Navigation method, system and equipment for exciting robot to approach right to avoid obstacle and storage medium

Abstract

The invention discloses a navigation method, system and device for exciting a robot to avoid obstacles on the right and a storage medium, and belongs to the technical field of robotics.The method comprises the steps that an MPPI controller is used for generating a plurality of navigation tracks, a cost function for exciting the robot to avoid obstacles on the right is established, the total cost value of each navigation track is evaluated, and the cost value of each navigation track is calculated. And driving the robot to avoid the obstacle along the navigation track with the minimum total cost value. According to the invention, the robot follows the behavior mode that the human beings approach to the right to avoid obstacles, and the efficiency and the intelligent degree of man-machine interaction and interaction between robots are improved.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a navigation method, system, device and storage medium for encouraging a robot to circumvent obstacles to the right. Background Art

[0002] With the popularization of mobile robot applications, the density of mobile robots in public places is getting higher and higher, and robots encounter people or other robots more and more frequently during navigation. In the existing technology, when a robot encounters an obstacle (pedestrians or other robots) in the opposite direction during navigation, it usually bypasses the obstacle from a random direction, either from the left or from the right. The uncertainty caused by this navigation behavior that does not comply with the human convention of driving on the right not only affects people's travel efficiency, but also when multiple robots meet in opposite directions in a narrow corridor, they may not give way to each other, eventually blocking the corridor and shutting down the entire multi-robot system.

[0003] MPPI controller stands for Model Predictive Path Integral Controller. The MPPI controller is a predictive controller and the successor to the TEB and pure path following MPC controllers. It uses a sampling-based approach to select the optimal trajectory, which is optimized in successive iterations. It contains plugin-based cost functions that are easy to customize and extend to accommodate a variety of behaviors and behavior properties. The controller is currently applicable to differential, omnidirectional, and Ackermann type robots.

[0004] The MPPI algorithm is a variant of MPC that finds control velocities for the robot through an iterative approach. Using the optimal control sequence from the previous time step and the current state of the robot, a set of random perturbations from a Gaussian distribution are applied. Based on the robot motion model, these noisy control signals generate a set of trajectories through forward simulation. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention provides a navigation method, system, device and storage medium for encouraging a robot to circumvent obstacles to the right.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A navigation method for encouraging a robot to avoid obstacles to the right uses an MPPI controller to generate multiple navigation trajectories, establishes a cost function for encouraging the robot to avoid obstacles to the right, evaluates the total cost of each navigation trajectory based on the cost function, and drives the robot to avoid obstacles along the navigation trajectory with the smallest total cost.

[0008] To optimize the above technical solutions, the specific measures taken also include:

[0009] Furthermore, the cost function for establishing an incentive for the robot to avoid obstacles to the right is specifically:

[0010] Assume that the first point where the reference path intersects the dynamic obstacle is the path point P (x0, y0, θ0), where x0 is the abscissa of the path point P, y0 is the ordinate of the path point P, and θ0 is the direction angle of the direction vector at the path point P;

[0011] Calculate the distance L1 from the end point T (x1, y1) of the navigation track to the straight line where the path point P (x0, y0, θ0) is located; x1 is the horizontal coordinate of the end point T of the navigation track, and y1 is the vertical coordinate of the end point T of the navigation track;

[0012] Calculate the Euclidean distance L2 from the end point T(x1,y1) of the navigation trajectory to the foresight point F(x2,y2). The calculation formula is as follows:

[0013]

[0014] In the formula, x2 represents the horizontal coordinate of the foresight point, and y2 represents the vertical coordinate of the foresight point;

[0015] Convert the distance L1 from the end point of the navigation trajectory to the straight line where the path point is located into the first cost function:

[0016]

[0017] Where Cost1 represents the first cost function, w represents the weight coefficient, a represents the first attenuation coefficient, and b represents the second attenuation coefficient;

[0018] Convert the Euclidean distance L2 from the end point of the navigation trajectory to the foresight point into the second cost function:

[0019]

[0020] Where Cost2 is the second cost function;

[0021] Calculate the total cost function:

[0022]

[0023] In the formula, Cost is the total cost function, and the total cost value of the navigation trajectory is calculated by the total cost function.

[0024] Furthermore, the distance L1 from the end point T(x1, y1) of the navigation trajectory to the straight line where the path point P(x0, y0, θ0) is located is calculated as:

[0025] Direction vector at path point P It is expressed as:

[0026]

