INS / OD integrated navigation system and method based on vehicle kinematic model assistance

Through the combination of vehicle kinematic model and Wheel-IMU, the navigation accuracy problem of traditional INS/OD navigation system in the case of vehicle turning, slipping, etc. is solved, and the navigation accuracy and robustness are achieved, and it is suitable for a variety of carriers .

CN118936484APending Publication Date: 2024-11-12SOUTHEAST UNIV
View PDF 1 Cites 0 Cited by

Patent Information

Application Number
CN202411021542.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-29
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

Traditional INS/OD combined navigation systems are difficult to accurately navigate when the vehicle turns, slips, and idles, and the parameters are not versatile. They cannot fully utilize the vehicle kinematic model to improve navigation accuracy and robustness.

Method used

Wheel-IMU is used to replace the traditional odometer, and static judgment and Kalman filtering are performed through vehicle kinematics model combining the information of IMU and OD modules to reduce the number of sensors and improve the robustness and accuracy of the navigation system.

Benefits of technology

It effectively improves the positioning accuracy and robustness of the navigation system in complex vehicles, avoids the impact of wheel slip on navigation, and is suitable for unmanned vehicles and wheeled robots.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118936484A_ABST
    Figure CN118936484A_ABST
Patent Text Reader

Abstract

According to the INS / OD integrated navigation system and method based on vehicle kinematic model assistance, different from a traditional CAN bus sensor, a Wheel-IMU (W-IMU) is used for replacing a front tire odometer, the steering angle of a wheel is obtained under the condition that information measured by a wheel angle sensor is not depended on, and therefore the steering angle of the wheel is obtained; necessary observation information is not lost while the number of sensors is reduced. The two-dimensional speed and the course angular rate of vehicle motion can be obtained by combining the forward and transverse speeds provided by the W-IMU and the speed measurement information of the rear wheel OD, the fault state of the odometer can be diagnosed by adding static judgment into the system, and the sensor data availability under different vehicle conditions can be effectively judged. Therefore, the usability of integrated navigation positioning is further improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, and in particular to an INS / OD integrated navigation system and method based on the assistance of a vehicle kinematics model. Background Art

[0002] The Global Navigation Satellite System (GNSS) can provide users with high-precision positioning information around the world around the clock. However, active non-autonomous GNSS positioning technology is susceptible to the complex urban environment and electromagnetic interference equipment, and cannot provide accurate, continuous and reliable location information. From the current application point of view, the integration of GNSS / Inertial Navigation System (INS) / vehicle CAN (controller area network) bus (providing wheel speed and steering), and integrating vehicle motion constraint information, is the simplest and most effective vehicle-borne low-cost high-precision navigation solution. The INS / wheel odometer (OD) combined navigation system does not need to interact with the outside world for data, nor does it need to obtain information from the surrounding environment. By measuring the motion state of the carrier, it can achieve fully autonomous navigation. In most cases, vehicle motion can be simplified into a two-dimensional plane motion. Implicit information such as the restricted motion of ground vehicles can provide virtual external observation correction for the passive autonomous dead reckoning system. The INS / OD vehicle-borne combined navigation system can improve the accuracy maintenance capability of the vehicle-borne navigation system's fusion positioning in complex environments when the satellite signal is poor.

[0003] At present, the traditional INS / OD integrated navigation system widely uses nonholonomic constraints (NHC) to construct virtual observations to constrain vehicle motion. In actual vehicle navigation, factors such as vehicle motion state, body vibration, IMU installation position and installation angle will make it impossible to meet the assumption that the vehicle's lateral and vertical speeds are zero, resulting in errors in the NHC virtual observations, which in turn affects the suppression effect of inertial error estimation.

