CN112462769A - Robot positioning method and device, computer readable storage medium and robot - Google Patents

Abstract

The present application belongs to the field of robotics, and in particular, relates to a robot positioning method, a device, a computer-readable storage medium, and a robot. The method acquires the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculates the pose of the robot according to the first odometer data and the second odometer data Increment; obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment; obtain the robot at the current moment The laser data collected at the current moment is transformed into a grid map according to the second pose to obtain the transformed laser data; according to the transformed laser data and the grid map The second pose is iteratively updated with the obstacle distance information, and the positioning result of the robot at the current moment is obtained, which significantly improves the positioning accuracy.

Description

Robot positioning method, device, computer-readable storage medium, and robot

technical field

The present application belongs to the field of robotics, and in particular, relates to a robot positioning method, a device, a computer-readable storage medium, and a robot.

Background technique

The robot navigation and positioning technology is based on the map that the robot has established, obtains the laser data of the currently detected obstacles through the sensor, and locates the robot's pose according to the laser data and the obstacles on the map, so that the subsequent robot can move. However, in the existing robot positioning methods, the accuracy of the positioning results is often low, and it is difficult to adapt to various complex and diverse application environments.

SUMMARY OF THE INVENTION

In view of this, the embodiments of the present application provide a robot positioning method, device, computer-readable storage medium and robot, in order to solve the problem that the accuracy of the positioning results in the existing methods is often low, and it is difficult to adapt to various complex and diverse application environments. question.

A first aspect of the embodiments of the present application provides a method for positioning a robot, which may include:

Obtain the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculate the pose increment of the robot according to the first odometer data and the second odometer data;

Obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment;

Acquiring the laser data collected by the robot at the current moment, and transforming the laser data into a grid map according to the second pose to obtain the transformed laser data;

The second pose is iteratively updated according to the transformed laser data and the obstacle distance information in the grid map to obtain the positioning result of the robot at the current moment.

Further, the transforming the laser data into a grid map according to the second pose may include:

Transform the laser data into the raster map according to:

为所述第二位姿中的姿态角，

为第i个激光点的位置坐标，S_i(ξ)为第i个激光点变换至所述栅格地图中的位置坐标，i为所述激光数据中的激光点的序号，1≤i≤n，n为所述激光数据中的激光点的数目。Among them, ξ is the second pose, (p _x , p _y ) is the position coordinate in the second pose,

is the attitude angle in the second pose,

is the position coordinate of the i-th laser point, S _i (ξ) is the position coordinate of the i-th laser point transformed into the grid map, i is the serial number of the laser point in the laser data, 1≤i≤ n, n is the number of laser spots in the laser data.

Further, the iteratively updating the second pose according to the transformed laser data and the obstacle distance information in the grid map may include:

Calculate the pose update amount according to the transformed laser data and the obstacle distance information in the grid map;

updating the second pose according to the pose update amount to obtain an updated second pose;

If the pose update amount is greater than the preset threshold, return to the step of calculating the pose update amount according to the transformed laser data and the obstacle distance information in the grid map and its subsequent steps, until the pose update amount is less than or equal to the threshold;

If the pose update amount is less than or equal to the threshold, the second pose after the last update is determined as the positioning result of the robot at the current moment.

Further, the calculation of the pose update amount according to the transformed laser data and the obstacle distance information in the grid map may include:

The pose update amount is calculated according to the following formula:

Δξ=H ^-1 dTr

Wherein, M(S _i (ξ)) is a preset function regarding the distance between Si ₍ ξ) and the nearest obstacle, and Δξ is the pose update amount.

Further, the preset function can be set according to the following formula:

为与S_i(ξ)距离最近的障碍物在所述栅格地图中的位置坐标，λ为预设的系数。in,

is the position coordinate of the obstacle closest to S _i (ξ) in the grid map, and λ is a preset coefficient.

Further, the updating of the second pose according to the pose update amount may include:

The second pose is updated according to the following formula:

ξ′=ξ+Δξ

Wherein, ξ is the second pose, Δξ is the pose update amount, and ξ′ is the updated second pose.

Further, the setting process of the obstacle distance information in the grid map may include:

Use the region growing method to traverse each grid in the grid map, insert the index of each grid into the preset priority queue in turn, and determine that the obstacle closest to the grid is in the grid map until the priority queue is empty, a grid map with obstacle distance information is obtained.

A second aspect of the embodiments of the present application provides a robot positioning device, which may include:

The pose increment calculation module is used to obtain the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculate according to the first odometer data and the second odometer data the pose increment of the robot;

a pose calculation module, configured to obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment;

a data transformation module, configured to acquire the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data;

A pose iterative update module is configured to iteratively update the second pose according to the transformed laser data and the obstacle distance information in the grid map, so as to obtain the positioning result of the robot at the current moment.

Further, the data transformation module is specifically configured to transform the laser data into the grid map according to the following formula:

为所述第二位姿中的姿态角，

为第i个激光点的位置坐标，S_i(ξ)为第i个激光点变换至所述栅格地图中的位置坐标，i为所述激光数据中的激光点的序号，1≤i≤n，n为所述激光数据中的激光点的数目。Among them, ξ is the second pose, (p _x , p _y ) is the position coordinate in the second pose,

is the attitude angle in the second pose,

is the position coordinate of the i-th laser point, S _i (ξ) is the position coordinate of the i-th laser point transformed into the grid map, i is the serial number of the laser point in the laser data, 1≤i≤ n, n is the number of laser spots in the laser data.

Further, the pose iterative update module may include:

a pose update amount calculation unit, configured to calculate the pose update amount according to the transformed laser data and the obstacle distance information in the grid map;

a pose updating unit, configured to update the second pose according to the pose update amount to obtain an updated second pose;

A positioning result determination unit, configured to determine the second pose after the last update as the positioning result of the robot at the current moment if the update amount of the pose is less than or equal to the threshold.

Further, the pose update amount calculation unit is specifically configured to calculate the pose update amount according to the following formula:

Δξ=H ^-1 dTr

Wherein, M(S _i (ξ)) is a preset function regarding the distance between Si ₍ ξ) and the nearest obstacle, and Δξ is the pose update amount.

The preset function can be set according to the following formula:

为与S_i(ξ)距离最近的障碍物在所述栅格地图中的位置坐标，λ为预设的系数。in,

is the position coordinate of the obstacle closest to S _i (ξ) in the grid map, and λ is a preset coefficient.

Further, the pose updating unit is specifically configured to update the second pose according to the following formula:

ξ′=ξ+Δξ

Wherein, ξ is the second pose, Δξ is the pose update amount, and ξ′ is the updated second pose.

Further, the robot positioning device may also include:

The obstacle distance information setting module is used to traverse each grid in the grid map by using the region growing method, insert the index of each grid into the preset priority queue in turn, and determine the closest distance to the grid The position coordinates of the obstacles in the grid map are obtained until the priority queue is empty, and a grid map with obstacle distance information is obtained.

A third aspect of the embodiments of the present application provides a computer-readable storage medium, where the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, implements the steps of any of the foregoing robot positioning methods.

A fourth aspect of the embodiments of the present application provides a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the computer program when the processor executes the computer program. The steps of any one of the above robot positioning methods.

A fifth aspect of the embodiments of the present application provides a computer program product, which, when the computer program product runs on a robot, causes the robot to perform the steps of any one of the above robot positioning methods.

Compared with the prior art, the embodiment of the present application has the following beneficial effects: the embodiment of the present application obtains the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and according to the first odometer data Calculate the pose increment of the robot according to the odometer data and the second odometer data; obtain the first pose of the robot at the last moment, and calculate the pose increment based on the first pose and the pose increment The second pose of the robot at the current moment; obtain the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data ; Iteratively update the second pose according to the transformed laser data and the obstacle distance information in the grid map to obtain the positioning result of the robot at the current moment. Through the embodiments of the present application, the positioning accuracy of the robot during navigation and positioning can be significantly improved, so that the robot has more robust and robust positioning performance in various complex and diverse application environments.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present application more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only for the present application. In some embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.

FIG. 1 is a flowchart of an embodiment of a robot positioning method in an embodiment of the application;

Fig. 2 is a schematic flow chart of iteratively updating the pose of the robot;

3 is a structural diagram of an embodiment of a robot positioning device in an embodiment of the application;

