Multi-sensor fusion positioning method suitable for multi-axle steering vehicle

Abstract

The invention relates to a multi-sensor fusion positioning method suitable for a multi-axle steering vehicle, and the method comprises the following steps: S1, installing a plurality of sensors on a multi-axle all-wheel steering vehicle platform, collecting IMU data, a laser radar real-time scanning point cloud, a GNSS signal, the reading of each wheel angle sensor, and the reading of each wheel speed meter, and carrying out the real-time scanning of the IMU data, the laser radar real-time scanning point cloud, the GNSS signal, the reading of each wheel angle sensor, and the reading of each wheel speed meter; and the timestamps of all the collected signals are aligned. And S2, establishing a kinematic model of a to-be-estimated state, and performing distortion removal on the laser radar point cloud data according to the IMU data to realize motion compensation of the point cloud. And S3, establishing an error state discrete model, a laser radar observation model, a multi-axis steering vehicle kinematics observation model and a GNSS observation model. And S4, considering the influence caused by multi-wheel non-Ackerman steering, and providing a method for adaptively adjusting the observation noise covariance of the multi-axle steering vehicle so as to ensure the correct weighting of chassis observation data. And S5, fusing IMU, GNSS, laser radar, and rotation angle and rotation speed observation information of each wheel of the multi-axis steering vehicle, constructing a sequential iterative error state Kalman filter, and obtaining an optimal positioning estimation result through iterative calculation.

Description

Technical Field

[0001] The invention relates to the technical field of multi-sensor fusion positioning, and in particular to a multi-sensor fusion positioning method suitable for a multi-axis steering vehicle. Background Art

[0002] Multi-axis steering vehicles are increasingly used in large-scale engineering applications. They can effectively carry 100-meter-long wind turbine blades, long-distance transport of super-heavy bridge structures / chemical equipment, etc. They are core engineering equipment to ensure the complete transportation of "indivisible" large items and meet various special operation requirements. In the process of realizing autonomous operation of intelligent multi-axis steering vehicles, the prerequisite is positioning and navigation. Only after the intelligent multi-axis steering vehicle accurately estimates its own posture and establishes the external environment can it automatically complete subsequent planning, decision-making, control and other tasks. However, engineering vehicles often face environments with harsh positioning conditions, such as tunnels, mines, etc., where there are obstructions and the environment has a single structure, GNSS and Lidar will degrade, which brings certain challenges to the high-precision positioning of multi-axis steering vehicles. Therefore, an innovative multi-sensor fusion algorithm is needed to enable multi-axis steering vehicles to ensure a certain accuracy under harsh positioning conditions. At present, there are few studies on multi-sensor fusion positioning algorithms for multi-axis steering vehicles, and there are more studies on positioning algorithms on ordinary two-axle vehicles. There are existing methods for vehicle positioning with strong environmental adaptability. For example, reference patent CN202311068502 proposes a multi-sensor fusion positioning and mapping method based on graph optimization, which adds a wheel speed odometer on the basis of lidar, IMU, and GPS, and achieves good positioning accuracy through nonlinear optimization methods. Another example is a ground-constrained multi-sensor fusion positioning and mapping method proposed in patent CN202410108411, which constructs an IMU and encoder measurement model. The back-end module optimizes the factor graph by constructing a lidar-IMU odometer factor, a ground plane factor, an IMU and encoder pre-integration factor, and a closed-loop factor to obtain a high-precision pose. Although these positioning methods have good positioning accuracy and environmental adaptability on two-axis vehicles, there are still some shortcomings and limitations in their direct application to multi-axis vehicles, which are mainly manifested as follows:

[0003] (1) Traditional two-axle vehicles usually introduce wheel speed meters as observation quantities to enhance positioning robustness in harsh positioning environments. They generally use measurement models based on vehicle dynamics and vehicle kinematics. However, in actual driving, the dynamic model of multi-axle steering vehicles is complex, and the observation accuracy depends largely on the nonlinear tire model, road friction sparseness, and the accuracy of vehicle parameters. The acquisition cost of these parameters is high and very difficult, requiring complex calibration tests. At the same time, the body structure of multi-axle steering vehicles is significantly different from that of ordinary vehicles. They have unique kinematic models and a very flexible angle distribution method. The kinematic model of traditional two-axle vehicles is no longer applicable.

[0004] (2) The kinematics of traditional two-axle vehicles generally adopts a bicycle model, in which the steering center is located at the intersection of the front wheel normal and the rear wheel normal. However, since multi-axle steering vehicles have multiple wheels, the intersection of the normals of the wheels usually cannot converge to a point during the steering process. Therefore, the steering center cannot be determined. In addition, the wheels will slip due to failure to satisfy the Acker steering theorem, which reduces the credibility of the kinematic model and affects the positioning accuracy. Summary of the invention

[0005] In view of this, an object of the present invention is to provide a multi-sensor fusion positioning method suitable for multi-axis steering vehicles.

