Universal navigation method, medium and device applicable to vehicles with various motion models

Abstract

The present application relates to the field of artificial intelligence automatic driving technology, in particular to a kind of general navigation method suitable for multiple motion model vehicle, medium and equipment, method includes the following steps: establishing observation equation, updating correction navigation error state, obtain updated estimation error state vector;Take the navigation component of updated estimation error state vector and update correction recursive nominal navigation state, obtain updated recursive nominal navigation state.The present application solves the problem that all wheels exist swing angle when full-wheel steering motion vehicle in rotation and translation motion in prior art, violates the important assumption of IMU / Wheel rigid connection, the present application proposes general, accurate wheel speed meter full parameter observation equation suitable for Ackermann and arbitrary shaft full-wheel steering multiple motion model, in addition, based on inertia unit auxiliary single wheel measurement, a complete, unified integrated navigation filter is established to realize multiple model adaptive fusion navigation state estimation.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of artificial intelligence automatic driving technology, in particular to a universal navigation method suitable for vehicles of multiple motion models, a medium and equipment. BACKGROUND

[0002] Robust and accurate state estimation is a basic and core task of mobile robots, which directly determines the realization of subsequent planning and control functions. Simultaneous localization and mapping (SLAM) based on relative positioning technology of matching environmental features is widely used in vehicle robots, such as autonomous driving, intelligent mine cars, automatic port towing vehicles, and factory automatic handling vehicles. However, visual SLAM is limited by sudden changes in light, and laser SLAM is prone to divergence in geometric self-similar environments and rainy and foggy weather. This inevitable degradation of the environment has seriously limited the performance of the navigation system.

[0003] Inertial measurement unit (IMU) / wheel is an autonomous dead reckoning technology, which has become a key complementary technology in SLAM degradation scenarios due to its advantage of being immune to external environmental interference. Most existing works are oriented towards Ackermann motion model vehicles (bicycle model), and wheel encoders are usually installed on fixed rear wheels to directly measure the speed of the robot, which is used to correct the cumulative error of the IMU. This important premise is assumed to be rigidly connected to the IMU. Unlike the Ackermann motion model vehicle with fixed rear wheels, the full-wheel steering motion model robot has all wheels independently steering and driving, and can perform special motion modes such as arbitrary radius rotation and arbitrary direction translation by flexibly distributing the steering angles of each wheel, greatly relieving the constraints of narrow environmental areas. However, this violates the key assumption of the IMU / Wheel combined navigation method, making existing methods no longer applicable and greatly limiting the application of the IMU / Wheel combined navigation method.

[0004] Specifically, unlike the Ackermann model vehicle with fixed rear wheels, the wheel encoder is rigidly connected to the vehicle, which directly reflects the carrier speed, so it can be directly used as a speed observation of the IMU fixed to the vehicle. In full-wheel steering motion, due to the change of wheel encoder deflection angle, the wheel encoder does not directly measure the carrier speed, especially when rotating in place, the IMU speed at the center of the vehicle is 0, while the wheel speed is obviously not 0. Therefore, this assumption is obviously not true when the full-wheel steering vehicle robot is steering, and directly applying it when steering is not applicable, which makes it difficult to apply the IMU / Wheel combined navigation method to full-wheel steering model vehicle robots. SUMMARY

[0005] The purpose of this invention is to provide a universal navigation method, medium, and device applicable to various motion model vehicles, in order to solve the technical problem in the prior art where all wheels of all-wheel steering vehicles exhibit sway angles during rotational and translational motion, violating the important assumption of rigid IMU / Wheel connection. The specific technical solution is as follows:

[0006] This invention provides a universal navigation method applicable to various motion model vehicles, comprising the following steps:

[0007] S1. Define the coordinate system;

[0008] S2. Real-time acquisition of data from multiple sensors based on a defined coordinate system, including real-time data from the wheel speed meter;

[0009] S3, in one Kalman filter update cycle In the middle, the inertial measurement unit data acquired based on S2 and the first Given the nominal navigation state at time t, perform a one-step recursion on the nominal navigation state to obtain the nth time. The recursive nominal navigation state at each moment;

[0010] S4. Based on the inertial measurement unit data obtained in S2, make a one-step prediction of the navigation error state;

[0011] S5. Based on the real-time data of the wheel speedometer collected in S2, establish a general observation equation and update and correct the estimated navigation error state;

