An intelligent vehicle cooperative positioning method combining ranging, direction finding and speed measurement

Abstract

The application provides a kind of intelligent vehicle cooperative positioning method of combination of ranging, direction finding, velocity measurement, comprising: measuring the linear motion information and angular motion information of vehicle based on inertial measurement unit, obtaining the nominal vehicle pose recurrence equation of discrete time;Based on the error state variable measured by inertial measurement unit, the recurrence equation under the discrete time of error state is established, and the prior estimation of vehicle error state is obtained;Real-time ranging, direction finding and velocity measurement are carried out on the vehicle by roadside unit, the observation equation is obtained, the state of vehicle is estimated based on Kalman filtering equation, and the posterior estimation of vehicle error state is obtained;The posterior estimation of error state is injected into the nominal vehicle pose recurrence equation, and the vehicle positioning estimation is obtained.The cumulative positioning error of vehicle can be corrected by roadside unit in the area where GNSS signal is not good, and the positioning accuracy of vehicle is improved.

Description

Technical Field

[0001] This invention relates to the field of intelligent transportation technology, specifically to an intelligent vehicle cooperative positioning method that combines distance measurement, direction measurement, and speed measurement. Background Technology

[0002] Intelligent vehicles are considered to have enormous potential in improving road safety and traffic efficiency, and have received widespread attention and research in recent years. Currently, unmanned vehicles using RTK-GNSS and INS integrated navigation and positioning can achieve centimeter-level positioning that meets driving requirements under ideal conditions. However, in urban environments, when tall buildings block GNSS signals, the positioning accuracy of the integrated navigation deteriorates, becoming insufficient to guarantee safe vehicle operation.

[0003] Therefore, how to provide a collaborative positioning method for intelligent vehicles that combines distance measurement, direction measurement, and speed measurement based on roadside unit assistance to improve vehicle positioning accuracy is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0004] In view of this, the present invention proposes an intelligent vehicle cooperative positioning method that combines distance measurement, direction measurement, and speed measurement, which is not limited by road environment and ensures safe driving of vehicles.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A collaborative positioning method for intelligent vehicles that combines ranging, direction finding, and speed measurement includes the following steps:

[0007] Based on the linear and angular motion information of the vehicle measured by the inertial measurement unit, the discrete-time recursive equation of the vehicle's nominal pose is obtained.

[0008] Based on the error state variables measured by the inertial measurement unit, a recursive equation for the discrete-time error state is established to obtain a priori estimate of the vehicle error state.

[0009] Real-time distance, direction and speed measurement of vehicles is performed using roadside units to obtain observation equations. Based on the Kalman filter equation, the state of the vehicle is estimated to obtain the posterior estimate of the vehicle error state.

[0010] The posterior estimate of the error state is injected into the vehicle nominal pose recursive equation to obtain the vehicle positioning estimate.

[0011] Preferably, an IMU is used to measure the vehicle's linear motion information and angular motion information, wherein the linear motion information includes acceleration measurements and their biases, and the angular motion information includes angular velocity measurements and their biases.

[0012] Based on the vehicle's initial pose and the pose recursive equation, the vehicle's pose is derived. When the acceleration and angular velocity measurements have a fixed bias, the discrete-time nominal vehicle pose recursive equation is obtained as follows:

[0013]

[0014] v k+1 =v k +[R k (a mk -a bk )+g k ]Δt

[0015]

[0016] a b(k+1) =a bk

[0017] ω b(k+1) =ω bk

[0018] In the formula, the subscripts k and k+1 represent two adjacent moments, with a corresponding time interval Δt; p and v represent the vehicle's position and velocity in the navigation coordinate system, respectively; R represents the rotation matrix from the body coordinate system to the navigation coordinate system; a m a b These represent the measured acceleration value and its offset, respectively; g represents the gravity at the vehicle's location; q represents the rotation quaternion from the body coordinate system to the navigation coordinate system; q k {(ω mk -ω bk )Δt} represents the axis-angle vector (ω) mk -ω bk The quaternion corresponding to Δt; ω m ω b These represent the angular rate measurement value and its bias, respectively.

[0019] Preferably, the error state variable to be estimated is:

[0020] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0021] In the formula, δp, δv, and δθ represent the vehicle's position error, velocity error, and attitude error, respectively; δa b and δω bThese represent the errors in acceleration bias estimation and angular velocity bias estimation, respectively.

[0022] The recursive equation for the discrete-time error state is established as follows:

[0023] δp k+1 =δp k +δv k Δt

[0024]

[0025]

[0026]

[0027]

