A global movement method of mobile robot

Abstract

The present invention provides a global movement method of a mobile robot. In the process of an initial node approaching a target node, a node with the smallest cost value is searched, and then the mobile robot moves before the node with the smallest cost value. The moving distance and moving time of the mobile robot are short. At the same time, for known obstacles, the cost values ​​of nodes other than the obstacle nodes are calculated. The mobile robot avoids the obstacle nodes during movement, and the known obstacles are avoided. For newly appeared obstacles, an obstacle avoidance path is planned during the process of the mobile robot approaching the newly appeared obstacles, and unknown obstacles are avoided.

Description

Technical Field

[0001] The invention relates to the technical field of path planning, and in particular to a global movement method of a mobile robot. Background Art

[0002] A mobile robot is a device that can be controlled to move and perform various tasks. With the continuous development of artificial intelligence, mobile robots have gradually been able to replace humans in some tasks. Compared with humans, robots have the advantage of being able to work for a long time and with high efficiency. Mobile robots, especially service-oriented mobile robots, such as sweeping robots and delivery robots, have gradually become common in daily life.

[0003] In the technical field of mobile robots, planning and control is an important research field. The more commonly used planning algorithms include the A* algorithm and the DWA (dynamic window method) algorithm. In the A* algorithm, the evaluation function F(n) is simply obtained by weighting G(n) and H(n). G(n) is the cost value from the starting point to node n in the state space, and H(n) is the estimated cost value of the path from node n to the target point. In this way, each time the node with the highest priority is selected from the search area as the next node to be traversed, and for some nodes that cannot find the target node, the destination cannot be reached, and the amount of calculation is large. For the DWA algorithm, it mainly samples multiple groups of speeds in the speed space, simulates the motion trajectories of these speeds in a certain period of time, and scores these motion trajectories through multi-layer evaluation functions to output the optimal speed sample. The existing DWA algorithm reduces the path safety due to the fixed parameters of speed and deviation angle changes; the weight of the speed evaluation function is fixed and is always smaller than the weights of other evaluation sub-functions. The speed in the algorithm cannot guarantee a fast and smooth arrival at the destination, and the resulting moving trajectory is not smooth; if an obstacle suddenly appears at the target point, the robot will move very slowly or even freeze when it is about to reach the target point. Summary of the invention

[0004] The present invention provides a global movement method of a mobile robot. The method plans a path between an initial node and a target node according to known obstacles. During the movement, a speed evaluation weight is changed according to the distance between the mobile robot and a newly appeared obstacle. If the mobile robot is far away from the obstacle, it approaches the end point smoothly. If the mobile robot is close to the obstacle, it can quickly avoid the newly appeared obstacle.

[0005] To achieve the above object, the technical solution of the present invention is: a global movement method of a mobile robot, comprising the following steps:

[0006] S1. Preset a map, construct a global coordinate system in the map, and construct the robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node.

[0007] The initial node, target node and obstacle node are preset. The initial node is the starting point of the mobile robot, the target node is the end point of the mobile robot, and the obstacle node is the location of the known obstacle.

[0008] An open list and a closed list are preset. The open list is used to store data of nodes that the mobile robot can reach, and the closed list is used to store data of nodes with the shortest distance between the initial node and the target node; the initial node and the target node are added to the closed list.

[0009] S2. Construct a motion model of the mobile robot. The motion model is used to obtain the moving direction of the mobile robot and the deflection angle between the robot coordinate system and the global coordinate system.

[0010] S3. Construct a speed sampling model of the mobile robot, where the speed sampling model is used to obtain the current speed of the mobile robot.

[0011] S4. Set a search area with the current node where the mobile robot is located as the center, and add all nodes in the search area to the open list.

[0012] S5. Calculate the cost values ​​of the remaining nodes in the search area except the obstacle node, delete the node with the smallest cost value except the obstacle node in the search area from the open list and add it to the closed list; then proceed to S6.

