CN111487960A - A Path Planning Method for Mobile Robots Based on Positioning Capability Estimation - Google Patents

Abstract

The invention relates to a mobile robot path planning method based on positioning capability estimation, which not only considers the influences of the distance of a path navigated to a target point, dynamic obstacles in the environment and the like on the path planning, but also considers the influences of different environmental characteristics and map noise on the robot positioning by fusing the positioning capability in a global map cost function. The path obtained by the invention can ensure that the mobile robot can navigate in an area with relatively strong positioning capability. Compared with the traditional path planning algorithm, the method can guide the robot to bypass dynamic obstacles or inform the robot that the robot cannot pass and needs to stop for waiting, and also enhances the positioning precision and the robustness in the navigation process of the mobile robot, thereby improving the moving performance. And finally, the algorithm is optimized by combining the actual navigation requirement of the robot, so that the algorithm can meet the real-time requirement when the path planning is carried out aiming at the large-range environment.

Description

A Path Planning Method for Mobile Robots Based on Positioning Capability Estimation

technical field

The invention relates to a mobile robot path planning method based on positioning capability estimation, which studies how to improve the path planning algorithm on the basis of considering the positioning capability of the mobile robot in different navigation areas, and belongs to the technical field of mobile robot navigation planning.

Background technique

In the future, mobile robots will work in increasingly complex dynamic and unstructured natural environments. If the positioning accuracy of mobile robots is reduced or even lost, they will not be able to continue to effectively perform autonomous navigation tasks. In traditional path planning, it is not enough to consider only the shortest path without collision. Because if only the shortest path is pursued without considering the influence of complex environment, it will bring more risks to navigation, such as the inability to bypass areas with poor locatability, the loss of the robot position, and the failure of the mission. To this end, the present invention proposes an algorithm for analyzing the positioning capability of the mobile robot and optimizing the path planning by using the analysis result.

SUMMARY OF THE INVENTION

The technical problem solved by the present invention is: to overcome the deficiencies of the prior art, a mobile robot path planning method based on positioning capability estimation is proposed, which solves the influence of map features or mapping noise on robot positioning, that is, positioning capability, It can also provide analytical solutions and introduce the analytical results into the path planning algorithm.

The technical solution of the present invention is:

First, Fisher's information matrix and CRB theorem are used to evaluate the impact of the known environment on positioning, and the impact is numerically represented, and after normalization, it is defined as positioning capability. Secondly, the influence of robot positioning ability, Euler distance, obstacle boundary and sensor information are jointly introduced into the gradient method cost allocation system, and then the path planning algorithm is given according to the new cost function. Finally, the algorithm is optimized according to the actual navigation requirements of the robot, so as to ensure that the algorithm can also meet the real-time requirements for path planning for a large-scale environment.

Specifically, the present invention proposes a path planning method for a mobile robot based on positioning capability estimation. The steps are as follows:

(1) Evaluate and calculate the positioning capability of the mobile robot;

(2) Constructing a path planning cost function;

(3) The path planning of the mobile robot is carried out by the path planning method based on the estimation of the positioning ability;

(4) Optimizing the real-time performance of the path planning method.

Further, the mobile robot uses an odometer and a laser rangefinder as sensors, and the map used is a probabilistic grid map.

Further, the step (1) evaluates and calculates the positioning capability of the mobile robot, specifically:

(1.1) Use Fisher's information matrix and CRB theorem to estimate the positioning capability of the mobile robot in the known environment; and then based on the characteristics of the probability grid map, discretize the above effects to obtain the positioning capability matrix;

(1.2) Use the determinant of the positioning ability matrix to reflect the numerical positioning ability of the robot under the pose.

Further, the positioning capability estimation is defined as:

Fisher's information matrix and CRB theorem are used to evaluate the impact of the known environment on positioning, and the impact is numerically represented, and after normalization, it is defined as positioning ability; positioning ability reflects the environmental characteristics and map noise on robot positioning. impact on accuracy and robustness.

