Tracked robot trajectory tracking method based on corner control quantity optimization

Abstract

The invention discloses a tracked robot trajectory tracking method based on rotation angle control quantity optimization, and the method comprises the steps: obtaining a navigation point of a target route of a tracked robot through a positioning device, carrying out the initial path planning through the navigation point, and obtaining an initial planning path; the current position, the current course angle and the current linear speed of the tracked robot are obtained in real time, and the nearest distance between the current position and the initial planning path is determined; according to the determined nearest distance and the current linear speed, a rotation angle control quantity is obtained by improving a track planning algorithm of the tracked robot, and the linear speed and the angular speed of the tracked robot are adjusted through the rotation angle control quantity; and controlling the tracked robot to move according to the adjusted linear velocity and angular velocity of the tracked robot, so that the tracked robot tracks the initial planned path. A controller is improved and optimized, a rotation angle control quantity is used as a setting parameter of an output speed, a control algorithm is finally improved, the trajectory tracking precision of the tracked robot is improved, and particularly the path tracking precision during steering is improved.

Description

Technical Field

[0001] The invention relates to robot path planning, and in particular to a track tracking method of a crawler robot based on optimization of a turning angle control amount. Background Art

[0002] When agricultural spraying robots operate autonomously, they must have high-precision positioning, trajectory planning and control capabilities. The development of agricultural spraying robots has made certain progress and achievements, but there is still a need to further improve the positioning and navigation technology of tracked robots. The Stanley path control algorithm can be applied in agricultural scenarios with continuous paths and low-speed operation. Currently, the Stanley algorithm is applicable to four-wheeled vehicles. It is rarely used in the automatic navigation of tracked robots and the accuracy is not high. Summary of the invention

[0003] Purpose of the invention: In view of the above shortcomings, the present invention provides a high-precision tracked robot trajectory tracking method based on the optimization of the angle control amount.

[0004] The present invention also provides a high-precision track tracking system for a crawler robot based on optimization of a turning angle control amount.

[0005] Technical solution: To solve the above problems, the present invention adopts a track tracking method of a crawler robot based on the optimization of the turning angle control amount, comprising the following steps: (1) Obtaining the navigation points of the target route of the crawler robot through the positioning device, performing initial path planning based on the navigation points, and obtaining the initial planned path; (2) Obtain the current position, heading angle, and linear velocity of the crawler robot in real time, and determine the shortest distance between the current position and the initial planned path; (3) According to the determined shortest distance and the current linear velocity, the angle control value is obtained by improving the Stanley controller, and the linear velocity and angular velocity of the crawler robot are adjusted by the angle control value; The linear speed adjustment formula of the crawler robot is: ; The angular velocity adjustment formula of the tracked robot is: ; in, Tracked robot The linear speed at the moment, Tracked robot The linear speed at the moment, is the linear velocity coefficient, is the angle control quantity, Tracked robot The angular velocity at time, and are the angular velocity coefficients, Tracked robot The lateral error of the moment, Tracked robot lateral error of the moment; (4) Control the movement of the crawler robot according to the adjusted linear velocity and angular velocity of the crawler robot so that the crawler robot follows the initial planned path.

[0006] Furthermore, according to the navigation points obtained in the step (1), the initial planned path is obtained by using a cubic spline interpolation optimization algorithm: the navigation points are divided into N small intervals, and a cubic polynomial is fitted between every two navigation points in one interval to obtain a set of interpolation functions. The boundary conditions are set to iteratively obtain the interpolation function group to generate the initial planned path.

[0007] Furthermore, a set of interpolation functions includes: The cubic polynomial between every two navigation points : ; Cubic Polynomial The first derivative form of and the second-order derivative form : ; ; in, Represents a cubic polynomial The independent variable, Represents the horizontal coordinate of the i-th navigation point, , , , are constants, ; Boundary conditions include: Interpolation conditions: ; Continuity conditions: ; First-order derivative continuity condition: ; Second-order derivative continuity conditions: ; Natural boundary conditions: , ; in, Indicates the ordinate of the i-th navigation point, Indicates the horizontal axis The function value at .

