A method and system for positioning a trailer based on extended kalman filtering

Abstract

A kind of tow positioning method and system based on extended Kalman filter, the positioning information of tractor is used to initialize the positioning vector of tow, the inertial navigation information of tow is used to predict the positioning vector of next time point of the tow in combination with motion model;Using the positioning information of tractor, the predicted positioning vector of the tow is corrected using extended Kalman filter algorithm, complete the whole positioning process, scheme is easy to implement, process is simple, without additional installation other sensors, cost is lower.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving positioning compensation, and in particular to a towed positioning method and system based on extended Kalman filtering. Background Technology

[0002] Driverless container trucks at ports are an innovative mode of transportation specifically designed for transporting containers in ports and logistics centers. Compared to traditional trucks, they feature automated driving capabilities, typically based on autonomous driving technologies such as LiDAR, cameras, sensors, and advanced computer vision systems. These systems allow the trucks to autonomously perceive, navigate, and control themselves within the port environment.

[0003] Positioning technology is a crucial component of autonomous driving. In ports, driverless container trucks with large trailers, which typically lack their own power systems and are significantly larger than the tractor unit, are highly susceptible to collisions with obstacles on the road due to inaccurate positioning, as the vehicle's control may not account for the trailer. While installing a multi-sensor fusion positioning system on the trailer could achieve precise positioning, it is prohibitively expensive for practical enterprise applications. Using machine vision methods, which employ images from two cameras to calculate the angle between the trailer and the tractor unit, suffers from poor nighttime performance, hindering safe operation of container truck trailers at night or in low-light conditions.

[0004] Due to cost control requirements for enterprise applications, using a single sensor for trailer positioning can indeed effectively reduce costs; for example, inertial navigation sensors are commonly used for positioning. However, without the constraint of the trailer's angle and orientation, the trailer's angle and orientation can deviate significantly, and the accuracy will decrease noticeably over time. Therefore, a low-cost, high-precision positioning solution for truck trailers is needed. Summary of the Invention

[0005] The main objective of this invention is to overcome the aforementioned deficiencies in the prior art and propose a trailer positioning method and system based on extended Kalman filtering. This method utilizes the precise positioning information of the tractor as an observation value, avoiding the problem of lacking stable and accurate observation information and ensuring the accuracy and stability of trailer positioning.

[0006] The present invention adopts the following technical solution:

[0007] A towed positioning method based on extended Kalman filtering, characterized by comprising:

[0008] When the tractor starts, the positioning vector of the trailer is initialized using the positioning information of the tractor.

[0009] The positioning vector of the towed vehicle at the next time point is predicted by using the inertial navigation information of the towed vehicle and combining it with the motion model.

[0010] The extended Kalman filter algorithm is used to correct the predicted positioning vector of the trailer using the positioning information of the tractor.