Further, the positioning capability matrix is specifically:

是在该点处第i条激光射线的期望测量距离，σ_i ²为对应的测量方差，N_o表示激光射线数，

为机器人沿着各个坐标轴方向移动Δ距离后，激光测距仪的观测变化。Among them, p=(x, y, θ) represents the position and attitude angle of the robot on the map,

is the expected measurement distance of the i-th laser ray at this point, σ _i ² is the corresponding measurement variance, N _o represents the number of laser rays,

It is the observation change of the laser range finder after the robot moves Δ distance along each coordinate axis direction.

的行列式反映移动机器人在位姿p下的数值化定位能力，即：Positioning Capability Matrix

The determinant reflects the numerical positioning ability of the mobile robot under the pose p, namely:

为

的特征根，L(p)数值越大，表示机器人在当前位姿下可以获取的有益定位的信息越多，该位置所对应的路径代价越小；反之，可以获取的有益定位信息就越少，路径代价就越大。in,

for

The larger the value of L(p), the more useful positioning information the robot can obtain under the current pose, and the smaller the path cost corresponding to the position; on the contrary, the less useful positioning information can be obtained. , the higher the path cost.

Further, for any path P on the map space, it consists of several discrete points p _i , and the cost function C is defined as:

Among them, the cost O represents the influence of the obstacle when the robot moves to the adjacent point p _i+1 , and the cost of passing the point closer to the boundary of the obstacle is greater; the cost D represents that the robot moves to the adjacent point p _i+ When the distance is ₁ , the influence of the distance, the longer the moving distance, the greater the cost; the cost L represents the influence of the positioning ability when the robot moves to the adjacent point p _i+1 , and the point with the weaker positioning ability is the greater the cost; In the above-mentioned situations, otherwise the cost will be lower.

Further, follow the steps below for path planning:

(3.1) Initialize the cost function value of each location point on the map: the initial cost at the target point is set to 0, and the initial cost at other points is set to infinity;

(3.2) Traverse from the target point: establish a list to be traversed, take the target point as the first point to be traversed, and put it into the list to be traversed. According to the principle of first-in, first-out, the relative value of each point in the traversed list is treated. Neighbors are detected;

(3.3) For the point _pi taken from the list to be traversed, there are 8 adjacent points pi ₊₁ around it, calculate the cost from the target point to the 8 adjacent points pi ₊₁ respectively, for any phase For the adjacent p _i+1 point, if the calculated cost is less than the currently allocated cost of this point, it means that there is a path with a lower cost from this point to the target point, then, replace the original point with a smaller cost At the same time, put the point that has undergone cost replacement into the list to be traversed, and it is necessary to re-traverse and detect the path passing through the point;

(3.4) If and only if the list to be traversed is empty, the algorithm jumps out of the loop.

Further, after the algorithm completes the traversal detection, the cost function value assigned to each point is the minimum cost of the path from the point to the target point; starting from the initial point of the robot, backtracking, according to the direction in which the cost function value decreases the fastest, find A series of continuous points can eventually be traced back to the target point, and the trajectory formed by these continuous points is the planned optimal path.

Further, the step (4) optimizes the real-time performance of the path planning method, specifically: on the updated map, retraverse the cost caused by the obstacle boundary, and fuse the sensor information and analyze the obstacles. In the process of re-traversing the cost caused by the boundary, according to the size and moving speed of the robot, set a square area with the robot as the center, and re-traverse the cost caused by the obstacle boundary only in this area; When it is outside the set area, its influence does not need to be introduced into the cost, that is, the path planning is not affected; on the contrary, when the obstacle is within the set area, its influence is introduced into the cost, that is, the planned path will bypass the obstacle. .

Compared with the prior art, the present invention has the following advantages:

