CN-121979222-A - AGV path tracking method, equipment and medium based on inertial navigation

CN121979222ACN 121979222 ACN121979222 ACN 121979222ACN-121979222-A

Abstract

The invention discloses an AGV path tracking method, equipment and medium based on inertial navigation, and relates to the technical field of automatic guided vehicle control, comprising the steps of collecting AGV motion state observation data, and performing inertial navigation calculation to obtain an AGV pose estimation initial value and a vehicle body linear speed estimation initial value; and calculating the transverse deviation and the heading deviation based on the corrected pose estimation value, calculating the comprehensive steering wheel corner by combining the real-time heading deviation weight coefficient and the real-time transverse deviation weight coefficient, and driving the AGV to execute path tracking. The method and the device effectively improve the robustness and the smoothness of the path tracking under the low confidence working condition.

Inventors

SONG LINSEN
LIU JINGRU
LI PING
DAI XIANGBO
GU LIDONG
ZHANG AIWEN
WANG CHEN
WANG HONGPING
LIANG LONGKAI
LI RENJUN
ZHAO BAOYAN
LIU XINYANG

Assignees

长春理工大学
一汽模具制造有限公司

Dates

Publication Date: 20260505
Application Date: 20260209

Claims (10)

1. An AGV path tracking method based on inertial navigation is characterized by comprising the following steps of, Acquiring AGV motion state observation data, and performing inertial navigation calculation to acquire an AGV pose estimation initial value and a vehicle body linear speed estimation initial value; based on the AGV pose estimation initial value and the vehicle body linear speed estimation initial value, an extended Kalman filtering state vector is constructed to update time, and the stability of the Z-axis angular speed, the volatility of the acceleration module and the synchronism of the left wheel speed and the right wheel speed are synchronously monitored; judging a motion mode of the Z-axis angular velocity stability, the acceleration mode long volatility and the left wheel speed synchronism, triggering zero offset online correction, and generating a corrected zero offset estimated value; According to the corrected zero offset estimated value, performing zero offset compensation and re-integration on the triaxial angular velocity data to obtain a corrected pose estimated value, and inputting an extended Kalman filter to execute measurement updating to obtain a real-time course deviation weight coefficient and a real-time transverse deviation weight coefficient; And calculating the transverse deviation and the course deviation based on the corrected pose estimation value, combining the real-time course deviation weight coefficient and the real-time transverse deviation weight coefficient, calculating the comprehensive steering wheel corner, and driving the AGV to execute path tracking.
2. The AGV path tracking method based on inertial navigation according to claim 1 wherein the AGV motion state observation data comprises three-axis acceleration data, three-axis angular velocity data, a left drive wheel rotational speed pulse signal and a right drive wheel rotational speed pulse signal.
3. The AGV path tracking method based on inertial navigation according to claim 2 wherein the steps of obtaining an AGV pose estimation initial value and a vehicle body linear velocity estimation initial value are as follows, Inertial navigation calculation is carried out on the triaxial acceleration data and the triaxial angular velocity data, and an AGV pose estimation initial value is obtained; And obtaining an estimated initial value of the vehicle body linear speed through kinematic calculation according to the left driving wheel rotating speed pulse signal and the right driving wheel rotating speed pulse signal.
4. The AGV path tracking method based on inertial navigation according to claim 3 wherein said constructing an extended Kalman filter state vector performs time updating and synchronously monitors Z-axis angular velocity stability, acceleration module long volatility and left and right wheel speed synchronicity by the steps of, Extracting an abscissa estimated value, an ordinate estimated value, a course angle estimated value, a car body linear speed estimated initial value, Z-axis angular speed data and a zero offset initial estimated value from an AGV pose estimated initial value and a car body linear speed estimated initial value, and constructing an extended Kalman filtering state vector; Based on the extended Kalman filtering state vector and the process noise covariance matrix, performing time update to generate a prediction state vector and a prediction covariance matrix; Carrying out consistency check on the predicted state vector and the AGV pose estimation initial value, marking the predicted state vector as a trusted state when the predicted state vector is smaller than a safety threshold value, and marking the AGV pose estimation initial value as an untrusted state otherwise; Monitoring the change amplitude of the Z-axis angular velocity data in a sliding time window, marking the Z-axis angular velocity data as a stable state when the Z-axis angular velocity data is lower than an angular velocity stability threshold value, and otherwise marking the Z-axis angular velocity data as an unstable state; Calculating an acceleration module length sequence based on the triaxial acceleration data, monitoring fluctuation degree in a sliding time window, and marking the acceleration module length sequence as a stable state when the fluctuation degree is lower than an acceleration fluctuation threshold value, otherwise marking the acceleration module length sequence as a fluctuation state; And monitoring synchronous deviation of the left driving wheel rotating speed pulse signal and the right driving wheel rotating speed pulse signal, and marking the synchronous state when the synchronous deviation is lower than a wheel speed synchronous threshold value, or else, marking the synchronous state as an asynchronous state.
5. The AGV path tracking method based on inertial navigation according to claim 4 wherein the generating the corrected zero offset estimate comprises the steps of, When the state is predicted to be a trusted state, the Z-axis angular speed is a stable state, the acceleration module length is a stable state and the left wheel speed and the right wheel speed are synchronous, judging that the AGV is in a static state, otherwise, judging that the AGV is in a moving state; When the AGV is judged to be in a static state and the duration time reaches the static judgment time, generating a zero offset correction trigger signal; When the zero offset correction trigger signal is in an effective state, reading Z-axis angular velocity data in a static period, and carrying out arithmetic average operation in a time window to obtain a static angular velocity average value; Injecting the rest angular velocity mean value into a zero offset estimation updating channel to generate a corrected zero offset estimation value.
6. The AGV path tracking method based on inertial navigation according to claim 5 wherein the step of zero offset compensating and re-integrating the three-axis angular velocity data based on the corrected zero offset estimate to obtain a corrected pose estimate comprises the steps of, Zero offset compensation is carried out based on the corrected zero offset estimated value and the triaxial angular velocity data, and compensated triaxial angular velocity data is generated; Performing time integral operation on the compensated triaxial angular velocity data to obtain a course angle correction value; and carrying out inertial navigation calculation based on the triaxial acceleration data, the compensated triaxial angular velocity data and the course angle correction value to obtain a corrected pose estimation value.
7. The AGV path tracking method based on inertial navigation according to claim 6 wherein the acquiring the real-time heading bias weight coefficient and the real-time lateral bias weight coefficient comprises the steps of, Inputting the corrected pose estimation value and the predicted covariance matrix into an extended Kalman filter, executing measurement update, and outputting an updated covariance matrix; Extracting a heading angle uncertainty characterization quantity from the updated covariance matrix to generate a heading confidence level; Projecting the updated covariance matrix to the normal direction of the path, extracting the uncertainty characterization quantity of the transverse position, and generating a transverse confidence level; and dynamically adjusting the deviation response intensity based on the course confidence level and the transverse confidence level, and reading a real-time course deviation weight coefficient and a real-time transverse deviation weight coefficient.
8. The AGV path tracking method based on inertial navigation according to claim 7 wherein the driving AGV performs path tracking by the steps of, Calculating the shortest distance from the current position of the AGV to a preset path and the drop point based on the abscissa correction value, the ordinate correction value and the preset path information in the corrected pose estimation value, and acquiring the transverse deviation; Obtaining course deviation according to the course angle correction value in the corrected pose estimation value and the tangential direction angle of the preset path at the foot drop point; applying a real-time lateral deviation weight coefficient to the lateral deviation to obtain a weighted lateral deviation, and applying a real-time heading deviation weight coefficient to the heading deviation to obtain a weighted heading deviation; Inputting the weighted transverse deviation and the weighted course deviation into a steering control law of a steering wheel to generate a comprehensive steering wheel angle; And carrying out parameter compensation on the comprehensive steering wheel corner through the AGV wheelbase, generating a steering wheel corner instruction, and driving the AGV to execute path tracking.
9. A computer device comprises a memory and a processor, wherein the memory stores a computer program, and the computer program is characterized in that the processor realizes the steps of the AGV path tracking method based on inertial navigation according to any one of claims 1-8 when executing the computer program.
10. A computer readable storage medium having a computer program stored thereon, wherein the computer program when executed by a processor performs the steps of the inertial navigation-based AGV path tracking method of any one of claims 1-8.