[0012] S6. Update and correct the navigation components based on the estimated error state from S5. At time S3, the recursive nominal navigation state is calculated, and the updated recursive nominal navigation state is obtained.

[0013] A further improvement of the universal navigation method applicable to various motion model vehicles of the present invention is that S1 specifically includes: the inertial measurement unit is installed at the geometric center of the Ackerman or all-wheel steering vehicle robot; and the strapdown inertial navigation coordinate system is defined as follows: The system's origin is located at the center of the inertial measurement unit. The coordinate axes follow the right-front-top order; the wheel speedometer coordinate system is defined as follows. The system originates at the point of contact between the tested wheel and the ground. The coordinate axes follow a right-front-top order; the navigation system is defined as the local northeast-sky coordinate system. The geocentric inertial coordinate system is defined as follows: The Earth coordinate system is defined as follows: The lever arm vector of the inertial measurement unit relative to the measurement center of the wheel speed gauge is obtained based on the mechanical installation. .

[0014] A further improvement of the universal navigation method applicable to various motion model vehicles of the present invention is that S2 specifically includes: real-time synchronous acquisition of data at the same frequency... Angular velocity output by the inertial measurement unit acceleration and wheel sway angle data The wheel speed meter outputs real-time data as follows: , The sampling time interval, ;

[0015] Given the initial latitude, longitude, and altitude of an Ackerman or all-wheel steering vehicle robot. Initial attitude matrix Initial velocity Error covariance matrix of initial navigation state and navigation error state .

[0016] A further improvement of the universal navigation method of the present invention, applicable to various motion model vehicles, lies in that, by performing a one-step recursive deduction on the nominal navigation state, the following is obtained: When determining the recursive nominal navigation state at time step, the following steps are included:

[0017] Recursive nominal posture, the first The attitude matrix estimated one step at a time The expression is as follows:

[0018] ;

[0019] in, express The estimated pose matrix is ​​updated continuously. express The estimated gyroscope bias error is updated continuously. Indicates an exponential mapping;

[0020] Recursive nominal speed, the first The speed of one-step recursive estimation at any given time The expression is as follows:

[0021] ;

[0022] in, express The estimated speed is updated in real time. yes The accelerometer zero bias error is constantly updated. Let be the gravity vector, where It is the acceleration due to gravity. express The estimated pose matrix is ​​updated continuously;

[0023] Recursive nominal position, step The position estimated at time step The expression is as follows:

[0024] ;

[0025] Wherein, Indicates The position estimated at time step is updated;

[0026] The nominal attitude, nominal velocity and nominal position obtained by one-step recursion are combined to form the recursive nominal navigation state at time step .

[0027] The further improvement of the general navigation method of the application applicable to various motion model vehicles is that S4 comprises the following steps:

[0028] The error state of the strapdown inertial navigation system is selected as the attitude angle error , the velocity error , the position error , the gyro zero offset error , the accelerometer zero offset error , the wheel speed sensor scale error , the wheel speed sensor heading installation angle error , and the wheel speed sensor pitch installation angle error A total of 18-dimensional error state vectors, the error state vector X The expression is as follows:

[0029] ;

[0030] Wherein, T Indicates the matrix transpose operator;

[0031] The expression of the continuous state transition equation is as follows:

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] ;

[0037] Wherein, Indicates the error state rate of change; Indicates the error state Jacobian matrix; Indicates the error state transition process noise vector, which is subject to Gaussian distribution; denotes navigation system; denotes the nominal velocity at time in navigation system; axis component, ; denotes the earth radius; g denotes the gravitational acceleration; denotes the angular velocity of the navigation system relative to the earth system ; denotes the axis component, of the angular velocity of the earth rotation in the navigation system ; denotes the axis component, denotes the angular velocity of the navigation system relative to the geocentric inertial coordinate system in the navigation system ; denotes the axis component, ; denotes the axis component, of the accelerometer output measurement in the navigation system ; ; denotes the measurement noise of the three gyroscopes; denotes the measurement noise of the three accelerometers; N denotes the Gaussian distribution symbol; denotes the process noise variance matrix;

[0038] According to the error state Jacobian matrix, the error covariance matrix of the navigation error state is calculated one-step prediction:

[0039] ;

[0040] wherein, denotes the error covariance matrix of the updated estimation at time .