[0006] To achieve the above object, the present invention adopts the following technical solution: a multi-sensor fusion positioning method suitable for a multi-axis steering vehicle, comprising the following steps:

[0007] Step S1: installing a variety of sensors on the multi-axis all-wheel steering vehicle platform to collect IMU data, laser radar real-time scanning point cloud, GNSS signals, each wheel angle sensor reading and each wheel speed meter reading, and aligning the timestamps of all collected signals;

[0008] Step S2: Establish a kinematic model of the state to be estimated, and dedistort the lidar point cloud data according to the IMU data to achieve motion compensation of the point cloud;

[0009] Step S3: establishing an error state discrete model, a lidar observation model, a multi-axis steering vehicle kinematic observation model, and a GNSS observation model;

[0010] Step S4: Considering the impact of multi-wheel non-Ackerman steering, a method for adaptively adjusting the observation noise covariance of multi-axis steering vehicles is proposed to ensure the correct weighting of chassis observation data;

[0011] Step S5: Integrate the IMU, GNSS, lidar, and the wheel angle and speed observation information of the multi-axis steering vehicle to construct a sequential iterative error state Kalman filter, and obtain the optimal positioning estimation result through iterative calculation.

[0012] In a preferred embodiment, the implementation method of step S1 is:

[0013] On the multi-axis steering vehicle platform, the communication subscription modules of IMU, GNSS, LiDAR and multi-axis all-wheel steering chassis are established based on the ROS robot operating system, which are used to collect the acceleration signal of IMU. i With the angular velocity signal ω i , GNSS latitude, longitude, and altitude informationλ i ,φ i ,hi , the point cloud information of the laser radar {o1,o2,…,o m} i , where o i is the coordinate information of the point in the laser radar coordinate system, the signal of the multi-axis all-wheel steering chassis {n1, n2, ... n m ,α1,α2,…α m} i , where n m is the speed information of the mth tire, α m is the turning angle information of the mth tire;

[0014] After collecting data within a certain period of time, the end time of each point cloud frame scan of the lidar is used as the timestamp alignment point, and the speed and angle signals of each wheel of the IMU, GNSS, and multi-axis all-wheel steering vehicle chassis are estimated by linear interpolation method. The measurement value at that time point.

[0015] In a preferred embodiment, the implementation method of step S2 is:

[0016] The state variable x to be estimated is expressed as:

[0017]

[0018] in G · I Represents the representation of the IMU coordinate system I in the world coordinate system G; the origin of the world coordinate system G is the position of the IMU in the first frame, the x-axis points to the east, the y-axis points to the north, and the z-axis satisfies the right-hand coordinate system rule; φ is represented as a three-dimensional rotation matrix, p represents the three-dimensional position vector, v represents the three-dimensional velocity vector, and b ω represents the gyroscope's three-dimensional angular velocity zero bias vector, b a represents the three-dimensional acceleration zero bias vector of the accelerometer, and g represents the gravity acceleration vector;

[0019] Considering IMU output Accelerometer noise n a , gyroscope noise n ω , Accelerometer bias noise and gyro bias noise n bω The noise influence is constructed to construct the noise matrix And build the state model:

[0020]

[0021] in· i Indicates time i, Represents the mapping of vector to antisymmetric matrix. Acceleration, gyroscope, acceleration bias and gyroscope bias noise are modeled as Gaussian white noise. Then, the forward propagation equation is established according to the forward kinematics method:

[0022]

[0023] in Indicates the measured value, represents the measurement state at the i-th moment, Δt I Represents the time difference between two adjacent frames of IMU data; the state at the end of the lidar scan is roughly estimated by the forward propagation method, and then the motion compensation of each point is estimated by backward propagation:

[0024]

[0025] Since the acquisition frequency of points is greater than the acquisition frequency of IMU, there are usually multiple point measurements between each IMU frame, so the state of the IMU frame at the last moment is taken as the standard; the obtained point pose relative to the end time of the scan is defined as in Represents the relative rotation transformation matrix between the IMU coordinate system at time k and the IMU coordinate system at time j, Represents the relative position transformation matrix between the IMU coordinate system at time k and the IMU coordinate system at time j. Projection to scan end time:

[0026]

[0027] In the formula Represents the coordinates of the point projected to the end time of the scan, I T L Indicates the coordinate system transformation from LiDAR to IMU.

[0028] In a preferred embodiment, the implementation method of step S3 is:

[0029] IMU error state model: In the motion state variable x imu The error δx is defined based on imu :

[0030]

[0031] Where x is the real state quantity, is the state passed through estimation, δ· is the difference between the true quantity and the estimated quantity; Represents a small deviation between the true pose and the estimated pose, Log() is a logarithmic mapping, and the linear error model is as follows:

[0032]

[0033] The linearized form of the error state equation can be expressed as:

[0034]

[0035] The Jacobian matrix after linearization of the error state is:

[0036]

[0037] GNSS observation model: The state observation model of GNSS can be expressed as:

[0038] z gnss =h gnss (x)+w gnss ,w gnss ~N(0,W gnss )

[0039] in W gnss is the covariance matrix of the GNSS measurement noise; the Jacobian matrix of the GNSS observation equation relative to the error state is:

[0040]

[0041] LiDAR observation model: In step S3, a frame of point cloud {o1, o2, …, o m}, for each sampling point o j You can find the set A of adjacent closest points j :

[0042] A j =[o1 o2 … o i ]

[0043] The plane fits the neighboring point cloud A and calculates its normal vector:

[0044]

[0045] where b j =-[I 3×1 … I 3×1 ], and the measurement residual of the laser radar is calculated as

[0046]

[0047] Where q j is any point on the plane, n oi Represents the measurement noise of the point; the Jacobian matrix of the lidar observation equation relative to the error state:

[0048]

[0049] The observation Jacobian matrix H of each point oi It is expressed as:

[0050]

[0051] Kinematic observation model of multi-axis vehicle:

[0052] Assume that the steering error and speed error states of the tire on the i-th axis are:

[0053] x a(i) =[s R(i) s L(i) δα R(i) δα L(i) ]

[0054] Where i represents the i-th axis, s R(i) represents the speed error factor of the right wheel on the i-th axis, s L(i) represents the speed error factor of the left wheel on the i-th axis, α R(i) represents the right wheel turning angle, δα R(i) represents the steering angle error factor of the right wheel on the i-th axis, α L(i) represents the left wheel turning angle δα L(i) represents the steering angle error factor of the left wheel on the i-th axis;

[0055] Taking the axis midpoint as the reference point, the following relationship is obtained based on the kinematic principle:

[0056]

[0057] where v R(i) With v L(i) represents the wheel speed of the right and left wheels of the i-th axis, v a(i) represents the reference speed of the midpoint of the i-th axis; The rotation matrix representing the right wheel angle and the axis midpoint coordinates is expressed as:

[0058]

[0059] Represents the rotation matrix of the left wheel turning angle and the axis midpoint coordinates; then considering the odometer scale factor and speed measurement, it is expressed as:

[0060]

[0061] Where X R(i) It is expressed as:

[0062]

[0063] The state estimation error of the virtual center wheel is:

[0064]

[0065] in Represents the rotation matrix from the imu coordinate system to the i-th axis coordinate system, ω in To measure the angular velocity, ω ib is the Earth's rotation speed, and the speed measurement equation at the center wheel is written as:

[0066]

[0067] So the velocity observation equation is:

[0068]

[0069] The observation model of the multi-axle vehicle chassis is then expressed as:

[0070]

[0071] In a preferred embodiment, the implementation method of step S4 is: designing a noise covariance adaptive factor model of a multi-axle steering vehicle as follows:

[0072]

[0073] c min With c max represents the upper and lower bounds of the adaptive factor, λ1 and λ2 are constants; where ω a(i) is the angular velocity calculated by the speed difference between the two wheels of the i-th axis:

[0074]

[0075] Where 2d represents the axis length; Δr a(i) It represents the distance between the steering center of the i-th axle and the virtual steering center of the multi-axle vehicle, and is defined as:

[0076]

[0077] Where L j It is expressed as:

[0078]

[0079] l j Indicates the wheelbase to the first axis, r a(i) It is expressed as:

[0080]

[0081] The observed noise covariance of the multi-axle vehicle chassis is then expressed as:

[0082]

[0083] In a preferred embodiment, the implementation method of step S5 is:

[0084] The error state and measurement model of the integrated system is expressed as:

[0085] δx k+1 =f(δx k )+w k ,w~N(0,W)

[0086] z k =h(δx k )+v k ,v~N(0,V)

[0087] The linearized form of the equation of motion is expressed as:

[0088] δx k+1 =0+Fδx k +w k ,w~N(0,W)

[0089] z k =0+Hδx k +v k ,v~N(0,V)

[0090] F,H represent the Jacobian matrices of the motion state and observation model respectively, where the error state quantity is set as:

[0091]

[0092] Where m is the number of axes, and the noise covariance W during the error motion state propagation process is expressed as:

[0093] W=diag(Cov(n ω ),03,03Cov(n a ),Cov(n bω ),Cov(n ba ),03)

[0094] The measurement matrix of different sensors for the error state is H gnss , H Lidar , H can , the respective observation noise co-square is V gnss , V Lidar , V can , and then solve the optimal state estimation based on the sequential iterative error extended Kalman filter. The prediction process is:

[0095] δx pred =Fδx

[0096] P pred=FPF T +W

[0097] Update process: Initialize and set iteration k = 0, P0 = P pred , δx0 = 0, observation sensor type i = GNSS, Lidar, can iterative update process is as follows:

[0098]

[0099] Where K k is the Kalman gain, J k It is expressed as:

[0100]

[0101] Log() represents the logarithmic mapping. When the iteration reaches our preset threshold ε , we have ||δx k+1 -δx k When ||≤ε, exit the iteration and calculate the error covariance propagation matrix based on the last iteration, that is:

[0102]

[0103] Then, the data collected by different types of sensors are updated sequentially until all sensor data are updated. The optimal state error estimation result δx is as follows:

[0104]

[0105] Then the optimal state estimation result for:

[0106]

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

[0108] 1) The present invention aims to solve the problem that the positioning accuracy of a multi-axis all-wheel steering vehicle decreases when the multi-axis steering vehicle positioning system is facing a GNSS degraded environment. The GNSS, lidar, IMU, and the wheel speed and steering angle information of the multi-axis steering vehicle chassis are tightly coupled through iterative error extended Kalman filtering, thereby ensuring that the multi-axis steering vehicle positioning system can achieve high-precision positioning in a GNSS return environment.

[0109] 2) In order to address the problem that during the steering process of a multi-axle steering vehicle, the wheels cannot satisfy the Ackermann steering theorem, resulting in tire slippage, which further affects the accuracy of the wheel speed and turning angle observations of the multi-axle steering vehicle chassis, the present invention designs a method for adaptively adjusting the observation noise covariance of the multi-axle steering vehicle, thereby ensuring the correct weighting of the chassis observation data, and providing a new solution for improving the positioning performance of multi-axle steering vehicles and coping with the challenges of the assignment environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0110] Figure 1 This is an overall architecture diagram of multi-sensor fusion positioning in an embodiment of the present invention;

[0111] Figure 2 A kinematic observation model diagram of a multi-axis steering vehicle in an embodiment of the present invention;

[0112] Figure 3 It is a positioning effect diagram of a multi-axle steering vehicle in an embodiment of the present invention. DETAILED DESCRIPTION

[0113] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0114] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0115] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0116] A multi-sensor fusion positioning method suitable for multi-axis steering vehicles, reference Figure 1-3 , including the following steps:

[0117] Step S1: Install various sensors on the multi-axis all-wheel steering vehicle platform to collect IMU data, lidar real-time scanning point cloud, GNSS signals, wheel angle sensor readings and wheel speedometer readings, and align the timestamps of all collected signals.

[0118] Step S2: Establish a kinematic model of the state to be estimated, and dedistort the lidar point cloud data according to the IMU data to achieve motion compensation of the point cloud.

[0119] Step S3: establishing an error state discrete model, a lidar observation model, a multi-axis steering vehicle kinematic observation model, and a GNSS observation model.

[0120] Step S4: Considering the impact of multi-round non-Ackerman steering, a method for adaptively adjusting the observation noise covariance of multi-axis steering vehicles is proposed to ensure the correct weighting of chassis observation data.

[0121] Step S5: Integrate the IMU, GNSS, lidar, and the wheel angle and speed observation information of the multi-axis steering vehicle to construct a sequential iterative error state Kalman filter, and obtain the optimal positioning estimation result through iterative calculation.

[0122] like Figure 1 As shown in the figure, a multi-sensor fusion positioning method architecture suitable for multi-axis steering vehicles is as follows: first, the lidar point cloud is motion compensated based on the IMU kinematic state model to remove distortion, and then the lidar observation residual model, GNSS observation residual model, and multi-axis steering vehicle chassis observation residual model are established respectively. Based on the Ackerman steering theorem, a covariance adaptive method for the multi-axis steering vehicle chassis observation noise is designed to ensure the correct weighting of the chassis observation data, thereby ensuring the correct weighting of the chassis observation data. Finally, all sensor information is fused by a sequential iterative error extended Kalman filter to obtain the optimal positioning estimation result.

[0123] In this embodiment, taking the three-axis vehicle test bench built in the laboratory as an example, the vehicle parameters and sensor parameters are as follows:

[0124]

[0125] The following is a further detailed explanation of the relevant contents involved in this method.

[0126] In step S1, it is necessary to establish a sensor information collection and time alignment system. The specific process is as follows:

[0127] On the multi-axis steering vehicle platform, the communication subscription modules of IMU, GNSS, LiDAR and multi-axis all-wheel steering chassis are established based on the ROS robot operating system, which are used to collect the acceleration signal of IMU. i With the angular velocity signal ω i , GNSS latitude, longitude, and altitude informationλ i ,φ i ,h i , the point cloud information of the laser radar {o1,o2,…,o m} i , where o i is the coordinate information of the point in the laser radar coordinate system, the signal of the multi-axis all-wheel steering chassis {n1, n2, ... n m ,α1,α2,…αm} i , where n m is the speed information of the mth tire, α m is the steering angle information of the mth tire.

