Yaw rate-based trajectory tracking method for tracked vehicle

Abstract

The present invention relates to the technical field of autonomous driving, and provides a yaw rate-based trajectory tracking method for a tracked vehicle. The method comprises: obtaining a target deflection angle by means of performing geometric analysis on a transverse error of a current position and attitude of a vehicle and performing a weighting operation together with a heading error, converting the target deflection angle into a target yaw rate by means of kinematics of the vehicle, and sending same to a manipulator mechanism for execution. The present method can implement real-time feedback control of yaw rate of a vehicle.

Description

A track tracking method for tracked vehicles based on yaw angular velocity Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a track tracking method for a tracked vehicle based on yaw angular velocity. Background Art

[0002] An autonomous driving system for tracked vehicles uses actuators to control the movement of the left and right tracks based on posture information. The movement of a tracked vehicle depends on the coordination of the left and right tracks. The overall motion of the tracked vehicle is determined by the interaction of the respective motion states of the left and right tracks. Therefore, the vehicle's lateral and longitudinal motion are coupled. However, the actuators and operating environment of tracked vehicles are often nonlinear, making motion control of tracked vehicles challenging.

[0003] Tracked vehicles operate through a coupled lateral and longitudinal motion process. Therefore, longitudinal motion must be considered during lateral control. Tracking accuracy and motion stability are both crucial considerations during trajectory tracking. This requires that tracked vehicles maintain minimal lateral error and minimize yaw angle while maintaining accuracy.

[0004] Ideally, there would be no slip between the drive wheel and the track, or between the track and the ground. In this case, the linear velocity of the drive wheel rim could be considered the track speed. By installing a Hall effect sensor on the drive wheel, for example, it is possible to measure the drive wheel rotational speed and thus calculate the track winding speed. However, this approach is complex and expensive, and its accuracy and lifespan are affected in muddy environments. Furthermore, slip between the drive wheel and the track, and between the track and the ground, can lead to control errors. Measuring track speed alone cannot address the issue of slip errors.

[0005] In an ideal model of a tracked vehicle, actuator movement affects the speeds of the left and right tracks, which can be considered as the longitudinal speed and differential speed components. The longitudinal control mechanism is typically the throttle, while the lateral actuator is typically the steering wheel or a joystick. Movement of the lateral control mechanism controls the speeds of the left and right tracks through mechanical transmission or electronic control, thereby achieving differential steering of the vehicle. However, due to transmission delays between the control, transmission, and actuator mechanisms, using solutions to control the heading of tracked vehicles cannot meet the requirements of high-precision control. Furthermore, this delay can lead to dangerous situations during autonomous driving.

[0006] Furthermore, tracked vehicles have different actuator structures, either centralized or distributed. Control methods developed based on a distributed chassis can directly control both the left and right tracks. However, centralized drive tracked vehicles have mutually coupled powertrains on both sides, making control more difficult.

[0007] Summary of the Invention

[0008] To address the issues of insufficient trajectory tracking control accuracy and stability for tracked vehicles, as well as the high cost and large latency of existing measurement methods, the present invention provides a target yaw rate calculation method for tracked vehicles based on geometry and kinematics to achieve high-precision trajectory tracking, as well as a method for tracking the target yaw rate.

[0009] The present invention aims to provide a tracked vehicle trajectory tracking method based on yaw rate to address the issues raised in the aforementioned background art. This method geometrically analyzes the lateral error in the vehicle's current position and weights it with the heading error to obtain a target yaw angle. The target yaw angle is then converted into a target yaw rate using the vehicle's kinematics, and the resulting yaw rate is transmitted to the control mechanism for execution.

[0010] In order to solve the above technical problems, the present invention provides the following technical solution: a track tracking method for a tracked vehicle based on yaw angular velocity, characterized in that the tracking method comprises:

[0011] A track tracking method for a tracked vehicle based on yaw angular velocity, the tracking method comprising:

[0012] S100: Calculate vehicle position and attitude based on GNSS information;

[0013] S200: Determine the lateral error and heading error of the vehicle;

[0014] S300: Calculating a target deflection angle of the vehicle;

[0015] S400: Calculating a target differential speed ratio of the vehicle;

[0016] S500: Calculating a target yaw rate of the vehicle;

[0017] S600: Control the operation of the control mechanism to complete yaw angular velocity tracking.