FIG. 4 is a schematic block diagram of a robot in an embodiment of the present application.

Detailed ways

In order to make the purpose, features and advantages of the invention of the present application more obvious and understandable, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the following The described embodiments are only some, but not all, embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

It is to be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described feature, integer, step, operation, element and/or component, but does not exclude one or more other features , whole, step, operation, element, component and/or the presence or addition of a collection thereof.

It should also be understood that the terminology used in the specification of the application herein is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural unless the context clearly dictates otherwise.

It should also be further understood that, as used in this specification and the appended claims, the term "and/or" refers to and including any and all possible combinations of one or more of the associated listed items .

As used in this specification and the appended claims, the term "if" may be contextually interpreted as "when" or "once" or "in response to determining" or "in response to detecting" . Similarly, the phrases "if it is determined" or "if the [described condition or event] is detected" may be interpreted, depending on the context, to mean "once it is determined" or "in response to the determination" or "once the [described condition or event] is detected. ]" or "in response to detection of the [described condition or event]".

In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.

In a specific implementation of the embodiment of the present application, a correlation matching or a matching method based on the gradient of an obstacle probability map may be used to locate the robot.

The correlation matching positioning method is to calculate the matching scores of the laser points and the obstacles on the map under each array pose along the array in each direction of the pose near the initial pose estimate value of the robot, and calculate the highest matching score obtained. The pose corresponding to the score is used as the current pose of the robot. This method is similar to the exhaustive traversal method, with a large amount of calculation, and the positioning accuracy depends on the accuracy of the above-mentioned arrays in each direction.

The matching and positioning method based on the probability map gradient of the obstacle is to calculate the gradient of the probability change of the obstacle on the map where the laser point is located, and iteratively move along the gradient direction. Since the change of the probability gradient of the map obstacle only has a large change near the obstacle, after a certain distance from the obstacle, the change of the probability gradient of the map obstacle is very small or even zero. The localization method of gradient change cannot output high-precision localization results. Therefore, this method relies on the initial pose accuracy of the robot, and it is necessary to make the laser point as close to the map obstacles as possible.

In another specific implementation of the embodiment of the present application, a residual equation based on the distance between the laser point and the nearest obstacle may be constructed to optimize the robot pose. The robot pose optimization algorithm can perform higher-precision robot positioning. At the same time, compared with other laser positioning algorithms, this method overcomes the problem that the positioning algorithm is sensitive to the robot's initial pose and the problem of a large amount of calculation, and the positioning operation is more stable. Efficient. In this process, the nearest obstacle information at any position in the map can be quickly and efficiently calculated based on the combination of priority queue and region growing method.

Referring to FIG. 1, an embodiment of a robot positioning method in the embodiment of the present application may include:

Step S101: Obtain the first odometry data of the robot at the previous moment and the second odometer data at the current moment, and calculate the pose of the robot according to the first odometer data and the second odometer data Increment.

The odometer data used in the embodiments of the present application may be data measured by a wheeled odometer of the robot, or data measured by an inertial measurement unit (Inertial Measurement Unit, IMU) of the robot.

The robot may perform an update of the positioning result at regular intervals. Here, the timings for updating the positioning result are sequentially recorded as: time 1, time 2, ..., time t-1, time t, time t+ 1, ..., and so on. If the current time is time t, the previous time is time t-1, and the odometer data of the robot at the previous time (denoted as the first odometry data) and the odometer data at the current time ( Denoted as the second odometer data), the pose increment of the robot can be estimated according to the difference between the two.

Step S102: Obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment.

Specifically, the second pose can be calculated according to the following formula:

ξ _t =ξ _t-1 +Δξ _t-1,t

Among them, ξ _t-1 is the pose of the robot at the previous moment, that is, the first pose, Δξ _{t-1, t} is the pose increment, and ξ _t is the current moment of the robot. pose, that is, the second pose.

It should be noted that due to the existence of sensor error and robot pose error, the calculated ξ _t is only an estimated value, not an accurate value. It needs to be matched according to the laser data and the raster map to further optimize the robot pose , to get a more accurate pose.

Step S103: Acquire the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data.

