Mobile robot path planning method under situation delay

Abstract

The invention discloses a mobile robot path planning method under situation delay, and the method comprises the steps: firstly obtaining situation position information and moving target point position information under situation delay, and carrying out the estimation of an actual position of a mobile robot based on a Kalman filtering method; and carrying out mobile robot path planning based on the estimated actual position of the mobile robot, updating a new target position point coordinate of the mobile robot, updating a target position point coordinate based on an original target position point, and carrying out repeated iteration until the robot moves to a target end point. According to the scheme, a mobile robot path planning framework under the situation delay is provided, the situation of the mobile robot is estimated through a Kalman filtering method, path planning is carried out based on the estimated situation, and an effective solution can be provided for path planning of safe driving of the mobile robot under the situation delay.

Description

Technical Field

[0001] The invention belongs to the field of mobile robot path planning, and in particular relates to a mobile robot path planning method under situation delay. Background Art

[0002] Robot planning is an important research field in robotics and an important intersection of artificial intelligence, robotics, and control theory. The term "planning" is usually understood as planning the steps before taking action, and the basic task of robot planning is to require the robot to automatically generate a control program for the action sequence, motion path, and trajectory from the initial working state to the target state at a specific time and in a specific working area.

[0003] The existing mobile robot path planning methods have not yet solved the path planning problem of mobile robots under situation delay. Due to problems such as GPS signal problems, network communication delays, mobile robot system processing delays, and mobile robot sensor failures, the situation position information obtained by the mobile robot is compared with the actual situation position. There is a "delay" problem, that is, the position is distorted or inaccurate, which may cause the mobile robot to "collide" or "drive outside the feasible area" when performing movement under situation delay, which seriously affects the driving safety of the mobile robot. In addition, in an emergency situation of situation delay, the continuous movement of the mobile robot without relevant processing measures will affect a series of safety such as human life in the area. To this end, the present invention proposes a mobile robot path planning scheme under situation delay based on the Kalman filtering method. The scheme provides a mobile robot path planning framework for solving situation delay. The situation of the mobile robot is estimated by the Kalman filtering method, and the path is planned based on the estimated situation. It can provide an effective solution for the path planning of the mobile robot to drive safely under situation delay. Summary of the invention

[0004] In view of the above problems, the object of the present invention is to provide a mobile robot path planning method under situation delay.

[0005] The specific technical solution for achieving the purpose of the present invention is:

[0006] A mobile robot path planning method under situation delay comprises the following steps:

[0007] Step 1: Obtain the situation position information Z fed back by the k mobile robot sensor at the current moment under situation delay k , the mobile robot's moving target point location information Target k ;

[0008] Step 2: Estimate the “actual” position of the mobile robot based on the Kalman filter method

[0009] Step 3: Determine whether the mobile robot has reached the final target position. If so, end the task; otherwise, go to step 4.

[0010] Step 4: Determine whether the mobile robot has reached the target position. If so, go to step 5; otherwise, go to step 7.

[0011] Step 5: Based on the estimated actual position X of the mobile robot k Perform path planning for mobile robots;

[0012] Step 6: Update the coordinates of the new target position of the mobile robot based on the path planned in step 5;

[0013] Step 7: Update the coordinates of the target location point based on the original target location point;

[0014] Step 8: The mobile robot performs the movement, k=k+1, and goes to step 1 to execute the next moment process.

[0015] Compared with the prior art, the present invention has the following beneficial effects:

[0016] (1) Compared with the prior art, the solution of the present invention adopts the Kalman filtering method to predict and estimate the "actual" position of the mobile robot under situation delay, and plans the path of the mobile robot based on the predicted and estimated position, thereby providing a feasible solution for the path planning of the mobile robot under situation delay; it makes up for the problem of the lack of "real position" in the path planning of the mobile robot under situation delay, thereby avoiding the occurrence of "collision with people" and "collision with objects" as much as possible.

[0017] The present invention is further described below in conjunction with specific implementation modes. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a flow chart of the mobile robot path planning method under situation delay of the present invention.

[0019] Figure 2 This is a flow chart of the path planning based on Kalman filtering and ant colony algorithm of the present invention.

[0020] Figure 3 In the embodiment of the present invention, Figure 1 Comparison of the path planning effect with direct planning without using this solution.

[0021] Figure 4 In the embodiment of the present invention, Figure 1 Comparison chart of the path planning and the actual trajectory of the mobile robot.

[0022] Figure 5In the embodiment of the present invention, Figure 1 Comparison of measurement error and prediction error in path planning.