(1) Compared with the traditional path planning algorithm, this algorithm can not only guide the robot to avoid dynamic obstacles, or notify the robot that it cannot pass and need to stop and wait, but also can effectively ensure that the robot can navigate and move in an area with strong positioning ability, thereby The positioning accuracy and robustness in the navigation process of the mobile robot are enhanced, and the mobile performance is improved. At the same time, the algorithm is optimized in combination with the actual navigation requirements of the robot to ensure that the algorithm can also meet the real-time requirements for path planning for a large-scale environment.

(2) The method of the present invention considers the influence of different environmental features and map noise on the robot positioning by integrating the positioning capability in the global map cost function.

(3) The method of the present invention also considers the influence of the distance of the route to the target point on the route planning.

(4) The method of the present invention also considers the influence of dynamic obstacles in the environment on path planning.

(5) The method of the present invention uses Fisher's information matrix and CRB theorem to evaluate the impact of the known environment on positioning, and numerically expresses the impact, and after normalization, it is defined as positioning capability. The localization ability reflects the influence of environmental features and map noise on the robot's localization accuracy and robustness.

(6) The method of the present invention optimizes the algorithm in combination with the actual navigation requirements of the robot, ensuring that the algorithm can also meet the real-time requirements when performing path planning for a large-scale environment. There is no substantial danger to the robot. Therefore, in the process of considering the influence of dynamic obstacles in the environment on the path planning, according to the size, moving speed, control cycle, etc. of the robot, the robot is the center to set a Map area, the algorithm only considers the influence of dynamic obstacles in this area.

Description of drawings

Fig. 1 is the overall scheme diagram of the algorithm of the present invention;

2 is a schematic diagram of a certain point and its adjacent points based on a grid map of the present invention;

3 is a schematic diagram of the planning algorithm steps of the present invention;

FIG. 4 is a schematic diagram of the result of the improved path planning algorithm of the present invention.

Detailed ways

The invention relates to a mobile robot path planning method based on positioning capability estimation. The positioning capability reflects the influence of environmental characteristics and map noise on the positioning accuracy and robustness. The present invention not only considers the influence of the distance of the path to the target point, the dynamic obstacles in the environment, etc. on the path planning. The global map cost function integrates the localization capability, and also considers the impact of different environmental features and map noise on robot localization. The path obtained by the present invention can ensure that the mobile robot can navigate in an area with relatively strong positioning capability. Compared with the traditional path planning algorithm, it can not only guide the robot to avoid dynamic obstacles, or notify the robot that it cannot pass and need to stop and wait, but also enhance the positioning accuracy and robustness of the mobile robot during the navigation process, thereby improving the mobile performance. Finally, the algorithm is optimized according to the actual navigation requirements of the robot, so as to ensure that the algorithm can also meet the real-time requirements for path planning for a large-scale environment.

As shown in FIG. 1 , a mobile robot path planning method based on positioning capability estimation proposed by the present invention, the steps are as follows:

(1) Evaluate and calculate the positioning capability of the mobile robot;

(2) Constructing a path planning cost function;

(3) The path planning of the mobile robot is carried out by the path planning method based on the estimation of the positioning ability;

(4) Optimizing the real-time performance of the path planning method.

1. Evaluate and quantify the positioning capability of mobile robots

The mobile robot of the present invention uses an odometer and a laser rangefinder as sensors, and the used map is a probability grid map. Firstly, Fisher's information matrix and CRB theorem are used to evaluate the influence of the known environment on the positioning of mobile robots; then, based on the characteristics of the probability grid map, the above influences are discretized, and the positioning capability matrix can be obtained:

是在该点处第i条激光射线的期望测量距离，σ_i ²为对应的测量方差，N_o表示激光射线数，

为机器人沿着各个坐标轴方向移动Δ距离后，激光测距仪的观测变化。定位能力矩阵本质上就是对移动机器人在已知地图上、某一位姿下的期望观测信息给定位带来的不确定性影响进行评估。Among them, p=(x, y, θ) represents the position and attitude angle of the robot on the map,