[0008] Furthermore, the improved Stanley crawler robot trajectory planning algorithm formula is: ; in, is the gain parameter.

[0009] The present invention also adopts a track tracking system of a crawler robot based on the optimization of the turning angle control amount, comprising: The path planning module is used for the positioning device to obtain the navigation points of the target route of the crawler robot, and to perform initial path planning through the navigation points to obtain the initial planned path; The error determination module is used to obtain the current position, current heading angle and current linear speed of the crawler robot in real time, and determine the shortest distance between the current position and the initial planned path; An adjustment amount determination module is used to obtain a turning angle control amount by improving the Stanley crawler robot trajectory planning algorithm according to the determined shortest distance and the current linear velocity, and adjust the linear velocity and angular velocity of the crawler robot by the turning angle control amount; The linear speed adjustment formula of the crawler robot is: ; The angular velocity adjustment formula of the tracked robot is: ; in, Tracked robot The linear speed at the moment, Tracked robot The linear speed at the moment, is the linear velocity coefficient, is the angle control quantity, Tracked robot The angular velocity at time, and are the angular velocity coefficients, Tracked robot The lateral error of the moment, Tracked robot lateral error of the moment; The path tracking module is used to control the movement of the crawler robot according to the adjusted linear velocity and angular velocity of the crawler robot, so that the crawler robot tracks the initial planned path.

[0010] Furthermore, the positioning device adopts an RTK module, the RTK module collects original NMEA data, and the path planning module converts the NMEA data into a WGS84 coordinate system to obtain coordinate data.

[0011] Furthermore, the path planning module obtains the initial planned path through the cubic spline interpolation optimization algorithm: the navigation points are divided into N small intervals, and in one interval, a cubic polynomial is fitted between every two navigation points to obtain a set of interpolation functions, and the boundary conditions are set to iteratively obtain the interpolation function group to generate the initial planned path.

[0012] Furthermore, the improved Stanley crawler robot trajectory planning algorithm formula is: ; in, is the gain parameter.

[0013] The present invention also adopts a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0014] The present invention also adopts a computer-readable storage medium on which a computer program is stored, and the computer program implements the steps of the above method when executed by a processor.

[0015] Beneficial effect: Compared with the prior art, the present invention has the following significant advantages: the Stanley controller is improved and optimized, the angle control amount is used as the setting parameter of the output speed, and finally the Stanley control algorithm is improved to improve the trajectory tracking accuracy of the crawler robot, especially the path tracking accuracy during turning. When the driving speed is set to 0.5m / s, compared with the situation of the unplanned algorithm and the Stanley algorithm, the cubic spline-Stanley trajectory planning and control algorithm reduces the average error by 0.12m and the maximum error by 0.49m in the short-distance scene 1 with open space and continuous turns, and reduces the average error by 0.02m and the maximum error by 0.44m in the medium-distance scene 2 with narrow roads and small turning space, and reduces the maximum error by 0.66m in the scene 3 on the road with open environment and long distance driving. Therefore, the cubic spline-Stanley trajectory planning and control algorithm proposed in the present invention effectively optimizes the global trajectory, and the path control capability is significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a schematic diagram of the overall flow of the trajectory tracking method of the present invention.

[0017] Figure 2 It is a schematic diagram of the motion trajectory of a tracked robot without a planning algorithm in the prior art.

[0018] Figure 3 It is a schematic diagram of the motion trajectory of the crawler robot of the planning algorithm in the present invention.

[0019] Figure 4 It is a schematic diagram of the position error between the current position of the crawler robot and the initial planned path in the present invention.

[0020] Figure 5 It is a schematic diagram of the initial planning path of the crawler robot in the present invention.

[0021] Figure 6 It is a schematic diagram comparing the running trajectories of the crawler robot under different path planning algorithms and control algorithms in the present invention.

[0022] Figure 7 It is a comparison curve diagram of the lateral error of the running trajectory of the crawler robot under different path planning algorithms and control algorithms in the present invention. DETAILED DESCRIPTION