[0041] Further improvement of the universal navigation method of the application applicable to various motion model vehicles is that when establishing the universal observation equation, the velocity observation equation is expressed as follows:

[0042] ;

[0043] wherein, the expression of the wheel speedometer observation Jacobian matrix is as follows:

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] in, Indicates the first The calculation matrix at any given moment consists of the relevant installation error angles and error quantities; Indicates the first The wheel speedometer outputs real-time data. This represents a calculation matrix consisting of the installation error angle, the error amount, and the measured values ​​output by the wheel speed gauge. The observed noise vector is represented by a Gaussian distribution. The variance matrix representing the observation noise; I Represents the identity matrix. Indicates the first The heading installation angle error of the wheel speed gauge at any given time; Indicates the first The pitch angle error of the wheel speed gauge at all times.

[0049] A further improvement of the universal navigation method of the present invention applicable to various motion model vehicles is that, when updating and correcting the navigation error state, the following steps are included:

[0050] Calculate the Kalman filter gain matrix :

[0051] ;

[0052] Update the estimated navigation error state vector :

[0053] ;

[0054] Update the estimated navigation error state covariance :

[0055] .

[0056] A further improvement of the universal navigation method of the present invention applicable to various motion model vehicles is that S6 specifically includes:

[0057] Attitude update, based on the updated estimated navigation error state vector Extracting attitude error The updated pose matrix is ​​obtained. : ;

[0058] Velocity update, based on the updated estimated navigation error state vector Extraction speed error Get the updated speed : = ;

[0059] Position update, based on the updated estimated navigation error state vector Extracting position error Get the updated position : ;

[0060] Gyroscope bias error update is performed based on the updated estimated navigation error state vector. Extracting the zero bias error of the gyroscope ,in express The estimated gyroscope bias error is updated continuously.

[0061] Accelerometer bias error update, based on the updated estimated navigation error state vector. Extracting the zero bias error of the accelerometer ,in express The accelerometer zero bias error is constantly updated;

[0062] The updated attitude matrix, updated velocity, updated position, updated gyroscope bias error, and updated accelerometer bias error are combined to form the updated recursive nominal navigation state.

[0063] The present invention also provides a readable storage medium storing a computer program adapted to be loaded by a processor and executed as described above for a universal navigation method applicable to various motion model vehicles.

[0064] The present invention also provides a computer device, the computer device including a memory and a processor, the memory storing a computer program, which, when executed by the processor, runs the universal navigation method applicable to various motion model vehicles as described above.

[0065] The application of the technical solution of the present invention has the following beneficial effects:

[0066] This invention provides a universal navigation method applicable to various motion model vehicles. By introducing an inertial measurement auxiliary wheel speed meter to uniformly measure the speed of Ackerman / all-wheel steering vehicle robots, a universal speed observation model is established. Furthermore, an 18-dimensional ESKF (Error State Kalman Filter) navigation filter is constructed to achieve optimal fusion of the IMU and a single wheel. This solves the technical problem in existing technologies where all wheels have sway angles during rotational and translational motion of all-wheel steering vehicles, violating the important assumption of rigid connection between the IMU and the wheel.

[0067] This invention establishes a universal speed observation model for Ackermann / all-wheel steering vehicles:

[0068] 1) It breaks through the original assumption of rigid connection, making the method applicable to all-wheel steering robots, no longer explicitly distinguishing between Ackerman and all-wheel steering models, and thus eliminating the need to establish observation equations separately according to model category;

[0069] 2) Introducing inertial measurement information to uniformly express the motion state of the all-wheel steering model with arbitrary radius rotation and arbitrary direction parallelism, without the need to distinguish motion states and establish separate observation equations;

[0070] 3) Inertial information assistance only requires measurement data from one wheel speedometer, which greatly reduces the potential problem of spatiotemporal consistency in multi-sensor data acquisition;

[0071] 4) In addition, the influence of the odometer pitch installation angle on navigation accuracy was considered, and the pitch installation angle error was introduced into the estimated state error.

[0072] Based on the general velocity observation model, an 18-dimensional unified navigation filter was established to uniformly express the motion mode of the Ackerman / all-wheel steering robot with arbitrary radius rotation and arbitrary direction translation. This eliminates the need to design observation equations for different models and motion states, reduces the complexity of the navigation filter, and improves the accuracy and stability of the filter while reducing switching.

