Robot dynamic path tracking method and system

Abstract

The invention discloses a robot dynamic path tracking method and system, and the method comprises the steps: constructing a nonlinear model prediction control model for a differential robot chassis; building a target cost function by considering the obstacle environment information around the robot; adjusting a control input vector in the nonlinear model predictive control model, performing real-time tracking control on the robot, calculating a target cost function value, taking the corresponding control input vector as an optimal control input vector when the target cost function value is minimum, and performing real-time tracking control on the robot. Performing dynamic path tracking control on the robot by using the optimal control input vector; the method has the advantages that real-time obstacle avoidance is reliably carried out while the robot tracks the path, complicated path planning calculation steps are not needed, the calculation process is simple, and the efficiency is high.

Description

Technical Field

[0001] The present invention relates to the field of robot path tracking, and in particular to a robot dynamic path tracking method and system. Background Art

[0002] The robot navigates in an unknown complex dynamic scene. After the global planning outputs the global path, the robot needs to track the global path in real time. The traditional tracking only realizes the robot position tracking global path under the robot kinematics and dynamics at the control level. In the tracking process, it responds to static or dynamic obstacles that may be encountered, and continues to track the global path after avoiding the obstacles. For example, Chinese Patent Publication No. CN116185018A discloses a path planning and control method for an amphibious robot, comprising the following steps: Step 1: According to the environmental terrain elevation model that has been imported into the amphibious robot system, the HHO algorithm is used to calculate the global optimal path from the starting point to the target point; Step 2: According to the real-time geographical location of the amphibious robot, it starts to navigate according to the global optimal path; Step 3: When the amphibious robot encounters an obstacle, it determines whether the obstacle is a known obstacle, and controls the amphibious robot to avoid the obstacle according to the specific situation of the obstacle.

[0003] Therefore, a real-time and efficient dynamic obstacle avoidance and path tracking method is of great significance for the autonomous navigation of robots in unknown environments. However, the current dynamic obstacle avoidance generally adopts the local path method, and then sends the local path to the control tracking module, so the calculation steps are complicated and the efficiency is low. Summary of the invention

[0004] The technical problem to be solved by the present invention is that the calculation steps of the existing dynamic obstacle avoidance and path tracking methods are complex and inefficient.

[0005] The present invention solves the above technical problems by the following technical means: a robot dynamic path tracking method, comprising the following steps:

[0006] Step 1: For the differential robot chassis, a nonlinear model predictive control model is constructed;

[0007] Step 2: Consider the obstacle environment information around the robot to construct the target cost function;

[0008] Step 3: Adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. Use the optimized control input vector to perform dynamic path tracking control on the robot.

[0009] Furthermore, the step 1 comprises:

[0010] Assume that the state of the robot in the global system is

[0011]

[0012] Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system;

[0013] For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows

[0014]

[0015] in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and R T represents the transposed matrix of R, and u represents the control input vector of the robot.

[0016] Furthermore, the expression of the rotation matrix is

[0017]

[0018] The expression of the control input vector of the robot is:

[0019]

[0020] Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

[0021] Furthermore, the step 2 includes:

[0022] Construct the target cost function, the formula is as follows

[0023]

[0024] Among them, j represents the jth moment, N c represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, || || represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, The third weight.

[0025] Furthermore, the step 2 further includes:

[0026] Considering the obstacle environment information around the robot, the collision cost is obtained. The formula is as follows

[0027]

[0028] Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold;

[0029] Add the collision cost to the target cost function, and the final target cost function is

[0030]

[0031] Among them, W dis The fourth weight.

[0032] The present invention also provides a robot dynamic path tracking system, comprising:

[0033] A model building module is used to build a nonlinear model predictive control model for a differential robot chassis;

[0034] The cost function construction module is used to construct the target cost function by considering the obstacle environment information around the robot;

[0035] The rolling optimization control module is used to adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. The optimized control input vector is used to perform dynamic path tracking control on the robot.

[0036] Furthermore, the model building module is also used for:

[0037] Assume that the state of the robot in the global system is

[0038]

[0039] Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system;

[0040] For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows

[0041]

[0042] in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and RT represents the transposed matrix of R, and u represents the control input vector of the robot.

[0043] Furthermore, the expression of the rotation matrix is

[0044]

[0045] The expression of the control input vector of the robot is:

[0046]

[0047] Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

[0048] Furthermore, the cost function construction module is also used for:

[0049] Construct the target cost function, the formula is as follows

[0050]

[0051] Among them, j represents the jth moment, N c represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, || || represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, The third weight.

[0052] Furthermore, the cost function construction module is also used for:

[0053] Considering the obstacle environment information around the robot, the collision cost is obtained. The formula is as follows