[0004] In the traditional scheme, it is difficult to determine the corresponding explicit expression using the NHC method, and it is difficult to give the optimal configuration scheme by manual parameter adjustment alone. Moreover, the parameters are not universal on different vehicles, and the restraining effect of constraints and OD on inertial estimation errors cannot be fully exerted. Compared with NHC, adding redundant sensors to improve the vehicle kinematic model can abandon the premise that the vehicle's lateral speed is 0, and effectively improve the auxiliary effect of OD on inertial navigation when the vehicle is turning. Since the scheme uses a corner sensor, it is not applicable to carriers such as unmanned vehicles and wheeled robots, and the scheme does not fully consider the situation of tire slippage, bumps, idling, locking, etc.

[0005] After searching, the Chinese invention patent with the authorization announcement number CN110221333A proposed a measurement error compensation method for a vehicle-mounted INS / OD combined navigation system. On a distributed drive electric smart car, the vehicle-mounted odometer is used to assist the inertial navigation system for vehicle combined navigation. The design takes into account the main error sources in the combined navigation system, such as inertial navigation error, odometer scale coefficient error, lever arm error, etc., and considers the influence of vehicle driving conditions on the odometer scale coefficient and the validity of measurement data, and designs parameters to judge them. In addition, the system uses a dual odometer, and the combination of the two odometer measurement data can improve the anti-interference of the odometer measurement data to a certain extent. The optimized odometer data is used as the measurement quantity in the combined navigation system measurement equation for correction, and a filtering system with inertial navigation position, speed, attitude error, random constant error of inertial device, left and right odometer scale coefficient error, and lever arm as state quantity is constructed, and feedback correction is performed to achieve the improvement of combined navigation positioning accuracy based on measurement error compensation.

[0006] The technical comparison between this application and the above-mentioned reference documents is as follows:

[0007] Although it also involves the combination of an odometer (OD) and an inertial navigation system (INS) on a vehicle, the present application is an INS / OD combined navigation system and method based on the assistance of a vehicle kinematic model. Compared with the above patents, the main differences are as follows: First, the number and layout of the odometers. The present application uses an IMU as a wheel odometer (W-IMU). Compared with traditional odometers, W-IMU can provide two-dimensional speed, and its measurement information can effectively improve the availability of measurement data when the vehicle slips. In addition, compared with the left and right distribution of the odometer, the front and back distribution designed by the application can make the measurement data of the odometer system have better reliability and anti-interference. Second, the availability of odometer data. Compared with not using unqualified odometer measurement information to correct the combined navigation system, the present application improves the availability of measurement information. Through the designed vehicle kinematic model, the present application can improve the robustness and navigation accuracy of the combined navigation system when the vehicle is side-slipping, stuck in the air, and locked. The present application can effectively solve the measurement error compensation problem raised in the comparative patent by determining the availability of sensor data under different vehicle conditions and improving data availability and reliability through kinematic models or parameter learning methods, thereby improving the positioning accuracy and robustness of the entire combined navigation system. Summary of the invention

[0008] To solve the above technical problems, the present invention proposes an INS / OD combined navigation system and method based on the assistance of vehicle kinematic model, which can reduce the number of sensors without losing necessary observation information, and constrain the drift of inertial calculation through specific expressions, which can effectively avoid the influence of wheel slip on the navigation and positioning availability of the combined system, and further improve the robustness of the combined navigation system and the accuracy and availability of positioning by adding static judgment and judging the vehicle motion state through IMU acceleration standard deviation, speed and OD speed.

[0009] To achieve the above object, the technical solution adopted by the present invention is:

[0010] An INS / OD integrated navigation system based on vehicle kinematic model assistance is characterized by comprising an INS module, an OD module and an integrated navigation module, wherein the INS module and the OD module realize information fusion through the integrated navigation module to obtain the positioning result of the integrated navigation.

[0011] The INS module includes an IMU, which is mounted on the vehicle frame. The INS module obtains high-frequency autonomous navigation results through the dead reckoning capability of the IMU, and provides acceleration standard deviation and speed information to complete the static determination of the vehicle;