[0073] In addition to the objectives, features, and advantages described above, the present invention has other objectives, features, and advantages. The invention will now be described in further detail with reference to the figures. Attached Figure Description

[0074] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0075] Figure 1 This is a flowchart of a universal navigation method applicable to various motion model vehicles according to the present invention;

[0076] Figure 2This is a schematic diagram illustrating the application of the universal navigation method of the present invention, applicable to various motion model vehicles, to the steering of an Ackerman model robot;

[0077] Figure 3 This is a schematic diagram of an all-wheel steering robot rotating in place in existing technology;

[0078] Figure 4 It is the actual trajectory of a sports car in the all-wheel steering model of the park environment;

[0079] Figure 5 This is the actual trajectory of a sports car in an urban environment using an Ackerman steering model. Detailed Implementation

[0080] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0081] See Figure 1 As shown, a universal navigation method applicable to various motion model vehicles includes the following steps:

[0082] S1. Define the coordinate system;

[0083] S2. Real-time acquisition of data from multiple sensors based on a defined coordinate system, including real-time data from the wheel speed meter;

[0084] S3, in one Kalman filter update cycle In the middle, the inertial measurement unit data acquired based on S2 and Given the nominal navigation state at time t, perform a one-step recursive process on the nominal navigation state to obtain... The recursive nominal navigation state at each moment;

[0085] S4. Based on the inertial measurement unit data obtained in S2, make a one-step prediction of the navigation error state;

[0086] S5. Based on the real-time data of the wheel speedometer collected in S2, establish the observation equation and update and correct the navigation error status;

[0087] S6. Update and correct the navigation components based on the estimated error state from S5. At time S3, the recursive nominal navigation state is calculated, and the updated recursive nominal navigation state is obtained.

[0088] This invention no longer distinguishes motion modes but uses a unified expression, which simplifies the complexity of the model. It only requires information from a single wheel speedometer and considers the impact of two installation angle errors on navigation accuracy. It is applicable not only to Ackerman vehicle robots but also to all-wheel steering vehicle robots.

[0089] This indicates the measurement speed of the SINS (IMU) at... Components on the coordinate axes of the system; This indicates the measured speed of the wheel speed gauge. Components on the coordinate axes of the system; It serves as the robot's center of rotation; Let be the robot's rotation radius. Figure 2 When the robot rotates, the rear wheels of the Ackerman model robot are fixedly mounted, with no change in swing angle, and remain fixedly connected to the IMU coordinate system. The wheel speed measured by the wheel speed meter is the same as the carrier speed measured by the IMU. Meanwhile... Figure 3 When the all-wheel steering robot rotates, all wheels have a swing angle, and the wheel speed measured by the wheel speed meter is obviously not equal to the carrier speed measured by the IMU.

[0090] Furthermore, S1 specifically includes: the inertial measurement unit being installed at the geometric center of the Ackerman or all-wheel steering vehicle robot; and defining the strapdown inertial navigation coordinate system as follows: The system's origin is located at the center of the inertial measurement unit. The coordinate axes follow the right-front-top order; the wheel speedometer coordinate system is defined as follows. The system originates at the point of contact between the tested wheel and the ground. The coordinate axes follow a right-front-top order; the navigation system is defined as the local northeast-sky coordinate system. The geocentric inertial coordinate system is defined as follows: The Earth coordinate system is defined as follows: The lever arm vector of the inertial measurement unit relative to the measurement center of the wheel speed gauge is obtained based on the mechanical installation. .

[0091] Furthermore, S2 specifically includes: real-time synchronous acquisition of data at the same frequency. Angular velocity output by the inertial measurement unit acceleration and wheel sway angle data The wheel speed meter outputs real-time data as follows: , The sampling time interval, (Typical 10ms);

[0092] The initial latitude, longitude, and altitude of the Ackerman or all-wheel steering vehicle robot are given manually or using an external information source (such as GNSS, Global Navigation Satellite System). Initial attitude matrix Initial velocity Error covariance matrix of initial navigation state and navigation error state .

[0093] Preferably, by performing a recursive step on the nominal navigation state, the following steps are obtained: When determining the recursive nominal navigation state at time step, the following steps are included:

[0094] Recursive nominal posture, the first The attitude matrix estimated one step at a time The expression is as follows:

[0095] ;