[0028] In the formula, the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval being Δt; R, R T Represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose, respectively; a m a b These represent the measured acceleration value and its offset, respectively; δθ represents the attitude error in the vehicle body coordinate system. ω represents the disturbance vector for velocity error estimation. m ω b These represent the measured angular rate value and its bias, respectively. The disturbance pulse vector represents the attitude error estimation. These represent the perturbation pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively.

[0029] Preferably, a priori estimates of the vehicle error state are obtained based on the recursive equation for the discrete-time error state. and its corresponding covariance

[0030]

[0031]

[0032]

[0033]

[0034]

[0035] In the formula, I is a 3×3 identity matrix; 0 is a 3×3 zero matrix; Q k This represents the covariance matrix of the disturbance impulse vector n. These represent velocity random walk and angle random walk, respectively. These represent the power spectral density of the dynamic zero bias of the accelerometer and gyroscope, respectively.

[0036] Preferably, the observation equation is:

[0037]

[0038]

[0039] In the formula, Z is the observation matrix; These are the actual distance measurement, angle measurement, and speed measurement of the vehicle by the roadside unit; These represent the calculated distance, calculated angle, and calculated speed between the vehicle and the roadside unit, respectively; w represents the observation noise of the roadside unit.

[0040] Posterior estimation of vehicle error state and its corresponding covariance for:

[0041]

[0042]

[0043]

[0044]

[0045] In the formula, I is a 3×3 identity matrix; These are the variances of the probability distributions that the distance measurement error, direction measurement error, and speed measurement error between the roadside unit and the vehicle follow.

[0046] Preferably, the posterior estimate of the error state is injected into the vehicle nominal pose recursive equation to obtain the vehicle positioning estimate as follows:

[0047]

[0048]

[0049]

[0050] In the formula, p k+1 v k+1 These are the vehicle's nominal position estimation and nominal speed estimation, respectively; These are the posterior estimates of vehicle position error and velocity error, respectively; q k+1 The quaternion corresponding to the vehicle's nominal attitude; Operator represents the posterior estimate of attitude error The corresponding quaternion.

[0051] Preferably, after obtaining the vehicle positioning estimate at the current moment, the error state is reset for estimation in the next filtering cycle:

[0052]

[0053] Each time the roadside unit receives distance, direction, and speed information about a vehicle, a corresponding filtering cycle is activated.

[0054] As can be seen from the above technical solution, compared with the prior art, the beneficial effects of the present invention include:

[0055] This invention enables vehicle pose recursive calculation based on inertial measurement unit (IMU) measurements. Simultaneously, in areas with poor GNSS signal, roadside units assist in communication with the vehicle, as well as ranging, direction finding, and speed measurement. An error-state Kalman filter algorithm is used to filter the vehicle pose, correcting the accumulated positioning error of the IMU and improving the vehicle's positioning accuracy, thus ensuring safe driving. Attached Figure Description

[0056] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0057] Figure 1 A flowchart of a collaborative positioning method for intelligent vehicles that combines ranging, direction finding, and speed measurement, provided in an embodiment of the present invention;

[0058] Figure 2 This is a schematic diagram illustrating the state of a roadside unit performing distance, direction, and speed measurements on a vehicle, as provided in an embodiment of the present invention. Detailed Implementation

[0059] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0060] See appendix Figure 1 This invention provides a cooperative positioning method for intelligent vehicles that combines ranging, direction finding, and speed measurement, implemented according to the following process:

[0061] Based on the linear and angular motion information of the vehicle measured by the inertial measurement unit, the discrete-time recursive equation of the vehicle's nominal pose is obtained.

[0062] Based on the error state variables measured by the inertial measurement unit, a recursive equation for the discrete-time error state is established to obtain a priori estimate of the vehicle error state.

[0063] Real-time distance, direction and speed measurement of vehicles is performed using roadside units to obtain observation equations. Based on the Kalman filter equation, the state of the vehicle is estimated to obtain the posterior estimate of the vehicle error state.

[0064] The posterior estimate of the error state is injected into the vehicle nominal pose recursive equation to obtain the vehicle positioning estimate.

[0065] In one embodiment, the inertial navigation-based dead reckoning positioning step specifically includes:

[0066] An inertial measurement unit (IMU) is installed on the vehicle. The IMU measures the vehicle's linear and angular motion information. Based on the vehicle's initial pose and pose recursion equations, the vehicle's pose is deduced. Ignoring IMU measurement noise, and assuming that the accelerometer and gyroscope measurements have only one fixed bias, the vehicle's state at this point is called the nominal state. The discrete-time nominal pose recursion equation is:

[0067]

[0068] v k+1 =v k +[R k (a mk -a bk )+g k ]Δt

[0069]