[0128] After collecting data within a certain period of time, the end time of each point cloud frame scan of the laser radar is used as the timestamp alignment point, and the speed and angle signals of each wheel of the IMU, GNSS, and multi-axis all-wheel steering vehicle chassis are estimated by linear interpolation to obtain the measured values ​​at that time point. The linear interpolation is expressed as follows:

[0129]

[0130] Where y represents the estimated value to be solved at time t, (y0, t0) and (y1, t1) represent the previous frame data and the next frame data at time t respectively.

[0131] In step S2, since the observed data will be distorted when the LiDAR point cloud is in motion, a kinematic model of the state to be estimated is established, and the LiDAR point cloud data is dedistorted according to the IMU data to achieve motion compensation of the point cloud. The specific process is as follows:

[0132] The state variable x to be estimated is expressed as:

[0133]

[0134] in G · I Represents the representation of the IMU coordinate system I in the world coordinate system G. The origin of the world coordinate system G is the position of the IMU in the first frame, the x-axis points to the east, the y-axis points to the north, and the z-axis satisfies the right-hand coordinate system rule. φ is represented as a three-dimensional rotation matrix, p represents the three-dimensional position vector, v represents the three-dimensional velocity vector, and b ω represents the gyroscope's three-dimensional angular velocity zero bias vector, b a represents the three-dimensional acceleration zero bias vector of the accelerometer, and g represents the gravitational acceleration vector.

[0135] Considering IMU output Accelerometer noise n a , gyroscope noise n ω , Accelerometer bias noise and gyro bias noise n bω The influence of Build the state model:

[0136]

[0137] in· i Indicates time i, Represents the mapping of vector to antisymmetric matrix. Acceleration, gyroscope, acceleration bias and gyroscope bias noise are modeled as Gaussian white noise. Then, the forward propagation equation is established according to the kinematic method:

[0138]

[0139] in Indicates the measured value, represents the measurement state at the i-th moment, Δt I Represents the time difference between two adjacent frames of IMU data. The state at the end of the lidar scan is roughly estimated by the forward propagation method, and then the motion compensation of each point is estimated by the backward propagation method:

[0140]

[0141] Since the acquisition frequency of points is greater than the acquisition frequency of IMU, there are usually multiple point measurements between each IMU frame, so the state of the IMU frame at the last moment is used as the reference. The obtained point pose relative to the end time of the scan is defined as in Represents the relative rotation transformation matrix between the IMU coordinate system at time k (the end of the scan) and the IMU coordinate system at time j, Represents the relative position transformation matrix between the IMU coordinate system at time k and the IMU coordinate system at time j. Projection to scan end time:

[0142]

[0143] In the formula Represents the coordinates of the point projected to the end time of the scan, I T L Indicates the coordinate system transformation from LiDAR to IMU.

[0144] In step S3, the error state model, GNSS observation model, lidar observation model, and multi-axis steering vehicle chassis observation model are designed:

[0145] Error state model: In the motion state variable x imu The error δx is defined based on imu :

[0146]

[0147] Where x is the real state quantity, is the estimated state, and δ· is the difference between the true value and the estimated value. Represents a small deviation between the true pose and the estimated pose, Log() is a logarithmic mapping, and the linear error model is as follows:

[0148]

[0149] The linearized form of the error state equation can be expressed as:

[0150]

[0151] The Jacobian matrix after linearization of the error state is:

[0152]

[0153] GNSS observation model: The state observation model of GNSS can be expressed as:

[0154] z gnss =h gnss (x)+w gnss ,w gnss ~N(0,W gnss )

[0155] in W gnss is the covariance matrix of the GNSS measurement noise. The Jacobian matrix of the GNSS observation equation relative to the error state is:

[0156]

[0157] LiDAR observation model: In step S3, a frame of point cloud {o1, o2, …, o m}, for each sampling point o j You can find the set A of adjacent closest points j :

[0158] A j =[o1 o2 … o i ]

[0159] The plane fits the neighboring point cloud A and calculates its normal vector:

[0160]

[0161] where b j =-[I 3×1 … I 3×1 ], and the measurement residual of the laser radar is calculated as

[0162]

[0163] Where q j is any point on the plane, n oi Represents the measurement noise of the point. The Jacobian matrix of the lidar observation equation relative to the error state:

[0164]

[0165] The observation Jacobian matrix H of each point oi It is expressed as:

[0166]

[0167] Kinematic observation model of multi-axis vehicle:

[0168] Assume that the steering error and speed error states of the tire on the i-th axis are:

[0169] x a(i) =[s R(i) s L(i) δα R(i) δα L(i) ]

[0170] Where i represents the i-th axis, s R(i) represents the speed error factor of the right wheel on the i-th axis, s L(i) represents the speed error factor of the left wheel on the i-th axis, α R(i) represents the right wheel turning angle, δα R(i) represents the steering angle error factor of the right wheel on the i-th axis, α L(i) represents the left wheel turning angle δα L(i) Represents the steering angle error factor of the left wheel on the i-th axis.