[0011] The positioning vector of the towed vehicle is μ. t =[x 1,t ,y 1,t ,θ 1,t v 1,t a 1,t w 1,t When the tractor starts, the initial value of the positioning vector of the trailer is obtained based on the bicycle motion model:

[0012] θ 1,t =θ 0,t

[0013] x 1,t =x 0,t -dsinθ 1,t

[0014] y 1,t =y 0,t -dcosθ 1,t

[0015] v 1,t =0

[0016] a 1,t =0

[0017] w 1,t =0

[0018] Where, x 0,t and y 0,t These are the XY coordinates of the tractor, and θ is the y coordinate. 0,t The angle of the tractor unit; x 1,t and y 1,t Let θ be the XY coordinate value of the towed vehicle. 1,t The angle of the tow truck, v 1,t The absolute value of the forward speed of the towed vehicle, a 1,t w is the absolute value of the towed acceleration. 1,t Absolute value of angular velocity.

[0019] The positioning vector of the towed vehicle at the next time point is predicted using inertial navigation information and a motion model, specifically as follows:

[0020] Based on the initialized positioning vector value, use the state transition function g(u) t ,μ t-1 Predict the location vector value;

[0021] When the angular velocity of the trailer is less than the set value, i.e., it is moving straight, the CA motion model is used to predict the positioning vector of the trailer at the next time point.

[0022] When the angular velocity of the trailer exceeds a set value and it turns, the CTRA model is used to predict the positioning vector of the trailer at the next time point.

[0023] The CA motion model is used to predict the positioning vector of the towed trailer at the next time point, and the specific expression is as follows:

[0024]

[0025] Where, x t-1 and y t-1 Let θ be the coordinate value of the previous time point of the towed vehicle. t-1 v represents the angular orientation of the towed vehicle at the previous point in time. t-1 Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and T be the time interval between each frame of inertial navigation information. t ,w t These are the absolute values ​​of the acceleration and angular velocity of the current tractor unit, respectively.

[0026] The CTRA model is used to predict the positioning vector of the towed trailer at the next time point. The specific expression is as follows:

[0027]

[0028] Where, x t-1 and y t-1 Let θ be the coordinate value of the previous time point of the towed vehicle. t-1 v represents the angular orientation of the towed vehicle at the previous point in time. t-1 Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and T be the time interval between each frame of inertial navigation information. t ,w t These are the absolute values ​​of the acceleration and angular velocity of the current tractor.

[0029] The extended Kalman filter algorithm is used to correct the predicted positioning vector of the trailer using the positioning state of the tractor, specifically including the following:

[0030] The orientation θ of the tractor at time t is calculated using the angular orientation of the tractor in the previous frame and the angular orientation of the trailer in the previous frame. 1,t ;

[0031] According to the angle of the tractor at time t, θ 1,t Calculate the state variable update function

[0032] The positioning information of the tractor is used as the measurement value z. t The predicted value of the positioning vector is then fed into the state variable update function and compared with the measured value z. t Calculate the difference, and then use the difference and the Kalman coefficient K. t The predicted value of the positioning vector is corrected to obtain the corrected positioning vector μ of the trailer. t :

[0033]

[0034] Among them, the measured value For the predicted localization vector, μ t This is the corrected positioning vector.

[0035] Also includes:

[0036] Update function for the state variable Perform the Jacobian operation to obtain the measurement matrix H. t ,

[0037] Combined with the process excitation noise covariance matrix Q t and prior estimation of covariance matrix Obtain the Kalman coefficient K t :

[0038]

[0039] Using Kalman coefficient K t and measurement matrix H t The posterior estimated covariance matrix Σ is obtained. t Posterior estimation of covariance matrix ∑ t Used to calculate the next Kalman coefficient.

[0040]

[0041] Where I is the identity matrix.

[0042] Using the Jacobian matrix and the measurement noise covariance matrix R t The prior estimated covariance matrix is ​​obtained.

[0043]

[0044] Among them G t Let ∑ be the state transition matrix. t-1 This is the posterior covariance matrix calculated in the previous frame.

[0045] A trailer positioning system based on extended Kalman filtering, characterized in that it includes:

[0046] The initialization module initializes the positioning vector of the trailer using the positioning information of the tractor when the vehicle starts.

[0047] The positioning prediction module uses inertial navigation information and combines it with a motion model to predict the positioning vector of the towed trailer at the next time point;

[0048] The correction module uses the extended Kalman filter algorithm to correct the predicted positioning vector of the trailer using the positioning information of the tractor.

[0049] As can be seen from the above description of the present invention, compared with the prior art, the present invention has the following beneficial effects:

[0050] In this invention, the positioning vector of the trailer is initialized using the positioning information of the tractor, and the positioning vector of the trailer at the next time point is predicted using the inertial navigation information of the trailer and combined with the motion model. The extended Kalman filter algorithm is used to correct the predicted positioning vector of the trailer using the positioning information of the tractor, thus completing the entire positioning process. The solution is easy to implement, the process is simple, no additional sensors are required, and the cost is low.

[0051] This invention eliminates the need for camera angle measurement, achieving positioning by integrating the positioning information of the tractor and the inertial navigation information on the trailer. Dim lighting does not affect the accuracy of the positioning information, and the stability is strong.

[0052] Compared to traditional camera positioning solutions, this invention has no requirements on the shape of the vehicle, is easier to replicate on other truck trailers, and is easier to promote. Attached Figure Description

[0053] Figure 1 This is the main flowchart of the present invention;

[0054] Figure 2 Top view of the tractor and trailer;

[0055] Figure 3 This is a diagram showing the turning of a tractor and trailer.

[0056] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. Detailed Implementation

[0057] The present invention will be further described below through specific embodiments.

[0058] In this invention, the tractor is equipped with various sensors, such as lidar, cameras, inertial navigation sensors, and GNSS. See also... Figures 2-3The tractor and trailer are connected by a towing pin. The tractor uses a multi-sensor fusion positioning algorithm to obtain accurate positioning information, including positioning status, position information, and orientation information. The trailer is equipped with inertial navigation sensors to obtain inertial navigation information, including absolute acceleration and absolute angular velocity.

[0059] See Figure 1 This invention proposes a towed positioning method based on extended Kalman filtering, comprising:

[0060] 1) When the tractor starts, the positioning vector of the trailer is initialized using the positioning information of the tractor.