[0012] The OD module includes OD and W-IMU. The OD module obtains the wheel speed and the distance the wheel moves through the number of pulses output by the OD and the radius of the wheel. The OD is installed on the rear wheel of the vehicle and provides the forward speed of the rear wheel of the vehicle through the OD. The W-IMU is installed on the front wheel of the vehicle and provides the forward and lateral speeds of the front wheel of the vehicle through the W-IMU. The information fusion of the INS module and the OD module can obtain the vehicle kinematic model.

[0013] In the above structure: the present invention proposes an INS / OD combined navigation system based on the assistance of vehicle kinematic model. Different from the traditional CAN bus sensor, the present invention uses Wheel-IMU (W-IMU) to replace the odometer in the front wheel, and obtains the wheel steering angle without relying on the wheel angle sensor measurement information, which can reduce the number of sensors without losing the necessary observation information. The two-dimensional speed and heading angular rate of the vehicle movement can be obtained by combining the forward and lateral speeds provided by the W-IMU with the speed measurement information of the rear wheel OD. By adding static judgment to the system, the availability of sensor data under different vehicle conditions can be determined, and the possible fault conditions of the combined navigation system can be diagnosed and processed, thereby further improving the availability and accuracy of the combined navigation positioning.

[0014] An INS / OD integrated navigation method based on vehicle kinematic model assistance is characterized by comprising the following steps: Step 1: Modeling the standard deviation σ of IMU output acceleration when the vehicle starts, as follows:

[0015] Set the number of acceleration frames acquired in the window to n, and obtain the acceleration standard deviation model σ0 under the initial state of the vehicle. The standard deviation formula is as follows:

[0016]

[0017] Step 2: Determine the vehicle and OD status, as follows:

[0018] At time t, the forward velocity v of the IMU I With OD forward speed v o The difference is less than the speed threshold λ v , then the vehicle is considered to be driving normally, and the following relationship holds true:

[0019] |v I -v o |<λ v (2);

[0020] Step 3: Establish the vehicle kinematic model during normal driving of the vehicle, as follows:

[0021] The vehicle kinematic model includes a body frame and wheels. The wheels include front wheels and rear wheels, where the front wheels are steering wheels and the rear wheels are driving wheels. The IMU is installed on the body frame for navigation solution, and the odometer is installed on the rear wheel to measure the forward speed of the rear wheel. The W-IMU is installed at the center of the front wheel on the same side as the odometer to provide the forward speed and lateral speed of the front tire. Different coordinate systems are established on the front wheel, rear wheel and body frame, respectively. The coordinate system m is the body frame coordinate system, w represents the tire system, mf represents the projection of the component in the W-IMU installation tire coordinate system in the vehicle coordinate system, mo represents the projection of the component in the OD coordinate system in the vehicle coordinate system, and the forward speed vectors of the front and rear tires are respectively

[0022]

[0023] represents the vehicle heading angular rate, l If Indicates the lever arm from the front wheel center to the IMU installation location, l Io Represents the lever arm from the rear wheel center to the IMU installation location, l mx Indicates the arm from the odometer installation position to the rear wheel center, l my Represents the lever arm from the front wheel center to the rear wheel center,

[0024] Assuming that the coordinate systems of the W-IMU, odometer, and vehicle frame coincide with each other or the installation error angle compensation has been completed, the front wheel steering angle during steering is ε It can be solved by formula (3) (4) to get:

[0025]

[0026] According to the vehicle kinematic model, when the vehicle is traveling on a horizontal plane, the velocity vectors of the front and rear wheels in the vehicle frame coordinate system are:

[0027]

[0028] When the sensor frequency is high enough, the vehicle's heading angular rate The integral within the window time is approximately equal to the wheel angle ε, and the vehicle rotates around a center of a circle, then the lateral velocity of the rear wheel of the vehicle is as follows:

[0029]

[0030] Considering the problem of different speeds of two wheels, a dual-track model was established. The vehicle steering can be determined by the angular velocity output of the IMU vertical gyroscope. Considering the influence of the lever arm in different rotation directions on the IMU speed estimation, the speed at the IMU installation position in the vehicle coordinate system is obtained according to the W-IMU and OD outputs. The following relationship holds true:

[0031]

[0032] Step 4: Determine the vehicle and OD motion status, as follows:

[0033] When the difference between the IMU and OD output speeds is too large during vehicle motion, and step 2 is not satisfied, the standard deviation model established in step 1 is compared with the observed value to make a static judgment, as follows:

[0034] Static judgment includes the following two situations:

[0035] a. When the wheels spin, including those caused by vehicle jamming, slipping or bouncing, the vehicle is not moving forward and the wheels spin, and the OD measurement information cannot be used for INS assistance. When the wheels spin due to bouncing, the vehicle is still moving forward, but the drive wheels are temporarily stuck in the air and spin, and the OD measurement information is unreliable;

[0036] b. When the wheels are locked but the vehicle is still moving forward, the OD output is 0, and the OD measurement information is unavailable. The vehicle's motion state is statically determined based on the IMU acceleration σ, while λ σis the acceleration σ standard deviation threshold, and the judgment expression is as follows:

[0037] |σ t -σ0|≤λ σ (9)

[0038] If the judgment is established, the wheels may be spinning. At this time, the vehicle is stationary or moving at a constant speed. To avoid misjudgment, speed information is added to assist in judgment. vI is the IMU speed threshold, then the judgment expressions for wheel spin caused by vehicle stuck / sinking, skidding or bouncing are shown in (10) and (11) respectively:

[0039]

[0040] |σ tz -σ0|≤λ σz (11)

[0041] If the judgment of formula (9) is not true, the wheel may be in a locked state. At this time, the OD output speed is judged again, λ vo is the OD output speed threshold, and its expression is as follows:

[0042]

[0043] Finally, the weight K of IMU and OD measurements in the combined system is assigned according to the result of static judgment feedback;

[0044] Step 5: Combine the results of steps 1-4, obtain auxiliary speed information through static judgment, vehicle kinematic model and OD combined vehicle motion constraints, combine the speed information with IMU speed information for Kalman filtering, and the complete Kalman filter state vector is:

[0045]

[0046] In the formula, Indicates the attitude misalignment angle, δv n Represents the three-way velocity error, δr n Indicates the errors of longitude, latitude and elevation, b a is the accelerometer zero bias, b g Indicates the gyroscope bias,

[0047] In the INS / OD combination, OD provides velocity measurement. Considering the arm error in the carrier coordinate system, the measurement equation and corresponding measurement matrix of INS / OD combined navigation are:

[0048]

[0049] H INS / OD =[0 3×3 I3×3 0 3×9 ] T (15)

[0050] In the formula, is the attitude transformation matrix from the IMU coordinate system to the navigation coordinate system, is the attitude transformation matrix from the IMU coordinate system to the vehicle coordinate system,

[0051] Finally, the INS navigation result is compensated by the Kalman filter solution to obtain the final result of INS / OD integrated navigation.

[0052] As a preferred technical solution of the present invention: in step 3: the odometer is installed on the right rear wheel, and the W-IMU is installed at the center of the right front wheel.

[0053] Compared with the prior art, the present invention has the following beneficial effects:

[0054] Based on the traditional INS / OD combination, the present invention uses vehicle motion to construct a vehicle kinematic constraint model, which can reduce the number of sensors without losing necessary observation information, and constrain the drift of inertial calculation through specific expressions, which can effectively avoid the influence of wheel slip on the navigation and positioning availability of the combined system. On this basis, static judgment is added, and the vehicle motion state is judged by the IMU acceleration standard deviation, speed and OD speed, and the influence of wheel idling and locking on OD measurement is fully considered, which can further improve the robustness of the combined navigation system and the accuracy and availability of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 Schematic diagram of the vehicle kinematic model.

[0056] Figure 2 This is the system block diagram for static judgment.