[0013] S6, the mobile robot moves to the node with the smallest cost in the search area; during the movement, if it is found that there is a new obstacle that will block the movement of the mobile robot, then proceed to S7; if there is no new obstacle that blocks the movement of the mobile robot, then proceed to S9.

[0014] S7. Calculate an obstacle avoidance path by using the current speed of the mobile robot, the deviation angle between the moving direction of the current moving trajectory of the mobile robot at the current speed and the target node, and the distance between the mobile robot and the newly appeared obstacle.

[0015] S8. The mobile robot moves along the obstacle avoidance path to avoid new obstacles.

[0016] S9, repeat S2-S8, the mobile robot resets the search area and moves towards the target node.

[0017] Furthermore, S5 is specifically as follows: by using the formula F(n) = G(n) + H(n) * H(n); calculating the F(n) values ​​of the remaining nodes in the search area except the obstacle node; G(n) is the cost from the initial node to a node, and H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; deleting the node with the smallest F(n) value except the obstacle node in the search area from the open list and adding it to the closed list; and then proceeding to S6.

[0018] Furthermore, through the formula Calculate the value of H(n); (x g ,y g ) is the coordinate of the target node, (x n ,y n ) is the coordinate of a node.

[0019] Furthermore, S2 is specifically as follows: constructing a motion model of the mobile robot; x t+1 =x t +vΔtcosθ t ,y t+1 =y t +vΔtsinθ t ,θ t+1 =θ t ωΔt;x t ,y t ,θ t is the position at time t; θ t is the deflection angle between the robot coordinate system and the global coordinate system.

[0020] Furthermore, the speed sampling model in S3 includes:

[0021] The speed constraint is:

[0022] v a = {(v,ω)|v∈[v min , v max ],ω∈[ω min ,ω max ]}.

[0023] The acceleration and deceleration constraints are:

[0024] v a = {(v,ω)|v∈[v c -v b Δt,v c +v a Δt],ω∈[ω c -ω b Δt,ω c +ω a Δt]}.

[0025] The braking distance constraint is:

[0026] v a ={(v,ω)| v≤(2dist(v,ω)v b ) 1 / 2 ,ω≤(2dist(v,ω)ω b ) 1 / 2}.

[0027] v max is the maximum value of linear velocity, v min is the minimum value of linear velocity, ω max is the maximum value of angular velocity, ω min is the minimum value of angular velocity; v c is the current speed, ω c is the current angular velocity; v a is the maximum acceleration, ω a is the maximum angular acceleration; v b is the maximum deceleration, ω b Maximum deceleration angle.

[0028] Furthermore, S7 is specifically: through the formula

[0029] E(v, ω)={σ(αhead(v, ω)+βdist(v, ω)+εDvel(v, ω))}; calculates the obstacle avoidance path of the mobile robot; σ is a smoothing function; α and β are weighting coefficients; head(v, ω) is the deviation angle between the moving direction of the current moving trajectory of the mobile robot and the target node at the current speed; dist(v, ω) is the distance between the mobile robot and the newly appeared obstacle; vel(v, ω) is the current speed of the mobile robot; εD is the weight value of vel(v, ω) that is dynamically adjusted according to the distance between the newly appeared obstacle and the mobile robot at the current speed.

[0030] The above method searches for the node with the smallest cost value in the process of the initial node approaching the target node, and then the mobile robot moves before the node with the smallest cost value; the moving distance and moving time of the mobile robot are short; at the same time, for known obstacles, the cost values ​​of nodes other than the obstacle nodes are calculated; the mobile robot avoids the obstacle nodes during movement to avoid the known obstacles; for the newly appeared obstacles, the obstacle avoidance path is planned in the process of the mobile robot approaching the newly appeared obstacles to avoid the unknown obstacles.