[0054]

[0055] Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold;

[0056] Add the collision cost to the target cost function, and the final target cost function is

[0057]

[0058] Among them, W dis The fourth weight.

[0059] The advantages of the present invention are as follows: the present invention considers the obstacle environment information around the robot to construct a target cost function. During autonomous navigation, when the global path is known, the robot can track the path and reliably avoid obstacles in real time by minimizing the target cost function. Only the control input vector needs to be adjusted to perform optimization and adjustment. No complicated path planning calculation steps are required, and the calculation process is simple and efficient. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 The present invention is a flowchart of a robot dynamic path tracking method disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are 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.

[0062] Example 1

[0063] like Figure 1 As shown, the present invention provides a robot dynamic path tracking method, comprising the following steps:

[0064] S1. For the differential robot chassis, a nonlinear model predictive control model is constructed; the specific process is as follows:

[0065] Assume that the state of the robot in the global system is

[0066]

[0067] Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system;

[0068] For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows

[0069]

[0070] in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and R T represents the transposed matrix of R, and u represents the control input vector of the robot.

[0071] The expression of the rotation matrix is

[0072]

[0073] The expression of the control input vector of the robot is:

[0074]

[0075] Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

[0076] The nonlinear model predictive control (NMPC) method has strong robustness to system model uncertainty and has been widely used in control-related fields. It also has obvious control effects on nonlinear systems and is an effective means to solve path tracking problems. The NMPC control algorithm divides complex problems into multiple stages and gradually obtains the optimal solution for each stage through a rolling optimization strategy, which reduces the complexity of calculations and has strong adaptability to dynamic environments. It optimizes the deviation cost function between the discrete state and control quantity predicted by the system model and the ideal reference through nonlinear optimization to obtain the control input that satisfies the minimum cost function. Generally, the differential equation of the system is as follows:

[0077]

[0078] Ideally, within a certain prediction period, the state predicted by the system model is equal to the reference path, which can be expressed as:

[0079] x e (t) = x r (t)-x(t)=0

[0080] The function of the nonlinear model predictive controller is to calculate the control amount sequence that minimizes the target cost function within the prediction period. Ideally, the state predicted by the above system model is equal to the reference path, but in practical applications, it is necessary to construct a target cost function to minimize the target cost function. The target cost function is introduced below.

[0081] S2. Consider the obstacle environment information around the robot to construct the target cost function; the specific process is as follows:

[0082] Construct the target cost function, the formula is as follows

[0083]

[0084] Among them, j represents the jth moment, Nc represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, || || represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, W Nc The third weight.

[0085] Through nonlinear optimization, the input sequence of the system is realized when the target cost function is minimized, and periodic rolling optimization is performed to track the target.

[0086] In the process of real-time obstacle avoidance in navigation in unknown environments, it is necessary to build a real-time map within the robot's sensor range, and the map used must meet the robot's real-time obstacle avoidance and efficiency requirements. By using the distance field to represent the distance value from each grid to the nearest obstacle as the environmental model for real-time obstacle avoidance, the distance field map SDF is constructed by using the Euclidean transformation algorithm based on EDT ((Euclidean Distance Transform)). The calculation process of the distance field in the SDF is actually the process of calculating the minimum distance from each idle voxel to the nearby obstacle voxel within the map update range. By constructing the SDF map centered on the robot in real time, the Euclidean distance of the robot's position from the nearest obstacle can be obtained. A distance value greater than zero indicates that the grid is outside the obstacle, less than zero indicates that the grid is within the obstacle, and equal to zero indicates that it is at the obstacle boundary. The Euclidean distance is introduced as a cost into the cost function of the nonlinear model prediction algorithm as a collision cost. Obviously, it is safest to have a collision cost of 0. What needs to be considered is that when the robot's distance from the obstacle reaches a certain threshold, the cost is equal to 0. The gradient of the distance field at each point can be approximately calculated by the finite difference method. Based on the above analysis, the present invention considers the obstacle environment information around the robot and obtains the collision cost. The formula is as follows

[0087]

[0088] Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold;

[0089] Add the collision cost to the target cost function, and the final target cost function is

[0090]

[0091] Among them, W dis The fourth weight.

[0092] S3. Adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. Use the optimized control input vector to perform dynamic path tracking control on the robot.

[0093] Through the above technical scheme, the present invention considers the obstacle environment information around the robot to construct a target cost function. During autonomous navigation, when the global path is known, the robot can track the path and reliably avoid obstacles in real time by minimizing the target cost function. Only the control input vector needs to be adjusted to perform optimization and adjustment. There is no need for complicated path planning calculation steps, and the calculation process is simple and efficient.

[0094] Example 2