[0023] Figure 6 In the embodiment of the present invention, Figure 2 Comparison of the path planning effect with direct planning without using this solution.

[0024] Figure 7 In the embodiment of the present invention, Figure 2 Comparison chart of the path planning and the actual trajectory of the mobile robot.

[0025] Figure 8 In the embodiment of the present invention, Figure 2 Comparison chart of path planning measurement error and prediction error. DETAILED DESCRIPTION

[0026] Example

[0027] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0028] As shown in this application and claims, unless the context clearly indicates an exception, the words "a", "an", "an kind" and / or "the" do not refer to the singular and may also include the plural. Generally speaking, the terms "include" and "comprise" only indicate the inclusion of the steps and elements that have been clearly identified, and these steps and elements do not constitute an exclusive list. The method or device may also include other steps or elements.

[0029] Unless otherwise specifically stated, the relative arrangement, numerical expressions and numerical values ​​of the parts and steps set forth in these embodiments do not limit the scope of the present application. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to those of ordinary skill in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be considered as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so that once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0030] Combination Figure 1 , a mobile robot path planning method under situation delay, comprising the following steps:

[0031] Step 1: Obtain the situation position information Z fed back by the k mobile robot sensor at the current moment under situation delay k , the mobile robot's moving target point location information Target k :

[0032]

[0033] Among them, Position_zx k 、Position_zy k They represent the x-coordinate and y-coordinate of the robot at time k fed back by the sensor; target ,y target Respectively represent the x-coordinate and y-coordinate of the target location point;

[0034] Step 2: Estimate the “actual” position of the mobile robot based on the Kalman filter method

[0035] Step 2-1, determine the state transfer equation of the mobile robot:

[0036] X k =AX k-1 +Bu k-1 +w k-1

[0037] The measurement equation is:

[0038] Z k =HX k +v k

[0039] Where A is the state transfer matrix B is the control gain matrix Δt represents the time difference of the decision step of path planning, Position_x k 、position_y k Represents the x-coordinate and y-coordinate at time k respectively Velocity_x k 、Velocity_y k They represent the x-direction velocity component and the y-direction velocity component at time k respectively, and H is the gain matrix of the state variable to the measured variable w and v are process excitation noise and observation noise, and they are assumed to be independent of each other and obey the following distribution:

[0040] p(ω)~N(0,Q)

[0041] p(v)~N(0,R)

[0042] Among them, p(ω) represents the probability distribution of w, N represents the normal distribution, Q is the symbol for the variance in the normal distribution that w obeys; p(ν) represents the probability distribution of v, R is the symbol for the variance in the normal distribution that v obeys;

[0043] Step 2-2: Calculate the prior estimate of the mobile robot’s “actual” position

[0044]

[0045] in, represents the prior estimated covariance at time k, P k-1 represents the k-1 moment posterior estimated covariance, P k-1 Initialize to the identity matrix

[0046] Step 2-3. Calculate the posterior estimate of the mobile robot’s “actual” position

[0047]

[0048] Among them, K k represents the Kalman gain at time k;

[0049] Step 2-3: Determine the "actual" position X of the mobile robot k :

[0050]

[0051] Step 3: Determine whether the mobile robot has reached the final target position, i.e., the end point. If so, the task is terminated, otherwise, go to step 4:

[0052]

[0053] The dis function is used to calculate the Euclidean distance between the estimated position of the mobile robot and the end point, D is the set threshold parameter, Target end for the end point;

[0054] Step 4: Determine whether the mobile robot has reached the target position. If so, go to step 5; otherwise, go to step 7:

[0055]

[0056] Among them, the dis function is used to calculate the Euclidean distance between the estimated position point of the mobile robot and the end point, F is the set threshold parameter, Target kis the target position point of this movement;

[0057] Step 5: Based on the estimated actual position X of the mobile robot k Perform mobile robot path planning:

[0058] Combination Figure 2 ,This scheme uses the ant colony algorithm to plan the path of the mobile robot, which specifically includes the following processes:

[0059] Step 5-1, parameter initialization, including the grid nodes contained in the map and the edges connecting the nodes, the number of ants m in the ant colony algorithm, the number of iterations T, the pheromone volatility factor ρ and the pheromone addition constant Q;

[0060] Step 5-2: Ant colony algorithm ant position setting, initialization position is the unmanned platform starting point position X k , the end point is the final target point of the movement end ;