[0023] like Figure 1 As shown, in this embodiment, a track tracking method of a tracked robot based on optimization of a turning angle control amount comprises the following steps: (1) Use the RTK module to collect navigation points on the target route. First, obtain the original NMEA data, convert it into the WGS84 coordinate system after processing, and save it.

[0024] The conventional method directly obtains the planned path through the obtained navigation points, such as Figure 2 As shown, in this embodiment, a cubic spline trajectory planning algorithm is designed to read the saved navigation point file, divide the navigation points into N small intervals, fit a cubic polynomial between every two navigation points in an interval, set boundary conditions and iterate to solve the equation group, and then generate a smooth path, such as Figure 3 shown.

[0025] The M known navigation points will be saved in the file Divided into N small intervals, we can get a set of functions , a cubic polynomial is fitted between every two navigation points. For example, the interpolation function between each cell can be expressed as , and also includes the function The first-order derivative form and the second-order derivative form are shown below: ; ; ; in, Representation function The independent variable, , , , Piecewise functions In the The coefficients of each segment curve, .

[0026] According to the definition of cubic interpolation, there are n cubic polynomials, so there are 4n unknown coefficients. 4n-2 equations can be determined by spline conditions and interpolation conditions. The natural boundary conditions still lack two conditions for solving the coefficient equations, and then the matching boundary conditions are selected. The boundary conditions are shown below.

[0027] Interpolation conditions: ; Continuity conditions: ; First-order derivative continuity condition: ; Second-order derivative continuity conditions: ; Natural boundary conditions: , ; in, Indicates that the navigation point is The vertical coordinate of Represents an interval function The horizontal coordinate of the navigation point The function value at Interval Functions The second derivative of the horizontal coordinate of the navigation point The function value at .

[0028] The system of equations is solved iteratively. When the corresponding coefficient matrix equation is determined, the matrix equation is solved iteratively to obtain the coefficient of each cell, generate a smooth and continuous path, and thus construct a complete path.

[0029] First, substitute the formulas in the spline conditions, interpolation conditions, and natural boundary conditions into , and obtain the following equation: ; Again Subtract the first and second derivatives of . , , and then substitute into the above formula to get and , we get the following equation: ; ; Substituting the above equation back into the equation for subtracting derivatives, we can get the following equation based on the natural boundary conditions: ; Then transform the equations into matrix form as follows: ; Finally, the piecewise function is obtained by iteratively solving the above matrix equation. In the The coefficient variables on the segment curve , , , , we can get denser navigation points and thus obtain the optimal planning trajectory of the tracked robot.

[0030] (2) The RTK module updates the positioning and heading to determine the current position at time t With heading angle , calculate the shortest distance between the optimal planning trajectory generated by the trajectory planning algorithm optimized by cubic spline interpolation and the current position , that is, the lateral error.

[0031] (3) Transverse error , gain parameter and current speed input Improve the Stanley controller and change the output angle control As the setting parameters of linear speed and angular speed, it is used to set the linear speed sent to the crawler robot at the next moment. and angular velocity , control the movement of the crawler robot. In the process of controlling the movement of the crawler robot, the RTK module updates the positioning and heading, and updates the current position and heading to With heading angle , the trajectory control of the tracked robot is achieved through continuous updating and iteration.

[0032] like Figure 4 As shown in the figure, point P1 is a point on the optimal planning trajectory, which is the point closest to the planned path of the crawler robot. The tangent line of point P1 intersects with point P2 of the target route. The distance between point P1 and point P2 is , which turns the tracked robot from its current heading to the target heading. Defined as the lateral tracking error angle, the lateral tracking error angle Without considering the heading tracking error angle In the case of , when the crawler robot body is parallel to the tangent direction of point P1 on the optimal planning trajectory, the angle between the current body and the line connecting point P1 and the expected target point P2. According to the geometric relationship of the model, the nonlinear proportional function that controls the lateral tracking error can be solved as shown in the following formula: ; Among them, the distance between point P1 and point P2 Is a speed The relevant values ​​are expressed in terms of speed and gain parameter k, as shown below: ; According to the lateral error angle , its rate of change over time is given by: ; in According to the geometric relationship of the model, the following formula can be obtained: ; Therefore, when the lateral error is very small (less than 0.1), the following formula is obtained: .