[0057] Figure 3 It is a schematic diagram of the INS / OD integrated navigation device and system operation based on the vehicle kinematic model. DETAILED DESCRIPTION

[0058] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0059] To solve the following two problems existing in the prior art:

[0060] 1. It is difficult to determine the corresponding explicit expression using the traditional NHC method. It is difficult to give the optimal configuration solution by manual parameter adjustment alone. The parameters are not universal on different vehicles, and the constraints and OD cannot fully exert their inhibitory effect on inertial estimation errors.

[0061] 2. Adding redundant sensors to improve the vehicle kinematic model can abandon the premise that the vehicle's lateral velocity is 0, but the solution is aimed at vehicles with angle sensors and is not applicable to unmanned vehicles and wheeled robots. The solution also does not fully consider situations such as tire slippage and idling. This method does not fully utilize the complementary advantages of IMU and OD measurement information.

[0062] like Figure 1-3 As shown, the present invention proposes an INS / OD integrated navigation system and method based on the assistance of vehicle kinematic model.

[0063] Among them, an INS / OD integrated navigation system based on vehicle kinematic model assistance is characterized by comprising an INS module, an OD module and an integrated navigation module, wherein the INS module and the OD module realize information fusion through the integrated navigation module to obtain the positioning result of the integrated navigation,

[0064] The INS module includes an IMU, which is mounted on the vehicle frame. The INS module obtains high-frequency autonomous navigation results through the dead reckoning capability of the IMU, and provides acceleration standard deviation and speed information to complete the static determination of the vehicle;

[0065] The OD module includes OD and W-IMU. The OD module obtains the wheel speed and the distance the wheel moves through the number of pulses output by the OD and the radius of the wheel. The OD is installed on the rear wheel of the vehicle and provides the forward speed of the rear wheel of the vehicle through the OD. The W-IMU is installed on the front wheel of the vehicle and provides the forward and lateral speeds of the front wheel of the vehicle through the W-IMU. The information fusion of the INS module and the OD module can obtain the vehicle kinematic model.

[0066] The present invention proposes an INS / OD combined navigation system based on the assistance of vehicle kinematic model. Different from the traditional CAN bus sensor, the present invention uses Wheel-IMU (W-IMU) to replace the odometer in the front wheel, and obtains the wheel steering angle without relying on the wheel angle sensor measurement information, which can reduce the number of sensors without losing the necessary observation information. The forward and lateral speeds provided by the W-IMU are combined with the speed measurement information of the rear wheel OD to obtain the two-dimensional speed and heading angular rate of the vehicle. By adding static judgment to the system, the availability of sensor data under different vehicle conditions is judged, and the possible fault conditions of the combined navigation system can be diagnosed and processed, thereby further improving the availability and accuracy of the combined navigation positioning.

[0067] An INS / OD integrated navigation method based on vehicle kinematic model assistance includes the following steps:

[0068] Step 1: Model the standard deviation σ of the IMU output acceleration when the vehicle starts, as follows:

[0069] Set the number of acceleration frames acquired in the window to n, and obtain the acceleration standard deviation model σ0 under the initial state of the vehicle. The standard deviation formula is as follows:

[0070]

[0071] Step 2: Determine the vehicle and OD status, as follows:

[0072] At time t, the forward velocity v of the IMU I With OD forward speed v o The difference is less than the speed threshold λ v , then the vehicle is considered to be driving normally, and the following relationship holds true:

[0073] |v I -v o |<λ v (2);

[0074] Step 3: Establish the vehicle kinematic model during normal driving of the vehicle, as follows:

[0075] The vehicle kinematic model includes a body frame and wheels. The wheels include front wheels and rear wheels, where the front wheels are steering wheels and the rear wheels are driving wheels. The IMU is installed on the body frame for navigation solution, and the odometer is installed on the rear wheel to measure the forward speed of the rear wheel. The W-IMU is installed at the center of the front wheel on the same side as the odometer to provide the forward speed and lateral speed of the front tire. Different coordinate systems are established on the front wheel, rear wheel and body frame, respectively. The coordinate system m is the body frame coordinate system, w represents the tire system, mf represents the projection of the component in the W-IMU installation tire coordinate system in the vehicle coordinate system, mo represents the projection of the component in the OD coordinate system in the vehicle coordinate system, and the forward speed vectors of the front and rear tires are respectively