[0171] Taking the axis midpoint as the reference point, the following relationship can be obtained based on the kinematic principle:

[0172]

[0173] where v R(i) With v L(i) represents the wheel speed of the right and left wheels of the i-th axis, v a(i) Indicates the reference speed of the midpoint of the i-th axis. The rotation matrix representing the right wheel angle and the axis midpoint coordinates is expressed as:

[0174]

[0175] Represents the rotation matrix of the left wheel turning angle and the axis midpoint coordinates. Then considering the odometer scale factor and speed measurement, it can be expressed as:

[0176]

[0177] Where X R(i) It is expressed as:

[0178]

[0179] The state estimation error of the virtual center wheel is:

[0180]

[0181] in Represents the rotation matrix from the imu coordinate system to the i-th axis coordinate system, ω in To measure the angular velocity, ω ib is the Earth's rotation speed, and the speed measurement equation at the center wheel can be written as:

[0182]

[0183] So the velocity observation equation is:

[0184]

[0185] The observation model of the multi-axle vehicle chassis can be expressed as:

[0186]

[0187] In step S4, considering the impact of multi-wheel non-Ackerman steering, a method for adaptively adjusting the observation noise covariance of multi-axis steering vehicles is proposed to ensure the correct weighting of chassis observation data. The specific process is as follows:

[0188] Since the turning centers of multi-axis vehicles are usually difficult to gather on the same turning center, the single-axis speed measurement covariance inevitably has errors. Therefore, an adaptive factor is applied to the single-axis measurement covariance to ensure that the measured data is correctly weighted. Therefore, the noise covariance adaptive factor modeling of the multi-axis steering vehicle is designed as follows:

[0189]

[0190] c min With c max represents the upper and lower bounds of the adaptive factor, λ1 and λ2 are constants. a(i) is the angular velocity calculated by the speed difference between the two wheels of the i-th axis:

[0191]

[0192] Where 2d represents the axis length. Δr a(i) It represents the distance between the steering center of the i-th axle and the virtual steering center of the multi-axle vehicle, and is defined as:

[0193]

[0194] Where L j It is expressed as:

[0195]

[0196] l j Indicates the wheelbase to the first axis, r a(i) It can be expressed as:

[0197]

[0198] The observed noise covariance of the multi-axle vehicle chassis can then be expressed as:

[0199]

[0200] In step S5, the error state and measurement model of the integrated system is expressed as:

[0201] δx k+1 =f(δx k )+w k ,w~N(0,W)

[0202] z k =h(δx k )+v k ,v~N(0,V)

[0203] The linearized form of the equation of motion is expressed as:

[0204] δx k+1 =0+Fδx k +w k ,w~N(0,W)

[0205] z k =0+Hδx k +v k ,v~N(0,V)

[0206] F, H represent the Jacobian matrices of the motion state and observation model respectively, where the error state quantity is set as:

[0207]

[0208] Where m is the number of axes, and the noise covariance W during the error motion state propagation process is expressed as:

[0209] W=diag(Cov(n ω ),03,03Cov(n a ),Cov(n bω ),Cov(n ba ),03)

[0210] The measurement matrix of different sensors for the error state is H gnss , H Lidar , H can , the respective observation noise co-square is Vgnss , V Lidar , V can , and then solve the optimal state estimation based on the sequential iterative error extended Kalman filter. The prediction process is:

[0211] δx pred =Fδx

[0212] P pred =FPF T +W

[0213] Update process: Initialize and set iteration k = 0, P0 = P pred , δx0 = 0, observation sensor type i = GNSS, Lidar, can iterative update process is as follows:

[0214]

[0215] Where K k is the Kalman gain, J k It is expressed as:

[0216]

[0217] Log() represents the logarithmic mapping. When the iteration reaches our preset threshold ε , we have ||δx k+1 -δx k When ||≤ε, exit the iteration and calculate the error covariance propagation matrix based on the last iteration, that is:

[0218]

[0219] Then, the data collected by different types of sensors are updated sequentially until all sensor data are updated. The optimal state error estimation result δx is as follows:

[0220]

[0221] Then the optimal state estimation result for:

[0222]

[0223] The present invention provides a multi-sensor fusion positioning method suitable for multi-axis steering vehicles. Aiming at the problem of positioning accuracy of multi-axis steering vehicles under degraded environments of sensors such as GNSS, an iterative error Kalman filter fusion positioning system based on a sequential method suitable for multi-axis steering vehicles is constructed, and an adaptive method for the observation noise covariance of the chassis sensor system of the multi-axis steering vehicle is designed, which can effectively weight the chassis observation, thereby effectively solving the problem of reduced positioning system accuracy caused by interference of multi-axis steering vehicles in sensor degraded environments such as tunnels and mines, and realizing high-precision positioning of multi-axis steering vehicles.

[0224] Figure 3 This is a positioning effect diagram of the multi-axis steering vehicle in this embodiment.