[0096] in, express The estimated pose matrix is ​​updated continuously. express The estimated gyroscope bias error is updated continuously. Indicates an exponential mapping;

[0097] Recursive nominal speed, the first The speed of one-step recursive estimation at any given time The expression is as follows:

[0098] ;

[0099] in, express The estimated speed is updated in real time. yes The accelerometer zero bias error is constantly updated. The gravity vector It is the acceleration due to gravity. , T This represents the matrix transpose operator. express The estimated pose matrix is ​​updated continuously;

[0100] The nominal position of the recursion, the first Position estimated step by step The expression is as follows:

[0101] ;

[0102] in, express The estimated location (longitude, latitude, and altitude) is updated continuously.

[0103] The nominal attitude, nominal velocity, and nominal position obtained by one step of recursion are combined to form The recursive nominal navigation state at each moment.

[0104] Preferably, S4 includes the following steps:

[0105] The error state of the strapdown inertial navigation system (SINS) is selected as attitude angle error. Speed ​​error Position error Gyroscope zero bias error Accelerometer zero bias error Wheel speed gauge scaling error Wheel speed gauge heading installation angle error And wheel speed gauge pitch installation angle error A total of 18-dimensional error state vectors, error state vectors The expression is as follows:

[0106] ;

[0107] in, T This represents the matrix transpose operator;

[0108] The expression for the continuous state transition equation is as follows:

[0109] ;

[0110] ;

[0111] ;

[0112] ;

[0113] ;

[0114] in, Indicates the rate of change of error state; The Jacobian matrix represents the error state; The noise vector representing the error state transition process follows a Gaussian distribution. express Navigation system; Indicates the first The nominal speed of time recursion is at Navigation system Axial components, ; Indicates the Earth's radius; g Represents gravitational acceleration; Navigation system Relative to Earth system angular velocity; Indicates the Earth's rotational angular velocity at Navigation system Axial components, ; Navigation system Relative geocentric inertial coordinate system angular velocity at Navigation system Axial components, ; Indicates the accelerometer output measurement at Navigation system Axial components, ; This indicates the measurement noise of the three gyroscopes; The measurement noise of the three accelerometers is represented by a Gaussian distribution; N Indicates the symbol for Gaussian distribution; Represents the process noise variance matrix;

[0115] Calculate the error covariance matrix of the navigation error state based on the Jacobian matrix of the error state. One-step prediction:

[0116] ;

[0117] in, express The estimated error covariance matrix is ​​updated at all times.

[0118] Preferably, when establishing the general observation equation, the velocity observation equation The expression is as follows:

[0119] ;

[0120] in, Represents the lever arm vector; the Jacobian matrix is ​​observed by the wheel speed gauge. The expression is as follows:

[0121] ;

[0122] ;

[0123] ;

[0124] It follows a Gaussian distribution;

[0125] in, Indicates the first The calculation matrix at any given moment consists of the relevant installation error angles and error quantities; Indicates the first The wheel speedometer outputs real-time data. This represents a calculation matrix consisting of the installation error angle, the error amount, and the measured values ​​output by the wheel speed gauge. The observed noise vector is represented by a Gaussian distribution. The variance matrix representing the observation noise; I Represents the identity matrix. Indicates the first The heading installation angle error of the wheel speed gauge at any given time; Indicates the first The pitch angle error of the wheel speed gauge at all times.

[0126] Preferably, updating and correcting the navigation error state includes the following steps:

[0127] Calculate the Kalman filter gain matrix :

[0128] ;

[0129] Update the estimated navigation error state vector :

[0130] ;

[0131] Update the estimated navigation error state covariance :

[0132] ;

[0133] in, I Represents the identity matrix.

[0134] Furthermore, S6 specifically includes:

[0135] Attitude update, based on the updated estimated navigation error state vector Extracting attitude error The updated pose matrix is ​​obtained. : ;

[0136] Velocity update, based on the updated estimated navigation error state vector Extraction speed error Get the updated speed : = ;

[0137] Position update, based on the updated estimated navigation error state vector Extracting position error Get the updated position : ;

[0138] Gyroscope bias error update is performed based on the updated estimated navigation error state vector. Extracting the zero bias error of the gyroscope ,in express The estimated gyroscope bias error is updated continuously.

[0139] Accelerometer bias error update, based on the updated estimated navigation error state vector. Extracting the zero bias error of the accelerometer ,in express The accelerometer zero bias error is constantly updated;