[0033] The lateral error , set the gain parameters and line speed , input the improved Stanley controller to get the angle control value , the output angle control quantity of the improved Stanley controller is as shown below: ; Conventional wheeled robots are generally considered to turn instantly, with their speed only affected by control, and the control information received by the crawler robot is not only the linear speed In addition, there is the angular velocity Therefore, the angle control of the tracked robot cannot be considered to be completed instantaneously. It is necessary to send the angular velocity to the tracked robot at the same time. Complete the turn. Use the obtained turning angle control value to adjust the linear speed of the crawler robot chassis at the next moment. and angular velocity Adjust the robot to track the track of the robot and the linear speed of the robot chassis at the next moment. and angular velocity They are shown as follows: ; ; in is the linear velocity coefficient, is the angular velocity coefficient of one, is the angular velocity coefficient of one. , , and gain factor The size of the coefficient is adjusted according to the actual engineering scenario. The RTK module updates the position and heading, and updates the current position and heading to With heading angle , through continuous updating and iteration, the optimized trajectory planning of the tracked robot is achieved.

[0034] (4) The linear velocity and angular velocity output by the improved Stanley controller are sent to the crawler robot chassis through CAN bus communication to control the movement of the crawler robot chassis.

[0035] First, save the linear velocity and angular velocity data in an 8-byte byte array. Store the high and low eight bits of the linear velocity data in byte[0] and byte[1] respectively, store the high and low eight bits of the angular velocity data in byte[2] and byte[3] respectively, and set the values ​​of byte[4], byte[5], byte[6], and byte[7] to 0x00. Among them, byte[0], byte[1], byte[2], and byte[3] are integer type data, the linear velocity unit is mm / s, and the angular velocity unit is 0.001rad / s.

[0036] The obtained byte array, data length 0x08, and the motion control instruction ID 0x111 of the crawler robot itself are sent to the chassis with a period of 20ms through the differential data transmission CAN communication protocol, using the CAN high and CAN low lines to control the crawler robot to track the initial planned trajectory. Since the method studies crawler robots, it is different from the conventional wheeled robot model. The conventional wheeled robot model is composed of front wheels and rear wheels. The crawler robot has no self-positioning wheels and no steering mechanism. It can only rely on the driving wheel to force the driving wheel to roll the crawler under the tension of the crawler, and the guide wheel then lays the crawler on the ground, so that the crawler robot follows the crawler track and uses the speed difference between the left and right crawlers to achieve turning.

[0037] like Figure 5 The figure shows the optimal trajectory planned by using the cubic spline interpolation optimization trajectory planning algorithm in the actual application scenario, setting the gain coefficient , angular velocity coefficient 1 , angular velocity coefficient 2 , Linear velocity coefficient The improvement effect and advantages are proved through experimental comparison and verification.

[0038] like Figure 6 As shown in the figure, different path planning algorithms and control algorithms are used to compare the running trajectories of the crawler robot. The comparison curves of the lateral errors of the crawler robot under the moving path are shown in Figure 7As shown, the experimental data are processed and summarized to obtain Table 1: Table 1

[0039] From Table 1, it can be seen that in an open, short distance with continuous turns, the trajectory control algorithm provided in this embodiment has a total travel distance of 37.25m, and the length closest to the planned path is 36.8m. In addition, the average error of trajectory control is 0.04m, and the maximum error is 0.08m, which is much smaller than the average error and maximum error of the other three algorithms, and the algorithm has been verified.

Claims