The grid map divides the environment where the robot is located into a series of grids, and records the states of each grid in them, and the states can include occupied states (that is, occupied by obstacles), free states ( i.e. not occupied by obstacles) and unknown state.

The robot can detect the surrounding environment through lidar, and its working principle is to transmit a detection signal (laser) to the target, and then compare the received signal reflected from the target with the detection signal, and after proper processing, it can be obtained. information about the target, so as to detect, track and identify the target. Generally, the robot can collect laser data according to a preset data collection frequency, and collect a frame of laser data every certain time interval. The specific data collection frequency may be set according to the actual situation, which is not specifically limited in this embodiment of the present application.

After acquiring the laser data collected by the robot at the current moment, the laser data can be transformed into the grid map according to the following formula:

为所述第二位姿中的姿态角，

为第i个激光点的位置坐标，S_i(ξ)为第i个激光点变换至所述栅格地图中的位置坐标，i为所述激光数据中的激光点的序号，1≤i≤n，n为所述激光数据中的激光点的数目。根据上式将i从1遍历到n，即可得到所述激光数据中的各个激光点变换至所述栅格地图中的位置坐标，也即所述变换后的激光数据。Among them, ξ is the second pose, (p _x , p _y ) is the position coordinate in the second pose,

is the attitude angle in the second pose,

is the position coordinate of the i-th laser point, S _i (ξ) is the position coordinate of the i-th laser point transformed into the grid map, i is the serial number of the laser point in the laser data, 1≤i≤ n, n is the number of laser spots in the laser data. Traversing i from 1 to n according to the above formula, can obtain the position coordinates of each laser point in the laser data transformed to the grid map, that is, the transformed laser data.

Step S104: Iteratively update the second pose according to the transformed laser data and the obstacle distance information in the grid map to obtain the positioning result of the robot at the current moment.

As shown in FIG. 2, step S104 may specifically include the following processes:

Step S1041: Calculate the pose update amount according to the transformed laser data and the obstacle distance information in the grid map.

In order to perform matching between the laser data and the map more accurately and efficiently, in this embodiment of the present application, a residual equation based on the distance between the laser point and the nearest obstacle can be constructed to optimize the robot pose. In this process, the pose of the robot is iteratively calculated by the Gauss-Newton method, so that the distance sum of each laser point under the pose at the current moment to the nearest obstacle at the point in the map is the smallest. When all the laser points in the laser data coincide with the obstacles in the map (that is, the distance from the point to the nearest obstacle is 0), the pose at this time is the ideal pose of the robot.

Through the above analysis, in the embodiment of the present application, the following optimization function can be constructed, and the precise pose of the robot can be calculated by iteratively solving it:

Among them, M(S _i (ξ)) is a preset function about the distance between Si ₍ ξ) and the nearest obstacle, which can be set according to the following formula:

为与S_i(ξ)距离最近的障碍物在所述栅格地图中的位置坐标，λ为预设的系数，其具体取值可以根据实际情况进行设置，本申请实施例对此不做具体限定。

is the position coordinate of the obstacle closest to S _i (ξ) in the grid map, λ is a preset coefficient, and its specific value can be set according to the actual situation, which is not specified in the embodiment of the present application. limited.

When the distance is smaller, it can be considered that the laser data matches the map better, the pose of the robot is more accurate, and the error is closer to 0, namely:

Perform a first-order Taylor expansion on it, we can get:

可以通过对M(S_i(ξ))求导得到，

可以通过对M(S_i(ξ))求导得到。in,

It can be obtained by derivation of M(S _i (ξ)),

It can be obtained by derivation of M(S _i (ξ)).

Then, derivation of the above formula, we can get:

make:

Then, the pose update amount Δξ can be calculated according to the following formula:

Δξ=H ^-1 dTr

Step S1042: Update the second pose according to the pose update amount to obtain an updated second pose.

Specifically, the second pose can be updated according to the following formula:

ξ′=ξ+Δξ

Wherein, ξ′ is the updated second pose.

Step S1043, judging whether the pose update amount is greater than a preset threshold.