[0031] When planning the global path, the estimated cost of reaching the target node from one node is calculated through the coordinates of the target node and the coordinates of a node, so that the straight-line distance between the target node and the node can be calculated; and then the node with a smaller F(n) value can be calculated; at the same time, when calculating the F(n) value, the weight of H(n) is increased, the redundant stages are reduced, and the calculation efficiency and flexibility are improved.

[0032] When planning a local path, the weight of the current moving speed of the mobile robot in the formula is adjusted by the distance between the mobile robot and the newly appeared obstacle;

[0033] When the mobile robot is far from the obstacle, the mobile robot approaches the end point smoothly, and when the mobile robot is close to the obstacle, the mobile robot can quickly avoid the newly appeared obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 A schematic diagram of the path planning of the existing A-Star algorithm on a 30*30mm grid map.

[0035] Figure 2 A schematic diagram of the path planning of the existing A-Star algorithm on a 40*40mm grid map.

[0036] Figure 3 Schematic diagram of path planning of the existing DWA algorithm on a 30*40mm grid map.

[0037] Figure 4 Schematic diagram of path planning of the existing DWA algorithm on a 40*40mm grid map.

[0038] Figure 5 The figure is a schematic diagram of path planning on a 30*30 mm grid map using the A-Star algorithm of the present invention.

[0039] Figure 6 The figure is a schematic diagram of path planning on a 40*40 mm grid map using the A-Star algorithm of the present invention.

[0040] Figure 7 The figure is a schematic diagram of path planning on a 30*30 mm grid map using the DWA algorithm of the present invention.

[0041] Figure 8 The figure is a schematic diagram of path planning on a 40*40mm grid map using the DWA algorithm of the present invention.

[0042] Fig. 9 It is the road map of the present invention.

[0043] Fig.10Schematic diagram of the change in the distance between the mobile robot and the newly appeared obstacle and the value of εD. DETAILED DESCRIPTION

[0044] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0045] like Figure 1-10 As shown; a global movement method of a mobile robot, the mobile robot has a visual recognition device, and the visual recognition device is a camera. The mobile robot moves through global path planning and local path planning; the global path planning enables the mobile robot to move between an initial node and a target node at the shortest distance while avoiding known obstacles. The local path planning is used for the mobile robot to avoid unknown obstacles during the process of moving along the global path.

[0046] The global movement method of the mobile robot includes the following steps:

[0047] S1. Preset a map, construct a global coordinate system in the map, and construct the robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node.

[0048] The initial node, target node and obstacle node are preset. The initial node is the starting point of the mobile robot, the target node is the end point of the mobile robot, and the obstacle node is the location of the known obstacle.

[0049] An open list and a closed list are preset. The open list is used to store data of nodes that the mobile robot can reach, and the closed list is used to store data of nodes with the shortest distance between the initial node and the target node; the initial node and the target node are added to the closed list.

[0050] S2. Construct a motion model of the mobile robot. The motion model is used to obtain the moving direction of the mobile robot and the deflection angle between the robot coordinate system and the global coordinate system. Specifically: through the motion model of the mobile robot; x t+1 =x t +vΔtcosθ t ,y t+1 =y t +vΔtsinθ t ,θ t+1 =θ t ωΔt;x t ,y t ,θ t is the position at time t; θ t is the deflection angle between the robot coordinate system and the global coordinate system.

[0051] S3. Construct a speed sampling model of the mobile robot, where the speed sampling model is used to obtain the current speed of the mobile robot.

[0052] Specifically, the speed sampling model includes: speed constraints, acceleration and deceleration constraints, and braking distance constraints.

[0053] The speed constraint is:

[0054] v a = {(v,ω)|v∈[v min , v max ],ω∈[ω min ,ω max ]}.

[0055] The acceleration and deceleration constraints are:

[0056] v a = {(v,ω)|v∈[v c -v b Δt,v c +v a Δt],ω∈[ω c -ω b Δt,ω c +ω a Δt]}.

[0057] The braking distance constraint is:

[0058] v a ={(v,ω)|v≤(2dist(v,ω)v b ) 1 / 2 ,ω≤(2dist(v,ω)ω b ) 1 / 2}.

[0059] v max is the maximum value of linear velocity, v min is the minimum value of linear velocity, ω max is the maximum value of angular velocity, ω min is the minimum value of angular velocity; v c is the current speed, ω c is the current angular velocity; v a is the maximum acceleration, ω a is the maximum angular acceleration; v b is the maximum deceleration, ω b Maximum deceleration angle.

[0060] S4. Set a search area with the current node where the mobile robot is located as the center, and add all nodes in the search area to the open list.

[0061] S5. Calculate the F(n) values ​​of the remaining nodes in the search area except the obstacle node through the formula F(n) = G(n) + H(n) * H(n); G(n) is the cost from the initial node to a node, H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; delete the node with the smallest F(n) value except the obstacle node in the search area from the open list and add it to the closed list; then proceed to S6.

[0062] S6, the mobile robot moves to the node with the smallest cost in the search area; during the movement, if it is found that there is a new obstacle that will block the movement of the mobile robot, then proceed to S7; if there is no new obstacle that blocks the movement of the mobile robot, then proceed to S9.

[0063] S7, through the formula

[0064] E(v, ω)={σ(αhead(v, ω)+βdist(v, ω)+εDvel(v, ω))}; calculates the obstacle avoidance path of the mobile robot; σ is a smoothing function; α and β are weighting coefficients; head(v, ω) is the deviation angle between the moving direction of the current moving trajectory of the mobile robot and the target node at the current speed; dist(v, ω) is the distance between the mobile robot and the newly appeared obstacle; vel(v, ω) is the current speed of the mobile robot; εD is the weight value of vel(v, ω) that is dynamically adjusted according to the distance between the newly appeared obstacle and the mobile robot at the current speed; εD= 0.4 - 0.1* dist(v, ω); the maximum value of 0.1*dist(v, ω) is 0.3.

[0065] In the formula, since E(v, W) is a standard value, when the value of the distance dist(v, ω) between the mobile robot and the newly appeared obstacle is small, the value of the current speed vel(v, ω) of the mobile robot is large; when the value of the distance dist(v, ω) between the mobile robot and the newly appeared obstacle is large, the value of the current speed vel(v, ω) of the mobile robot is small. That is, when the distance between the mobile robot and the newly appeared obstacle is short, the mobile robot will bypass the newly appeared obstacle at a faster speed. When the distance between the mobile robot and the newly appeared obstacle is far, the mobile robot will approach the end point smoothly. The maximum threshold and minimum threshold of the distance between the mobile robot and the newly appeared obstacle are preset respectively; when the distance between the mobile robot and the newly appeared obstacle is greater than the maximum threshold, εD is 0.1; when the distance between the mobile robot and the newly appeared obstacle is less than the minimum threshold, εD is 0.4. In this embodiment, the maximum threshold is 3m and the minimum threshold is 0.

[0066] When the distance dist(v, ω) between the mobile robot and the newly appeared obstacle is between the maximum threshold and the minimum threshold and decreases, the weight of εD increases; when the distance dist(v, ω) between the mobile robot and the newly appeared obstacle is between the maximum threshold and the minimum threshold and increases, the weight of εD decreases.

[0067] S8. The mobile robot moves along the obstacle avoidance path to avoid new obstacles.

[0068] S9, repeat S2-S8, the mobile robot resets the search area and moves towards the target node.

[0069] In the above method, in the process of global path planning, when the initial node approaches the target node, the node with the smallest cost value is searched, and then the mobile robot moves before the node with the smallest cost value; the moving distance and moving time of the mobile robot are short; at the same time, for known obstacles, the cost values ​​of nodes other than the obstacle nodes are calculated; the mobile robot avoids the obstacle nodes during movement, and avoids the known obstacles. For new obstacles, local path planning is used; in the process of the mobile robot approaching the new obstacles, an obstacle avoidance path is planned to avoid unknown obstacles.