[0061] Step 5-3: Initialize the pheromone concentration τ on the edge from the current node i where the ant is located to the adjacent node j, denoted as j∈neighbors(i) ij , initialize τ ij =1, each ant maintains a visited node list visited_nodes[];

[0062] Step 5-4: For m ants, calculate the sum of pheromones on the edges from the current ant's node i to all adjacent nodes j:

[0063] total_sum=∑ j∈nighbors(i) τ ij

[0064] Step 5-5, calculate the transition probability from node i to node j:

[0065]

[0066] According to the transition probability, based on a probabilistic sampling method such as roulette, the next node j is selected, the current ant moves to the selected node j, and the node is added to the visited node list visited_nodes[];

[0067] If the current ant reaches the target node Target end , then mark the path as complete and move to the next ant, otherwise go to step 5-4;

[0068] Step 5-6: Sort the paths obtained by all ants from short to long, and record the path length as L. For each connection (i, j) in each path, update the pheromone value according to the formula:

[0069]

[0070] For connections formed by other points not on the path, pheromone volatilization: τ ij =(1-ρ)τ ij ;

[0071] Step 5-7: Determine whether the number of iterations T has been reached. If not, return to step 5-4. If yes, sort the shortest paths of each iteration, select the overall shortest path Shortest_Route as the final path and output it to the mobile robot. Clear the visited_nodes[] parameter for the next path planning:

[0072]

[0073] Step 6: Update the coordinates of the new target position of the mobile robot based on the path planned in step 5:

[0074] Target k+1 =Shortest_Route[0]

[0075] Where Shortest_Route[0] represents the first target point of the newly planned path of the mobile robot;

[0076] Step 7: Update the coordinates of the target location based on the original target location:

[0077] Target k+1 =Target k

[0078] Step 8: The mobile robot performs the movement, k=k+1, and goes to step 1 to execute the next moment process.

[0079] like Figure 3 As shown, this is based on the ground in the embodiment of this scheme Figure 1 The comparison chart of the effect of planning the path by using the scheme and planning without using the scheme directly shows that the scheme can avoid a collision at the grid with X coordinates of 6-7 and Y coordinates of 1-2.

[0080] like Figure 4 As shown, this is based on the ground in the embodiment of this scheme Figure 1 The comparison diagram of the path planned by the mobile robot and the actual trajectory of the mobile robot shows that the error between the method based on Kalman filtering and the actual trajectory is small, indicating that the method estimates the motion trajectory based on the situation delay data better;

[0081] like Figure 5As shown, this is based on the ground in the embodiment of this scheme Figure 1 The comparison diagram of measurement error and prediction error in path planning in . It can be seen from the figure that the prediction error based on this scheme is smaller than the measurement error.

[0082] Figure 6 In this embodiment of the scheme, based on Figure 2 The effect comparison diagram of planning the path by using the scheme and planning directly without using the scheme is shown in the figure. It can be seen from the figure that the method avoids the collision of the two grids with X coordinates of 9-10 and Y coordinates of 5-6 and X coordinates of 13-14 and Y coordinates of 4-5.

[0083] Figure 7 In this embodiment of the scheme, based on Figure 2 The comparison diagram of the path planned by the mobile robot and the actual trajectory of the mobile robot shows that the error between the method based on Kalman filtering and the actual trajectory is small, indicating that the method estimates the motion trajectory based on the situation delay data better;

[0084] Figure 8 In the embodiment of the present invention, Figure 2 Comparison diagram of measurement error and prediction error in path planning. It can be seen from the figure that the prediction error is smaller than the measurement error.

[0085] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A mobile robot path planning method under situation delay, characterized in that: The following steps are involved: Step 1: Obtain the situation position information Z fed back by the k mobile robot sensor at the current moment under situation delay k , the mobile robot's moving target point location information Target k ; Step 2: Estimate the "actual" position of the mobile robot based on the Kalman filter method Step 3: Determine whether the mobile robot has reached the final target position. If so, end the task; otherwise, go to step 4. Step 4: Determine whether the mobile robot has reached the target position. If so, go to step 5; otherwise, go to step 7. Step 5: Based on the estimated actual position X of the mobile robot k Perform path planning for mobile robots; Step 6: Update the coordinates of the new target position of the mobile robot based on the path planned in step 5; Step 7: Update the coordinates of the target location point based on the original target location point; Step 8: The mobile robot performs the movement, k=k+1, and goes to step 1 to execute the next moment process.