[0070] a b(k+1) =a bk

[0071] ω b(k+1) =ω bk

[0072] In the formula, the subscripts k and k+1 represent two adjacent moments, with a corresponding time interval Δt; p and v represent the vehicle's position and velocity in the navigation coordinate system, respectively; R represents the rotation matrix from the body coordinate system to the navigation coordinate system; a m a b These represent the measured acceleration value and its offset, i.e., linear motion information; g represents the gravity at the vehicle's location; q represents the rotation quaternion from the body coordinate system to the navigation coordinate system; q k {(ω mk-ω bk )Δt} represents the axis-angle vector (ω) mk -ω bk The quaternion corresponding to Δt; ω m ω b These represent the angular rate measurement value and its bias, i.e., angular motion information.

[0073] In this embodiment, during the actual movement of the vehicle, due to measurement noise, the acceleration measurement bias 'a' is... b and angular rate measurement bias ω b It is time-varying and needs to be estimated in conjunction with observations from other sensors; otherwise, cumulative errors will occur. The error state variable to be estimated is selected as follows:

[0074] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0075] In the formula, δp, δv and δθ represent the vehicle's position error, velocity error and attitude error, respectively; and These represent the errors in acceleration bias estimation and angular velocity bias estimation, respectively.

[0076] The recursive equation for the discrete-time error state is established as follows:

[0077] δp k+1 =δp k +δv k Δt

[0078]

[0079]

[0080]

[0081]

[0082] In the formula, the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval being Δt; R, R T Represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose, respectively; a m a b These represent the measured acceleration value and its offset, respectively; δθ represents the attitude error in the vehicle body coordinate system. ω represents the disturbance vector for velocity error estimation. mω b These represent the measured angular rate value and its bias, respectively. The disturbance pulse vector represents the attitude error estimation. These represent the perturbation pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively.

[0083] In one embodiment, vehicle cooperative positioning using roadside units that combine ranging, direction finding, and speed measurement specifically includes:

[0084] The vehicle can communicate with the roadside unit and perform distance, direction and speed measurements to correct the vehicle's cumulative positioning error.

[0085] The roadside unit performs distance, direction, and speed measurements on vehicles and estimates the vehicle's state using the Kalman filter equation.

[0086] In this embodiment, a priori estimate of the vehicle error state is obtained based on the established recursive equation for the discrete-time error state. and its corresponding covariance

[0087]

[0088]

[0089]

[0090]

[0091]

[0092] In the formula, I is a 3×3 identity matrix; 0 is a 3×3 zero matrix; Q k This represents the covariance matrix of the disturbance impulse vector n. These represent velocity random walk and angle random walk, respectively. These represent the power spectral density of the dynamic zero bias of the accelerometer and gyroscope, respectively.

[0093] In this embodiment, the distance, direction, and speed measurements between the roadside unit and the vehicle are used as observations, and the observation equation is:

[0094]

[0095]

[0096] In the formula, Z is the observation matrix; These are the actual distance measurement, angle measurement, and speed measurement of the vehicle by the roadside unit; These represent the calculated distance, calculated angle, and calculated speed between the vehicle and the roadside unit, respectively; w represents the observation noise of the roadside unit.

[0097] According to the Kalman filter equation, the posterior estimate of the vehicle error state and its corresponding covariance for:

[0098]

[0099]

[0100]

[0101]

[0102] In the formula, I is a 3×3 identity matrix; These are the variances of the probability distributions that the distance measurement error, direction measurement error, and speed measurement error between the roadside unit and the vehicle follow.

[0103] In this embodiment, the filtered error state estimate is injected into the nominal state estimate to obtain a more accurate vehicle positioning estimate:

[0104]

[0105]

[0106]

[0107] In the formula, p k+1 v k+1 These are the vehicle's nominal position estimation and nominal speed estimation, respectively; These are the posterior estimates of vehicle position error and velocity error, respectively; q k+1 The quaternion corresponding to the vehicle's nominal attitude; Operator represents the posterior estimate of attitude error The corresponding quaternion.

[0108] After error state injection, the state is reset for estimation in the next filtering cycle.

[0109] Each time the distance, direction, and speed information of the vehicle from the roadside unit is received, a corresponding filtering cycle is activated. That is, when the distance, direction, and speed information of the vehicle from the roadside unit is received again, the error state of cycle k+1 is reset, and the vehicle positioning estimate of cycle k+2 is calculated.

[0110] After repeated calculations over multiple cycles, the real-time positioning results of the vehicle in discrete time are obtained.