[0076]

[0077] represents the vehicle heading angular rate, l If Indicates the lever arm from the front wheel center to the IMU installation location, l Io Represents the lever arm from the rear wheel center to the IMU installation location, l mx Indicates the arm from the odometer installation position to the rear wheel center, l my Represents the lever arm from the front wheel center to the rear wheel center,

[0078] Assuming that the coordinate systems of the W-IMU, odometer, and vehicle frame coincide with each other or the compensation of the installation error angle has been completed, the front wheel steering angle ε during steering can be solved by formula (3) (4):

[0079]

[0080] According to the vehicle kinematic model, when the vehicle is traveling on a horizontal plane, the velocity vectors of the front and rear wheels in the vehicle frame coordinate system are:

[0081]

[0082] When the sensor frequency is high enough, the vehicle's heading angular rate The integral within the window time is approximately equal to the wheel angle ε, and the vehicle rotates around a center of a circle, then the lateral velocity of the rear wheel of the vehicle is as follows:

[0083]

[0084] Considering the problem of different speeds of two wheels, a dual-track model was established. The vehicle steering can be determined by the angular velocity output of the IMU vertical gyroscope. Considering the influence of the lever arm in different rotation directions on the IMU speed estimation, the speed at the IMU installation position in the vehicle coordinate system is obtained according to the W-IMU and 0D outputs. The following relationship holds true:

[0085]

[0086] Step 4: Determine the vehicle and OD motion status, as follows:

[0087] When the difference between the IMU and 0D output speeds is too large during vehicle motion, and step 2 is not satisfied, the standard deviation model established in step 1 is compared with the observed value to make a static judgment, as follows:

[0088] Static judgment includes the following two situations:

[0089] a. When the wheels spin, including those caused by vehicle jamming, slipping or bouncing, the vehicle is not moving forward and the wheels spin, and the OD measurement information cannot be used for INS assistance. When the wheels spin due to bouncing, the vehicle is still moving forward, but the drive wheels are temporarily stuck in the air and spin, and the OD measurement information is unreliable;

[0090] b. When the wheels are locked but the vehicle is still moving forward, the OD output is 0, and the OD measurement information is unavailable. The vehicle's motion state is statically determined based on the IMU acceleration σ, while λ σis the acceleration σ standard deviation threshold, and the judgment expression is as follows:

[0091] |σ t -σ0|≤λ σ (9)

[0092] If the judgment is established, the wheels may be spinning. At this time, the vehicle is stationary or moving at a constant speed. To avoid misjudgment, speed information is added to assist in judgment. vI is the IMU speed threshold, then the judgment expressions for wheel spin caused by vehicle stuck / sinking, skidding or bouncing are shown in (10) and (11) respectively:

[0093]

[0094] |σ tz -σ0|≤λ σz (11)

[0095] If the judgment of formula (9) is not true, the wheel may be in a locked state. At this time, the OD output speed is judged again, λ vo is the OD output speed threshold, and its expression is as follows:

[0096]

[0097] Finally, the weight K of IMU and 0D measurement in the combined system is assigned according to the result of static judgment feedback;

[0098] Step 5: Combine the results of steps 1-4, obtain auxiliary speed information through static judgment, vehicle kinematic model and OD combined vehicle motion constraints, combine the speed information with IMU speed information for Kalman filtering, and the complete Kalman filter state vector is:

[0099]

[0100] In the formula, Indicates the attitude misalignment angle, δv n Represents the three-way velocity error, δr n Indicates the errors of longitude, latitude and elevation, b a is the accelerometer bias, b g Indicates the gyroscope bias,