[0027] The vector from the path point P(x0,y0,θ0) to the end point of the navigation trajectory T(x1,y1) It is expressed as:

[0028]

[0029] Where x1 is the horizontal coordinate of the end point T of the navigation trajectory, and y1 is the vertical coordinate of the end point T of the navigation trajectory;

[0030] The calculation formula of distance L1 is as follows:

[0031]

[0032] because , so the final distance formula is .

[0033] The present invention also provides a navigation system for encouraging a robot to avoid obstacles by keeping to the right, comprising:

[0034] MPPI controller, used to generate multiple navigation trajectories;

[0035] The cost function building module is used to build a cost function that encourages the robot to avoid obstacles to the right;

[0036] The trajectory selection module is used to evaluate the total cost value of each navigation trajectory based on the cost function, and drive the robot to avoid obstacles along the navigation trajectory with the smallest total cost value.

[0037] The present invention also proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the navigation method for encouraging a robot to navigate around obstacles to the right as described above is implemented.

[0038] The present invention also proposes a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the above-mentioned navigation method for encouraging a robot to navigate around obstacles to the right.

[0039] The beneficial effects of the present invention are as follows: when the reference path passes through the middle of the obstacle, the right side, or a little to the left of the middle, the obstacle is circumvented from the right side. When the reference path passes through the obstacle very far to the left, the obstacle is circumvented from the left side. When the obstacle is very large, the robot will not circumvent the obstacle indefinitely. When the maximum obstacle circumvention distance is reached, the robot will stop circumventing the obstacle, report, and wait for a new reference path. The present invention improves the efficiency and intelligence of human-machine interaction and interaction between robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 Schematic diagram of establishing a cost function that encourages the robot to avoid obstacles to the right.

[0041] Figure 2 is the image of the first cost function.

[0042] Figure 3 This is a schematic diagram of the mobile robot circumventing the obstacle from the right side when the reference path passes through the middle of the obstacle.

[0043] Figure 4 This is a schematic diagram of the mobile robot circumventing the obstacle from the right side when the reference path passes through the center of the obstacle to the right.

[0044] Figure 5 This is a schematic diagram of the mobile robot circumventing the obstacle from the right side when the reference path passes through the center and slightly left of the obstacle.

[0045] Figure 6 This is a schematic diagram of the mobile robot circumventing the obstacle from the left side when the reference path passes through the center of the obstacle and is very left of it.

[0046] Figure 7 Schematic diagram of the robot stopping on the side of the reference path and waiting for a new reference path when the obstacle is very large. DETAILED DESCRIPTION

[0047] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0048] Embodiment 1

[0049] The present invention proposes a navigation method for encouraging a robot to go around obstacles to the right, and uses an MPPI controller to generate multiple navigation trajectories.