[0018] Preferably, in step S200, the lateral error is obtained through the geometric relationship between the vehicle position and the target trajectory.

[0019] Preferably, in step S300, the target deflection angle is calculated as follows:

[0020] Where, δ: target deflection angle; ω h : weight of heading error; ω lat : weight of lateral error; The tangential direction angle of the target trajectory; The body direction angle of the controlled vehicle; e lat : lateral error; n: the number of cycles expected to achieve lateral error convergence; T: the length of each control cycle; V c : The current speed of the vehicle.

[0021] Preferably, in step S400, the target differential ratio is calculated as follows:

[0022] Wherein, k: target differential ratio; f(δ): user-defined function of target deflection angle δ; B: width of the center axis of the crawler chassis. Preferably, in step S400, the user-defined function of target deflection angle δ is:

[0023] Preferably, in the above S500, the target yaw angular velocity is calculated as follows:

[0024] Among them, ω T : target yaw rate.

[0025] Preferably, in the above S600, the yaw rate error is converged to 0 by controlling the control mechanism to complete the yaw rate tracking. The calculation formula of the yaw rate error is: ω =ω T -ω real

[0026] Among them, e ω : yaw angular velocity error; ω real : The actual yaw rate of the vehicle measured by the gyroscope.

[0027] Preferably, based on the above algorithm, the yaw rate of the vehicle can be feedback controlled in real time.

[0028] Preferably, the weights of the heading error and the lateral error are constants.

[0029] Preferably, the weights of the heading error and the lateral error are functions, which are:

[0030] Compared with the existing technology, the beneficial effects achieved by the present invention are: the present invention is not only widely applicable to two different types of tracked vehicles, centralized and distributed; it can also improve the trajectory tracking accuracy and stability of tracked vehicles, and improve the overall stability of the system by reducing the delay in vehicle status measurement, thereby avoiding the occurrence of dangerous situations.

[0031] The working principle of this invention is to measure the lateral error and heading error of the tracked vehicle, and then calculate the target yaw rate for achieving trajectory tracking based on the tracked vehicle's geometric model and the kinematic model of trajectory tracking. Then, the control mechanism is controlled to achieve tracking and fitting of the target yaw rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0033] FIG1 is a schematic flow diagram of the present invention;

[0034] FIG2 is a schematic diagram of track tracking of a tracked vehicle according to the present invention. DETAILED DESCRIPTION

[0035] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only 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 making creative efforts are within the scope of protection of the present invention.

[0036] The present invention provides a technical solution, and the specific flow chart is shown in FIG1 . The specific execution process of the method is as follows.

[0037] Tracked vehicles require a GNSS antenna and gyroscope. For example, the steering mechanism, such as a steering wheel, requires an encoder installed on the steering wheel motor. GNSS is used to obtain vehicle position and heading information. The gyroscope is used to obtain yaw rate, and the encoder is used to obtain steering wheel angle. The steering mechanism can also be alternative structures such as a joystick.

[0038] 1. Target deflection angle calculated based on geometry

[0039] Based on the position state between the vehicle and the track obtained by GNSS, the figure uses a deformed Cartesian coordinate system as an example, with the north direction as the X-axis and the east direction as the Y-axis. The coordinate system can also be other equivalent coordinate systems such as the Northeast Celestial Coordinate System, the Earth-centered Earth-fixed Coordinate System, and the Frenet Coordinate System. The equations in the present invention can be expressed in either continuous or discrete form with equal effect.

[0040] The target deflection angle of the crawler vehicle can be expressed as Equation 1: δ = ω h δ h (t)+ω lat δ lat (t) (1)

[0041] ωh 、ω lat : The weight of the heading error and the lateral error. In practical applications, it can be a fixed value or a function related to parameters such as speed and lateral error. For example, Equation 2:

[0042] In formula 1

[0043] δ h (t): target deflection angle required to correct heading error;

[0044] δ lat (t): target deflection angle required to correct lateral error.

[0045] The target deflection angle δ of the heading error h (t) is expressed as Equation 3:

[0046] The tangential direction angle of the target trajectory can be obtained by geometric calculation of the target trajectory;

[0047] The body orientation angle of the controlled vehicle can be obtained through the GNSS antenna.