[0101] In the INS / OD combination, OD provides velocity measurement. Considering the arm error in the carrier coordinate system, the measurement equation and corresponding measurement matrix of INS / OD combined navigation are:

[0102]

[0103] H INS / OD =[0 3×3 I3×3 0 3×9 ] T (15)

[0104] In the formula, is the attitude transformation matrix from the IMU coordinate system to the navigation coordinate system, is the attitude transformation matrix from the IMU coordinate system to the vehicle coordinate system,

[0105] Finally, the INS navigation result is compensated by the Kalman filter solution to obtain the final result of INS / OD integrated navigation.

[0106] The odometer is installed on the right rear wheel, and the W-IMU is installed at the center of the right front wheel.

[0107] Based on the traditional INS / OD combination, the present invention uses vehicle motion to construct a vehicle kinematic constraint model, which can reduce the number of sensors without losing necessary observation information, and constrain the drift of inertial calculation through specific expressions, which can effectively avoid the influence of wheel slip on the navigation and positioning availability of the combined system. On this basis, static judgment is added, and the vehicle motion state is judged by the IMU acceleration standard deviation, speed and OD speed, and the influence of wheel idling and locking on OD measurement is fully considered, which can further improve the robustness of the combined navigation system and the accuracy and availability of positioning.

[0108] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. An INS / OD integrated navigation system based on vehicle kinematic model assistance, Features: It includes an INS module, an OD module and an integrated navigation module. The INS module and the OD module realize information fusion through the integrated navigation module to obtain the positioning result of the integrated navigation. The INS module includes an IMU, which is mounted on the vehicle frame. The INS module obtains high-frequency autonomous navigation results through the dead reckoning capability of the IMU, and provides acceleration standard deviation and speed information to complete the static determination of the vehicle; The OD module includes OD and W-IMU. The OD module obtains the wheel speed and the distance the wheel moves through the number of pulses output by the OD and the radius of the wheel. The OD is installed on the rear wheel of the vehicle and provides the forward speed of the rear wheel of the vehicle through the OD. The W-IMU is installed on the front wheel of the vehicle and provides the forward and lateral speeds of the front wheel of the vehicle through the W-IMU. The information fusion of the INS module and the OD module can obtain the vehicle kinematic model.