[0050] In order to make the mobile robot imitate the human behavior of going to the right to avoid obstacles during navigation, a cost function is established to encourage the robot to go to the right to avoid obstacles. Figure 1 , Figure 1 The triangle in the middle is the mobile robot, the circle is the dynamic obstacle, the multiple divergent curves in front of the mobile robot are the multiple navigation trajectories generated by the MPPI controller, point T is the end point of the navigation trajectory, point F is the foresight point, and the reference path ( Figure 1 The first point where the navigation trajectory (the red line in the middle) intersects with the dynamic obstacle is the path point P (x0, y0, θ0), where x0 is the horizontal coordinate of the path point P, y0 is the vertical coordinate of the path point P, and θ0 is the direction angle of the direction vector at the path point P; L1 is the distance from the end point T of the navigation trajectory to the straight line where the path point P is located, and L2 is the Euclidean distance from the end point T of the navigation trajectory to the foresight point F.

[0051] Direction vector at path point P It is expressed as:

[0052]

[0053] The vector from the path point P(x0,y0,θ0) to the end point of the navigation trajectory T(x1,y1) It is expressed as:

[0054]

[0055] Where x1 is the horizontal coordinate of the end point T of the navigation trajectory, and y1 is the vertical coordinate of the end point T of the navigation trajectory;

[0056] Calculate the distance L1 from the end point T(x1,y1) of the navigation trajectory to the straight line where the path point P(x0,y0,θ0) is located; the calculation formula of the distance L1 is as follows:

[0057]

[0058] because , so the final distance formula is .

[0059] Based on the distance L1, we can determine whether the end point T(x1,y1) of the trajectory is on the left or right side of the line where the path point P(x0,y0,θ0) is located:

[0060] if , then the end point T of the trajectory is on the right side of the straight line;

[0061] if , then the end point T of the trajectory is on the left side of the straight line;

[0062] Calculate the Euclidean distance L2 from the end point T(x1,y1) of the navigation trajectory to the foresight point F(x2,y2)

[0063]

[0064] In the formula, x2 represents the horizontal coordinate of the foresight point, and y2 represents the vertical coordinate of the foresight point;

[0065] Convert the distance L1 from the end point of the navigation trajectory to the straight line where the path point is located into the first cost function:

[0066]

[0067] Wherein, Cost1 represents the first cost function, w represents the weight coefficient, a represents the first attenuation coefficient, and b represents the second attenuation coefficient.

[0068] Figure 2The horizontal axis represents L1, and the vertical axis represents Cost1. The weight coefficient w is 1.3, the first attenuation coefficient a is 16, and the second attenuation coefficient b is 28.

[0069] The cost Cost1 decreases as the absolute value of the distance L1 increases. When , the cost value decays to 0;

[0070] when When the trajectory end point T is on the right side of the path, the cost curve is Figure 2 In the middle blue curve on the left side of the Y axis, Cost1 decays faster;

[0071] when When , the end point T of the trajectory is on the left side of the path, and the cost curve is Figure 2 In the part of the red curve to the right of the Y-axis, Cost1 decays more slowly.

[0072] Convert the Euclidean distance L2 from the end point of the navigation trajectory to the foresight point into the second cost function:

[0073]

[0074] Wherein, Cost2 is the second cost function; the value of the second cost function Cost2 decreases as L2 decreases.

[0075] Calculate the total cost function:

[0076]

[0077] Where Cost is the total cost function.

[0078] Evaluate the total cost of each navigation trajectory and drive the robot to avoid obstacles along the navigation trajectory with the smallest total cost.

[0079] In order to minimize the cost, the MPPI controller will select the trajectory with the smallest Cost value.

[0080] Because Cost1 decays faster when the trajectory end point T is on the right side of the path, the MPPI controller will select the trajectory with the trajectory end point T on the right side of the path, thereby driving the robot to bypass the obstacle from the right side.

[0081] Because the closer the end point T of the trajectory is to the front, the smaller Cost2 is, so the trajectory will move forward immediately after bypassing the obstacle; When , Cost1 decays to 0, When the robot encounters a wide obstacle, it will not go around to the right without restriction.

[0082] By adjusting the weight coefficient The size of can adjust the weight of L1 and L2 in Cost. If it is too small, the robot will tend to follow the reference path instead of avoiding obstacles. If it is too large, the robot will just go around the obstacle instead of moving forward.

[0083] By adjusting the first attenuation coefficient a and the second attenuation coefficient b, the attenuation speed of the curve and the L1 value attenuated to 0 can be adjusted, thereby affecting the tendency to avoid obstacles to the right and the maximum obstacle avoidance distance.

[0084] Embodiment 2

[0085] The present invention proposes a navigation system for encouraging a robot to avoid obstacles to the right, corresponding to the method of the first embodiment, comprising:

[0086] MPPI controller, used to generate multiple navigation trajectories;

[0087] The cost function building module is used to build a cost function that encourages the robot to avoid obstacles to the right;

[0088] The trajectory selection module is used to evaluate the total cost value of each navigation trajectory based on the cost function, and drive the robot to avoid obstacles along the navigation trajectory with the smallest total cost value.

[0089] The implementation methods of each module and module function in the system are completely consistent with the steps of the method in Example 1, so they will not be repeated here.

[0090] Embodiment 3

[0091] The present invention proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the navigation method for encouraging a robot to navigate around obstacles to the right as described in Example 1 is implemented.

[0092] Embodiment 4

[0093] The present invention proposes a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the navigation method for encouraging a robot to navigate around obstacles to the right as described in the first embodiment.

[0094] The schematic diagram of the obstacle avoidance effect of the present invention is as follows Figures 3 to 7 When the reference path passes through the middle of the obstacle, the right side, or a little to the left of the middle, the obstacle will be circumvented from the right side. When the reference path passes through the very left side of the obstacle, the obstacle will be circumvented from the left side. When the obstacle is large, the robot will not circumvent the obstacle indefinitely. When the maximum obstacle circumvention distance is reached, the robot will stop circumventing the obstacle, report it, and wait for a new reference path. Figure 3It means that when the reference path passes through the middle of the obstacle, the mobile robot goes around the obstacle from the right side; Figure 4 It means that the reference path passes through the middle of the obstacle and slightly to the right, and the mobile robot goes around the obstacle from the right side; Figure 5 It means that the reference path passes through the middle of the obstacle and slightly to the left, and the mobile robot goes around the obstacle from the right side; Figure 6 It means that the reference path passes through the middle of the obstacle and is very far to the left. The mobile robot goes around the obstacle from the left side. Figure 7 It means that when the obstacle is very large, the obstacle avoidance distance will not increase indefinitely, and the robot will stop on the side of the reference path and wait for a new reference path.

[0095] The total cost function designed by the present invention can be used not only for model predictive control, but also for reinforcement learning: the cost function can be converted into a reward function by simply negating the Cost value: the robot is rewarded for avoiding obstacles to the right and punished for avoiding obstacles to the left, thereby training the neural network of reinforcement learning to form network parameters for avoiding obstacles to the right.

[0096] In the embodiments disclosed in the present application, the computer storage medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or apparatus. The computer storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or equipment, or any suitable combination of the foregoing. More specific examples of computer storage media may include electrical connections based on one or more lines, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0097] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0098] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A navigation method for motivating a robot to avoid obstacles to the right, using an MPPI controller to generate multiple navigation trajectories, characterized in that: A cost function is established to encourage the robot to avoid obstacles to the right, and the total cost value of each navigation trajectory is evaluated based on the cost function, so as to drive the robot to avoid obstacles along the navigation trajectory with the smallest total cost value.

