A fusion localization method that takes into account both vehicle dynamics and nonholonomic constraints

Abstract

This invention relates to the field of vehicle positioning and navigation technology, and provides a fusion positioning method that takes into account vehicle dynamics and nonholonomic constraints. The method includes the following steps: reading information provided by a GPS receiver, IMU sensor, vehicle CAN bus, etc.; establishing a coordinate system based on the read information; calculating position, velocity, and attitude information using an INS kinematic model based on information provided by the IMU sensor and GPS receiver (or UWB); establishing state equations and observation equations, estimating navigation error values ​​and IMU sensor bias values ​​using an error state Kalman filter, and using the results to correct the INS kinematic model; establishing a trajectory estimation model based on vehicle dynamics, establishing state equations and observation equations, and estimating position and heading angle information using an extended Kalman filter. This method can improve the accuracy of navigation and positioning when GNSS signals are interfered with or interrupted, effectively improving positioning accuracy and extending positioning time.

Description

Technical Field

[0001] This invention belongs to the field of vehicle positioning and navigation technology, and particularly relates to a fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints. Background Technology

[0002] The decision-making, trajectory planning, and motion control of intelligent vehicles heavily rely on accurate and continuous position information. GNSS is a recognized method for acquiring vehicle position, providing sub-decimeter positioning accuracy when combined with real-time kinematics (RTK). However, due to the complex urban environment, its high-precision positioning can degrade due to signal interruptions. INS can determine position, velocity, and attitude information in a short time, but biases and noise from accelerometers and gyroscopes cause errors to accumulate over time. By integrating GNSS and INS, a more accurate and robust navigation solution can be achieved.

[0003] However, because only INS operates during GNSS outages, GNSS / INS navigation systems cannot provide accurate positioning information in environments where GNSS is susceptible to interference. Track estimation systems can independently calculate position based on sensor information without relying on GNSS data, providing positioning information even with short-term GNSS signal interruptions. However, DR algorithms alone cannot meet the positioning accuracy requirements of intelligent vehicles because road surface bumps during actual driving cause the two-dimensional plane assumptions to fail, leading to significant errors. UWB can provide high-precision positioning information, but UWB devices require pre-installation, making it difficult to achieve full outdoor road coverage; it is generally used for indoor positioning. Summary of the Invention

[0004] The purpose of this invention is to provide a fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints, thereby addressing the problems mentioned in the background art.

[0005] The present invention is implemented as follows: a fusion localization method that takes into account both vehicle dynamics and nonholonomic constraints includes the following steps:

[0006] Step 1: Read the position, speed and attitude information provided by the GPS receiver, the acceleration and angular velocity information provided by the IMU sensor, the vehicle speed, wheel speed and front wheel steering angle information provided by the vehicle CAN bus, and the position information provided by the UWB device pre-installed indoors;

[0007] Step 2: Preprocess the read information and establish the corresponding coordinate system;

[0008] Step 3: Determine whether the GNSS signal is available based on the horizontal geometric precision factor, i.e., the HDOP value. If it is not available, read the location information provided by UWB.

[0009] Step 4: Based on the information provided by the IMU sensor and GPS receiver, or the IMU sensor and UWB, calculate the position, velocity and attitude information using the INS kinematic model;

[0010] Step 5: Establish the state equation and observation equation, and use the error state Kalman filter (ESKF) to estimate the navigation error value and IMU sensor bias value. The obtained navigation error estimate and IMU sensor bias estimate are used to correct the INS kinematic model.

[0011] Step 6: Establish a trajectory estimation model (DR) based on vehicle dynamics, establish state equations and observation equations, and use extended Kalman filter (EKF) to estimate position and heading angle information;

[0012] Step S7: Output the final estimated position and heading angle information.

[0013] In a further technical solution, in step 4, the INS kinematic model is as follows:

[0014]

[0015] Where, p k v k ε k The position, velocity, and attitude (expressed as unit quaternions) provided by the GPS receiver (or UWB) are represented in s. k ω k This represents the three-dimensional scale and angular velocity information provided by the inertial measurement unit (IMU). It is the rotation matrix from the body coordinate system to the navigation platform coordinate system.

[0016] In a further technical solution, in step 3, when HDOP is less than a threshold, position estimation is performed using GPS receiver signals; when HDOP is greater than a threshold, position estimation is performed using UWB signals.

[0017] A further technical solution involves the following steps in step 5: Building the state-space model of the error-state Kalman filter system.

[0018] Step 5.1: Establish the system state equations as follows:

[0019]

