A path generation method based on forward-looking points

Abstract

The present invention provides a path generation method based on a forward-looking point. After a mobile robot deviates from a target path, it is necessary to plan a path between the mobile robot and the target path. The shortest distance between the target path and the mobile robot is confirmed, and the distance the mobile robot moves toward the target path is limited to avoid the mobile robot exceeding the target path when moving toward the target path. The forward-looking point is confirmed forward by the position of a projection point, and the distance the mobile robot enters the target path is extended, and the turning radius of the mobile robot directly entering the target path is reduced, thereby improving the smoothness of the movement of the mobile robot.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path generation method based on a forward-looking point. Background Art

[0002] With the continuous development of artificial intelligence, mobile robots have been widely used in people's production and life due to their convenient mobility. For example, sweeping robots, delivery robots, etc. have gradually become very common in daily life. They have the advantages of high work efficiency and are favored by researchers.

[0003] In the technical field of mobile robots, path planning control is one of the path planning methods in mobile robots. If the path planning is good, the efficiency and reliability of the mobile robot's movement can be further improved.

[0004] For example, a patent document with application number 202010366446.1 and publication date 2020.06.26 in China discloses a mobile robot path planning method based on distance judgment and angle deflection, which includes the following steps: Step 1: Using the global grid map constructed by the SLAM mapping algorithm as prior information, it is used for global path planning to obtain a pre-planned path. In a static environment where the environment has not changed, the mobile robot moves according to the pre-planned path. Step 2: When the environment changes, that is, an obstacle appears on the pre-planned path, the mobile robot obtains distance information and obstacle angle range information based on the information scanned by the laser radar, guides the mobile robot to deflect and move, and achieves the purpose of returning to the global pre-planned path, thereby realizing the navigation of the robot in a dynamic environment. The method of the present invention obtains obstacle distance information and angle range information through the robot's existing sensors, solves triangles to calculate the obstacle avoidance trajectory, reduces the robot's calculation amount, enhances the real-time performance of the robot's movement, and improves the robot's movement efficiency.

[0005] However, in this document, after avoiding the obstacle, the robot uses its existing sensors to obtain the obstacle distance information and angle range information, thereby returning to the original path. However, if the obstacle distance information is close and the angle range information has a large deviation, this will cause the mobile robot to easily exceed the global path after moving, making it impossible to accurately return to the global path. Summary of the invention

[0006] The present invention provides a path generation method based on a look-ahead point. The look-ahead point is set to enable a mobile robot that deviates from a target path to enter the target path. The look-ahead point extends the distance for the mobile robot to enter the target path, thereby improving the reliability of the mobile robot's movement.

[0007] To achieve the above object, the technical solution of the present invention is: a path generation method based on a forward-looking point, used for a mobile robot, comprising the following steps: S1. Confirm the projection point of the mobile robot on the target path. The projection point is the node on the target path that is closest to the center of the mobile robot.

[0008] S2, along the direction of the target path, set the next node in front of the projection point as the forward point (x p ,y p ).

[0009] S3. Set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax].

[0010] The moving path between the mobile robot and the forward point is calculated through the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω).

[0011] α, β, γ and c are weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point angle and the body orientation angle.

[0012] dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point angle and the vehicle body orientation angle; vel(v, ω) is the cost value of the current velocity of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

[0013] According to the above method, after the mobile robot deviates from the target path, it is necessary to plan the path between the mobile robot and the target path; confirm the shortest distance between the target path and the mobile robot, limit the distance the mobile robot moves to the target path, and avoid the mobile robot exceeding the target path when moving to the target path; confirm the forward point forward through the position of the projection point, extend the distance the mobile robot enters the target path, reduce the turning radius of the mobile robot directly entering the target path, and improve the smoothness of the mobile robot's movement.