1. A track tracking method for a tracked robot based on optimization of a turning angle control amount, characterized in that: The following steps are involved: (1) Obtaining the navigation points of the target route of the crawler robot through the positioning device, performing initial path planning based on the navigation points, and obtaining the initial planned path; (2) Obtain the current position, heading angle, and linear velocity of the crawler robot in real time, and determine the shortest distance between the current position and the initial planned path; (3) According to the determined shortest distance and the current linear velocity, the angle control value is obtained by improving the Stanley controller, and the linear velocity and angular velocity of the crawler robot are adjusted by the angle control value; The linear speed adjustment formula of the crawler robot is: ； The angular velocity adjustment formula of the tracked robot is: ； in, Tracked robot The linear speed at the moment, Tracked robot The linear speed at the moment, is the linear velocity coefficient, is the angle control quantity, Tracked robot The angular velocity at time, and are the angular velocity coefficients, Tracked robot The lateral error of the moment, Tracked robot lateral error of the moment; (4) Control the movement of the crawler robot according to the adjusted linear velocity and angular velocity of the crawler robot so that the crawler robot follows the initial planned path.

2. The track tracking method of a crawler robot according to claim 1, characterized in that: According to the navigation points obtained in the step (1), the initial planned path is obtained by using a cubic spline interpolation optimization algorithm: the navigation points are divided into N small intervals, and a cubic polynomial is fitted between every two navigation points in one interval to obtain a set of interpolation functions. The boundary conditions are set to iteratively obtain the interpolation function group to generate the initial planned path.

3. The track tracking method of a crawler robot according to claim 2, characterized in that: A set of interpolation functions includes: The cubic polynomial between every two navigation points : ； Cubic Polynomial The first derivative form of and the second-order derivative form : ； ； in, Represents a cubic polynomial The independent variable, Represents the horizontal coordinate of the i-th navigation point, , , , are constants, ; Boundary conditions include: Interpolation conditions: ; Continuity conditions: ; First-order derivative continuity condition: ; Second-order derivative continuity conditions: ; Natural boundary conditions: , ; in, Indicates the ordinate of the i-th navigation point, Indicates the horizontal axis The function value at .

4. The track tracking method of a crawler robot according to claim 3, characterized in that: The improved Stanley controller formula is: ； in, is the gain parameter.

5. A track tracking system for a tracked robot based on optimization of the turning angle control amount, characterized in that: include: The path planning module is used to obtain the navigation points of the target route of the crawler robot through the positioning device, perform initial path planning through the navigation points, and obtain the initial planned path; The error determination module is used to obtain the current position, current heading angle and current linear speed of the crawler robot in real time, and determine the shortest distance between the current position and the initial planned path; An adjustment amount determination module is used to obtain a turning angle control amount by improving the Stanley controller according to the determined closest distance and the current linear velocity, and to adjust the linear velocity and angular velocity of the crawler robot by the turning angle control amount; The linear speed adjustment formula of the crawler robot is: ； The angular velocity adjustment formula of the tracked robot is: ； in, Tracked robot The linear speed at the moment, Tracked robot The linear speed at the moment, is the linear velocity coefficient, is the angle control quantity, Tracked robot The angular velocity at time, and are the angular velocity coefficients, Tracked robot The lateral error of the moment, Tracked robot lateral error of the moment; The path tracking module is used to control the movement of the crawler robot according to the adjusted linear velocity and angular velocity of the crawler robot, so that the crawler robot tracks the initial planned path.

6. The track tracking system of the crawler robot according to claim 5, characterized in that: The positioning device adopts an RTK module, the RTK module collects original NMEA data, and the path planning module converts the NMEA data into the WGS84 coordinate system to obtain coordinate data.

7. The track tracking system of the crawler robot according to claim 6, characterized in that: The path planning module obtains the initial planned path through the cubic spline interpolation optimization algorithm: the navigation points are divided into N small intervals, and a cubic polynomial is fitted between every two navigation points in one interval to obtain a set of interpolation functions. The boundary conditions are set to iteratively obtain the interpolation function group to generate the initial planned path.

8. The track tracking system of a crawler robot according to claim 5, characterized in that: The improved Stanley controller formula is: ； in, is the gain parameter.

9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 4 are implemented.

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

Images