The specific value of the threshold may be set according to the actual situation, which is not specifically limited in this embodiment of the present application. If the pose update amount is greater than the threshold, return to step S1041 and its subsequent steps, that is, calculate the pose update amount again. It should be noted that at this time, the ξ used in the calculation process is replaced with the updated ξ′, iterative calculation is performed repeatedly until the pose update amount is less than or equal to the threshold; if the pose update amount is less than or equal to the threshold, the iterative process is ended, and the last update The second pose is determined as the positioning result of the robot at the current moment.

在本申请实施例中，可以结合采用优先队列和区域生长法来在所述栅格地图中进行障碍物距离信息的设置。即使用区域生长法遍历所述栅格地图中的每个栅格，将每个栅格的索引依次插入到预设的优先队列中，确定与该栅格距离最近的障碍物在所述栅格地图中的位置坐标，直至所述优先队列为空为止，得到具有障碍物距离信息的栅格地图。其中，优先队列会对插入的元素与队列中已有的元素进行比较并排序，并将插入到合适的位置中，本实施例中优先队列的元素即为栅格的索引，对新插入队列的元素，依次比较该元素与列队中已有元素的距最近障碍物的距离。距离越小，该元素在队列中的位置越靠前，越先出队列；距离越大，该元素在队列中的位置越靠后，越后出队列。这样保证从优先队列前端取出的元素是整个队列中距最近障碍物的距离最小的栅格的索引。In the above process, it is necessary to calculate the position coordinates of the obstacle closest to the laser point S _i (ξ) in the grid map

In this embodiment of the present application, a priority queue and an area growing method can be combined to set the obstacle distance information in the grid map. That is, use the region growing method to traverse each grid in the grid map, insert the index of each grid into the preset priority queue in turn, and determine that the obstacle closest to the grid is in the grid position coordinates in the map, until the priority queue is empty, a grid map with obstacle distance information is obtained. The priority queue compares and sorts the inserted elements with the existing elements in the queue, and inserts them into appropriate positions. In this embodiment, the element of the priority queue is the index of the grid. element, compare the distance of the element to the nearest obstacle in turn with the elements already in the queue. The smaller the distance, the higher the position of the element in the queue, and the first to be dequeued; the larger the distance, the further back the element is in the queue, and the later out of the queue. This ensures that the element taken from the front of the priority queue is the index of the grid with the smallest distance from the nearest obstacle in the entire queue.

The specific setting process of the obstacle distance information in the grid map is as follows:

(1) Traverse the grid map, initialize the distances (denoted as obs_distance) of all grids to the nearest obstacle (denoted as obs_distance) to a fixed value (denoted as max_distance), and set the grid map occupied by obstacles The index of the grid is denoted as a set: Obs_cells={obs_1, obs_2, ..., obs_i, ..., obs_n}.

(2) Set the distances of all elements in Obs_cells from the nearest obstacle to 0 in turn, ie: obs_i.obs_distance=0, and insert these elements into the priority queue (denoted as Priority_quene).

(3) Determine whether Priority_quene is empty;

If it is empty, end the setting process; if it is not empty, execute step (4).

(4) Take out the frontmost element of Priority_quene and use it as the current element (denoted as Current_cell).

(5) The elements of Current_cell in the grid map are obtained in sequence, and recorded as a set: Neighbor_cells={UpCell, DownCell, LeftCell, RightCell}.

(6) Judge whether each element in Neighbor_cells has been traversed;

If the traversal is not completed, step (7) is performed; if the traversal is completed, step (10) is performed.

(7) Select an element from Neighbor_cells, and judge whether the flag bit of the element (denoted as Neighbor_cell.is_marked) has been set to true (true);

If yes, go back to step (6); if not, go to step (8).

(8) Calculate the distance of the element from the nearest obstacle according to the following formula (denoted as Neighbor_cell.obs_distance):

Neighbor_cell.obs_distance=||Neighbor_cell.pose－Neighbor_cell.obs_pose||

Among them, Neighbor_cell.pose is the position coordinate of the element in the grid map, and Neighbor_cell.obs_pose is the position coordinate of the nearest obstacle to the element in the grid map. The obstacle is approximately equivalent to the obstacle closest to Current_cell, then there are:

Neighbor_cell.obs_pose=Current_cell.obs_pose

Among them, Current_cell.obs_pose is the position coordinate of the nearest obstacle to Current_cell in the grid map, which is a known quantity;

Combining the above two formulas, we have:

Neighbor_cell.obs_distance=||Neighbor_cell.pose-Current_cell.obs_pose||.

(9) The marked position of the element is true, that is: Neighbor_cell.is_marked=true, and the element is inserted into the Priority_quene; then it returns to step (6).

(10) Delete the Current_cell from the Priority_quene; then return to step (3) until the Priority_quene is empty, ending the entire setting process.

且这一设置过程只在初始化地图时计算一次，后续导航定位过程中地图信息不再更新，故对于地图中任一点，最近障碍物也不会发生变化，导航过程中该信息可直接查询得到，无需进行任何计算，极大提升了定位效率。同时无论机器人提供的初始位姿精度如何，都能得到所有激光点距最近障碍物的距离，并进行匹配优化，对机器人初始优化位姿不存在依赖，解决了基于概率地图梯度匹配算法的问题。并且由于本申请实施例采用了基于指数的最近距离评价函数M(S_i(ξ))，机器人的位姿的微小变化即可引起M(S_i(ξ))较大的输出变化，因此在迭代过程中可以得到更高的定位精度。Through the embodiment of the present application, given any grid coordinate S _i (ξ) in the map, the nearest obstacle grid corresponding to the grid can be quickly obtained

And this setting process is only calculated once when the map is initialized, and the map information will not be updated in the subsequent navigation and positioning process, so for any point on the map, the nearest obstacles will not change, and the information can be directly queried during the navigation process. There is no need to perform any calculations, which greatly improves the positioning efficiency. At the same time, regardless of the initial pose accuracy provided by the robot, the distance between all laser points and the nearest obstacle can be obtained, and the matching optimization is carried out. There is no dependence on the initial optimized pose of the robot, which solves the problem based on the probability map gradient matching algorithm. And since the embodiment of this application adopts the index-based closest distance evaluation function M(S _i (ξ)), a small change in the pose of the robot can cause a large output change in M(S _i (ξ)), so in Higher positioning accuracy can be obtained in the iterative process.

To sum up, the embodiment of the present application obtains the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculates according to the first odometer data and the second odometer data The pose increment of the robot; obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment ; Acquire the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data; According to the transformed laser data and all The second pose is iteratively updated using the obstacle distance information in the grid map to obtain the positioning result of the robot at the current moment. Through the embodiments of the present application, the positioning accuracy of the robot during navigation and positioning can be significantly improved, so that the robot has more robust and robust positioning performance in various complex and diverse application environments.

It should be understood that the size of the sequence numbers of the steps in the above embodiments does not mean the sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

Corresponding to the robot positioning method described in the above embodiments, FIG. 3 shows a structural diagram of an embodiment of a robot positioning device provided by an embodiment of the present application.

In this embodiment, a robot positioning device may include:

The pose increment calculation module 301 is used to obtain the first odometer data of the robot at the previous moment and the second odometer data of the current moment, and according to the first odometer data and the second odometer data calculating the pose increment of the robot;

The pose calculation module 302 is used to obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment;

A data transformation module 303, configured to acquire the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data;

The pose iterative update module 304 is used to iteratively update the second pose according to the transformed laser data and the obstacle distance information in the grid map, so as to obtain the positioning result of the robot at the current moment .

Further, the data transformation module is specifically configured to transform the laser data into the grid map according to the following formula:

为所述第二位姿中的姿态角，

为第i个激光点的位置坐标，S_i(ξ)为第i个激光点变换至所述栅格地图中的位置坐标，i为所述激光数据中的激光点的序号，1≤i≤n，n为所述激光数据中的激光点的数目。Among them, ξ is the second pose, (p _x , p _y ) is the position coordinate in the second pose,

is the attitude angle in the second pose,

is the position coordinate of the i-th laser point, S _i (ξ) is the position coordinate of the i-th laser point transformed into the grid map, i is the serial number of the laser point in the laser data, 1≤i≤ n, n is the number of laser spots in the laser data.

Further, the pose iterative update module may include:

a pose update amount calculation unit, configured to calculate the pose update amount according to the transformed laser data and the obstacle distance information in the grid map;

a pose updating unit, configured to update the second pose according to the pose update amount to obtain an updated second pose;