[0020] Wherein, δp k δv k ε k δs represents the error between the actual and nominal values ​​of the navigation platform's position, velocity, and attitude, respectively; k ,δω kThese represent the proportional and angular velocity deviations measured by the slowly changing IMU, respectively. The measurement noise is represented as Gaussian white noise, and its covariance matrix is ​​Q. m ; The noise in the random walk process is represented by Gaussian white noise, and its covariance matrix is ​​Q. b Assume that the noise during the random walk is uncorrelated with the measurement noise; ε k Given an Euler angle vector, the unit quaternion q representing the actual attitude value of the navigation platform can be obtained according to equation (3):

[0021]

[0022] Step 5.2: Establish the system observation equations as follows:

[0023]

[0024] in, These are virtual measurements of velocity in the x and y directions, determined based on nonholonomic constraints. and Let these represent the measurement noise from the GPS receiver for position and velocity, respectively. Assume they are uncorrelated white Gaussian noise, and their covariance matrices are... and The noise used for vehicle speed measurement on the CAN bus is assumed to be Gaussian white noise, and its covariance matrix is ​​given by... It is the rotation matrix from the navigation platform coordinate system to the vehicle body coordinate system, which can be obtained through calibration. It is the rotation matrix from the local horizontal coordinate system to the navigation platform coordinate system. This means converting the navigation platform speed in the local horizontal coordinate system to the vehicle's center of gravity speed in the vehicle coordinate system.

[0025] A further technical solution is that, in step 5.2, the nonholonomic constraint assumes that the vehicle will not sideslip during planar motion and has no motion in the direction perpendicular to the road surface, that is, the velocity of the vehicle's center of mass along the y-axis and z-axis in the vehicle coordinate system is zero. However, in reality, due to sideslip during turning, road bumps, etc., the velocities in these two directions are not completely zero. Therefore, these constraints are relaxed according to equation (5):

[0026]

[0027] Among them, e nhlolo It is Gaussian white noise, and the covariance matrix is ​​R. nhlolo This is used to reflect the degree of relaxation of the corresponding constraints.

[0028] A further technical solution, in step 6, is as follows: The trajectory estimation model based on vehicle dynamics is as follows:

[0029]

[0030] Where X and Y represent the vehicle's coordinates in the local horizontal coordinate system, v x v y This represents the velocity in the vehicle coordinate system, where ψ represents the vehicle's yaw rate.

[0031] A further technical solution involves the following steps in step 6: Building the state-space model of the extended Kalman filter.

[0032] Step 6.1: Establish the nonlinear state equations of the system as follows:

[0033] x k =f(x) k-1 ,u k )+w k ,cov(w k )=Q k (7)

[0034] x k =f(x) k-1 ,u k Specifically, the equations are as follows:

[0035]

[0036]

[0037]

[0038]

[0039]

[0040] ψ k =ψ k-1 +T s r k-1

[0041] Among them, v x v y These are the components of the vehicle speed along the x and y axes of the vehicle coordinate system, respectively; r is the yaw rate of the vehicle; δ is the front wheel steering angle; and l... f l r These are the distances from the vehicle's center of gravity to the front and rear axles, respectively. αf C αr The lateral stiffness of a single front tire and a single rear tire, respectively;

[0042] Step 6.2: Establish the system observation equations as follows:

[0043]

[0044] Among them, v rL and v rR d represents the speed of the center of the left and right rear wheels respectively. r This represents the rear wheel track. The relationship between the rear wheel speed and the vehicle's longitudinal and yaw rates is used to establish the observation equations, incorporating measurements from the ABS wheel speed sensors.

[0045] A further technical solution is that, in step 6, when the GPS signal is interrupted, the INS positioning information will generate a large error. When establishing the observation equation, the navigation position information in the third and fourth rows of the observation values ​​is excluded, so that the result of the trajectory calculation model is not easily affected by the large error of INS, thereby improving the positioning accuracy.

[0046] The present invention provides a fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints. By considering both vehicle dynamics and nonholonomic constraints, and fusing GNSS, INS, DR and UWB for positioning, the accuracy of navigation and positioning can be improved when GNSS signals are interfered with or interrupted. Especially in scenarios where GNSS signals are completely blocked (such as underground parking lots), the positioning accuracy can be effectively improved and the positioning time can be extended. Attached Figure Description

[0047] Figure 1 A flowchart illustrating a fusion localization method that takes into account both vehicle dynamics and nonholonomic constraints, provided for an embodiment of the present invention;

[0048] Figure 2 This is a structural block diagram of a fusion positioning method that takes into account both vehicle dynamics and non-holonomic constraints, provided in an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0050] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0051] One embodiment of the present invention provides a fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints. Specifically, it is a GNSS / INS / DR / UWB fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints, comprising the following steps:

[0052] Step 1: Read the information provided by the GPS receiver and IMU sensor (including position, speed and attitude information provided by the GPS receiver, and acceleration and angular velocity information provided by the IMU), vehicle speed, wheel speed and front wheel steering angle information provided by the vehicle CAN bus, and position information provided by the UWB device pre-installed indoors (such as underground parking lots).

[0053] Step 2: Preprocess the read information and establish the corresponding coordinate system;

[0054] Step 3: Determine whether the GNSS signal is available based on the HDOP value. If it is not available, read the location information provided by UWB. Specifically, if the HDOP is less than the threshold, the location is estimated by relying on the GPS receiver signal; if the HDOP is greater than the threshold, the location is estimated by relying on the UWB signal.

[0055] Step 4: Based on the information provided by the GPS receiver (or UWB) and IMU, calculate the position, velocity, and attitude information using the INS kinematic model. The INS kinematic model is as follows:

[0056]

[0057] Where, p k v k ε k The position, velocity, and attitude (expressed as unit quaternions) provided by the GPS receiver (or UWB) are represented in s. k ω k This represents the three-dimensional scale and angular velocity information provided by the inertial measurement unit (IMU). It is the rotation matrix from the body coordinate system to the navigation platform coordinate system.

[0058] Step 5: Establish the state equation and observation equation, and use Error State Kalman Filter (ESKF) to estimate the navigation error and IMU sensor bias. The obtained navigation error estimates and IMU sensor bias estimates are used to correct the INS kinematic model.

[0059] The system state equations are as follows:

[0060]

[0061] Wherein, δp k δv k ε k δs represents the error between the actual and nominal values ​​of the navigation platform's position, velocity, and attitude, respectively; k ,δω k These represent the proportional and angular velocity deviations measured by the slowly changing IMU, respectively. The measurement noise is represented as Gaussian white noise, and its covariance matrix is ​​Q. m ; The noise in the random walk process is represented by Gaussian white noise, and its covariance matrix is ​​Q. b Assume that the noise during the random walk is uncorrelated with the measurement noise. ε k Given an Euler angle vector, the unit quaternion q representing the actual attitude value of the navigation platform can be obtained according to equation (3):

[0062]

[0063] The system observation equations are as follows:

[0064]

[0065] in, These are virtual measurements of velocity in the x and y directions, determined based on nonholonomic constraints. and e GPSr Let these represent the measurement noise from the GPS receiver for position and velocity, respectively. Assume they are uncorrelated white Gaussian noise, and their covariance matrices are... and The noise used for vehicle speed measurement on the CAN bus is assumed to be Gaussian white noise, and its covariance matrix is ​​given by... It is the rotation matrix from the navigation platform coordinate system to the vehicle body coordinate system, which can be obtained through calibration. It is the rotation matrix from the local horizontal coordinate system to the navigation platform coordinate system. This means converting the navigation platform speed in the local horizontal coordinate system to the vehicle's center of gravity speed in the vehicle coordinate system.

[0066] The nonholonomic constraint assumes that the vehicle will not sideslip during planar motion and will not move in the direction perpendicular to the road surface, i.e., the velocity at the vehicle's center of mass along the y-axis and z-axis in the vehicle coordinate system is zero. However, in reality, due to sideslip during turning, road bumps, etc., the velocities in these two directions are not completely zero. Therefore, according to equation (5), these constraints are relaxed:

[0067]

[0068] Among them, e nhlolo It is Gaussian white noise, and the covariance matrix is ​​R. nhlolo This is used to reflect the degree of relaxation of the corresponding constraints.

[0069] Using error Kalman filtering, navigation error estimates and IMU sensor bias estimates are obtained. These are then used to correct the navigation information obtained from the INS kinematic model, thus completing the estimation of navigation information. At the beginning of the next sampling period, the error value is reset to zero.

[0070] Step 6: Establish a trajectory estimation model (DR) based on vehicle dynamics, establish state equations and observation equations, and use extended Kalman filter (EKF) to estimate position and heading angle information.

[0071] The nonlinear state equations of the system are as follows:

[0072] x k =f(x) k-1 ,u k )+w k ,cov(w k )=Q k (7)

[0073] x k =f(x) k-1 ,u k Specifically, the equations are as follows:

[0074]

[0075]

[0076]

[0077]

[0078]

[0079] ψ k =ψ k-1 +T s r k-1