[0111] The above provides a detailed description of the intelligent vehicle cooperative positioning method combining ranging, direction finding, and speed measurement provided by the present invention. Specific examples are used in this embodiment to illustrate the principle and implementation of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation and application scope based on the idea of ​​the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

[0112] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined in these embodiments may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A cooperative positioning method for intelligent vehicles that combines ranging, direction finding, and speed measurement, characterized in that, Includes the following steps: The linear and angular motion information of the vehicle is measured using an inertial measurement unit (IMU). The linear motion information includes acceleration measurements and their biases, and the angular motion information includes angular velocity measurements and their biases. The vehicle's pose is recursively derived from its initial pose and pose recursive equations. When the acceleration and angular velocity measurements have fixed biases, the vehicle's state is called its nominal state. The discrete-time nominal pose recursive equations for the vehicle are then obtained. v k+1 =v k +[R k (a mk -a bk )+g k ]Δt a b(k+1) =a bk oh b(k+1) =ω bk In the formula, the subscripts k and k+1 represent two adjacent moments, with a corresponding time interval Δt; p and v represent the vehicle's position and velocity in the navigation coordinate system, respectively; R represents the rotation matrix from the body coordinate system to the navigation coordinate system; a m a b These represent the measured acceleration value and its offset, respectively; g represents the gravity at the vehicle's location; q represents the rotation quaternion from the body coordinate system to the navigation coordinate system; q k {(ω mk -ω bk )Δt} represents the axis-angle vector (ω) mk -ω bk The quaternion corresponding to Δt; ω m ω b These represent the measured angular rate value and its bias, respectively. Based on the error state variables measured by the inertial measurement unit, a recursive equation for the discrete-time error state is established to obtain a priori estimate of the vehicle error state, specifically including: The error state variable to be estimated is: δX=[(δp) T (dv) T (sth) T (da) b ) T (see b ) T ] T In the formula, δp, δv, and δθ represent the vehicle's position error, velocity error, and attitude error, respectively; δa b and δω b These represent the errors in acceleration bias estimation and angular velocity bias estimation, respectively. The recursive equation for the discrete-time error state is established as follows: δp k+1 =δp k +δv k Δt In the formula, the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval being Δt; R, R T Represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose, respectively; a m a b These represent the measured acceleration value and its offset, respectively; δθ represents the attitude error in the vehicle body coordinate system. ω represents the disturbance vector for velocity error estimation. m ω b These represent the measured angular rate value and its bias, respectively. The disturbance pulse vector represents the attitude error estimation. These represent the disturbance pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively; Based on the recursive equation for the discrete-time error state, a priori estimate of the vehicle error state is obtained. and its corresponding covariance In the formula, I is a 3×3 identity matrix; 0 is a 3×3 zero matrix; Q k This represents the covariance matrix of the disturbance impulse vector n. These represent velocity random walk and angle random walk, respectively. These represent the power spectral densities of the accelerometer and gyroscope, respectively, representing the dynamic zero bias. Real-time distance, direction, and speed measurements of vehicles are performed using roadside units to obtain observation equations. Based on the Kalman filter equations, the vehicle state is estimated to obtain a posterior estimate of the vehicle's error state, specifically including: The observation equation is: In the formula, Z is the observation matrix; These are the actual distance measurement, angle measurement, and speed measurement of the vehicle by the roadside unit; These represent the calculated distance, calculated angle, and calculated speed between the vehicle and the roadside unit, respectively; w represents the observation noise of the roadside unit. Posterior estimation of vehicle error state and its corresponding covariance for: In the formula, I is a 3×3 identity matrix; These are the variances of the probability distributions that the distance measurement error, direction measurement error, and speed measurement error between the roadside unit and the vehicle follow; The posterior estimate of the error state is injected into the vehicle nominal pose recursive equation to obtain the vehicle positioning estimate.

2. The intelligent vehicle cooperative positioning method combining ranging, direction finding, and speed measurement according to claim 1, characterized in that, The posterior estimate of the error state is injected into the vehicle nominal pose recursive equation to obtain the vehicle positioning estimate as follows: In the formula, p k+1 v k+1 These are the vehicle's nominal position estimation and nominal speed estimation, respectively; These are the posterior estimates of vehicle position error and velocity error, respectively; q k+1 The quaternion corresponding to the vehicle's nominal attitude; Operator represents the posterior estimate of attitude error The corresponding quaternion.

3. The intelligent vehicle cooperative positioning method combining ranging, direction finding, and speed measurement according to claim 1, characterized in that, After obtaining the vehicle positioning estimate at the current moment, the error state is reset for estimation in the next filtering cycle: Each time the roadside unit receives distance, direction, and speed information about a vehicle, a corresponding filtering cycle is activated.

Images