[0140] The updated attitude matrix, updated velocity, updated position, updated gyroscope bias error, and updated accelerometer bias error are combined to form the updated recursive nominal navigation state.

[0141] The present invention also provides a readable storage medium storing a computer program adapted to be loaded by a processor and executed as described above for a universal navigation method applicable to various motion model vehicles.

[0142] The present invention also provides a computer device, the computer device including a memory and a processor, the memory storing a computer program, which, when executed by the processor, runs the universal navigation method applicable to various motion model vehicles as described above.

[0143] A real-world test was conducted on a four-wheel all-wheel steering robot in a typical park environment. The test covered a distance of approximately 1.6 kilometers and lasted for more than 45 minutes. The entire test was conducted without GNSS information assistance and included typical operational maneuvers such as rotation with different radii, drifting, stationary rotation, and forward movement. Figure 4 and Figure 5 The actual trajectory of the sports car under a certain all-wheel steering model and the Ackermann model is shown. Table 1 gives the statistical results of three repeated all-wheel steering experiments.

[0144] Table 1. Statistical analysis of repeatability test results for the all-wheel steering model.

[0145]

[0146] A real-world test was conducted on the Ackerman all-wheel steering robot in a typical urban environment. The test covered a distance of approximately 13 kilometers and took more than 1 hour and 15 minutes. The entire test was conducted without GNSS information assistance and included scenarios such as tunnels, elevated roads, and tree-lined areas. Figure 5 It shows the actual trajectory of a sports car under the Ackermann model.

[0147] The results show that the general observation equation proposed in this invention is applicable to various typical maneuvering modes of Ackerman and all-wheel steering robots, and effectively maintains the accuracy of the robot's navigation system for a long time in complex environments such as GNSS rejection and SLAM degradation.

[0148] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A general navigation method for vehicles of various motion models, characterized by, Comprising the following steps: S1, defining a coordinate system; S2, collecting real-time sensor data based on the defined coordinate system, including real-time wheel speed data; S2 specifically includes: real-time synchronous acquisition of data at the same frequency. Angular velocity output by the inertial measurement unit acceleration and wheel sway angle data The wheel speed meter outputs real-time data as follows: , The sampling time interval, ; Given the initial latitude and longitude position of an Ackerman or omnidirectional vehicle robot , initial pose matrix , initial velocity , error covariance matrix of initial navigation state and navigation error state ; S3. In one Kalman filter update cycle based on the inertial measurement unit data acquired in S2 and the nominal navigation state at the time instant , a step of recursion of the nominal navigation state is performed to obtain a recursive nominal navigation state at the time instant . S4, predicting the navigation error state based on the inertial measurement unit data obtained in S2; S4 comprises the following steps: The error states of the SINS are chosen as attitude angle errors , velocity errors , position errors , gyro bias errors , accelerometer bias errors , wheel speed scale errors , wheel speed heading installation angle errors , and wheel speed pitch installation angle errors , a total of 18-dimensional error state vectors, the error state vectors X are expressed as follows: ； wherein T denotes the matrix transpose operator; The continuous state transition equation is expressed as follows: ； ； ； ； ； in, Indicates the rate of change of error state; The Jacobian matrix represents the error state; The noise vector representing the error state transition process follows a Gaussian distribution. express Navigation system; Indicates the first Nominal speed at each moment exist Navigation system Axial components, ; Indicates the Earth's radius; g Represents gravitational acceleration; Navigation system Relative to Earth system angular velocity; Indicates the Earth's rotational angular velocity at Navigation system Axial components, ; Navigation system Relative geocentric inertial coordinate system angular velocity at Navigation system Axial components, ; Indicates the accelerometer output measurement at Navigation system Axial components, ; This indicates the measurement noise of the three gyroscopes; This indicates the measurement noise of the three accelerometers; N Indicates the symbol for Gaussian distribution; Represents the process noise variance matrix; Computing an error covariance matrix of the navigation error states from the error state Jacobian matrix One-step prediction: ； wherein denotes updating the estimated error covariance matrix at the time instant S5, establishing a general observation equation based on the wheel speed real-time data collected in S2, and updating the corrected estimated navigation error state; S6, update correction of navigation component estimating error state from S5 The nominal navigation state is recursively updated at time S3 to obtain an updated recursive nominal navigation state.