[0061] In this invention, in order to use the tractor to calculate the positioning information of the trailer, the relationship between the trailer and the tractor is simplified to a bicycle motion model, which is used as a state variable measurement function.

[0062] The following is the formula derivation process:

[0063] θ 1,t+1 =θ 1,t +w1t

[0064]

[0065]

[0066]

[0067] x 1,t+1 =x 0,t+1 -dsinθ 1,t+1

[0068] y 1,t+1 =y 0,t+1 -dcosθ 1,t+1

[0069] Among them, x 0,t+1 and y 0,t+1 These are the XY coordinates of the tractor in the enu coordinate system, and θ is the y coordinate. 0,t The angle of the tractor unit; x 1,t+1 and y 0,t+1 For the XY coordinates of the dragged object in the enu coordinate system, θ 1,t θ 1,t+1 The towing angles at times t and t+1 are respectively, w 1,t The absolute value of the angular velocity of the trailer, R1 is the radius of rotation of the trailer, D is the distance the tractor travels per unit time as the step length, D=v0v*dt, d is the length from the center of the rear axle of the trailer to the center of the rear axle of the tractor, d=s2, v0 is the absolute value of the forward speed of the vehicle front.

[0070] Based on the last three formulas above, we can obtain the relationship between the orientation of the trailer and the tractor, and the relationship between the positioning of the trailer and the tractor head.

[0071] In this step, we assume the positioning vector of the towed vehicle is μ. t =[x 1,t ,y 1,t ,θ 1,t v 1,t a 1,t w 1,t When the tractor starts, the trailer and the tractor face the same direction, the trailer speed is 0, and the initial value of the state vector is obtained according to the bicycle motion model, that is, the initial value of the trailer's positioning vector is obtained according to the bicycle motion model:

[0072] θ 1,t =θ 0,t

[0073] x 1,t =x 0,t -dsinθ 1,t

[0074] y 1,t =y 0,t -dcosθ 1,t

[0075] v 1,t =0

[0076] a 1,t =0

[0077] w 1,t =0

[0078] Where, x 0,t and y 0,t These are the XY coordinates of the tractor in the enu coordinate system, and θ is the y coordinate. 0,t The angle of the tractor unit; x 1,t and y 1,t For the XY coordinates of the dragged object in the enu coordinate system, θ 1,t The angle of the tow truck, v 1,t The absolute value of the forward speed of the towed vehicle, a 1,t w is the absolute value of the towed acceleration. 1,t Absolute value of angular velocity.

[0079] 2) Utilize the inertial navigation information of the towed vehicle and combine it with the motion model to predict the positioning vector of the towed vehicle at the next time point.

[0080] The positioning vector of the towed vehicle at the next time point is predicted using inertial navigation information and a motion model, specifically as follows:

[0081] Based on the initialized positioning vector value, use the state transition function g(u) t ,μ t-1 Predicting location vector values

[0082] Towed positioning vector μ t =[x t ,y t ,θ t v t a t w t ], u t =[T,a t ,w t ]

[0083] T is the time interval dt = 0.01s for each frame of the inertial navigation sensor. t , t The absolute values ​​of acceleration and angular velocity from the inertial navigation sensor.

[0084] When the angular velocity of the towed trailer is less than the set value, i.e., it is moving straight, the CA motion model is used to predict the positioning vector of the towed trailer at the next time point:

[0085]

[0086] in, To predict the location vector.

[0087] The CA motion model is used to predict the positioning vector of the towed trailer at the next time point. The specific expression is as follows:

[0088]

[0089] Where, x t-1 and y t-1 Let θ be the coordinate value of the previous time point of the towed vehicle. t-1 v represents the angular orientation of the towed vehicle at the previous point in time. t-1 Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and T be the time interval between each frame of inertial navigation information. t ,w t These are the absolute values ​​of the acceleration and angular velocity of the current tractor unit, respectively.

[0090] When the towed trailer's angular velocity exceeds a set value (i.e., when turning), the CTRA model is used to predict the trailer's positioning vector at the next time point. The specific expression for predicting the trailer's positioning vector at the next time point using the CTRA model is as follows:

[0091]

[0092] Where, xt-1 and y t-1 Let θ be the coordinate value of the previous time point of the towed vehicle. t-1 v represents the angular orientation of the towed vehicle at the previous point in time. t-1 Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and T be the time interval between each frame of inertial navigation information. t ,w t These are the absolute values ​​of the acceleration and angular velocity of the current tractor.

[0093] 3) The predicted positioning vector of the towed trailer is corrected using the extended Kalman filter algorithm and the positioning status of the tractor. Specifically, this includes the following:

[0094] The orientation θ of the tractor at time t is calculated using the tractor's orientation in the previous frame and the towed vehicle's orientation in the previous frame. 1,t According to the angle of the tractor at time t, θ 1,t Calculate the state variable update function

[0095]

[0096]

[0097]

[0098] The positioning information of the tractor is used as the measurement value z. t The predicted value of the positioning vector Substitute the state variable into the state variable update function, and then multiply the output of the state variable update function by the measured value z. t The difference is calculated to obtain the error between the calculated result and the measurement result; based on the difference and the Kalman coefficient K... t The predicted value of the positioning vector is corrected to obtain the corrected positioning vector μ of the trailer. t :

[0099]

[0100] Among them, the measured value For the predicted localization vector, μ t This is the corrected positioning vector.

[0101] Furthermore, the present invention also includes a function for updating state variables. Perform the Jacobian operation to obtain the measurement matrix H. t :

[0102]

[0103] Then, combined with the process excitation noise covariance matrix Qt and prior estimation of covariance matrix Obtain the Kalman coefficient K t :

[0104]

[0105] Using Kalman coefficient K t and measurement matrix H t The posterior estimated covariance matrix ∑ is obtained. t Posterior estimation of covariance matrix ∑ t Used to calculate the next Kalman coefficient.

[0106]

[0107] Where I is the identity matrix.

[0108] Among them, the Jacobian matrix and the measurement noise covariance matrix R are used. t The prior estimated covariance matrix is ​​obtained.

[0109]

[0110] Among them G t Let ∑ be the state transition matrix. t-1 This is the posterior covariance matrix calculated in the previous frame. The state transition matrix G is then calculated using the state transition function. t That is, to Perform the Jacobian operation to obtain the state transition matrix G. t :

[0111] Based on this, the present invention also proposes a trailer positioning system based on extended Kalman filtering, comprising:

[0112] The initialization module initializes the positioning vector of the trailer using the positioning status, position information, and orientation information of the tractor when the vehicle starts.

[0113] The positioning prediction module uses inertial navigation information and combines it with a motion model to predict the positioning vector of the towed vehicle at the next time point.

[0114] The correction module uses the extended Kalman filter algorithm to correct the predicted positioning vector of the trailer based on the positioning status of the tractor.

[0115] The system of the present invention employs the above-described extended Kalman filter-based tow truck positioning method to achieve tow truck positioning. The initialization module is used to execute step 1), the positioning prediction module is used to execute step 2), and the correction module is used to execute step 3).

[0116] The above are merely specific embodiments of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantial modifications made to the present invention using this concept shall be considered as infringing upon the protection scope of the present invention.

Claims