[0014] When planning the moving path, affected by the speed, if the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle is large, the cost value will increase as the speed increases; then according to the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle, and the angle difference between the next position coordinates of the mobile robot at time t and the forward-looking point angle and the body orientation angle at the current speed, the speed is constrained; if the angle difference of the current position is less than the angle difference at time t, the cost of the mobile robot moving to the target path increases, and the speed needs to be reduced; if the angle difference of the current position is greater than the angle difference at time t, the cost of the mobile robot moving to the target path decreases, and the current speed can be maintained. At the same time, the penalty cost value of exceeding the target node is added to the formula to prevent the mobile robot from exceeding the target node, thereby ensuring that it can move to the target node of the path more reliably without the problem of exceeding the target node, ensuring the reliability of movement.

[0015] Furthermore, in S3, the calculation method of dis_angle(v, ω) is as follows: Calculate the coordinates of the mobile robot at time t (x t ,y t ) and the look-ahead point (x p ,y p ) between the third angle θ pt , by moving the robot's body toward angle θ at time t t The third angle θ pt Calculate dis_angle(v, ω).

[0016] Furthermore, in S3, the calculation method of dis_local(v, ω) is as follows: Calculate the current coordinates of the mobile robot (x 0 ,y 0 ) and the look-ahead point (x p ,y p ) between the fourth angle θ p0 , calculate the current body orientation angle θ of the mobile robot 0 The fourth angle θ p0 The angle difference is θ 0 With θ p0 The angle difference and the linear velocity range Vt are used to calculate dis_local(v, ω). The calculation formula is dis_local(v, ω) =| arctan[(y p –y 0 ) / (x p –x 0 )]- θ 0 |*Vt.

[0017] The above settings facilitate determining the dis_local (v, ω) situation at the current moment according to the angle range information.

[0018] Furthermore, in S3, the calculation method of dis_goal(v, ω) is specifically as follows: when the mobile robot is at the end node of the moving path, it is determined whether the node on the target path with the shortest distance to the mobile robot is the target node; if not, dis_goal(v, ω) is 0; if so, dis_goal(v, ω)=L*cosθ L ; L is the distance between the end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

[0019] In the above method, when the projection point is inconsistent with the target node, it means that the target node is in front of the projection point, and it does not interfere with the movement of the mobile robot to the forward point; when the projection point is consistent with the target node, if the mobile robot moves in front of the projection point, it will cause the mobile robot to exceed the target node, thus passing L and θ L Calculate the distance that the mobile robot exceeds the target node so that the mobile robot can accurately return to the target node.

[0020] Furthermore, Vt∈[V0-Dv*T,V0+Av*T], the linear velocity range Vt of the mobile robot at time t is calculated; V0 is the current linear velocity of the mobile robot; Dv is the maximum linear acceleration of the mobile robot; Av is the maximum linear deceleration of the mobile robot; and T is the simulation period.

[0021] Furthermore, in step S3, Wt∈[W0-Dw*T,W0+Aw*T], the angular velocity range Wt of the mobile robot at time t is calculated; W0 is the current angular velocity of the mobile robot; Dw is the maximum angular acceleration of the mobile robot; Aw is the minimum angular deceleration of the mobile robot; and T is the simulation period.

[0022] The above settings can determine the angular velocity range according to the maximum angular velocity of the mobile robot at different times, thereby improving accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 Schematic diagram of the projection point, forward-looking point and mobile robot in the target path of the present invention.

[0024] Figure 2 It is a schematic diagram of the mobile robot exceeding the target node in the present invention.

[0025] Figure 3 It is a flow chart of the present invention. DETAILED DESCRIPTION

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

[0027] like Figure 1-3 As shown, a path generation method based on a forward-looking point, for a mobile robot, comprises the following steps: S0. 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; a target path is set in the map, and the target path includes an initial node and a target node, the initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot.

[0028] S1. Confirm the projection point 1 of the mobile robot on the target path. Projection point 1 is the node on the target path that is closest to the center of the mobile robot.

[0029] S2, along the direction of the target path, set the next node in front of projection point 1 as forward point 2, the coordinates of forward point 2 are (x p ,y p ).

[0030] S3. Set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax].