[0048] The target deflection angle δ of the lateral error in Equation 1 is lat (t) is calculated by formula 4, and the lateral error e in formula 4 lat The current position of the vehicle is obtained through GNSS and the distance from the vehicle position to the trajectory is calculated:

[0049] e lat : The lateral error between the controlled vehicle and the target trajectory, obtained by the geometric relationship between the vehicle position and the target trajectory;

[0050] d(t): The distance that the velocity will advance in a certain period in the tangential direction of the trajectory. It can be expressed as Equation 5: d(t) = n V c T cosδ h (t) (5)

[0051] n: expected to achieve e lat The number of cycles of convergence, constant;

[0052] T: the length of each control cycle;

[0053] V c : The current speed of the vehicle.

[0054] Based on the above, Equation 1 can be expressed as:

[0055] The function in Equation 6 represents the ability of the target deflection angle to effectively track the target trajectory and the direction of the target trajectory. During the autonomous driving process of the tracked vehicle, the continuous fitting of the target deflection angle is the control process of the vehicle's lateral control.

[0056] 2. Kinematics-based differential motion decoupling method

[0057] According to the differential model kinematics of the tracked vehicle,

[0058] V L 、V R : Winding speed of left and right tracks;

[0059] ω c : yaw angular velocity;

[0060] B: The width of the center axis of the crawler vehicle chassis, obtained in advance through measurement.

[0061] Introduce variable k, defined as the ratio of left and right wheel speeds: V L =k V R (8)

[0062] Combining Equation 8 with the differential model Equation 7, the differential ratio of the tracked vehicle is obtained as:

[0063] The formula f(δ) can be regarded as an adjustable parameter, which is a custom function of the deflection angle δ. Application examples:

[0064] The target differential speed ratio of a tracked vehicle calculated using this method effectively represents the calculation method for vehicle motion targets. Compared with traditional kinematic models, this method reduces the computational complexity of the target value and more closely aligns with the underlying principles of tracked vehicle motion control.

[0065] 3. Calculation of track target yaw rate

[0066] The target yaw rate is the target deflection angle required to complete the steering action within the expected action time, so it is calculated by the following formula:

[0067] In formula 11, ΔV L / R Indicates the speed difference between the left and right tracks. The speed difference can also be calculated from the differential ratio:

[0068] Therefore, the target yaw rate calculation method is expressed as:

[0069] Therefore, the target yaw rate can be calculated using the above equation, the differential ratio calculated using the differential model, the vehicle width, and the current speed of the tracked vehicle. By using the target yaw rate to autonomously control the steering mechanism, the vehicle's motion response speed can be significantly improved. Compared to the target angular velocity obtained by differentiating the target yaw angle, the yaw rate obtained by this solution is smoother and more accurate. Compared to direct control using the yaw angle, the smoothness of the vehicle's motion is also significantly improved.

[0070] 4. Operation of the mechanism

[0071] The steering mechanism is controlled automatically by the target yaw rate. The steering wheel rotation can be read by the steering wheel encoder, and the vehicle yaw rate achieved by the steering wheel movement is read by the vehicle gyroscope. The steering error of the steering mechanism can be calculated by taking the difference between the target value and the actual value: ω =ω T -ω real (14)

[0072] ω real : The actual yaw angular velocity of the actual vehicle can be obtained by measuring the gyroscope.

[0073] The control of the steering mechanism can be tracked using the classic SISO control method, and the target ω is tracked by the action amount of the steering wheel motor. T Tracking to achieve e ω Converges to 0.

[0074] 2 , an embodiment is given below to illustrate the present invention in detail.

[0075] This solution requires a GNSS antenna and gyroscope installed on the tracked vehicle, along with an encoder on the steering wheel. GNSS is used to obtain the distance between the vehicle and the target track, as well as the angular difference between the vehicle's direction and the tangent direction of the target track. The encoder is used to obtain the steering wheel angle.

[0076] This solution needs to obtain the center wheelbase B of the tracks on both sides before starting operation.

[0077] This solution measures the tracked vehicle's lateral and heading errors, and then calculates the target yaw rate for trajectory tracking based on the tracked vehicle's geometric model and the kinematic model of trajectory tracking. The control mechanism is then controlled to achieve tracking and fitting of the target yaw rate.

[0078] The specific calculation process of the target yaw rate of the tracked vehicle is as follows:

[0079] Based on the vehicle position and target trajectory position obtained by GNSS, the lateral error can be calculated by calculating the distance from the point to the curve. The lateral error can be described by the angle that should be deflected to eliminate the lateral error based on the geometric relationship:

[0080] Where d(t) describes the distance the longitudinal component of the velocity will travel in the next n cycles. d(t) = nv c T cosδ h (t)

[0081] According to the difference between the vehicle body direction and the tangent direction of the trajectory curve, the target deflection angle required to correct the heading error can be calculated.

[0082] Combining the above two items: lateral direction and heading, the angle to be deflected can be calculated as:

[0083] Due to the response characteristics of the system, the adjustment weight ω is added to both the lateral and heading directions at the same time. h and ω lat .

[0084] According to the above formula, that is, according to the position and posture of the vehicle, based on the geometric model, the target deflection angle of the tracked vehicle to achieve error convergence is calculated.

[0085] The steering mechanism of a tracked vehicle directly controls the speed of the left and right tracks. In autonomous lateral control, the speed difference between the left and right tracks is the independent variable. This means that the steering wheel angle affects the differential speed ratio between the left and right tracks. The differential speed ratio is another representation of the yaw rate. Since the yaw angle is the integral of the yaw rate, feedback control of the yaw rate can reduce delays in executing actions.

[0086] The target yaw rate is calculated by the kinematic model of the tracked vehicle and is first defined as the left-right wheel speed ratio: V L =k V R

[0087] Substituting the above formula into the kinematic equation of differential motion, the differential ratio can be obtained as:

[0088] The target yaw rate can be expressed by the differential ratio:

[0089] This formula can be used to calculate the target yaw rate. Based on this value, the actuator is controlled by the steering mechanism. Specifically, in a vehicle with a steering wheel, the yaw rate of the entire vehicle is feedback-controlled by the steering wheel angle. The feedback error is: eω =ω T -ω real

[0090] where ω real The actual yaw rate of the vehicle is measured by the gyroscope. The yaw rate error is converged to zero by controlling the steering mechanism, which can be achieved using methods such as PID control.

[0091] It should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[0092] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art will be able to modify the technical solutions described in the aforementioned embodiments or substitute equivalents for some of the technical features. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A track tracking method for a tracked vehicle based on yaw angular velocity, characterized in that: The tracking methods include: S100: Calculate the vehicle position and attitude according to the GNSS information; S200: Determine a lateral error and a heading error of the vehicle; S300: Calculating a target deflection angle of the vehicle; S400: Calculating a target differential ratio of the vehicle; S500: Calculating a target yaw rate of the vehicle; S600: Control the operation of the control mechanism to complete the yaw angular velocity tracking.

2. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 1, characterized in that: In step S200, the lateral error is obtained through the geometric relationship between the vehicle position and the target trajectory.

3. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 2, characterized in that: In step S300, the target deflection angle is calculated as: Where, δ: target deflection angle; ω h : Weight of heading error; ω lat : weight of lateral error; The tangential direction angle of the target trajectory; The body direction angle of the controlled vehicle; lat : lateral error; n: the number of cycles expected to achieve lateral error convergence; T: the length of each control cycle; V c : Current vehicle speed.

4. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 3, characterized in that: In step S400, the target differential ratio is calculated as follows: Where, k: target differential ratio; f(δ): custom function of target deflection angle δ; B: width of the center axle of the crawler chassis.

5. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 4, characterized in that: In the step S400, the user-defined function of the target deflection angle δ is:

6. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 4, characterized in that: In the above-mentioned S500, the target yaw angular velocity is calculated as follows: Among them, ω T : Target yaw rate.

7. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 6, characterized in that: In the above S600, the yaw rate error is converged to 0 by controlling the control mechanism, and the yaw rate tracking is completed. The calculation formula of the yaw rate error is: ω =ω T -ω real Among them, e ω : yaw rate error; ω real : The actual yaw rate of the vehicle measured by the gyroscope.

8. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 7, characterized in that: Based on the above algorithm, the vehicle's yaw rate can be feedback controlled in real time.

9. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 3, characterized in that: The weights of the heading error and the lateral error are constants.

10. The track tracking method of a tracked vehicle based on yaw angular velocity according to claim 3, characterized in that: The weights of the heading error and the lateral error are functions, which are:

Images