2. The mobile robot path planning method under situation delay according to claim 1 is characterized in that: The situation position information Z in step 1 k , the mobile robot's moving target point location information Target k , specifically: Among them, Position_zx k 、Position_zy k They represent the x-coordinate and y-coordinate of the robot at time k fed back by the sensor; target ,y target Respectively represent the x-coordinate and y-coordinate of the target location point.

3. The mobile robot path planning method under situation delay according to claim 1 is characterized in that: The actual position of the mobile robot is estimated based on the Kalman filter method in step 2 to obtain Specifically: Step 2-1, determine the state transfer equation of the mobile robot: X k =AX k-1 +Bu k-1 +w k-1 The measurement equation is: Z k =HX k +v k Where A is the state transfer matrix B is the control gain matrix Δt represents the time difference of the decision step of path planning, Position_x k 、position_y k represent the x-coordinate and y-coordinate at time k respectively, Velocity_x k 、Velocity_y k They represent the x-direction velocity component and the y-direction velocity component at time k respectively, and H is the gain matrix of the state variable to the measured variable w and v are process excitation noise and observation noise, and they are assumed to be independent of each other and obey the following distribution: p(ω)~N(0,Q) p(v)~N(0,R) Among them, p(ω) represents the probability distribution of ω, N represents the normal distribution, Q is the symbol for the variance in the normal distribution that ω obeys; p(ν) represents the probability distribution of v, and R is the symbol for the variance in the normal distribution that v obeys; Step 2-2: Calculate the prior estimate of the "actual" position of the mobile robot in, represents the prior estimated covariance at time k, P k-1 represents the k-1 moment posterior estimated covariance, P k-1 Initialize to the identity matrix Step 2-3: Calculate the posterior estimate of the "actual" position of the mobile robot Among them, K k represents the Kalman gain at time k; Step 2-3: Determine the "actual" position X of the mobile robot k :

4. The mobile robot path planning method under situation delay according to claim 1 is characterized in that: The step 3 of determining whether the mobile robot has reached the final target position is as follows: The dis function is used to calculate the Euclidean distance between the estimated position of the mobile robot and the end point, D is the set threshold parameter, Target end For the end point.

5. The mobile robot path planning method under situation delay according to claim 1 is characterized in that: The step 4 of determining whether the mobile robot has reached the target position is as follows: Among them, the dis function is used to calculate the Euclidean distance between the estimated position point of the mobile robot and the end point, F is the set threshold parameter, Target k is the target position of this move.

6. The mobile robot path planning method under situation delay according to claim 1 is characterized in that: The mobile robot path planning in step 5 is specifically as follows: Step 5-1, parameter initialization, including the grid nodes contained in the map and the edges connecting the nodes, the number of ants m in the ant colony algorithm, the number of iterations T, the pheromone volatility factor ρ and the pheromone addition constant Q; Step 5-2: Ant colony algorithm ant position setting, initialization position is the unmanned platform starting point position X k , the end point is the final target point of the movement end ; Step 5-3: Initialize the pheromone concentration τ on the edge from the current node i where the ant is located to the adjacent node j, denoted as j∈neighbors(i) ij , initialize τ ij =1, each ant maintains a visited node list visited_nodes[ ]; Step 5-4: For m ants, calculate the sum of pheromones on the edges from the current ant's node i to all adjacent nodes j: total_sum=∑ j∈nighbors(i) t ij Step 5-5, calculate the transition probability from node i to node j: According to the transition probability, the next node j is selected, the current ant moves to the selected node j, and the node is added to the visited node list visited_nodes[ ]; If the current ant reaches the target node Target end , then mark the path as complete and move to the next ant, otherwise go to step 5-4; Step 5-6: Sort the paths obtained by all ants from short to long, and record the path length as L. For each connection (i, j) in each path, update the pheromone value according to the formula: For connections formed by other points not on the path, pheromone volatilization: τ ij =(1-ρ)τ ij ; Step 5-7: Determine whether the number of iterations T has been reached. If not, return to step 5-4. If yes, sort the shortest paths of each iteration, select the overall shortest path Shortest_Route as the final path and output it to the mobile robot. Clear the visited_nodes[ ] parameter for the next path planning:

7. The method for mobile robot path planning under situation delay according to claim 6, characterized in that: The updating of the coordinates of the new target position point of the mobile robot in step 6 is specifically as follows: Target k+1 =Shortest_Route[0] Where Shortest_Route[0] represents the first target point of the newly planned path of the mobile robot.

8. The method for mobile robot path planning under situation delay according to claim 7, characterized in that: The updating of the target position point coordinates based on the original target position point is specifically as follows: Target k+1 =Target k 。 9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

10. A computer storable medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images