[0031] In this embodiment, the linear velocity range Vt of the mobile robot at time t is calculated by the formula Vt∈[V0-Dv*T,V0+Av*T]; V0 is the current linear velocity of the mobile robot; Dv is the maximum linear acceleration of the mobile robot; Av is the maximum linear deceleration of the mobile robot; and T is the simulation period.

[0032] The linear angular velocity range Wt of the mobile robot at time t is calculated by the formula Wt∈[W0-Dw*T,W0+Aw*T]; W0 is the current angular velocity of the mobile robot; Dw is the maximum angular acceleration of the mobile robot; Aw is the minimum angular deceleration of the mobile robot; and T is the simulation period.

[0033] The moving path between the mobile robot and the forward point 2 is calculated by the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω).

[0034] α, β, γ and c are user-defined weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point 2 and the body orientation angle.

[0035] dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point 2 and the vehicle body orientation angle; vel(v, ω) is the cost value of the current velocity of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

[0036] After the mobile robot deviates from the target path, it is necessary to plan the path between the mobile robot and the target path; confirm the shortest distance between the target path and the mobile robot, limit the distance the mobile robot moves to the target path, and avoid the mobile robot exceeding the target path when moving to the target path; confirm the forward point 2 forward through the position of the projection point 1, extend the distance the mobile robot enters the target path, reduce the turning radius of the mobile robot directly entering the target path, and improve the smoothness of the mobile robot's movement.

[0037] When planning the moving path, affected by the speed, if the angle difference between the current position coordinates of the mobile robot and the forward-looking point 2 angle and the body orientation angle is large, the cost value will increase as the speed increases; then according to the angle difference between the current position coordinates of the mobile robot and the forward-looking point 2 angle and the body orientation angle, and the angle difference between the next position coordinates of the mobile robot at time t and the forward-looking point 2 angle and the body orientation angle at the current speed, the speed is constrained; if the angle difference of the current position is less than the angle difference at time t, the cost of the mobile robot moving to the target path increases, and the speed needs to be reduced; if the angle difference of the current position is greater than the angle difference at time t, the cost of the mobile robot moving to the target path decreases, and the current speed can be maintained. At the same time, the penalty cost value of exceeding the target node is added to the formula to prevent the mobile robot from exceeding the target node.

[0038] The calculation method of dis_angle(v, ω) is as follows: Calculate the coordinates of the mobile robot at time t (x t ,y t ) and the forward point 2 coordinates (x p ,y p ) between the third angle θ pt , refer to Figure 3 As shown in Figure 4, the mobile robot at time t is shown in Figure 5. t The third angle θ pt Calculate dis_angle(v, ω). The calculation formula is dis_angle(v, ω)=|θ pt -θ t |=| arctan[(y p -y t ) / (x p -x t )]- θ t |.

[0039] The calculation method of dis_local(v, ω) is as follows: Calculate the current coordinates of the mobile robot (x 0 ,y 0 ) and the forward point 2 coordinates (x p ,y p ) between the fourth angle θ p0 , refer to Figure 3 As shown in the figure, 3 is the current mobile robot, and the current body orientation angle θ of the mobile robot is calculated. 0 The fourth angle θ p0 The angle difference is θ 0 With θ p0 The angle difference and the linear velocity range Vt are used to calculate dis_local(v, ω). The calculation formula is dis_local(v, ω) = | arctan[(y p –y 0 ) / (x p –x 0 )]- θ 0 |*Vt.

[0040] The calculation method of dis_goal(v, ω) is as follows: when the mobile robot is at the end node of the moving path, determine whether the node on the target path with the shortest distance to the mobile robot is the target node; if not, dis_goal(v, ω) is 0; if so, dis_goal(v, ω)=L*cosθ L ; L is the distance between the end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

[0041] When projection point 1 is inconsistent with the target node, it means that the target node is in front of projection point 1, and it does not interfere with the movement of the mobile robot to forward point 2; when projection point 1 is consistent with the target node, if the mobile robot moves in front of projection point 1, it will cause the mobile robot to exceed the target node, thus passing L and θ L The distance LA that the mobile robot exceeds the target node is calculated so that the mobile robot can accurately return to the target node.