is the expected measurement distance of the i-th laser ray at this point, σ _i ² is the corresponding measurement variance, N _o represents the number of laser rays,

It is the observation change of the laser range finder after the robot moves Δ distance along each coordinate axis direction. The positioning capability matrix is essentially to evaluate the uncertainty impact of the mobile robot's expected observation information on a known map and a certain pose on the positioning.

We know that the covariance ellipse can be used to represent the uncertainty of variables, so it can also be used to evaluate the uncertainty of positioning. For example, a positioning covariance ellipse with a larger area indicates that the robot's possible poses are distributed in a larger area, that is, its positioning uncertainty is high, and vice versa. Of course, in order to draw the covariance ellipse, the covariance matrix must first be obtained. According to the CRB theorem, the desired positioning covariance matrix can be estimated by the positioning capability matrix (1):

Furthermore, the long and short axes of the positioning covariance ellipse can be solved according to equation (2). Taking the x-y covariance ellipse as an example, its semimajor axis is:

Its short semi-axis is:

The orientation angle of the major semi-axis is:

和_qxy是符号相反的，则令

令

和

为

的特征根，则：Among them, if

and _qxy are opposite in sign, then let

make

and

for

The characteristic root of , then:

The area size of the covariance ellipse can measure the uncertainty of positioning, and the area of the ellipse is proportional to the characteristic root:

的行列式来衡量当前位姿下x-y方向上的定位精度。据此类推，x-θ和y-θ协方差椭圆也可以根据公式(2)-(5)求解出来。综上，我们使用定位能力矩阵的行列式来反映机器人在位姿p下的数值化定位能力，即：Therefore, we can use

The determinant of is to measure the positioning accuracy in the xy direction under the current pose. By analogy, the x-θ and y-θ covariance ellipses can also be solved according to formulas (2)-(5). In summary, we use the determinant of the positioning ability matrix to reflect the numerical positioning ability of the robot under the pose p, namely:

为

的特征根。Among them, the larger the value of L(p), the more useful positioning information the robot can obtain under the current pose, and the smaller the path cost corresponding to the position; on the contrary, the less useful positioning information can be obtained, and the path the greater the cost, among which,

for

characteristic root of .

2. Construction of path planning cost function

Usually two-dimensional laser rangefinders are not omnidirectional sensors. Therefore, when the robot faces different directions, the positioning ability at the same position is also different. Therefore, in the process of path planning, the robot positioning ability fused by the cost function needs to be calculated in combination with the orientation of the robot. In order to ensure the real-time performance of the algorithm, the positioning capability can be pre-calculated and pre-stored according to the known map. During the actual execution of the algorithm, the corresponding value can be directly read by the look-up table method (during pre-calculation, the orientation of the robot is reflected by the map move from a previous grid to an adjacent grid).

Specifically, for any path P on the map space (composed of several discrete points p _i ), we define its cost function C as:

Among them, the cost O represents the influence of the obstacle when the robot moves to the adjacent point p _i+1 . Usually, the cost of passing the point closer to the boundary of the obstacle is greater; the cost D represents that the robot moves to the adjacent point p i+1. When the point p _i+1 , the influence of the distance, in general, the longer the moving distance, the greater the cost; the cost L represents the influence of the positioning ability when the robot moves to the adjacent point p _i+1 , through the positioning ability The weaker the point, the more expensive it is. For the above-mentioned situations, the cost is smaller on the contrary.

3. Path Planning Algorithm Based on Positioning Capability Estimation

During the navigation process, the robot will face the occlusion of dynamic obstacles, and should detour these dynamic obstacles during path planning. However, dynamic obstacles do not exist on the initial map, so it is necessary to update the map in real time according to sensor information, and calculate the cost O caused by the obstacle boundary at each grid (ie, location point), here On the basis, follow the steps below to plan the path.

First, initialize the cost function value of each location point on the map: the initial cost at the target point is set to 0, and the initial cost at other points is set to infinity.