[0225] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0226] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0227] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0228] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0229] The above is only a preferred embodiment of the present invention, and does not limit the present invention in other forms. Any technician familiar with the profession may use the above disclosed technical content to change or modify it into an equivalent embodiment with equivalent changes. However, any simple modification, equivalent change and modification made to the above embodiment according to the technical essence of the present invention without departing from the technical solution of the present invention still belongs to the protection scope of the technical solution of the present invention.

Claims

1. A multi-sensor fusion positioning method suitable for multi-axis steering vehicles, characterized in that: The following steps are involved: Step S1: installing a variety of sensors on the multi-axis all-wheel steering vehicle platform to collect IMU data, laser radar real-time scanning point cloud, GNSS signals, each wheel angle sensor reading and each wheel speed meter reading, and aligning the timestamps of all collected signals; Step S2: Establish a kinematic model of the state to be estimated, and dedistort the lidar point cloud data according to the IMU data to achieve motion compensation of the point cloud; Step S3: establishing an error state discrete model, a lidar observation model, a multi-axis steering vehicle kinematic observation model, and a GNSS observation model; Step S4: Considering the impact of multi-wheel non-Ackerman steering, a method for adaptively adjusting the observation noise covariance of multi-axis steering vehicles is proposed to ensure the correct weighting of chassis observation data; Step S5: Integrate the IMU, GNSS, lidar, and the wheel angle and speed observation information of the multi-axis steering vehicle to construct a sequential iterative error state Kalman filter, and obtain the optimal positioning estimation result through iterative calculation.

2. A multi-sensor fusion positioning method suitable for a multi-axle steering vehicle according to claim 1, characterized in that: The implementation method of step S1 is: On the multi-axis steering vehicle platform, the communication subscription modules of IMU, GNSS, LiDAR and multi-axis all-wheel steering chassis are established based on the ROS robot operating system, which are used to collect the acceleration signal of IMU. i With the angular velocity signal ω i , GNSS latitude, longitude, and altitude informationλ i ,φ i ,h i , the point cloud information of the laser radar {o1,o2,…,o m } i , where o i is the coordinate information of the point in the laser radar coordinate system, the signal of the multi-axis all-wheel steering chassis {n1, n2, ... n m ,α1,α2,…α m } i , where n m is the speed information of the mth tire, α m is the turning angle information of the mth tire; After collecting data within a certain period of time, the end time of each point cloud frame scan of the lidar is used as the timestamp alignment point, and the speed and angle signals of each wheel of the IMU, GNSS, and multi-axis all-wheel steering vehicle chassis are estimated by linear interpolation method. The measurement value at that time point.

3. The multi-sensor fusion positioning method applicable to a multi-axle steering vehicle according to claim 1, characterized in that: The implementation method of step S2 is: The state variable x to be estimated is expressed as: in Represents the representation of the IMU coordinate system I in the world coordinate system G; the origin of the world coordinate system G is the position of the IMU in the first frame, the x-axis points to the east, the y-axis points to the north, and the z-axis satisfies the right-hand coordinate system rule; φ is represented as a three-dimensional rotation matrix, p represents the three-dimensional position vector, v represents the three-dimensional velocity vector, and b ω represents the gyroscope's three-dimensional angular velocity zero bias vector, b a represents the three-dimensional acceleration zero bias vector of the accelerometer, and g represents the gravity acceleration vector; Considering IMU output Accelerometer noise n a , gyroscope noise n ω , Accelerometer bias noise and gyro bias noise n bω The influence of And build the state model: in· i Indicates time i, Represents the mapping of vector to antisymmetric matrix. Acceleration, gyroscope, acceleration bias and gyroscope bias noise are modeled as Gaussian white noise. Then, the forward propagation equation is established according to the forward kinematics method: in Indicates the measured value, represents the measurement state at the i-th moment, Δt I Represents the time difference between two adjacent frames of IMU data; the state at the end of the lidar scan is roughly estimated by the forward propagation method, and then the motion compensation of each point is estimated by backward propagation: Since the acquisition frequency of points is greater than the acquisition frequency of IMU, there are usually multiple point measurements between each IMU frame, so the state of the IMU frame at the last moment is taken as the standard; the obtained point pose relative to the end time of the scan is defined as in Represents the relative rotation transformation matrix between the IMU coordinate system at time k and the IMU coordinate system at time j; Represents the relative position transformation matrix between the IMU coordinate system at time k and the IMU coordinate system at time j. Projection to scan end time: In the formula Represents the coordinates of the point projected to the end time of the scan, I T L Indicates the coordinate system transformation from LiDAR to IMU.