A positioning result determination unit, configured to determine the second pose after the last update as the positioning result of the robot at the current moment if the update amount of the pose is less than or equal to the threshold.

Further, the pose update amount calculation unit is specifically configured to calculate the pose update amount according to the following formula:

Δξ=H ^-1 dTr

Wherein, M(S _i (ξ)) is a preset function regarding the distance between Si ₍ ξ) and the nearest obstacle, and Δξ is the pose update amount.

The preset function can be set according to the following formula:

为与S_i(ξ)距离最近的障碍物在所述栅格地图中的位置坐标，λ为预设的系数。in,

is the position coordinate of the obstacle closest to S _i (ξ) in the grid map, and λ is a preset coefficient.

Further, the pose updating unit is specifically configured to update the second pose according to the following formula:

ξ′=ξ+Δξ

Wherein, ξ is the second pose, Δξ is the pose update amount, and ξ′ is the updated second pose.

Further, the robot positioning device may also include:

The obstacle distance information setting module is used to traverse each grid in the grid map by using the region growing method, insert the index of each grid into the preset priority queue in turn, and determine the closest distance to the grid The position coordinates of the obstacles in the grid map are obtained until the priority queue is empty, and a grid map with obstacle distance information is obtained.

Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described devices, modules and units can be referred to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

In the foregoing embodiments, the description of each embodiment has its own emphasis. For parts that are not described or described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

FIG. 4 shows a schematic block diagram of a robot provided by an embodiment of the present application. For convenience of description, only parts related to the embodiment of the present application are shown.

As shown in FIG. 4 , the robot 4 of this embodiment includes a processor 40 , a memory 41 , and a computer program 42 stored in the memory 41 and executable on the processor 40 . When the processor 40 executes the computer program 42 , the steps in each of the above embodiments of the robot positioning method are implemented, for example, steps S101 to S104 shown in FIG. 1 . Alternatively, when the processor 40 executes the computer program 42, the functions of the modules/units in each of the foregoing apparatus embodiments, such as the functions of the modules 301 to 304 shown in FIG. 3, are implemented.

Exemplarily, the computer program 42 may be divided into one or more modules/units, and the one or more modules/units are stored in the memory 41 and executed by the processor 40 to complete the this application. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program 42 in the robot 4 .

Those skilled in the art can understand that FIG. 4 is only an example of the robot 4, and does not constitute a limitation to the robot 4. It may include more or less components than the one shown in the figure, or combine some components, or different components, such as The robot 4 may also include input and output devices, network access devices, buses, and the like.

The processor 40 may be a central processing unit (Central Processing Unit, CPU), other general-purpose processors, a digital signal processor (Digital Signal Processor, DSP), an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), Field-Programmable Gate Array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, and the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The memory 41 may be an internal storage unit of the robot 4 , such as a hard disk or a memory of the robot 4 . The memory 41 may also be an external storage device of the robot 4, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card equipped on the robot 4, Flash card (Flash Card) and so on. Further, the memory 41 may also include both an internal storage unit of the robot 4 and an external storage device. The memory 41 is used to store the computer program and other programs and data required by the robot 4 . The memory 41 can also be used to temporarily store data that has been output or will be output.

Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. Module completion, that is, dividing the internal structure of the device into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated in one processing unit, or each unit may exist physically alone, or two or more units may be integrated in one unit, and the above-mentioned integrated units may adopt hardware. It can also be realized in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing from each other, and are not used to limit the protection scope of the present application. For the specific working process of the units and modules in the above-mentioned system, reference may be made to the corresponding process in the foregoing method embodiments, which will not be repeated here.

In the foregoing embodiments, the description of each embodiment has its own emphasis. For parts that are not described or described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of this application.

In the embodiments provided in this application, it should be understood that the disclosed apparatus/robot and method may be implemented in other ways. For example, the device/robot embodiments described above are only illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods, such as multiple units or Components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated modules/units, if implemented in the form of software functional units and sold or used as independent products, may be stored in a computer-readable storage medium. Based on this understanding, the present application can implement all or part of the processes in the methods of the above embodiments, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer When the program is executed by the processor, the steps of the foregoing method embodiments can be implemented. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form, and the like. The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM, Read-Only Memory) ), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium, etc. It should be noted that the content contained in the computer-readable storage medium may be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction, for example, in some jurisdictions, according to legislation and patent practice, computer-readable Storage media exclude electrical carrier signals and telecommunications signals.