2. The universal navigation method for vehicles applicable to a plurality of motion models according to claim 1, characterized by, S1 specifically comprises: the inertial measurement unit is installed at the geometric center of the Ackerman or omnidirectional vehicle robot; a strapdown inertial navigation coordinate system is defined as , with the origin at the center of the inertial measurement unit , and the coordinate axes following the right-front-up order; a wheel speed meter coordinate system is defined as , with the origin at the contact point between the measured wheel and the ground , and the coordinate axes following the right-front-up order; a navigation coordinate system is defined as the local north-east-sky ; a geocentric inertial coordinate system is defined as ; an earth coordinate system is defined as ; and a lever arm vector of the inertial measurement unit relative to the measurement center of the wheel speed meter is obtained according to the mechanical installation .

3. The universal navigation method for vehicles applicable to a plurality of motion models according to claim 2, characterized by, In a one-step recursion of the nominal navigation state, one obtains In a one-step recursion of the nominal navigation state, one obtains In a one-step recursion of the nominal navigation state, one obtains Recursive nominal pose, part The pose matrix estimated at time step k is recursively computed as The expression is as follows: ； wherein, denotes the estimated pose matrix is updated at time instant denotes the estimated gyro bias error is updated at time instant denotes an exponential map; Recursive nominal velocity, part Velocity estimated one step ahead The expression is as follows: ； wherein denotes the estimated velocity is updated at time instant is the estimated accelerometer bias error is updated at time instant is the gravity vector, wherein is the gravitational acceleration, denotes the estimated attitude matrix is updated at time instant Recursive nominal position, part The position is estimated one step at a time The expression is as follows: ； wherein indicates updating the estimated position in real time; The nominal attitude, nominal velocity and nominal position combination resulting from the one-step recursion form the nominal navigation state at the time instant.

4. The universal navigation method for vehicles applicable to a plurality of motion models according to claim 3, characterized by, In establishing the general observation equation, the velocity observation equation The expression is as follows: ； where the wheel odometry observed Jacobian matrix is given by the expression ； ； ； ； wherein, represents the calculation matrix composed of the relevant installation error angle and error amount at the time t; represents the calculation matrix composed of the relevant installation error angle and error amount at the time t; represents the real-time data output by the wheel speed meter at the time t; represents the calculation matrix composed of the installation error angle, error amount and measurement value output by the wheel speed meter; represents the observation noise vector, which is subject to Gaussian distribution; represents the variance matrix of the observation noise; I represents the unit matrix, represents the installation angle error of the wheel speed meter heading at the time t; represents the installation angle error of the wheel speed meter heading at the time t; represents the installation angle error of the wheel speed meter pitch at the time t; represents the installation angle error of the wheel speed meter pitch at the time t.​ 5. The universal navigation method for vehicles applicable to a plurality of motion models according to claim 4, characterized by, When updating the corrected navigation error state, the following steps are included: Computing a Kalman filter gain matrix : ； updating the estimated navigation error state vector : ； updating the estimated navigation error state covariance : 。 6. The universal navigation method for vehicles applicable to a plurality of motion models according to claim 3, characterized by, S6 specifically includes: pose update, updating the estimated navigation error state vector extracting a pose error quantity , obtaining an updated pose matrix : ; velocity update, updating the estimated navigation error state vector extracting a velocity error quantity , obtaining an updated velocity : = ; Position update, updating the estimated navigation error state vector Extracting the position error quantity , obtaining an updated position : ; gyro bias error update, based on the updated estimate of the navigation error state vector extracting a gyro bias error quantity wherein denotes updating the estimate of the gyro bias error at the time instant accelerometer bias error update, the navigation error state vector is updated according to the updated estimation extracting an accelerometer bias error amount wherein denotes updating the estimated accelerometer bias error at the time instant The updated attitude matrix, updated velocity, updated position, updated gyro zero bias error and updated accelerometer zero bias error are combined to form an updated recursive nominal navigation state.

7. A readable storage medium characterized by, The readable storage medium stores a computer program, and the computer program is suitable for being loaded and executed by the processor to implement the general navigation method for vehicles with multiple motion models according to any one of claims 1-6.

8. A computer device, comprising: The computer device comprises a memory and a processor, and the memory stores a computer program, and the computer program is executed by the processor to implement the general navigation method for vehicles with multiple motion models according to any one of claims 1-6.

Images