[0070] When planning the global path at the same time, the estimated cost value of reaching the target node from the node is calculated by the coordinates of the target node and the coordinates of the node, so that the straight-line distance between the target node and the node can be calculated; and then the node with a smaller F(n) value can be calculated; at the same time, when calculating the F(n) value, the weight of H(n) is increased, the redundant stage is reduced, and the calculation efficiency and flexibility are improved. In this embodiment, the global path planning uses the A-Star algorithm.

[0071] When planning a local path, the weight of the current moving speed of the mobile robot in the formula is adjusted by the distance between the mobile robot and the newly appeared obstacle;

[0072] When the mobile robot is far from the obstacle, the mobile robot approaches the end point smoothly, and when the mobile robot is close to the obstacle, the mobile robot can quickly avoid the newly appeared obstacle. In this embodiment, the local path planning uses the DWA algorithm.

[0073] Table 1 is a comparison table of the A-Star algorithm of the present invention and the existing A-Star algorithm on a 30*30 mm grid map.

[0074] Iteration steps / step Running distance / mm Running time / s Traverse nodes / Existing A-Star algorithm 32 35.92 5.514 291 A-Star algorithm of the present invention 33 34.32 1.2123 28 Effect 3.12% reduction Reduced by 4.66% 454% shorter

[0075] Table 1

[0076] Reference Figure 1 and Figure 5As shown, compared with the traditional A-Sta algorithm, the A-Sta algorithm of the present invention reduces the running distance by 4.66% and shortens the time by 454%.

[0077] Table 2 is a comparison table of the A-Star algorithm of the present invention and the existing A-Star algorithm on a 40*40 mm grid map.

[0078] Iteration steps / step Running distance / mm Running time / s Traverse nodes / Existing A-Star algorithm 36 37.9859 14.78 386 A-Star algorithm of the present invention 38 37.714 10.53 91 Effect 5.5% reduction Reduced by 0.72% Shortened by 40.36%

[0079] Table 2

[0080] Reference Figure 2 and Figure 6 As shown, compared with the traditional A-Sta algorithm, the A-Sta algorithm of the present invention reduces the running distance by 0.72% and shortens the time by 40.36%.

[0081] Table 3 is a comparison table of the DWA algorithm of the present invention and the existing DWA algorithm on a 30*30 mm grid map.

[0082] Running distance / m Number of turns Running time / s Existing DWA Algorithms 35.35 5 161.43 DWA algorithm of the present invention 26.90 4 125.86 Effect Shortened by 76.09% 33% reduction Shortened by 77.96%

[0083] Table 3

[0084] Reference Figure 3 and Figure 7 As shown, compared with the traditional DWA algorithm, the DWA algorithm of the present invention reduces the running distance by 76.09% and shortens the time by 77.96%.

[0085] Table 4 is a comparison table of the DWA algorithm of the present invention and the existing DWA algorithm on a 40*40 mm grid map.

[0086] Running distance / m Number of turns Running time / s Existing DWA Algorithms 49.28 5 376.74 DWA algorithm of the present invention 46.96 4 321.52 Effect Shortened by 76.09% 33% reduction Shortened by 85.34%

[0087] Table 4

[0088] Reference Figure 4 and Figure 8 As shown, compared with the traditional DWA algorithm, the DWA algorithm of the present invention reduces the running distance by 76.09% and shortens the time by 85.34%.

Claims