2. The navigation method for encouraging a robot to navigate around obstacles to the right as claimed in claim 1, characterized in that: The cost function for establishing an incentive for the robot to avoid obstacles to the right is specifically: Assume that the first point where the reference path intersects the dynamic obstacle is the path point P (x0, y0, θ0), where x0 is the abscissa of the path point P, y0 is the ordinate of the path point P, and θ0 is the direction angle of the direction vector at the path point P; Calculate the distance L1 from the end point T (x1, y1) of the navigation track to the straight line where the path point P (x0, y0, θ0) is located; x1 is the horizontal coordinate of the end point T of the navigation track, and y1 is the vertical coordinate of the end point T of the navigation track; Calculate the Euclidean distance L2 from the end point T(x1,y1) of the navigation trajectory to the foresight point F(x2,y2). The calculation formula is as follows: ; In the formula, x2 represents the horizontal coordinate of the foresight point, and y2 represents the vertical coordinate of the foresight point; Convert the distance L1 from the end point of the navigation trajectory to the straight line where the path point is located into the first cost function: ; Where Cost1 represents the first cost function, w represents the weight coefficient, a represents the first attenuation coefficient, and b represents the second attenuation coefficient; Convert the Euclidean distance L2 from the end point of the navigation trajectory to the foresight point into the second cost function: ; Where Cost2 is the second cost function; Calculate the total cost function: ; In the formula, Cost is the total cost function, and the total cost value of the navigation trajectory is calculated by the total cost function.

3. The navigation method for encouraging a robot to navigate around obstacles to the right as described in claim 2, characterized in that: The method for calculating the distance L1 from the end point T(x1, y1) of the navigation trajectory to the straight line where the path point P(x0, y0, θ0) is located is as follows: Direction vector at path point P It is expressed as: ; The vector from the path point P(x0,y0,θ0) to the end point of the navigation trajectory T(x1,y1) It is expressed as: ; Where x1 is the horizontal coordinate of the end point T of the navigation trajectory, and y1 is the vertical coordinate of the end point T of the navigation trajectory; The calculation formula of distance L1 is as follows: ; because , so the final distance formula is .

4. A navigation system for encouraging a robot to avoid obstacles to the right, characterized in that: include: MPPI controller, used to generate multiple navigation trajectories; The cost function building module is used to build a cost function that encourages the robot to avoid obstacles to the right; The trajectory selection module is used to evaluate the total cost value of each navigation trajectory based on the cost function, and drive the robot to avoid obstacles along the navigation trajectory with the smallest total cost value.

5. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the navigation method for encouraging a robot to navigate around obstacles to the right is implemented as described in any one of claims 1 to 3.

6. A computer-readable storage medium storing a computer program, characterized in that: The computer program enables the computer to execute the navigation method for encouraging the robot to navigate around obstacles to the right as described in any one of claims 1-3.

Images