Second, traverse from the target point. A list to be traversed is established, and the target point is taken as the first point to be traversed, and placed in the list to be traversed. According to the principle of first-in, first-out, the adjacent points of each point in the to-be-traversed list are detected.

Third, for the point pi taken out from the list to be _traversed , it can be known that there are 8 adjacent points pi ₊₁ around it, as shown in FIG. 2 . According to formula (9), the cost from the target point to 8 adjacent points p _i+1 is calculated respectively. For any adjacent p _i+1 point, if the calculated cost is less than the currently allocated cost of the point, then It means that there is a path with a lower cost from this point to the target point, then, replace the original cost of this point with a smaller cost; at the same time, put the point whose cost has been replaced into the list to be traversed, and it is necessary to Path retraversal detection at this point.

During the traversal process, the calculation of the cost O and the cost D is the same as the traditional method, and will not be repeated. The cost L is calculated according to formula (8). It should be noted that for non-omnidirectional sensors, the positioning capability is directional.

Fourth, the algorithm breaks out of the loop if and only if the list to be traversed is empty.

Finally, it can be seen that when the algorithm completes the traversal detection, the cost function value assigned to each point is the minimum cost of the path from this point to the target point. Start backtracking from the initial point of the robot. According to the direction in which the cost function value decreases the fastest, a series of continuous points can be found, and finally the target point can be backtracked. The trajectory formed by these continuous points is the planned optimal (minimum cost) path. A simple, visual example is shown in Figure 3.

Fourth, the real-time improvement of the algorithm

It can be seen from the above path planning algorithm based on positioning capability estimation that with the fusion of sensor information, it is necessary to re-traverse the cost caused by the obstacle boundary on the updated map, which requires the real-time performance of the algorithm.

For this reason, in the process of fusing sensor information and retraversing the cost caused by the obstacle boundary, according to the size and moving speed of the robot, a square area is set with the robot as the center, and the obstacle boundary is only in this area. The resulting cost is re-traversed. When the obstacle is outside the set area, although the robot can observe the existence of the obstacle, its influence does not need to be introduced into the cost, so the path planning will not be affected; on the contrary, when the obstacle is within the set area , its influence will be introduced into the cost, so the planned path will bypass the obstacle, as shown in Figure 4.

V. Conclusion

Compared with the traditional path planning algorithm, this algorithm can not only guide the robot to avoid dynamic obstacles, or inform the robot that it cannot pass and need to stop and wait, but also can effectively ensure that the robot can navigate and move in the area with strong positioning ability, thus enhancing the movement. The positioning accuracy and robustness in the robot navigation process improve the mobile performance. At the same time, the algorithm is optimized in combination with the actual navigation requirements of the robot to ensure that the algorithm can also meet the real-time requirements for path planning for a large-scale environment.

Contents that are not described in detail in the specification of the present invention belong to the prior art known to those skilled in the art.

Claims (10)

1. A mobile robot path planning method based on positioning capability estimation is characterized by comprising the following steps:

(1) evaluating and calculating the positioning capacity of the mobile robot;

(2) constructing a path planning cost function;

(3) performing path planning of the mobile robot through a path planning method based on positioning capacity estimation;

(4) and optimizing the real-time performance of the path planning method.

2. The method for planning the path of the mobile robot based on the estimation of the positioning capability of claim 1, wherein: the mobile robot uses an odometer and a laser range finder as sensors, and the map used is a probability grid map.

3. The method for planning the path of the mobile robot based on the estimation of the positioning capability of claim 1, wherein: the step (1) of evaluating and calculating the positioning capability of the mobile robot specifically comprises the following steps:

(1.1) estimating the positioning capacity of the mobile robot in the known environment by using a Fisher's information matrix and a CRB theorem; discretizing the influence based on the characteristics of the probability grid map to obtain a positioning capacity matrix;

and (1.2) reflecting the numerical positioning capacity of the robot in the position posture by using a determinant of the positioning capacity matrix.