[0080] Among them, v x v y These are the components of the vehicle speed along the x and y axes of the vehicle coordinate system, respectively; r is the yaw rate of the vehicle; δ is the front wheel steering angle; and l... f l r These are the distances from the vehicle's center of gravity to the front and rear axles, respectively. αf C αr These are the lateral stiffness of a single front tire and a single rear tire, respectively.

[0081] The system observation equations are as follows:

[0082]

[0083] Among them, vrL and v rR d represents the speed of the center of the left and right rear wheels respectively. r This represents the rear wheel track. The relationship between the rear wheel speed and the vehicle's longitudinal and yaw rates is used to establish the observation equations, incorporating measurements from the ABS wheel speed sensors.

[0084] In step 6, the trajectory estimation model based on vehicle dynamics is as follows:

[0085]

[0086] Where X and Y represent the vehicle's coordinates in the local horizontal coordinate system, v x v y This represents the velocity in the vehicle coordinate system, where ψ represents the vehicle's yaw rate.

[0087] Step S7: Output the final estimated position and heading angle information.

[0088] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A fusion positioning method that takes into account both vehicle dynamics and nonholonomic constraints, characterized in that, The method comprises the following steps: Step 1, reading the position, speed and attitude information provided by the GPS receiver, the acceleration and angular velocity information provided by the IMU sensor, the vehicle speed, wheel speed and front wheel angle information provided by the vehicle CAN bus, and the position information provided by the indoor pre-installed UWB device; Step 2, preprocessing the read information and establishing the corresponding coordinate system; Step 3, judging the GPS receiver signal according to the horizontal geometric precision factor, i.e. HDOP value, and if it is not available, reading the position information provided by the UWB; Step 4, according to the information provided by the IMU sensor and the GPS receiver, or the IMU sensor and the UWB, the position, speed and attitude information is calculated through the INS kinematic model; Step 5, establishing the state equation and the observation equation, and using the error state Kalman filter to estimate the navigation error value and the IMU sensor deviation value, and the obtained navigation error estimation value and IMU sensor deviation estimation value are used to correct the INS kinematic model; Step 6, establishing a dead reckoning model based on vehicle dynamics, establishing the state equation and the observation equation, and using the extended Kalman filter to estimate the position and heading angle information; Step 7, outputting the final estimated position information and heading angle information; Non-holonomic constraints assume that the vehicle does not slip sideways during planar motion and has no motion in the direction normal to the road, i.e. the velocity of the vehicle's center of mass along the z-axis of the body coordinate system is zero The vehicle's center of mass moves along the x-axis of the body coordinate system at a constant speed v, and the vehicle's center of mass moves along the y-axis of the body coordinate system at a constant speed w. The vehicle's center of mass moves along the x-axis of the body coordinate system at a constant speed v, and the vehicle's center of mass moves along the y-axis of the body coordinate system at a constant speed w. The actual situation is that the velocities in these two directions are not completely zero due to the presence of side slip when turning and road bumps, so the constraints are relaxed according to equation (5): wherein is a Gaussian white noise with a covariance matrix to reflect the degree of relaxation of the corresponding constraint.

2. The fused localization method that takes into account vehicle dynamics and nonholonomic constraints of claim 1, wherein, In the step 3, when the HDOP is less than the threshold value, the position estimation is performed depending on the GPS receiver signal; when the HDOP is greater than the threshold value, the position estimation is performed depending on the UWB signal.

3. The fused localization method that takes into account vehicle dynamics and nonholonomic constraints of claim 1, wherein, In the step 6, the dead reckoning model based on vehicle dynamics is as follows: wherein , denotes the coordinate of the vehicle in the local horizontal coordinate system, , denotes the representation of its velocity in the body coordinate system, denotes the yaw angular velocity of the vehicle.

4. The fused localization method that takes into account vehicle dynamics and nonholonomic constraints of claim 3, wherein, In the step 6, the establishment of the state space model of the extended Kalman filter comprises the following steps: Step 6.1, establishing the system nonlinear state equation as follows: Specifically, the following equation: wherein, , are components of the vehicle speed along the body coordinate system axes and axes directions, respectively, is the yaw rate of the vehicle, is the front wheel steering angle, , are distances from the center of mass of the vehicle to the front and rear axles, respectively, , are cornering stiffnesses of the single front wheel tire and the single rear wheel tire, respectively; Step 6.2, establishing the system observation equation as follows: where, and Vr and Vl represent the velocities of the right and left rear wheel centers, respectively, represents the rear wheel track; the measured value of the ABS wheel speed sensor is also used for the establishment of the observation equation by using the relationship between the rear wheel speed and the longitudinal velocity and the yaw angular velocity of the vehicle.

Images