The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the above-mentioned embodiments, those of ordinary skill in the art should understand that: it can still be used for the above-mentioned implementations. The technical solutions described in the examples are modified, or some technical features thereof are equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions in the embodiments of the application, and should be included in the within the scope of protection of this application.

Claims (10)

1. a robot positioning method, is characterized in that, comprises: Obtain the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculate the pose increment of the robot according to the first odometer data and the second odometer data; Obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment; Acquiring the laser data collected by the robot at the current moment, and transforming the laser data into a grid map according to the second pose to obtain the transformed laser data; The second pose is iteratively updated according to the transformed laser data and the obstacle distance information in the grid map to obtain the positioning result of the robot at the current moment. 2. The robot positioning method according to claim 1, wherein the transforming the laser data into a grid map according to the second pose comprises: Transform the laser data into the raster map according to:

Among them, ξ is the second pose, (p _x , p _y ) is the position coordinate in the second pose,

is the attitude angle in the second pose,

is the position coordinate of the i-th laser point, S _i (ξ) is the position coordinate of the i-th laser point transformed into the grid map, i is the serial number of the laser point in the laser data, 1≤i≤ n, n is the number of laser spots in the laser data. 3. The robot positioning method according to claim 2, wherein the second pose is iteratively updated according to the transformed laser data and the obstacle distance information in the grid map, include: Calculate the pose update amount according to the transformed laser data and the obstacle distance information in the grid map; updating the second pose according to the pose update amount to obtain an updated second pose; If the pose update amount is greater than the preset threshold, return to the step of calculating the pose update amount according to the transformed laser data and the obstacle distance information in the grid map and its subsequent steps, until the pose update amount is less than or equal to the threshold; If the pose update amount is less than or equal to the threshold, the second pose after the last update is determined as the positioning result of the robot at the current moment. 4. The robot positioning method according to claim 3, wherein the calculation of the pose update amount according to the transformed laser data and the obstacle distance information in the grid map comprises: The pose update amount is calculated according to the following formula: Δξ=H ^-1 dTr

Wherein, M(S _i (ξ)) is a preset function regarding the distance between Si ₍ ξ) and the nearest obstacle, and Δξ is the pose update amount. 5. The robot positioning method according to claim 4, wherein the preset function is set according to the following formula:

in,

is the position coordinate of the obstacle closest to S _i (ξ) in the grid map, and λ is a preset coefficient. 6. The robot positioning method according to claim 3, wherein the updating the second pose according to the pose update amount comprises: The second pose is updated according to the following formula: ξ′=ξ+Δξ Wherein, ξ is the second pose, Δξ is the pose update amount, and ξ′ is the updated second pose. 7. The robot positioning method according to any one of claims 1 to 6, wherein the setting process of the obstacle distance information in the grid map comprises: Use the region growing method to traverse each grid in the grid map, insert the index of each grid into the preset priority queue in turn, and determine that the obstacle closest to the grid is in the grid map until the priority queue is empty, a grid map with obstacle distance information is obtained. 8. A robot positioning device, characterized in that, comprising: The pose increment calculation module is used to obtain the first odometer data of the robot at the previous moment and the second odometer data at the current moment, and calculate according to the first odometer data and the second odometer data the pose increment of the robot; a pose calculation module, configured to obtain the first pose of the robot at the previous moment, and calculate the second pose of the robot at the current moment according to the first pose and the pose increment; a data transformation module, configured to acquire the laser data collected by the robot at the current moment, and transform the laser data into a grid map according to the second pose to obtain the transformed laser data; A pose iterative update module is configured to iteratively update the second pose according to the transformed laser data and the obstacle distance information in the grid map, so as to obtain the positioning result of the robot at the current moment. 9. A computer-readable storage medium storing a computer program, characterized in that, when the computer program is executed by a processor, the robot according to any one of claims 1 to 7 is implemented The steps of the positioning method. 10. A robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the computer program as claimed in claim 1 when the processor executes the computer program Steps of the robot positioning method described in any one of to 7.

Images