2. According to claim 1, a vehicle kinematic model-assisted INS / OD integrated navigation method, Features: The steps include: Step 1: Model the standard deviation σ of the IMU output acceleration when the vehicle starts, as follows: Set the number of acceleration frames acquired in the window to n, and obtain the acceleration standard deviation model σ under the initial state of the vehicle 0 , then the standard deviation formula is as follows: Step 2: Determine the vehicle and OD status, as follows: At time t, the forward velocity v of the IMU I With OD forward speed v o The difference is less than the speed threshold λ v , then the vehicle is considered to be driving normally, and the following relationship holds true: |v I -v o |<λ v (2); Step 3: Establish the vehicle kinematic model during normal driving of the vehicle, as follows: The vehicle kinematic model includes a body frame and wheels. The wheels include front wheels and rear wheels, where the front wheels are steering wheels and the rear wheels are driving wheels. The IMU is installed on the body frame for navigation solution, and the odometer is installed on the rear wheel to measure the forward speed of the rear wheel. The W-IMU is installed at the center of the front wheel on the same side as the odometer to provide the forward speed and lateral speed of the front tire. Different coordinate systems are established on the front wheel, rear wheel and body frame, respectively. The coordinate system m is the body frame coordinate system, w represents the tire system, mf represents the projection of the component in the W-IMU installation tire coordinate system in the vehicle coordinate system, mo represents the projection of the component in the OD coordinate system in the vehicle coordinate system, and the forward speed vectors of the front and rear tires are respectively represents the vehicle heading angular rate, l If Indicates the lever arm from the front wheel center to the IMU installation location, l Io Represents the lever arm from the rear wheel center to the IMU installation location, l mx Indicates the arm from the odometer installation position to the rear wheel center, l my Represents the lever arm from the front wheel center to the rear wheel center, Assume that the coordinate systems of the W-IMU, odometer, and vehicle frame coincide with each other or the compensation of the installation error angle has been completed. The front wheel steering angle ε during steering can be solved by formula (3) (4): According to the vehicle kinematic model, when the vehicle is traveling on a horizontal plane, the velocity vectors of the front and rear wheels in the vehicle frame coordinate system are: When the sensor frequency is high enough, the vehicle's heading angular rate The integral within the window time is approximately equal to the wheel angle ε, and the vehicle rotates around a center of a circle, then the lateral velocity of the rear wheel of the vehicle is as follows: Considering the problem of different speeds of two wheels, a dual-track model was established. The vehicle steering can be determined by the angular velocity output of the IMU vertical gyroscope. Considering the influence of the lever arm in different rotation directions on the IMU speed estimation, the speed at the IMU installation position in the vehicle coordinate system is obtained according to the W-IMU and OD outputs. The following relationship holds true: Step 4: Determine the vehicle and OD motion status, as follows: When the difference between the IMU and OD output speeds is too large during vehicle motion, and step 2 is not satisfied, the standard deviation model established in step 1 is compared with the observed value to make a static judgment, as follows: Static judgment includes the following two situations: a. When the wheels spin, Including idling caused by vehicle jamming, sliding or bouncing. When the vehicle jams, slides or spins due to idling, the vehicle does not move forward and the wheels spin. The OD measurement information cannot be used for INS assistance. When the vehicle bounces due to idling, the vehicle is still moving forward, but the driving wheels are temporarily stuck in the air and spin. The OD measurement information is unreliable. b. When the wheels are locked but the vehicle is still moving forward, the OD output is 0 and the OD measurement information is unavailable. The vehicle's motion state is statically determined based on the IMU's acceleration σ, and λ σ is the acceleration σ standard deviation threshold, and the judgment expression is as follows: |s t -s 0 |≤λ σ (9) If the judgment is established, the wheels may be spinning. At this time, the vehicle is stationary or moving at a constant speed. To avoid misjudgment, speed information is added to assist in judgment. vI is the IMU speed threshold, then the judgment expressions for wheel spin caused by vehicle stuck / sinking, skidding or bouncing are shown in (10) and (11) respectively: |s tz -s 0 |≤λ σz (11) If the judgment of formula (9) is not true, the wheel may be in a locked state. At this time, the OD output speed is judged again, λ vo is the OD output speed threshold, and its expression is as follows: Finally, the weight K of IMU and OD measurements in the combined system is assigned according to the result of static judgment feedback; Step 5: Combine the results of steps 1-4, obtain auxiliary speed information through static judgment, vehicle kinematic model and OD combined vehicle motion constraints, combine the speed information with IMU speed information for Kalman filtering, and the complete Kalman filter state vector is: In the formula, Indicates the attitude misalignment angle, δv n Represents the three-way velocity error, δr n Indicates the errors of longitude, latitude and elevation, b a is the accelerometer bias, b g Indicates the gyroscope bias, In the INS / OD combination, OD provides velocity measurement. Considering the arm error in the carrier coordinate system, the measurement equation and corresponding measurement matrix of INS / OD combined navigation are: H INS / OD =[O 3×3 I 3×3 O 3×9 ] T (15) In the formula, is the attitude transformation matrix from the IMU coordinate system to the navigation coordinate system, is the attitude transformation matrix from the IMU coordinate system to the vehicle coordinate system, Finally, the INS navigation result is compensated by the Kalman filter solution to obtain the final result of INS / OD integrated navigation.

3. According to claim 2, a vehicle kinematic model-assisted INS / OD integrated navigation method, Features: In step 3: the odometer is installed on the right rear wheel, and the W-IMU is installed at the center of the right front wheel.

Citation Information

Patent Citations

  • Measurement error compensation method for vehicle-mounted INS / OD integrated navigation system

    CN110221333A