[0042] The working principle of the present invention is as follows: after the mobile robot deviates from the target path, it is necessary to plan the path between the mobile robot and the target path; confirm the shortest distance between the target path and the mobile robot, limit the distance the mobile robot moves toward the target path, and avoid the mobile robot exceeding the target path when moving toward the target path; confirm the forward point forward through the position of the projection point, extend the distance the mobile robot enters the target path, reduce the turning radius of the mobile robot directly entering the target path, and improve the smoothness of the mobile robot's movement.

[0043] When planning the moving path, affected by the speed, if the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle is large, the cost value will increase as the speed increases; then according to the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle, and the angle difference between the next position coordinates of the mobile robot at time t and the forward-looking point angle and the body orientation angle at the current speed, the speed is constrained; if the angle difference of the current position is less than the angle difference at time t, the cost of the mobile robot moving to the target path increases, and the speed needs to be reduced; if the angle difference of the current position is greater than the angle difference at time t, the cost of the mobile robot moving to the target path decreases, and the current speed can be maintained. At the same time, the penalty cost value of exceeding the target node is added to the formula to prevent the mobile robot from exceeding the target node, thereby ensuring that it can move to the target node of the path more reliably without the problem of exceeding the target node, ensuring the reliability of movement.

Claims

1. A path generation method based on forward-looking points, characterized in that: The following steps are involved: S1. Confirm the projection point of the mobile robot on the target path. The projection point is the node on the target path that is closest to the center of the mobile robot. S2, along the direction of the target path, set the next node in front of the projection point as the forward point (x p ,y p ); S3, set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax]; The moving path between the mobile robot and the forward point is calculated by the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω); α, β, γ and c are weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point and the body orientation angle; dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point angle and the body orientation angle; vel(v, ω) is the cost value of the current speed of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

2. The method for generating a path based on a forward-looking point according to claim 1, characterized in that: In S3, the calculation method of dis_angle(v, ω) is as follows: Calculate the coordinates of the mobile robot at time t (x t ,y t ) and the look-ahead point (x p ,y p ) between the third angle θ pt , by moving the robot's body toward angle θ at time t t The third angle θ pt Calculate dis_angle(v, ω).

3. The method for generating a path based on a forward-looking point according to claim 1, characterized in that: In S3, the calculation method of dis_local(v, ω) is as follows: Calculate the current coordinates (x0, y0) of the mobile robot and the forward point (x p ,y p ) between the fourth angle θ p0 , calculate the current body orientation angle θ0 of the mobile robot and the fourth angle θ p0 The angle difference between θ0 and θ p0 The angle difference and the linear velocity range Vt are used to calculate dis_local(v, ω). The calculation formula is dis_local(v, ω) =| arctan[(y p – y0) / (x p –x0)]- θ0|*Vt.

4. The method for generating a path based on a forward-looking point according to claim 1, characterized in that: In S3, the calculation method of dis_goal(v, ω) is as follows: when the mobile robot is at the end node of the moving path, determine whether the node on the target path with the shortest distance to the mobile robot is the target node; if not, dis_goal(v, ω) is 0; if so, dis_goal(v, ω)=L*cosθ L ; L is the distance between the end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

5. The method for generating a path based on a forward-looking point according to claim 1, characterized in that: In step S3, Vt∈[V0-Dv*T,V0+Av*T], the linear velocity range Vt of the mobile robot at time t is calculated; V0 is the current linear velocity of the mobile robot; Dv is the maximum linear acceleration of the mobile robot; Av is the maximum linear deceleration of the mobile robot; and T is the simulation period.

6. The method for generating a path based on a forward-looking point according to claim 1, characterized in that: In step S3, Wt∈[W0-Dw*T,W0+Aw*T], the angular velocity range Wt of the mobile robot at time t is calculated; W0 is the current angular velocity of the mobile robot; Dw is the maximum angular acceleration of the mobile robot; Aw is the minimum angular deceleration of the mobile robot; and T is the simulation period.

Images