Description

AGV path tracking method, equipment and medium based on inertial navigation Technical Field The invention relates to the technical field of automatic guided vehicle control, in particular to an AGV path tracking method, equipment and medium based on inertial navigation. Background The AGV path tracking technology based on inertial navigation occupies a vital position in the current intelligent logistics, flexible manufacturing and automatic storage fields, autonomous pose estimation and track tracking without external positioning dependence are realized by fusing the inertial measurement unit and wheel speed encoder observation data, the AGV path tracking technology becomes a core support for the reliable running of the AGV in a high-dynamic and weak GNSS environment, the related method generally adopts an extended Kalman filter to perform state estimation, and the path deviation calculation and steering wheel control instruction generation are completed by combining a kinematic model, so that the technical framework is widely applied to industrial-grade mobile robots. In the field of inertial navigation-based AGV path tracking, the traditional inertial navigation-based AGV path tracking method generally adopts an offline calibrated fixed gyroscope to carry out angular velocity compensation, is difficult to adapt to heading accumulated errors caused by zero bias time-varying drift in long-term operation, and meanwhile, a path tracking controller mostly adopts constant weight gain, does not dynamically adjust deviation response intensity according to real-time uncertainty of pose estimation, so that overshoot or oscillation is easy to generate under a low confidence working condition, and tracking robustness and smoothness are affected. Disclosure of Invention The present invention has been made in view of the above-described problems occurring in the prior art. Therefore, the invention provides an AGV path tracking method based on inertial navigation, which solves the problems of course estimation divergence caused by gyro zero bias time-varying drift and insufficient control robustness caused by deviation feedback weight solidification. In order to solve the technical problems, the invention provides the following technical scheme: In a first aspect, the present invention provides a inertial navigation-based AGV path tracking method, comprising, Acquiring AGV motion state observation data, and performing inertial navigation calculation to acquire an AGV pose estimation initial value and a vehicle body linear speed estimation initial value; based on the AGV pose estimation initial value and the vehicle body linear speed estimation initial value, an extended Kalman filtering state vector is constructed to update time, and the stability of the Z-axis angular speed, the volatility of the acceleration module and the synchronism of the left wheel speed and the right wheel speed are synchronously monitored; judging a motion mode of the Z-axis angular velocity stability, the acceleration mode long volatility and the left wheel speed synchronism, triggering zero offset online correction, and generating a corrected zero offset estimated value; According to the corrected zero offset estimated value, performing zero offset compensation and re-integration on the triaxial angular velocity data to obtain a corrected pose estimated value, and inputting an extended Kalman filter to execute measurement updating to obtain a real-time course deviation weight coefficient and a real-time transverse deviation weight coefficient; And calculating the transverse deviation and the course deviation based on the corrected pose estimation value, combining the real-time course deviation weight coefficient and the real-time transverse deviation weight coefficient, calculating the comprehensive steering wheel corner, and driving the AGV to execute path tracking. As an optimal scheme of the inertial navigation-based AGV path tracking method, the AGV motion state observation data comprise three-axis acceleration data, three-axis angular velocity data, a left driving wheel rotating speed pulse signal and a right driving wheel rotating speed pulse signal. As an optimal scheme of the AGV path tracking method based on inertial navigation, the method comprises the steps of obtaining an AGV pose estimation initial value and a vehicle body linear speed estimation initial value, Inertial navigation calculation is carried out on the triaxial acceleration data and the triaxial angular velocity data, and an AGV pose estimation initial value is obtained; And obtaining an estimated initial value of the vehicle body linear speed through kinematic calculation according to the left driving wheel rotating speed pulse signal and the right driving wheel rotating speed pulse signal. As a preferable scheme of the AGV path tracking method based on inertial navigation, the AGV pose estimation initial value and the vehicle body linear velocity estimation initial v