4. The method of claim 3, wherein the method comprises: the location capability estimate defined as:

estimating the influence of the known environment on positioning by using a Fisher's information matrix and a CRB theorem, numerically expressing the influence, and defining the influence as positioning capacity after normalization processing; the positioning capability reflects the influence of environmental characteristics and map noise on the positioning accuracy and robustness of the robot.

5. The method of claim 3, wherein the method comprises: the positioning capability matrix is specifically:

where p ═ x, y, θ denotes the position and attitude angle of the robot on the map,

is the desired measured distance, σ, of the ith laser ray at that point_i ²For the corresponding measured variance, N_oThe number of laser beams is shown as,

the observation change of the laser range finder is observed after the robot moves a delta distance along each coordinate axis direction.

6. A location based capability as claimed in claim 5The estimated mobile robot path planning method is characterized by comprising the following steps: using location capability matrices

The determinant of (a) reflects the numerical positioning capability of the mobile robot under the position attitude p, namely:

wherein,

is composed of

The greater the L (p) value is, the more useful positioning information the robot can acquire in the current pose is, the smaller the path cost corresponding to the position is, otherwise, the less useful positioning information the robot can acquire is, the larger the path cost is.

7. The method for planning the path of the mobile robot based on the estimation of the positioning capability of claim 1, wherein: for any path P in the map space, a plurality of discrete points P_iThe cost function C is defined as:

wherein the cost O represents when the robot moves to the adjacent point p_i+1In time, the influence brought by the obstacle is increased by the point cost which is closer to the boundary of the obstacle; the cost D represents that the robot moves to the adjacent point p_i+1The longer the moving distance is, the larger the cost is, and the cost L represents that the robot moves to the adjacent point p_i+1In time, the point cost is higher when the positioning capability is weaker; for the above cases, the cost is smaller in the opposite case.

8. The method for planning the path of the mobile robot based on the estimation of the positioning capability of claim 1, wherein: path planning is performed according to the following steps:

(3.1) initializing a cost function value of each position point on the map: setting the initial cost at the target point to be 0, and setting the initial costs at other points to be infinite;

(3.2) traversal is performed starting from the target point: establishing a list to be traversed, taking a target point as a first point to be traversed, putting the target point into the list to be traversed, and detecting adjacent points of all points in the list to be traversed according to a first-in first-out principle;

(3.3) for the point p taken from the list to be traversed_iWith 8 adjacent points p around it_i+1Respectively calculating from the target point to 8 adjacent points p_i+1For any one adjacent p_i+1If the calculated cost is less than the cost which is distributed at present, the point is indicated to have a path with lower cost when reaching the target point, and then the original cost of the point is replaced by the lower cost; meanwhile, the points subjected to cost replacement are put into a list to be traversed, and the paths passing through the points need to be traversed and detected again;

(3.4) if and only if the list to be traversed is empty, the algorithm jumps out of the loop.

9. The method of claim 8, wherein the method comprises: when the algorithm finishes traversal detection, the cost function values distributed by each point are the minimum cost of the path from the point to the target point; and backtracking is carried out from the initial point of the robot, a series of continuous points are found according to the direction that the cost function value is reduced most quickly, the target point can be backtracked finally, and the track formed by the continuous points is the planned optimal path.

10. The method for planning the path of the mobile robot based on the estimation of the positioning capability of claim 1, wherein: the step (4) of optimizing the real-time performance of the path planning method specifically comprises the following steps: on the updated map, re-traversing the cost caused by the barrier boundary, and in the process of fusing sensor information and re-traversing the cost caused by the barrier boundary, setting a square area by taking a robot as a center according to the size and the moving speed of the robot, and re-traversing the cost caused by the barrier boundary only in the area; when the obstacle is outside the set area, the influence of the obstacle does not need to be introduced into the cost, namely, the path planning is not influenced; conversely, when an obstacle is within the set area, its effect is introduced into the cost, i.e., the planned path will bypass the obstacle.

Images