4. The multi-sensor fusion positioning method applicable to a multi-axle steering vehicle according to claim 1, characterized in that: The implementation method of step S3 is: IMU error state model: In the motion state variable x imu The error δx is defined based on imu : In the formula, x is the true state quantity, x is the estimated state, and δ· is the difference between the true quantity and the estimated quantity; Represents a small deviation between the true pose and the estimated pose, Log() is a logarithmic mapping, and the linear error model is as follows: The linearized form of the error state equation can be expressed as: The Jacobian matrix after linearization of the error state is: GNSS observation model: The state observation model of GNSS is expressed as: z gnss =h gnss (x)+w gnss ,w gnss ~N(0,W gnss ) in W gnss is the covariance matrix of GNSS measurement noise; The Jacobian matrix of the GNSS observation equation relative to the error state is: LiDAR observation model: In step S3, a frame of point cloud {o1, o2, …, o m }, for each sampling point o j You can find the set A of adjacent closest points j : A j =[o1 o2…o i ] The plane fits the neighboring point cloud A and calculates its normal vector: where b j =-[I 3×1 …I 3×1 ], and the measurement residual of the laser radar is calculated as Where q j is any point on the plane, n oi Represents the measurement noise of the point; the Jacobian matrix of the lidar observation equation relative to the error state: The observation Jacobian matrix H of each point oi It is expressed as: Kinematic observation model of multi-axis vehicle: Assume that the steering error and speed error states of the tire on the i-th axis are: x a(i) =[s R(i) s L(i) yes R(i) yes L(i) ] Where i represents the i-th axis, s R(i) represents the speed error factor of the right wheel on the i-th axis, s L(i) represents the speed error factor of the left wheel on the i-th axis, α R(i) represents the right wheel turning angle, δα R(i) represents the steering angle error factor of the right wheel on the i-th axis, α L(i) represents the left wheel turning angle δα L(i) represents the steering angle error factor of the left wheel on the i-th axis; Taking the axis midpoint as the reference point, the following relationship is obtained based on the kinematic principle: where v R(i) With v L(i) represents the wheel speed of the right and left wheels of the i-th axis, v a(i) represents the reference speed of the midpoint of the i-th axis; The rotation matrix representing the right wheel angle and the axis midpoint coordinates is expressed as: Represents the rotation matrix of the left wheel turning angle and the axis midpoint coordinates; then considering the odometer scale factor and speed measurement, it is expressed as: Where X R(i) It is expressed as: The state estimation error of the virtual center wheel is: in Represents the rotation matrix from the imu coordinate system to the i-th axis coordinate system, ω in To measure the angular velocity, ω ib is the Earth's rotation speed, and the speed measurement equation at the center wheel is written as: So the velocity observation equation is: The observation model of the multi-axle vehicle chassis is then expressed as:

5. The multi-sensor fusion positioning method applicable to a multi-axle steering vehicle according to claim 1, characterized in that: The implementation method of step S4 is: designing the noise covariance adaptive factor model of the multi-axle steering vehicle as follows: c min With c max represents the upper and lower bounds of the adaptive factor, λ1 and λ2 are constants; where ω a(i) is the angular velocity calculated by the speed difference between the two wheels of the i-th axis: Where 2d represents the axis length; Δr a(i) It represents the distance between the steering center of the i-th axle and the virtual steering center of the multi-axle vehicle, and is defined as: Where L j It is expressed as: l j Indicates the wheelbase to the first axis, r a(i) It is expressed as: The observed noise covariance of the multi-axle vehicle chassis is then expressed as:

6. The multi-sensor fusion positioning method applicable to a multi-axle steering vehicle according to claim 1, characterized in that: The implementation method of step S5 is: The error state and measurement model of the integrated system is expressed as: δx k+1 =f(δx k )+w k ,w~N(0,W) with k =h(δx k )+v k ,v~N(0,V) The linearized form of the equation of motion is expressed as: δx k+1 =0+Fδx k +in k ,in~N(0,W) with k =0+Hδx k +v k ,v~N(0,V) F,H represent the Jacobian matrices of the motion state and observation model respectively, where the error state quantity is set as: Where m is the number of axes, and the noise covariance W during the error motion state propagation process is expressed as: W=diag(Cov(n ω ),03,03Cov(n a ),Cov(n bω ),Cov(n ba ),03) The measurement matrix of different sensors for the error state is H gnss , H Lidar , H can , the respective observation noise co-square is V gnss , V Lidar , V can , and then solve the optimal state estimation based on the sequential iterative error extended Kalman filter. The prediction process is: δx pred =Fδx P pred =FPF T +W Update process: Initialize and set iteration k = 0, P0 = P pred , δx0 = 0, observation sensor type i = GNSS, Lidar, can iterative update process is as follows: Where K k is the Kalman gain, J k It is expressed as: Where Log() represents the logarithmic mapping. When it iterates to our preset threshold ε, we have ||δx k+1 -δx k When ||≤ε, exit the iteration and calculate the error covariance propagation matrix based on the last iteration, that is: Then, the data collected by different types of sensors are updated sequentially until all sensor data are updated. The optimal state error estimation result δx is as follows: Then the optimal state estimation result x k+1 for: x k+1 =x k +δx k+1 。

Images