1. A towed positioning method based on extended Kalman filtering, characterized in that, include: When the tractor starts, the positioning vector of the trailer is initialized using the tractor's positioning information; the positioning vector of the trailer is... =[ , , , , , When the tractor starts, the initial value of the positioning vector of the trailer is obtained based on the bicycle motion model: ； ； ； ； ； ； in, and These are the X and Y coordinates of the tractor unit. The angle of the tractor unit; and The XY coordinates of the towed vehicle are... The angle of the trailer is facing. The absolute value of the forward speed of the towed vehicle. The absolute value of the towed acceleration. The absolute value of the angular velocity, where d is the length from the center of the rear axle of the trailer to the center of the rear axle of the tractor. The positioning vector of the towed vehicle at the next time point is predicted by using the inertial navigation information of the towed vehicle and combining it with the motion model. The extended Kalman filter algorithm is used to correct the predicted positioning vector of the trailer using the tractor positioning information, specifically including the following: The orientation of the tractor at time t is calculated using the angular orientation of the tractor in the previous frame and the angular orientation of the trailer in the previous frame. ; According to the angle of the tractor at time t Calculate the state variable update function ; The positioning information of the tractor unit is used as the measurement value. The predicted value of the positioning vector is then fed into the state variable update function and compared with the measured value. Calculate the difference, and then use the difference and the Kalman coefficients as the basis. The predicted value of the positioning vector is corrected to obtain the corrected positioning vector of the trailer. : ； Among them, the measured value , The predicted location vector, This is the corrected positioning vector.

2. The towed positioning method based on extended Kalman filtering as described in claim 1, characterized in that, The positioning vector of the towed vehicle at the next time point is predicted using inertial navigation information and a motion model, specifically as follows: Use the state transition function based on the initialized location vector value. Perform location vector value prediction; When the angular velocity of the trailer is less than the set value, i.e., it is moving straight, the CA motion model is used to predict the positioning vector of the trailer at the next time point. When the angular velocity of the trailer exceeds a set value and it turns, the CTRA model is used to predict the positioning vector of the trailer at the next time point.

3. The towed positioning method based on extended Kalman filtering as described in claim 1, characterized in that, The CA motion model is used to predict the positioning vector of the towed trailer at the next time point, and the specific expression is as follows: ； in, and The coordinates of the previous time point of the towed vehicle. The angle of the towed vehicle at the previous point in time. Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and let T be the time interval between each frame of inertial navigation information. These are the absolute values ​​of the acceleration and angular velocity of the current tractor unit, respectively.

4. The towed positioning method based on extended Kalman filtering as described in claim 1, characterized in that, The CTRA model is used to predict the positioning vector of the towed trailer at the next time point. The specific expression is as follows: ； in, and The coordinates of the previous time point of the towed vehicle. The angle of the towed vehicle at the previous point in time. Let T be the absolute value of the forward velocity of the towed vehicle at the previous time point, and let T be the time interval between each frame of inertial navigation information. These are the absolute values ​​of the acceleration and angular velocity of the current tractor.

5. The towed positioning method based on extended Kalman filtering as described in claim 1, characterized in that, Also includes: Update function for the state variable Perform Jacobi operations to obtain the measurement matrix. , Combined with process excitation noise covariance matrix and prior estimation of covariance matrix Obtain the Kalman coefficients : Using Kalman coefficients and measurement matrix The posterior estimated covariance matrix is ​​obtained. Posterior estimation of covariance matrix Used to calculate the next Kalman coefficient. ； Where I is the identity matrix.

6. The towed positioning method based on extended Kalman filtering as described in claim 5, characterized in that, Using the Jacobian matrix and the measurement noise covariance matrix The prior estimated covariance matrix is ​​obtained. : ； in State transition matrix, This is the posterior covariance matrix calculated in the previous frame.

7. A trailer positioning system based on extended Kalman filtering, characterized in that, A towed positioning method based on extended Kalman filtering, as described in any one of claims 1 to 6, comprises: The initialization module initializes the positioning vector of the trailer using the positioning information of the tractor when the vehicle starts. The positioning prediction module uses inertial navigation information and combines it with a motion model to predict the positioning vector of the towed trailer at the next time point; The correction module uses the extended Kalman filter algorithm to correct the predicted positioning vector of the trailer using the positioning information of the tractor.

Images