1. A global movement method of a mobile robot, Features: The following steps are involved: S1. Preset a map, construct a global coordinate system in the map, and construct the robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node; The initial node, target node and obstacle node are preset. The initial node is the starting point of the mobile robot, the target node is the end point of the mobile robot, and the obstacle node is the location of the known obstacle. An open list and a closed list are preset. The open list is used to store the data of the nodes that the mobile robot can reach, and the closed list is used to store the data of the node with the shortest distance between the initial node and the target node; the initial node and the target node are added to the closed list; S2. construct a motion model of the mobile robot, where the motion model is used to obtain the moving direction of the mobile robot and the deflection angle between the robot coordinate system and the global coordinate system; S3, constructing a speed sampling model of the mobile robot, where the speed sampling model is used to obtain the current speed of the mobile robot; S4, setting a search area with the current node where the mobile robot is located as the center, and adding all nodes in the search area to the open list; S5, calculate the cost values ​​of the remaining nodes in the search area except the obstacle node, delete the node with the smallest cost value except the obstacle node in the search area from the open list and add it to the closed list; the node with the smallest cost is the node closest to the target node; then proceed to S6; S6, the mobile robot moves to the node with the smallest cost value in the search area; during the movement process, if it is found that there is a new obstacle that will block the movement of the mobile robot, then S7 is performed; if there is no new obstacle that blocks the movement of the mobile robot, then S9 is performed; S7, calculating an obstacle avoidance path by using the current speed of the mobile robot, the deviation angle between the moving direction of the current moving trajectory of the mobile robot at the current speed and the target node, and the distance between the mobile robot and the newly appeared obstacle; S8, the mobile robot moves along the obstacle avoidance path to avoid the newly appeared obstacles; S9, repeat S2-S8, the mobile robot resets the search area and moves towards the target node.

2. A global movement method of a mobile robot according to claim 1, Features: S5 is specifically as follows: by using the formula F(n) = G(n) + H(n) * H(n); calculating the F(n) values ​​of the remaining nodes in the search area except the obstacle node; G(n) is the cost from the initial node to a node, and H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; deleting the node with the smallest F(n) value except the obstacle node in the search area from the open list and adding it to the closed list; and then proceeding to S6.

3. The global movement method of a mobile robot according to claim 2, Features: By formula Calculate the value of H(n); (x g ,y g ) is the coordinate of the target node, (x n ,y n ) is the coordinate of a node.

4. The global movement method of a mobile robot according to claim 1, Features: S2 is specifically: constructing a motion model of the mobile robot; t+1 =x t +vΔtcosθ t ,y t+1 =y t +vΔtsinθ t ,θ t+1 =θ t ωΔt;x t ,y t ,θ t is the position at time t; θ t is the deflection angle between the robot coordinate system and the global coordinate system.

5. The global movement method of a mobile robot according to claim 1, Features: The speed sampling model in S3 includes: the speed constraints are: v a ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}; The acceleration and deceleration constraints are: v a ={(v,ω)|v∈[v c -v b Δt, v c +v a Δt],ω∈[ω c -oh b Δt,ω c +oh a Δt]}; The braking distance constraint is: v a ={(v,ω)| v≤(2dist(v,ω)v b ) 1 / 2 ,ω≤(2dist(v,ω)ω b ) 1 / 2 }; v max is the maximum value of linear velocity, v min is the minimum value of linear velocity, ω max is the maximum value of angular velocity, ω min is the minimum value of angular velocity; v c is the current speed, ω c is the current angular velocity; v a is the maximum acceleration, ω a is the maximum angular acceleration; v b is the maximum deceleration, ω b Maximum deceleration angle.

6. A global movement method of a mobile robot according to claim 5, Features: S7 is specifically: through the formula E(v, ω)={σ(αhead(v, ω)+βdist(v, ω)+εDvel(v, ω))}; calculates the obstacle avoidance path of the mobile robot; σ is a smoothing function; α and β are weighting coefficients; head(v, ω) is the deviation angle between the moving direction of the current moving trajectory of the mobile robot and the target node at the current speed; dist(v, ω) is the distance between the mobile robot and the newly appeared obstacle; vel(v, ω) is the current speed of the mobile robot; εD is the weight value of vel(v, ω) that is dynamically adjusted according to the distance between the newly appeared obstacle and the mobile robot at the current speed.

Images