[0095] Based on embodiment 1, embodiment 2 of the present invention further provides a robot dynamic path tracking system, including:

[0096] A model building module is used to build a nonlinear model predictive control model for a differential robot chassis;

[0097] The cost function construction module is used to construct the target cost function by considering the obstacle environment information around the robot;

[0098] The rolling optimization control module is used to adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. The optimized control input vector is used to perform dynamic path tracking control on the robot.

[0099] Specifically, the model building module is also used for:

[0100] Assume that the state of the robot in the global system is

[0101]

[0102] Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system;

[0103] For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows

[0104]

[0105] in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and R T represents the transposed matrix of R, and u represents the control input vector of the robot.

[0106] More specifically, the expression of the rotation matrix is

[0107]

[0108] The expression of the control input vector of the robot is:

[0109]

[0110] Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

[0111] More specifically, the cost function building module is also used to:

[0112] Construct the target cost function, the formula is as follows

[0113]

[0114] Among them, j represents the jth moment, N c represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, || || represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, The third weight.

[0115] More specifically, the cost function building module is also used to:

[0116] Considering the obstacle environment information around the robot, the collision cost is obtained. The formula is as follows

[0117]

[0118] Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold;

[0119] Add the collision cost to the target cost function, and the final target cost function is

[0120]

[0121] Among them, W dis The fourth weight.

[0122] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot dynamic path tracking method, characterized in that: The following steps are involved: Step 1: For the differential robot chassis, a nonlinear model predictive control model is constructed; Step 2: Consider the obstacle environment information around the robot to construct the target cost function; Step 3: Adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. Use the optimized control input vector to perform dynamic path tracking control on the robot.

2. A robot dynamic path tracking method according to claim 1, characterized in that: The step one comprises: Assume that the robot's state in the global system is Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system; For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and R T represents the transposed matrix of R, and u represents the control input vector of the robot.

3. A robot dynamic path tracking method according to claim 2, characterized in that: The expression of the rotation matrix is The expression of the control input vector of the robot is: Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

4. A robot dynamic path tracking method according to claim 2, characterized in that: The second step comprises: Construct the target cost function, the formula is as follows Among them, j represents the jth moment, N c represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, |||| represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, The third weight.

5. A robot dynamic path tracking method according to claim 4, characterized in that: The step 2 also includes: Considering the obstacle environment information around the robot, the collision cost is obtained. The formula is as follows Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold; Add the collision cost to the target cost function, and the final target cost function is Among them, W dis The fourth weight.

6. A robot dynamic path tracking system, characterized in that: include: A model building module is used to build a nonlinear model predictive control model for the differential robot chassis; The cost function construction module is used to construct the target cost function by considering the obstacle environment information around the robot; The rolling optimization control module is used to adjust the control input vector in the nonlinear model predictive control model, perform real-time tracking control on the robot, calculate the target cost function value, and the control input vector corresponding to the minimum target cost function value is the optimized control input vector. The optimized control input vector is used to perform dynamic path tracking control on the robot.

7. A robot dynamic path tracking system according to claim 6, characterized in that: The model building module is also used to: Assume that the robot's state in the global system is Among them, x and y represent the horizontal and vertical coordinates of the robot, and θ represents the heading angle of the robot in the global system; For the differential robot chassis, a nonlinear model predictive control model is constructed, and the formula is as follows in, represents the derivative of X, R is the rotation matrix, which represents the transformation from the robot coordinate system to the world coordinate system, and R T represents the transposed matrix of R, and u represents the control input vector of the robot.

8. A robot dynamic path tracking system according to claim 7, characterized in that: The expression of the rotation matrix is The expression of the control input vector of the robot is: Among them, v x Indicates the robot's lateral velocity, v y represents the longitudinal velocity of the robot, and ω represents the angular velocity of the robot.

9. A robot dynamic path tracking system according to claim 8, characterized in that: The cost function building module is also used for: Construct the target cost function, the formula is as follows Among them, j represents the jth moment, N c represents the end time, x k represents the actual state variable at time k, represents the state variable of the target tracked at time k, |||| represents the binary norm symbol, u k represents the control input vector at time k, represents the expected control amount at time k, Indicates the end time status, represents the desired state at the end time, W x is the first weight, W u is the second weight, The third weight.

10. A robot dynamic path tracking system according to claim 9, characterized in that: The cost function building module is also used for: Considering the obstacle environment information around the robot, the collision cost is obtained. The formula is as follows Among them, α is the control cost amplitude, r is the control cost increase rate, d is the Euclidean distance of the grid to the nearest obstacle, and d0 is the minimum collision distance threshold; Add the collision cost to the target cost function, and the final target cost function is Among them, W dis The fourth weight.

Images