Patent Publication Number: US-11648946-B2

Title: Intelligent vehicle platoon lane change performance evaluation method

Description:
TECHNICAL FIELD 
     The present invention relates to the technical field of road trials and testing of intelligent driving vehicles, and in particular, to an intelligent vehicle platoon lane change performance evaluation method. 
     BACKGROUND 
     With continuous increase of traffic mileage in our country, explosive growth of vehicle holdings, as well as complex road conditions and traffic conditions in our country, traffic accidents frequently occur, resulting in traffic interruptions, property losses and casualties. More than 90% of road traffic accidents are caused by a driver factor. In this context, an intelligent driving technology that assists a driver in driving operations, partially replaces the driver&#39;s operation in emergency situations, or even fully realizes unmanned driving has become the focus of research at home and abroad. 
     As an important constituent part of the intelligent driving technology, intelligent vehicle platoon uses technologies such as collaborative control and vehicle-to-vehicle communication to organize vehicles traveling in the same direction uniformly, so that vehicles travel in columns with a small fixed distance. When encountering an obstacle, the following vehicles can receive obstacle avoidance information sent by a leading vehicle, and change lanes to avoid roadblocks like the path of leading vehicle. As an advanced mode of road transportation, intelligent vehicle platoon driving plays an important role in improving driving safety, improving road efficiency, alleviating traffic congestion, and reducing fuel consumption. At present, the intelligent vehicle platoon technology is mainly applied to large trucks with heavy loads and large volumes for road transportation. If the platoon vehicles have a traffic accident, it is very likely to cause serious consequences such as multi-vehicle rear-end collisions, group deaths and group injuries. In order to realize safe and efficient intelligent vehicle platoon driving, the test and evaluation of the functions and performance of intelligent vehicle platoons has been highly valued by the government and research institutions. 
     At present, the existing patent literature studies the test and evaluation of the function and performance of intelligent vehicle platoon, and builds an evaluation system for the platoon effect of intelligent vehicles, involving aspects such as safety and traffic efficiency. In terms of safety evaluation, a hazard analysis and risk evaluation method is utilized to analyze the effectiveness and stability of the platoon, a stochastic model is utilized to evaluate the collision probability of platoon vehicles, or a car-following model such as an Intelligent Driver Model (IDM) and a Cooperative Adaptive Cruise Control (CACC) model is utilized to model the vehicle platoon process, and distance-to-collision time, inverse distance-to-collision time, total time exposed to dangerous traffic conditions, a collision risk index, and velocity covariance and other indexes are utilized for safety evaluation. In terms of traffic efficiency evaluation, the efficiency evaluation is carried out using the intersection traffic flow, exit turning rate, operating velocity, traffic flow and other indexes. There is no evaluation research on the intelligent vehicle platoon lane change performance, especially the lack of evaluation research on quantifying the performance indexes of platoon lane-changing. 
     On the whole, a quantitative evaluation method and an index evaluation system for the intelligent vehicle platoon lane change performance are not formed, which cannot meet the needs of comprehensive, accurate and reliable quantitative evaluation of vehicle platoon performance, and further breakthroughs are needed. 
     SUMMARY 
     Purpose of the Invention 
     To solve the problem of the lack of quantitative evaluation of platoon lane change performance at present, an intelligent vehicle platoon lane change performance evaluation method is proposed. Based on actual road tests, the method can measure vehicle motion state parameters in a high-precision and comprehensive manner, quantify and output multi-dimensional platoon lane change performance evaluation indexes, and achieve comprehensive, accurate and reliable scientific quantitative evaluation for platoon lane change performance. 
     Technical Solution 
     To achieve the purpose of the present invention, the technical solution adopted in the present invention is: an intelligent vehicle platoon lane change performance evaluation method. First, an intelligent vehicle platoon lane change performance test scenario is established; Secondly, a three-degree of freedom nonlinear dynamics model is established according to motion characteristics of intelligent vehicles in a platoon lane change process. Further, an improved adaptive unscented Kalman filter algorithm is utilized to perform filter estimation on state variables of positions and velocities of platoon vehicles. Finally, based on accurately recursive vehicle motion state parameters, evaluation indexes for platoon lane change performance are proposed and quantified, and an evaluation system for platoon lane change performance is constructed, thereby achieving comprehensive, accurate, and reliable scientific quantitative evaluation for intelligent vehicle platoon lane change performance. The method includes the following steps. 
     Step I, an intelligent vehicle platoon lane change performance test scenario is established. 
     Regarding the problem of lacking intelligent vehicle platoon lane change performance evaluation at present, the present invention establishes an intelligent vehicle platoon lane change performance test scenario. first, selecting a high-grade highway as a test site, and secondly, establishing an intelligent vehicle platoon lane change performance test scenario, described as follows: 
     a leading vehicle is located in the middle of a test road and drives in a straight line at a set velocity, and following vehicles follow the leading vehicle at a set interval; when the leading vehicle passes through a test starting point, it is started to synchronously collect motion state parameters of the leading vehicle and the following vehicles; then, when encountering an obstacle, the leading vehicle conducts a lane change operation, the following vehicles receive lane change information and change lanes with a similar motion trajectory; and when the leading vehicle reaches an end point, a test is completed. 
     In the present invention, the leading vehicle refers to a first vehicle in platoon driving, generally in a manual driving mode, and the following vehicles refer to intelligent vehicles behind the leading vehicle in platoon driving, generally in an automatic driving mode. 
     Step II, a dynamic model for intelligent vehicle platoon lane change is established. 
     In the process of platoon lane change performance test, it is necessary to accurately obtain the motion state parameters such as the position and velocity of the leading vehicle and the following vehicles at each moment. In order to meet the measurement requirements of complete information and high precision, it is necessary to establish a dynamic model that can accurately describe the intelligent vehicle platoon lane change movement characteristics. For the field of application of the present invention, for front-wheel steered four-wheel vehicles in platoon driving, the following assumptions are made: 
     (1) ignoring pitch and roll motions of each vehicle, and ignoring the influence of a vehicle suspension on a tire axle; 
     (2) assuming that two tires of a front axle of the vehicle have the same steering angle, cornering angle, longitudinal force and lateral force, and assuming that two tires of a rear axle of the vehicle have the same steering angle, cornering angle, longitudinal force and side force to force; and 
     (3) assuming that the direction of the front wheels of the vehicle is consistent with the current velocity direction of the vehicle. 
     According to the above requirements and assumptions, the present invention adopts the three-degree of freedom model to perform dynamics modeling on the leading vehicle in the platoon vehicles. The three-degree of freedom dynamics model of the vehicle is defined, considering the longitudinal, lateral and yaw motions. Point G is the center of mass of the vehicle. The left and right wheels of the front axle are combined into one point, located at point A. The left and right wheels of the rear axle are combined into one point, located at point B. The GX axis is the same as the vehicle&#39;s forward direction, the GY axis is determined by the right-hand screw rule, and the GZ axis is perpendicular to the vehicle motion plane and points to the ground. The dynamics model of the leading vehicle is described as: 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 v 
                                 . 
                               
                               x 
                             
                             = 
                             
                               
                                 
                                   v 
                                   y 
                                 
                                 · 
                                 
                                   r 
                                   l 
                                 
                               
                               + 
                               
                                 a 
                                 x 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               a 
                               x 
                             
                             = 
                             
                               
                                 2 
                                 M 
                               
                               [ 
                               
                                 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 - 
                                 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   F 
                                   xr 
                                 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 v 
                                 . 
                               
                               y 
                             
                             = 
                             
                               
                                 
                                   - 
                                   
                                     v 
                                     x 
                                   
                                 
                                 · 
                                 
                                   r 
                                   l 
                                 
                               
                               + 
                               
                                 a 
                                 y 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               a 
                               y 
                             
                             = 
                             
                               
                                 2 
                                 M 
                               
                               [ 
                               
                                 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   F 
                                   
                                     y 
                                     ⁢ 
                                     r 
                                   
                                 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 r 
                                 . 
                               
                               l 
                             
                             = 
                             
                               
                                 2 
                                 
                                   I 
                                   z 
                                 
                               
                               [ 
                               
                                 
                                   
                                     l 
                                     f 
                                   
                                   ⁢ 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   
                                     l 
                                     f 
                                   
                                   ⁢ 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 - 
                                 
                                   
                                     l 
                                     r 
                                   
                                   ⁢ 
                                   
                                     F 
                                     
                                       y 
                                       ⁢ 
                                       r 
                                     
                                   
                                 
                               
                               ] 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
     In formula (1), the superscript “ ” represents the differential; {dot over (v)} x  represents the differential of v x ; r l , v x , v y , a x , and a y  represent the yaw angular velocity, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the leading vehicle, respectively; M, δ, and I z  represent the mass of the leading vehicle, a steering angle of the front wheel, and the moment of inertia around the vertical axis of a body coordinate system, respectively, l f  and l r  represent the distances from the center of mass of the vehicle to the front and rear axles, respectively, F xf , F xr , F yf , and F yr  represent the longitudinal force and lateral force on the front and rear wheels, respectively. 
     For vehicles running on general roads, when the tire cornering angle is small, the lateral force on the front and rear tires is represented as:
 
 F   yf   =C   αf ·α f   ,F   yr   =C   αr ·α r   (2).
 
In formula (2), C αf  and C αr  represent the cornering stiffness of the front and rear tires, respectively, α f  and α r  represent the cornering angle of the front and rear tires, respectively, and α f =δ−(v y +l f r)/v x ,α r =(l r r−v y )/v x .
 
     In order to calculate the tire longitudinal force in formula (2), the tire model can be adopted to determine. Common tire models include empirical models, theoretical models, and adaptive models. In order to ensure the accuracy and real-time performance of vehicle motion parameter measurement, the present invention adopts a brush tire model, and the longitudinal force of the front and rear tires is represented as: 
     
       
         
           
             
               
                 
                   
                     
                       F 
                       xf 
                     
                     = 
                     
                       
                         
                           C 
                           xf 
                         
                         ⁢ 
                         
                           s 
                           xf 
                         
                       
                       + 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xf 
                               
                               ⁢ 
                               
                                 s 
                                 xf 
                               
                             
                             ) 
                           
                           3 
                         
                         
                           27 
                           ⁢ 
                           
                             
                               ( 
                               
                                 
                                   μ 
                                   l 
                                 
                                 ⁢ 
                                 
                                   F 
                                   zf 
                                 
                               
                               ) 
                             
                             2 
                           
                         
                       
                       - 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xf 
                               
                               ⁢ 
                               
                                 s 
                                 xf 
                               
                             
                             ) 
                           
                           2 
                         
                         
                           3 
                           ⁢ 
                           
                             μ 
                             l 
                           
                           ⁢ 
                           
                             F 
                             zf 
                           
                         
                       
                     
                   
                   ⁢ 
                   
 
                   
                     
                       F 
                       xr 
                     
                     = 
                     
                       
                         
                           C 
                           xr 
                         
                         ⁢ 
                         
                           s 
                           xr 
                         
                       
                       + 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xr 
                               
                               ⁢ 
                               
                                 s 
                                 
                                   xr 
                                     
                                 
                               
                             
                             ) 
                           
                           3 
                         
                         
                           27 
                           ⁢ 
                           
                             
                               ( 
                               
                                 
                                   μ 
                                   l 
                                 
                                 ⁢ 
                                 
                                   F 
                                   zr 
                                 
                               
                               ) 
                             
                             2 
                           
                         
                       
                       - 
                       
                         
                           
                             
                               ( 
                               
                                 
                                   C 
                                   xr 
                                 
                                 ⁢ 
                                 
                                   s 
                                   xr 
                                 
                               
                               ) 
                             
                             2 
                           
                           
                             3 
                             ⁢ 
                             
                               μ 
                               l 
                             
                             ⁢ 
                             
                               F 
                               zr 
                             
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   3 
                   ) 
                 
               
             
           
         
       
     
     In formula (3), C xf  and C xr  represent the longitudinal stiffness of the front and rear tires, respectively, μ l  is a road adhesion coefficient, considering that the test road for the platoon lane change test is generally a dry and smooth asphalt road. The present invention takes that μ l =0.75, the vertical loads F zf  and F zr  of the front and rear tires, the longitudinal slip rates s xf  and s xr  of the front and rear tires are calculated by the following formulas: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             F 
                             zf 
                           
                           = 
                           
                             
                               9.8 
                               
                                 Ml 
                                 r 
                               
                             
                             
                               ( 
                               
                                 
                                   l 
                                   f 
                                 
                                 + 
                                 
                                   l 
                                   r 
                                 
                               
                               ) 
                             
                           
                         
                       
                       
                         
                           
                             F 
                             zr 
                           
                           = 
                           
                             
                               9.8 
                               
                                 Ml 
                                 f 
                               
                             
                             
                               ( 
                               
                                 
                                   l 
                                   f 
                                 
                                 + 
                                 
                                   l 
                                   r 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   . 
                 
               
               
                 
                   ( 
                   4 
                   ) 
                 
               
             
           
         
       
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             s 
                             xf 
                           
                           = 
                           
                             
                               
                                 
                                   ω 
                                   f 
                                 
                                 ⁢ 
                                 
                                   R 
                                   tyre 
                                 
                               
                               - 
                               
                                 v 
                                 xf 
                               
                             
                             
                               max 
                               ⁡ 
                               ( 
                               
                                 
                                   
                                     ω 
                                     f 
                                   
                                   ⁢ 
                                   
                                     R 
                                     tyre 
                                   
                                 
                                 , 
                                 
                                   v 
                                   xf 
                                 
                               
                               ) 
                             
                           
                         
                       
                       
                         
                           
                             s 
                             xr 
                           
                           = 
                           
                             
                               
                                 
                                   ω 
                                   r 
                                 
                                 ⁢ 
                                 
                                   R 
                                   tyre 
                                 
                               
                               - 
                               
                                 v 
                                 xr 
                               
                             
                             
                               max 
                               ⁡ 
                               ( 
                               
                                 
                                   
                                     ω 
                                     r 
                                   
                                   ⁢ 
                                   
                                     R 
                                     tyre 
                                   
                                 
                                 , 
                                 
                                   v 
                                   xr 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   . 
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
           
         
       
     
     In the formulas, R tyre  is the tire radius, ω f  and ω r  represent the rotational angular velocity of the front and rear wheels, respectively, obtained by calculating a linear velocity measured by a wheel velocity sensor, v xf  and v xr  represent the velocities of the front and rear axles along the tire direction, respectively, and v xr =v x , and v xf =v x  cos δ+(v y +l f r l )sin δ. 
     At the same time, the longitudinal and lateral velocities v x  and v y  of the vehicle, and the east and north positions p e  and p n  satisfy the following relationship:
 
 {dot over (p)}   e   =v   e =√{square root over ( v   x   2   +v   y   2 )}sin β {dot over (p)}   n   =v   n =√{square root over ( v   x   2   +v   y   2 )}cos β  (6).
 
In formula (6), v e  and v n  represent the east and north velocities of the vehicle, respectively, and β represents the azimuth of the vehicle&#39;s motion direction relative to the due north direction, and the following relationship is satisfied:
 
     
       
         
           
             β 
             = 
             
               { 
               
                 
                   
                     
                       
                         
                           
                             
                               when 
                               ⁢ 
                                   
                               arc 
                               ⁢ 
                               
                                 tan 
                                 ⁡ 
                                 ( 
                                 
                                   
                                     v 
                                     e 
                                   
                                   / 
                                   
                                     v 
                                     n 
                                   
                                 
                                 ) 
                               
                             
                           
                           
                             
                               
                                 v 
                                 n 
                               
                               &gt; 
                               0 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           
                             
                               
                                 when 
                                 ⁢ 
                                     
                                 
                                   arctan 
                                   ⁡ 
                                   ( 
                                   
                                     
                                       v 
                                       e 
                                     
                                     / 
                                     
                                       v 
                                       n 
                                     
                                   
                                   ) 
                                 
                               
                               - 
                               π 
                             
                           
                           
                             
                               
                                 
                                   v 
                                   n 
                                 
                                 &lt; 
                                 0 
                               
                               , 
                               
                                 
                                   v 
                                   e 
                                 
                                 &lt; 
                                 0 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           
                             
                               
                                 when 
                                 ⁢ 
                                     
                                 arc 
                                 ⁢ 
                                 
                                   tan 
                                   ⁡ 
                                   ( 
                                   
                                     
                                       v 
                                       e 
                                     
                                     / 
                                     
                                       v 
                                       n 
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               π 
                             
                           
                           
                             
                               
                                 
                                   v 
                                   n 
                                 
                                 &lt; 
                                 0 
                               
                               , 
                               
                                 
                                   v 
                                   e 
                                 
                                 &gt; 
                                 0 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           
                             
                               when 
                               ⁢ 
                                   
                               π 
                               / 
                               2 
                             
                           
                           
                             
                               
                                 
                                   v 
                                   n 
                                 
                                 = 
                                 0 
                               
                               , 
                               
                                 
                                   v 
                                   e 
                                 
                                 &gt; 
                                 0 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           
                             
                               when 
                                   
                               - 
                               
                                 π 
                                 / 
                                 2 
                               
                             
                           
                           
                             
                               
                                 
                                   v 
                                   n 
                                 
                                 = 
                                 0 
                               
                               , 
                               
                                 
                                   v 
                                   e 
                                 
                                 &lt; 
                                 
                                   0 
                                   ⁢ 
                                       
                                   and 
                                 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           
                             0 
                           
                           
                             
                               
                                 v 
                                 n 
                               
                               = 
                               
                                 
                                   v 
                                   e 
                                 
                                 = 
                                 0 
                               
                             
                           
                         
                       
                     
                   
                 
                 . 
               
             
           
         
       
     
     For the intelligent vehicle platoon lane change process, taking a system state vector X=[p e  p n  v x  v y  r l ] T , the superscript T of the matrix in the present invention represents the transposition of the matrix, and T represents the discrete period. According to the dynamics model described by formula (1), the system state equation is established:
 
 X=f ( X,U,W ,γ)  (7).
 
     In formula (7), f( ) is a 5-dimensional vector function, W is the zero-mean system Gaussian white noise, γ is the zero-mean Gaussian white noise corresponding to the external, input of the system, U is an external input vector of the system and U=[δ F xf  F sr ] T , where a steering angle of the front wheel is δ=ε l /ρ l , the steering wheel angle ε l  is obtained by a body CAN bus, ρ l  is a transmission ratio of the steering system, and in the present invention, ρ l =10, the tire longitudinal forces F xf  and F sr  are determined by the brush tire model. 
     Step III, vehicle motion state estimation based on an improved unscented Kalman filter algorithm 
     The present invention adopts a UKF algorithm to perform recursive estimation on the vehicle motion state parameters. 
     The observation equation of the system is represented as:
 
 Z ( t )= h ( x ( t ), V ( t ))  (8).
 
     In formula (8), h is the observation equation, t is the time, and the system observation vector Z=[p eg  p ng  v x_m  ω z_m ] T , where p eg  and p ng  represent the observed values of the east and north positions of the vehicle, respectively, and are obtained by converting the latitude and longitude coordinates collected by a centimeter-level high-precision satellite positioning system (such as GPS and Beidou); and v x_m  and ω z_m  represent the longitudinal forward velocity and yaw angular velocity of the vehicle, respectively, and are measured by an inertial measurement unit. 
     In the actual filtering recursion process, it is necessary to adopt a discretized filtering model. To this end, discretization is performed on formulas (7) and (8), and the system state equation and observation equation after discretization are respectively: 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               X 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               f 
                               ⁡ 
                               ( 
                               
                                 
                                   X 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   U 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   W 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   γ 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               ) 
                             
                           
                         
                       
                       
                         
                           
                             
                               Z 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               h 
                               ⁡ 
                               ( 
                               
                                 
                                   X 
                                   ⁡ 
                                   ( 
                                   k 
                                   ) 
                                 
                                 , 
                                 
                                   V 
                                   ⁡ 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
           
         
       
     
     In formula (9), k is the discretization time, and the system process noise W=[w 1  w 2  w 3  w 4  w 5 ] T , where w 1 , w 2 , w 3 , w 4 , and w 5  represent five system Gaussian white noise components, respectively, a Gaussian white noise covariance matrix corresponding to W(k−1) is Q(k−1)=diag[σ w     1     2  σ w     2     2  σ w     3     2  σ w     4     2  σ w     5     2 ], where σ w     1     2 , σ w     2     2 , σ w     3     2 , σ w     4     2 , and σ w     5     2  represent the variances corresponding to Gaussian white noises w 1 , w 2 , w 3 , w 4 , and w 5 , respectively. The system observation noise V=[v 1  v 2  v 3  v 4 ] T , where v 1 , v 2 , v 3 , and v 4  represent four system Gaussian white noise components, respectively, a measured white Gaussian noise covariance matrix corresponding to V(k) is R(k)=diag[σ v     1     2  σ v     2     2  σ v     3     2  σ v     4     2 ], where σ v     1     2 , σ v     2     2 , σ v     3     2 , and σ v     4     2 , respectively represent the variances corresponding to the Gaussian white noises v 1 , v 2 , v 3 , and v 4 , and are determined according to the statistical characteristics of the measurement noise of the sensor&#39;s position, velocity, and yaw angular velocity. The external input noise of the system is γ=[w δ  w F     xf    w F     sr   ] T , where w δ , w F     d   , and w F     sr    represent the zero-mean Gaussian white noise components corresponding to δ, F xf , and F sr , respectively, and these white noises are implied in the three external inputs of the system of the system state function f. The system state function is: 
     
       
         
           
             	 
             
               
                 
                   f 
                   ⁡ 
                   ( 
                   
                     X 
                     , 
                     U 
                     , 
                     W 
                     , 
                     γ 
                   
                   ) 
                 
                 = 
                 
                   [ 
                   
                     
                       
                         
                           
                             f 
                             1 
                           
                           ( 
                           
                             
                               X 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               U 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               W 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               γ 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           
                             f 
                             2 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             3 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             4 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             5 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   ] 
                 
               
               ⁢ 
               
 
               	 
               
                 where 
                 , 
               
             
           
         
       
       
         
           
             
               
                 f 
                 1 
               
               ( 
               
                 
                   X 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   U 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   W 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   γ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               ) 
             
             = 
             
               
                 
                   p 
                   e 
                 
                 ( 
                 
                   k 
                   - 
                   1 
                 
                 ) 
               
               + 
               
                 
                   
                     
                       
                         v 
                         x 
                         2 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     + 
                     
                       
                         v 
                         y 
                         2 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                 
                 ⁢ 
                 sin 
                 ⁢ 
                 
                   β 
                   · 
                   T 
                 
               
               + 
               
                 w 
                 1 
               
             
           
         
       
       
         
           
             
               
                 f 
                 2 
               
               ( 
               
                 
                   X 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   U 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   W 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   γ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               ) 
             
             = 
             
               
                 
                   p 
                   n 
                 
                 ( 
                 
                   k 
                   - 
                   1 
                 
                 ) 
               
               + 
               
                 
                   
                     
                       
                         v 
                         x 
                         2 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     + 
                     
                       
                         v 
                         y 
                         2 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                 
                 ⁢ 
                 cos 
                 ⁢ 
                 
                   β 
                   · 
                   T 
                 
               
               + 
               
                 w 
                 2 
               
             
           
         
       
       
         
           
             
               
                 f 
                 3 
               
               ( 
               
                 
                   X 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   U 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   W 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   γ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               ) 
             
             = 
             
               
                 
                   v 
                   x 
                 
                 ( 
                 
                   k 
                   - 
                   1 
                 
                 ) 
               
               + 
               
                 
                   T 
                   M 
                 
                 [ 
                 
                   
                     
                       
                         Mv 
                         y 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     ⁢ 
                     
                       
                         r 
                         l 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   + 
                   
                     2 
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         f 
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             v 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         + 
                         
                           
                             l 
                             f 
                           
                           ⁢ 
                           
                             
                               r 
                               l 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                       
                         
                           v 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       δ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                 
                 ] 
               
               + 
               
                 
                   
                     2 
                     ⁢ 
                     T 
                   
                   M 
                 
                 [ 
                 
                   
                     
                       F 
                       xf 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       F 
                       xr 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                 
                 ] 
               
               + 
               
                 w 
                 3 
               
             
           
         
       
       
         
           
             
               
                 f 
                 4 
               
               ( 
               
                 
                   X 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   U 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   W 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   γ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               ) 
             
             = 
             
               
                 
                   v 
                   y 
                 
                 ( 
                 
                   k 
                   - 
                   1 
                 
                 ) 
               
               + 
               
                 
                   T 
                   M 
                 
                 [ 
                 
                   
                     
                       - 
                       
                         
                           Mv 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       
                         r 
                         l 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   + 
                   
                     2 
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         f 
                       
                     
                     ⁢ 
                     
                       δ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   - 
                   
                     2 
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         f 
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             v 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         + 
                         
                           
                             l 
                             f 
                           
                           ⁢ 
                           
                             
                               r 
                               l 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                       
                         
                           v 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                   + 
                   
                     2 
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         r 
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             l 
                             r 
                           
                           ⁢ 
                           
                             
                               r 
                               l 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                         - 
                         
                           
                             v 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       
                         
                           v 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                 
                 ] 
               
               + 
               
                 
                   
                     2 
                     ⁢ 
                     T 
                   
                   M 
                 
                 ⁢ 
                 
                   
                     F 
                     xf 
                   
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 ⁢ 
                 
                   δ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               + 
               
                 w 
                 4 
               
             
           
         
       
       
         
           
             
               
                 f 
                 5 
               
               ( 
               
                 
                   X 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   U 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
                   W 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 , 
                 
 
                 
                   γ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               ) 
             
             = 
             
               
                 
                   r 
                   l 
                 
                 ( 
                 
                   k 
                   - 
                   1 
                 
                 ) 
               
               + 
               
                 
                   T 
                   
                     I 
                     z 
                   
                 
                 [ 
                 
                   
                     2 
                     ⁢ 
                     
                       l 
                       f 
                     
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         f 
                       
                     
                     ⁢ 
                     
                       δ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   - 
                   
                     2 
                     ⁢ 
                     
                       l 
                       f 
                     
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         f 
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             v 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         + 
                         
                           
                             l 
                             f 
                           
                           ⁢ 
                           
                             
                               r 
                               l 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                       
                         
                           v 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                   - 
                   
                     2 
                     ⁢ 
                     
                       l 
                       r 
                     
                     ⁢ 
                     
                       C 
                       
                         α 
                         ⁢ 
                         r 
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             l 
                             r 
                           
                           ⁢ 
                           
                             
                               r 
                               l 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                         - 
                         
                           
                             v 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       
                         
                           v 
                           x 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                 
                 ] 
               
               + 
               
                 
                   
                     2 
                     ⁢ 
                     
                       l 
                       f 
                     
                     ⁢ 
                     T 
                   
                   
                     I 
                     z 
                   
                 
                 ⁢ 
                 
                   
                     F 
                     xf 
                   
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
                 ⁢ 
                 
                   δ 
                   ⁡ 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
               + 
               
                 
                   w 
                   5 
                 
                 . 
               
             
           
         
       
     
     Secondly, according to the system state equation and observation equation described by formula (9), the unscented Kalman filtering recursive process is established, and the filtering recursion is performed by using time update and measurement update: 
     (1) initializing the input variable and performing parameter calculation 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 X 
                                 ^ 
                               
                               0 
                             
                             = 
                             
                               E 
                               [ 
                               
                                 X 
                                 0 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               P 
                               0 
                             
                             = 
                             
                               E 
                               [ 
                               
                                 
                                   ( 
                                   
                                     
                                       X 
                                       0 
                                     
                                     - 
                                     
                                       
                                         X 
                                         ^ 
                                       
                                       0 
                                     
                                   
                                   ) 
                                 
                                 · 
                                 
                                   
                                     ( 
                                     
                                       
                                         X 
                                         0 
                                       
                                       - 
                                       
                                         
                                           X 
                                           ^ 
                                         
                                         0 
                                       
                                     
                                     ) 
                                   
                                   T 
                                 
                               
                               ] 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
     In formula (10), P 0  is an initial error variance matrix, the variable with the superscript symbol {circumflex over ( )} represents a filtering estimated value of the variable, and {circumflex over (X)} 0  is a filtering estimated value representing the initial value X 0  of the input variable. 
     (2) State Estimation 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   ξ 
                                   i 
                                 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               = 
                               
                                 
                                   X 
                                   ^ 
                                 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               i 
                               = 
                               0 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                         ) 
                                       
                                     
                                     = 
                                     
                                       
                                         
                                           X 
                                           ^ 
                                         
                                         ⁢ 
                                         
                                           ( 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                           ) 
                                         
                                       
                                       + 
                                       
                                         
                                           ( 
                                           
                                             
                                               
                                                 ( 
                                                 
                                                   
                                                     x 
                                                     dim 
                                                   
                                                   + 
                                                   λ 
                                                 
                                                 ) 
                                               
                                               ⁢ 
                                               
                                                 P 
                                                 ⁡ 
                                                 ( 
                                                 
                                                   k 
                                                   - 
                                                   1 
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ) 
                                         
                                         i 
                                       
                                     
                                   
                                   , 
                                 
                               
                             
                             
                               
                                 
                                   
                                     i 
                                     = 
                                     1 
                                   
                                   , 
                                   2 
                                   , 
                                   … 
                                       
                                   , 
                                   
                                     x 
                                     dim 
                                   
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                         ) 
                                       
                                     
                                     = 
                                     
                                       
                                         
                                           X 
                                           ^ 
                                         
                                         ⁢ 
                                         
                                           ( 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                           ) 
                                         
                                       
                                       - 
                                       
                                         
                                           ( 
                                           
                                             
                                               
                                                 ( 
                                                 
                                                   
                                                     x 
                                                     dim 
                                                   
                                                   + 
                                                   λ 
                                                 
                                                 ) 
                                               
                                               ⁢ 
                                               
                                                 P 
                                                 ⁡ 
                                                 ( 
                                                 
                                                   k 
                                                   - 
                                                   1 
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ) 
                                         
                                         i 
                                       
                                     
                                   
                                   , 
                                 
                               
                             
                             
                               
                                 
                                   
                                     i 
                                     = 
                                     
                                       
                                         x 
                                         dim 
                                       
                                       + 
                                       1 
                                     
                                   
                                   , 
                                   
                                     
                                       x 
                                       dim 
                                     
                                     + 
                                     2 
                                   
                                   , 
                                   … 
                                       
                                   , 
                                   
                                     2 
                                     ⁢ 
                                     
                                       x 
                                       dim 
                                     
                                   
                                 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     In formula ξ i (k−1) is the Sigma point, (√{square root over ((x dim +λ)P(k−1))}) i  is the i-th column of a square root of a weighted covariance matrix, and x dim  is the number of dimensions of the state vector. The present invention takes x dim =5. 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               ω 
                               0 
                               
                                 ( 
                                 e 
                                 ) 
                               
                             
                             = 
                             
                               λ 
                               
                                 
                                   x 
                                   dim 
                                 
                                 + 
                                 λ 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               0 
                               
                                 ( 
                                 c 
                                 ) 
                               
                             
                             = 
                             
                               
                                 ω 
                                 0 
                                 
                                   ( 
                                   e 
                                   ) 
                                 
                               
                               + 
                               
                                 ( 
                                 
                                   1 
                                   - 
                                   
                                     α 
                                     2 
                                   
                                   + 
                                   ε 
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               i 
                               
                                 ( 
                                 c 
                                 ) 
                               
                             
                             = 
                             
                               
                                 ω 
                                 i 
                                 
                                   ( 
                                   e 
                                   ) 
                                 
                               
                               = 
                               
                                 1 
                                 
                                   2 
                                   ⁢ 
                                   
                                     ( 
                                     
                                       
                                         x 
                                         dim 
                                       
                                       + 
                                       λ 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
           
         
       
     
     In formula (12), λ is a distance parameter, and λ=x dim (α 2 −1), α is a first scale factor, ε is a second scale factor, taking ε=2, ω i   (c)  and ω i   (e)  represent the weight coefficients of the mean and variance, respectively. 
     (3) Time Update
 
ξ i ( k,k− 1)= f [ξ i ( k,k− 1)], i =0,1, . . . ,2 x   dim   (13).
 
 {circumflex over (X)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) ξ i ( k,k− 1)  (14).
 
 P ( k,k− 1)=Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)]·[ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)] T   +Q ( k )  (15).
 
     In the formulas, {circumflex over (X)}(k−1) is the optimal estimate at time k−1, and P(k,k−1) is a one-step prediction error variance matrix at time k. 
     (4) Observation Update
 
χ i ( k,k− 1)= h [ξ i ( k,k− 1)]  (16).
 
 {circumflex over (Z)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) χ i ( k,k− 1)  (17).
 
 P   {tilde over (Z)} =Σ i=0   2x     dim   ω i   (c) [χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   +R ( k )  (18).
 
 P   XZ =Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   (19).
 
     In the formulas, χ i (k,k−1) represents the value of the observation equation transformed from the Sigma point set, {circumflex over (Z)}(k,k−1) represents the one-step predicted observation value at time k recursively from time k−1, P {tilde over (Z)}  is the predicted value covariance, and P XZ  is the covariance of the state value and the measured value. 
     (5) Filter Update
 
 K ( k )= P   XZ   ·P   {tilde over (Z)}   −1   (20).
 
 {circumflex over (X)} ( k )= {circumflex over (X)} ( k,k− 1)+ K ( k )·[ Z ( k )− {circumflex over (Z)} ( k,k− 1)]  (21).
 
 P ( k )= P ( k,k− 1)− K ( k )· P   {tilde over (Z)}   ·K ( k ) T   (22).
 
     In the formulas, K(k) is a filter gain matrix, {circumflex over (X)}(k) is a state quantity estimated value, and P(k) is an estimated error variance matrix. 
     In the actual test process, there are differences in the motion characteristics of different platoon vehicles, so that the noise statistical characteristics of the system cannot be consistent. When dealing with nonlinear problems, although the UKF algorithm has good parameter estimation accuracy, it still has certain shortcomings, that is, the prior statistical characteristics of system observation noise and process noise need to be accurately known. 
     Regarding the above problems, the present invention combines an AKF filter with an UKF filter, and makes targeted improvements to improve the accuracy of the system state estimation. Specifically: 
     (1) In order to reduce the influence of the system observation noise and process noise uncertainty on the filtering accuracy, a noise estimator with time-varying performance is introduced. (2) In order to improve the accuracy of the one-step prediction error variance matrix, and formulas (14-15) and formulas (17-18) are replaced with formulas (23-26):
 
 {circumflex over (X)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) ξ i ( k,k− 1)+ {circumflex over (q)} ( k− 1)  (23).
 
 P ( k,k− 1)=τ f Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)][ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)] T   +{circumflex over (Q)} ( k )  (24).
 
 {circumflex over (Z)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) χ i ( k,k− 1)+ {circumflex over (r)} ( k )  (25).
 
 P   {tilde over (Z)} =Σ i=0   2x     dim   ω i   (c) [χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   +{circumflex over (R)} ( k )  (26).
 
     where the means and variance matrixes of the observation noise and system noise {circumflex over (r)}(k), {circumflex over (R)}(k), {circumflex over (q)}(k), {circumflex over (Q)}(k), and forgetting factors are: 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 q 
                                 ^ 
                               
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               
                                 
                                   
                                     δ 
                                     d 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                                 [ 
                                 
                                   
                                     
                                       X 
                                       ^ 
                                     
                                     ( 
                                     k 
                                     ) 
                                   
                                   - 
                                   
                                     
                                       ∑ 
                                       
                                         i 
                                         = 
                                         0 
                                       
                                       
                                         2 
                                         ⁢ 
                                         
                                           x 
                                           dim 
                                         
                                       
                                     
                                     
                                       
                                         ω 
                                         i 
                                         
                                           ( 
                                           e 
                                           ) 
                                         
                                       
                                       ⁢ 
                                       
                                         
                                           ξ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                                 ] 
                               
                               + 
                               
                                 
                                   [ 
                                   
                                     1 
                                     - 
                                     
                                       
                                         δ 
                                         d 
                                       
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                   ] 
                                 
                                 ⁢ 
                                 
                                   
                                     q 
                                     ^ 
                                   
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 Q 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             = 
                             
                               
                                 δ 
                                 d 
                               
                               ⁢ 
                               
                                 ( 
                                 k 
                                 ) 
                               
                               ⁢ 
                               
                                 { 
                                 
                                   K 
                                   ⁢ 
                                   
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                     [ 
                                     
                                       
                                         Z 
                                         ⁢ 
                                         
                                           ( 
                                           k 
                                           ) 
                                         
                                       
                                       - 
                                       
                                         
                                           ∑ 
                                           
                                             i 
                                             = 
                                             0 
                                           
                                           
                                             2 
                                             ⁢ 
                                             
                                               x 
                                               dim 
                                             
                                           
                                         
                                         
                                           
                                             ω 
                                             i 
                                             
                                               ( 
                                               e 
                                               ) 
                                             
                                           
                                           ⁢ 
                                           
                                             χ 
                                             i 
                                           
                                           ⁢ 
                                           
                                             ( 
                                             
                                               k 
                                               , 
                                               
                                                 k 
                                                 - 
                                                 1 
                                               
                                             
                                             ) 
                                           
                                         
                                       
                                     
                                     ] 
                                   
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             [ 
                             
                               
                                 Z 
                                 ⁡ 
                                 ( 
                                 k 
                                 ) 
                               
                               - 
                               
                                 
                                   
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           
                                             χ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                     ] 
                                   
                                   T 
                                 
                                 ⁢ 
                                 
                                   
                                     K 
                                     T 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                       
                       
                         
                           
                             
                               P 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             - 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   0 
                                 
                                 
                                   2 
                                   ⁢ 
                                   
                                     x 
                                     dim 
                                   
                                 
                               
                               
                                 
                                   ω 
                                   i 
                                   
                                     ( 
                                     c 
                                     ) 
                                   
                                 
                                 [ 
                                 
                                   
                                     
                                       ξ 
                                       i 
                                     
                                     ( 
                                     
                                       k 
                                       , 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                   - 
                                   
                                     
                                       X 
                                       ^ 
                                     
                                     ( 
                                     
                                       k 
                                       , 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                 
                                 ] 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   [ 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ( 
                                       
                                         k 
                                         , 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                       
                                       ) 
                                     
                                     - 
                                     
                                       
                                         X 
                                         ^ 
                                       
                                       ( 
                                       
                                         k 
                                         , 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                       
                                       ) 
                                     
                                   
                                   ] 
                                 
                                 T 
                               
                               } 
                             
                             + 
                             
                               
                                 [ 
                                 
                                   1 
                                   - 
                                   
                                     
                                       δ 
                                       d 
                                     
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                 ] 
                               
                               ⁢ 
                               
                                 
                                   Q 
                                   ^ 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 r 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             = 
                             
                               
                                 
                                   [ 
                                   
                                     1 
                                     - 
                                     
                                       
                                         δ 
                                         d 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         k 
                                         ) 
                                       
                                     
                                   
                                   ] 
                                 
                                 ⁢ 
                                 
                                   r 
                                   ^ 
                                 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   δ 
                                   d 
                                 
                                 ⁢ 
                                 
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                   [ 
                                   
                                     
                                       Z 
                                       ⁢ 
                                       
                                         ( 
                                         k 
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           χ 
                                           i 
                                         
                                         ⁢ 
                                         
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 R 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             = 
                             
                               
                                 δ 
                                 d 
                               
                               ⁢ 
                               
                                 ( 
                                 k 
                                 ) 
                               
                               ⁢ 
                               
                                 { 
                                 
                                   [ 
                                   
                                     
                                       Z 
                                       ⁢ 
                                       
                                         ( 
                                         k 
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           
                                             χ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 [ 
                                 
                                   
                                     Z 
                                     ⁡ 
                                     ( 
                                     k 
                                     ) 
                                   
                                   - 
                                   
                                     
                                       ∑ 
                                       
                                         i 
                                         = 
                                         0 
                                       
                                       
                                         2 
                                         ⁢ 
                                         
                                           x 
                                           dim 
                                         
                                       
                                     
                                     
                                       
                                         ω 
                                         i 
                                         
                                           ( 
                                           e 
                                           ) 
                                         
                                       
                                       ⁢ 
                                       
                                         
                                           χ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                                 ] 
                               
                               T 
                             
                             - 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   0 
                                 
                                 
                                   2 
                                   ⁢ 
                                   
                                     x 
                                     dim 
                                   
                                 
                               
                               
                                 
                                   ω 
                                   i 
                                   
                                     ( 
                                     c 
                                     ) 
                                   
                                 
                                 [ 
                                 
                                   
                                     
                                       χ 
                                       i 
                                     
                                     ( 
                                     
                                       k 
                                       , 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                   - 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         Z 
                                         ^ 
                                       
                                       ( 
                                       
                                         k 
                                         , 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                       
                                       ) 
                                     
                                     ] 
                                   
                                   [ 
                                   
                                     
                                       
                                         χ 
                                         i 
                                       
                                       ( 
                                       
                                         k 
                                         , 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                       
                                       ) 
                                     
                                     - 
                                     
                                       
                                         Z 
                                         ^ 
                                       
                                       ( 
                                       
                                         k 
                                         , 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                       
                                       ) 
                                     
                                   
                                   ] 
                                 
                                 T 
                               
                               } 
                             
                             + 
                             
                               
                                 [ 
                                 
                                   1 
                                   - 
                                   
                                     
                                       δ 
                                       d 
                                     
                                     ⁢ 
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                 
                                 ] 
                               
                               ⁢ 
                               
                                 R 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   27 
                   ) 
                 
               
             
           
         
       
       
         
           
             
               
                 
                   
                     τ 
                     f 
                   
                   = 
                   
                     max 
                     ⁢ 
                     
                       
                         { 
                         
                           1 
                           , 
                           
                             
                               trace 
                               ⁡ 
                               ( 
                               
                                 
                                   P 
                                   
                                     Z 
                                     ~ 
                                   
                                 
                                 ⁢ 
                                 
                                   P 
                                   
                                     Z 
                                     ~ 
                                   
                                   T 
                                 
                               
                               ) 
                             
                             
                               trace 
                               ⁢ 
                               
                                 { 
                                 
                                   
                                     
                                       [ 
                                       
                                         
                                           
                                             ϰ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                         - 
                                         
                                           
                                             Z 
                                             ^ 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                       ] 
                                     
                                     [ 
                                     
                                       
                                         
                                           ϰ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                       - 
                                       
                                         
                                           Z 
                                           ^ 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                     ] 
                                   
                                   T 
                                 
                                 } 
                               
                             
                           
                         
                         } 
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
           
         
       
     
     In the formulas, δ d (k) represents an adaptive weighting parameter at time k, and δ d (k)=1/k, τ f  is the forgetting factor. 
     In conclusion, formulas (10-13), formula (16), and formulas (19-28) constitute the improved adaptive unscented Kalman filter algorithm. 
     Finally, after the above-mentioned improved filtering recursive calculation, the output state estimate {circumflex over (X)}(k) is the east position, north position, longitudinal velocity, lateral velocity and yaw angular velocity information of the leading vehicle at time k. 
     Similarly, the dynamics model of each following vehicle is established by using the method described in step II and step III, and filtering recursion is performed, to output the motion state parameters of each following vehicle. 
     For the intelligent vehicles in the platoon, the output longitudinal velocity information is V x_j ={v x_j (0), v x_j  (1), . . . , v x_j (k)}, the lateral velocity information is V y_j ={v y_j (0), v y_j  (1), . . . , v y_j (k)}, the yaw angular velocity information is R j ={r l_j (0), r l_j  (1), . . . , r l_j (k)}, and the position information is P j ={(p e_j (0), p n_j (0)), (p e_j (1), p n_j (1)), . . . , (p e_j (k), p n_j (k)), (j=1, 2, 3 . . . , a). 
     The subscript j represents the j-th platoon vehicle in the platoon, for example, j=1 represents that the vehicle is the leading vehicle, and j=2 represents that the vehicle is the first following vehicle, a is the total number of platoon vehicles, p e_j (k), p n_j (k), v x_j (k), v y_j (k), and r l_j (k) represent the east position, the north position, the longitudinal velocity, the lateral velocity and the yaw angular velocity of the j-th intelligent vehicle at time k, respectively. 
     Step IV, the evaluation indexes for platoon lane change performance are proposed and quantified. 
     Regarding the following vehicles in the platoon driving, first, the present invention proposes multi-dimensional platoon lane change performance evaluation indexes: lane change yaw stability, velocity consistency, safety distance margin, and average vehicle clearance. Secondly, based on the position and velocity motion state parameters of the leading vehicle and the following vehicles output in step II, the above performance evaluation indexes are quantified. Specifically: 
     (1) Lane Change Yaw Stability 
     
       
         
           
             
               
                 
                   
                     σ 
                     j 
                   
                   = 
                   
                     
                       
                         
                           1 
                           s 
                         
                         ⁢ 
                         
                           
                             ∑ 
                             
                               k 
                               = 
                               1 
                             
                             s 
                           
                           
                             
                               [ 
                               
                                 
                                   
                                     r 
                                     j 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                                 - 
                                 
                                   
                                     r 
                                     
                                       E 
                                       ⁢ 
                                       _ 
                                       ⁢ 
                                       j 
                                     
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               ] 
                             
                             2 
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   29 
                   ) 
                 
               
             
           
         
       
     
     In formula (29), σ j  represents the lane change yaw stability of the j-th following vehicle, which reflects the aggressiveness of the following vehicle to change lanes, r E_j (k) is an expected value of the yaw angular velocity at time k, and r E_j (k)=√{square root over (v x_j   2 (k)+v y_j   2 (k))}/R road , the unit is rad/s, R road  is the radius of curvature of the road, calculated from the position and velocity information output in step III, and s is the number of sampling points in the test process. 
     (2) Velocity Consistency 
     
       
         
           
             
               
                 
                   
                     τ 
                     platoon 
                   
                   = 
                   
                     
                       1 
                       
                         j 
                         - 
                         1 
                       
                     
                     ⁢ 
                     
                       ∑ 
                       
                         
                           τ 
                           j 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
           
         
       
     
     In formula (30), τ platoon  represents the velocity consistency in the platoon lane change process, τ j  represents the average relative velocity of the j-th following vehicle and the vehicle ahead in the same direction, and 
                 τ   j     =       1   s     ⁢       ∑     k   =   1     s       [         v     res   ⁢   _   ⁢   j       (   k   )     -       v       res   ⁢   _   ⁢   j     -   1       (   k   )       ]           ,         
v res_j (k) is the combined velocity of the j-th following vehicle at time k, in m/s.
 
     (3) Safety Distance Margin 
                     L   j     =             [         p     e   ⁢   _   ⁢   j       (   k   )     -       p       e   ⁢   _   ⁢   j     -   1       (   k   )       ]     2     +       [         p     n   ⁢   _   ⁢   j       (   k   )     -       p       n   ⁢   _   ⁢   j     -   1       (   k   )       ]     2         -       L   v     .               (   31   )               
In formula (31), L j  represents the safety distance margin between the j-th following vehicle and the vehicle ahead in the same direction, and L v  is the body length of the following vehicle, and the unit is m.
 
     (4) Average Vehicle Clearance 
     
       
         
           
             
               
                 
                   
                     L 
                     avg 
                   
                   = 
                   
                     
                       1 
                       
                         j 
                         - 
                         1 
                       
                     
                     ⁢ 
                     
                       ∑ 
                       
                         
                           L 
                           j 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   32 
                   ) 
                 
               
             
           
         
       
     
     In formula (32), L avg  is the average value of the platoon vehicle clearance, and the unit is m. 
     In evaluation of the intelligent vehicle platoon lane change performance, first, in the test scenario constructed in “step I”, “step II” and “step III” are utilized to perform filtering recursion on each motion state parameter of the leading vehicle and the following vehicles. Secondly, according to “step IV”, the quantitative values of the performance evaluation indexes are calculated. Finally, the lane change performance of the platoon vehicles is analyzed by quantitative evaluation, achieving comprehensive, accurate, and reliable scientific quantitative evaluation for intelligent vehicle platoon lane change performance. 
     Beneficial effects: compared with the prior art, the technical solution of the present invention has the following beneficial technical effects: 
     1) Through the test method proposed by the present invention and the constructed platoon lane change performance evaluation system, the current problem of lack of quantitative evaluation of platoon lane change performance is solved, and a comprehensive and reliable scientific quantitative evaluation of intelligent vehicle platoon lane change performance is achieved. 
     2) Compared with the evaluation method of virtual simulation, the platoon lane change performance evaluation method based on an actual road test proposed by the present invention can better ensure the safety and reliability of the platoon lane change of the intelligent vehicles, and is more accurate and persuasive. 
     3) The measurement accuracy of vehicle motion state parameters and platoon lane change performance indexes is high, and the position measurement accuracy is less than or equal to 0.0295 m (RMS), and the accuracy can meet the high-precision test requirements of platoon lane change performance. 
     4) The environmental adaptability is high, under complex conditions such as wet and slippery roads, nights and rainy days, the method proposed by the present invention can complete the platoon lane change performance test with the same accuracy as the dry road surface. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG.  1    is a schematic diagram of a technical route of the present invention; 
         FIG.  2    is a schematic diagram of a dynamics model of platoon vehicles; 
         FIG.  3    is a schematic diagram of motion trajectories of a leading vehicle and following vehicles in an intelligent vehicle platoon lane change performance test; and 
         FIG.  4    is a graph showing the change of the safety distance margin of the following vehicles over time in an intelligent vehicle platoon lane change performance test. 
     
    
    
     DETAILED DESCRIPTION 
     The technical solution of the present invention is further described below with reference to the accompanying drawings and embodiments. 
     With the continuous increase of traffic mileage in our country and the explosive growth of vehicle holdings, together with the complex road conditions and traffic conditions in our country, traffic accidents frequently occur, resulting in traffic interruptions, property losses and casualties. More than 90% of road traffic accidents are caused by drivers. In this context, the intelligent driving technology that assists the driver in driving operations, partially replaces the driver&#39;s operation in emergency situations, or even fully realizes unmanned driving has become the focus of research at home and abroad. 
     As an important part of intelligent driving technology, intelligent vehicle platoon uses technologies such as collaborative control and vehicle-to-vehicle communication to organize vehicles traveling in the same direction uniformly, so that vehicles travel in columns with a small fixed distance. When encountering an obstacle, the following vehicle can receive lane change information sent by a leading vehicle, and change lanes to avoid roadblocks like the path of leading vehicle. As an advanced mode of road transportation, intelligent vehicle platoon driving plays an important role in improving driving safety, improving road efficiency, alleviating traffic congestion, and reducing fuel consumption. At present, the intelligent vehicle platoon technology is mainly applied to large trucks with heavy loads and large volumes for road transportation. If the platoon vehicles have a traffic accident, it is very likely to cause serious consequences such as multi-vehicle rear-end collisions, group deaths and group injuries. In order to realize safe and efficient intelligent vehicle platoon driving, the test and evaluation of the functions and performance of intelligent vehicle platoons has been highly valued by the government and research institutions. 
     To this end, our country has formulated a series of standards and regulations to test and evaluate the performance of intelligent vehicle platoons. The National Ministry of Industry and Information Technology, the Ministry of Public Security, and the Ministry of Communications jointly issue the “AUTONOMOUS DRIVING FUNCTION TEST REGULATIONS FOR INTELLIGENT CONNECTED VEHICLES”, which clearly stipulates the test scenarios and test methods for intelligent vehicle platoons. Lane changing is one of the most common and complex actions in platoon driving, and the performance thereof is particularly important for realizing safe and efficient vehicle platoon. However, this standard only conducts functional tests for platoon driving and platoon switching in intelligent vehicle platoons, and does not relate to the test and evaluation of platoon lane-changing performance. The Guangzhou Municipal Transportation Commission issues the “GUIDING OPINIONS ON THE WORK RELATED TO THE ROAD TEST OF INTELLIGENT CONNECTED VEHICLES”, which makes clear requirements for the test vehicles, test conditions and test environment in the platoon driving test, but does not relate to the specific test methods and test procedures for platoon driving functions and performance. China Transportation Intelligent Transportation Industry Alliance issues the T/ITS0101-2019 standard “AUTONOMOUS DRIVING COMMERCIAL VEHICLE TEST SITE CONSTRUCTION AND AUTOMATED TEST SPECIFICATIONS”, which describes the test methods and evaluation criteria for the platoon driving ability of commercial vehicles. However, this standard only tests and evaluates the safety during platoon driving, and does not quantitatively evaluate the lane-changing performance during platoon driving. It can be seen that there is still lack of perfect evaluation methods and unified national standards for the quantitative test and evaluation of the lane-changing performance of intelligent vehicles. 
     At present, the existing patent literature studies the test and evaluation of the function and performance of intelligent vehicle platoon, and builds an evaluation system for the platoon effect of intelligent vehicles, involving aspects such as safety and traffic efficiency. In terms of safety evaluation, a hazard analysis and risk evaluation method is utilized to analyze the effectiveness and stability of the platoon, a stochastic model is utilized to evaluate the collision probability of platoon vehicles, or a car-following model such as an Intelligent Driver Model (IDM) and a Cooperative Adaptive Cruise Control (CACC) model is utilized to model the vehicle platoon process, and distance-to-collision time, inverse distance-to-collision time, total time exposed to dangerous traffic conditions, a collision risk index, and velocity covariance and other indexes are utilized for safety evaluation. In terms of traffic efficiency evaluation, the efficiency evaluation is carried out using the intersection traffic flow, exit turning rate, operating velocity, traffic flow and other indexes. There is no evaluation research on the intelligent vehicle platoon lane change performance, especially the lack of evaluation research on quantifying the performance indexes of platoon lane-changing. 
     On the whole, a quantitative evaluation method and an index evaluation system for the intelligent vehicle platoon lane change performance are not formed, which cannot meet the needs of comprehensive, accurate and reliable quantitative evaluation of vehicle platoon performance, and further breakthroughs are needed. 
     In order to comprehensively and accurately obtain vehicle motion parameters in the process of intelligent vehicle platoon lane change, multi-dimensional platoon lane change performance evaluation indexes are quantified and output, and comprehensive, accurate, and reliable scientific quantitative evaluation for platoon lane change performance is achieved, to fill a gap in the intelligent vehicle platoon lane change performance evaluation technology in practical applications. The present invention proposes an intelligent vehicle platoon lane change performance evaluation method for large trucks traveling in platoon. First, an intelligent vehicle platoon lane change performance test scenario is established. Secondly, a three-degree of freedom nonlinear dynamics model is established according to motion characteristics of intelligent vehicles in a platoon lane change process. Further, an improved adaptive unscented Kalman filter algorithm is utilized to perform filter estimation on state variables of positions and velocities of platoon vehicles. Finally, based on accurately recursive vehicle motion state parameters, evaluation indexes for platoon lane change performance are proposed and quantified, and an evaluation system for platoon lane change performance is constructed, thereby achieving comprehensive, accurate, and reliable scientific quantitative evaluation for intelligent vehicle platoon lane change performance. The technical route of the present invention is as shown in  FIG.  1   , and the concrete steps are as follows: 
     Step I, an intelligent vehicle platoon lane change performance test scenario is established. For the current lack of intelligent vehicle platoon lane change performance evaluation, the present invention establishes an intelligent vehicle platoon lane change performance test scenario. First, a high-grade highway is selected as the test site, and secondly, an intelligent vehicle platoon lane change performance test scenario is established. The detailed description is as follows: the leading vehicle is located in the middle of the test road and drives straight at a set velocity, the following vehicles follow the leading vehicle at a set distance, and when the leading vehicle passes through the test starting point, it is started to collect the motion state parameters of the leading vehicle and the following vehicles synchronously. Then, when encountering an obstacle, the leading vehicle conducts a lane change operation, the following vehicles receive lane change information and change lanes with a similar motion trajectory. When the leading vehicle reaches an end point, a test is completed. 
     In the present invention, the leading vehicle refers to a first vehicle in platoon driving, generally in a manual driving mode, and the following vehicles refer to intelligent vehicles behind the leading vehicle in platoon driving, generally in an automatic driving mode. 
     Step II, a dynamic model for intelligent vehicle platoon lane change is established. 
     In the process of platoon lane change performance test, it is necessary to accurately obtain the motion state parameters such as the position and velocity of the leading vehicle and the following vehicles at each moment. In order to meet the measurement requirements of complete information and high precision, it is necessary to establish a dynamic model that can accurately describe the intelligent vehicle platoon lane change movement characteristics. For the field of application of the present invention, for front-wheel steered four-wheel vehicles in platoon driving, the following assumptions are made: 
     (1) ignoring pitch and roll motions of each vehicle, and ignoring the influence of a vehicle suspension on a tire axle; 
     (2) assuming that two tires of a front axle of the vehicle have the same steering angle, cornering angle, longitudinal force and lateral force, and assuming that two tires of a rear axle of the vehicle have the same steering angle, cornering angle, longitudinal force and side force to force; and 
     (3) assuming that the direction of the front wheels of the vehicle is consistent with the current velocity direction of the vehicle. 
     Compared with the vehicle kinematics model, the dynamics model can describe the vehicle platoon lane change process in a more refined manner. Therefore, according to the above requirements and assumptions, the dynamics modelling of the large trucks traveling in platoon is carried out. Due to the complexity of the dynamics model of the truck, some parameters involved are difficult to obtain, and need to be simplified appropriately. Considering that the single-degree of freedom and two-degree of freedom dynamics models are too simple, the influence of factors such as the nonlinear characteristics of tires on vehicle motion is ignored, and the vehicle motion characteristics in the process of platoon lane change cannot be accurately described. Therefore, under the condition of taking into account the model accuracy and parameter complexity, the present invention adopts a three-degree of freedom model to carry out dynamics modeling of the leading vehicle in the platoon vehicle. 
       FIG.  2    defines a three-degree of freedom dynamics model of the vehicle, considering the longitudinal, lateral and yaw motions. Point G is the center of mass of the vehicle. The left and right wheels of the front axle are combined into one point, located at point A. The left and right wheels of the rear axle are combined into one point, located at point B. The GX axis is the same as the vehicle&#39;s forward direction, the GY axis is determined by the right-hand screw rule, and the GZ axis is perpendicular to the vehicle motion plane and points to the ground. According to  FIG.  2   , the dynamics model of the leading vehicle is described as: 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 v 
                                 . 
                               
                               x 
                             
                             = 
                             
                               
                                 
                                   v 
                                   y 
                                 
                                 · 
                                 
                                   r 
                                   l 
                                 
                               
                                 
                               + 
                               
                                 a 
                                 x 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               a 
                               x 
                             
                             = 
                             
                               
                                 2 
                                 M 
                               
                               [ 
                               
                                 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 - 
                                 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   F 
                                   xr 
                                 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 v 
                                 . 
                               
                               y 
                             
                             = 
                             
                               
                                 
                                   - 
                                   
                                     v 
                                     x 
                                   
                                 
                                 · 
                                 
                                   r 
                                   l 
                                 
                               
                                 
                               + 
                               
                                 a 
                                 y 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               a 
                               y 
                             
                             = 
                             
                               
                                 2 
                                 M 
                               
                               [ 
                               
                                 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   F 
                                   yr 
                                 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 r 
                                 . 
                               
                               l 
                             
                               
                             = 
                             
                               
                                 2 
                                 
                                   I 
                                   z 
                                 
                               
                               [ 
                               
                                 
                                   
                                     l 
                                     f 
                                   
                                   ⁢ 
                                   
                                     F 
                                     xf 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   δ 
                                 
                                 + 
                                 
                                   
                                     l 
                                     f 
                                   
                                   ⁢ 
                                   
                                     F 
                                     yf 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   δ 
                                 
                                 - 
                                 
                                   
                                     l 
                                     r 
                                   
                                   ⁢ 
                                   
                                     F 
                                     yr 
                                   
                                 
                               
                               ] 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
     In formula (1), the superscript “ ” represents the differential; {dot over (v)} x  represents the differential of v x ; r l , v x , v y , a x , and a y  represent the yaw angular velocity, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the leading vehicle, respectively; M, δ, and I z  represent the mass of the leading vehicle, a steering angle of the front wheel, and the moment of inertia around the vertical axis of a body coordinate system, respectively, l f  and l r  represent the distances from the center of mass of the vehicle to the front and rear axles, respectively, F xf , F xr , F yf , and F yr  represent the longitudinal force and lateral force on the front and rear wheels, respectively. 
     For vehicles running on general roads, when the tire cornering angle is small, the lateral force on the front and rear tires is represented as:
 
 F   yf   =C   αf ·α f   ,F   yr   =C   αr ·α r   (2).
 
In formula (2), C αf  and C αr  represent the cornering stiffness of the front and rear tires, respectively, α f  and α r  represent the cornering angle of the front and rear tires, respectively, and α f =δ−(v y +l f r)/v x ,α r =(l r r−v y )/v x .
 
     In order to calculate the tire longitudinal force in formula (2), the tire model can be adopted to determine. Common tire models include empirical models, theoretical models, and adaptive models. In order to ensure the accuracy and real-time performance of vehicle motion parameter measurement, the present invention adopts a brush tire model, and the longitudinal force of the front and rear tires is represented as: 
     
       
         
           
             
               
                 
                   
                     
                       F 
                       xf 
                     
                     = 
                     
                       
                         
                           C 
                           xf 
                         
                         ⁢ 
                         
                           s 
                           xf 
                         
                       
                       + 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xf 
                               
                               ⁢ 
                               
                                 s 
                                 xf 
                               
                             
                             ) 
                           
                           3 
                         
                         
                           2 
                           ⁢ 
                           7 
                           ⁢ 
                           
                             
                               ( 
                               
                                 
                                   μ 
                                   l 
                                 
                                 ⁢ 
                                 
                                   F 
                                   zf 
                                 
                               
                               ) 
                             
                             2 
                           
                         
                       
                       - 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xf 
                               
                               ⁢ 
                               
                                 s 
                                 xf 
                               
                             
                             ) 
                           
                           2 
                         
                         
                           3 
                           ⁢ 
                           
                             μ 
                             l 
                           
                           ⁢ 
                           
                             F 
                             zf 
                           
                         
                       
                     
                   
                   ⁢ 
                     
                   
 
                   
                     
                       F 
                       xr 
                     
                     = 
                     
                       
                         
                           C 
                           xr 
                         
                         ⁢ 
                         
                           s 
                           xr 
                         
                       
                       + 
                       
                         
                           
                             ( 
                             
                               
                                 C 
                                 xr 
                               
                               ⁢ 
                               
                                 s 
                                 xr 
                               
                             
                             ) 
                           
                           3 
                         
                         
                           2 
                           ⁢ 
                           7 
                           ⁢ 
                           
                             
                               ( 
                               
                                 
                                   μ 
                                   l 
                                 
                                 ⁢ 
                                 
                                   F 
                                   zr 
                                 
                               
                               ) 
                             
                             2 
                           
                         
                       
                       - 
                       
                         
                           
                             
                               ( 
                               
                                 
                                   C 
                                   xr 
                                 
                                 ⁢ 
                                 
                                   s 
                                   xr 
                                 
                               
                               ) 
                             
                             2 
                           
                           
                             3 
                             ⁢ 
                             
                               μ 
                               l 
                             
                             ⁢ 
                             
                               F 
                               zr 
                             
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   3 
                   ) 
                 
               
             
           
         
       
     
     In formula (3), C xf  and C xr  represent the longitudinal stiffness of the front and rear tires, respectively, μ l  is a road adhesion coefficient, considering that the test road for the platoon lane change test is generally a dry and smooth asphalt road. The present invention takes that μ l =0.75, the vertical loads F zf  and F zr  of the front and rear tires, the longitudinal slip rates s xf  and s xr  of the front and rear tires are calculated by the following formulas: 
     
       
         
           
             
               
                 
                   
                     F 
                     zf 
                   
                   = 
                   
                     
                       
                         
                           9.8 
                           
                             Ml 
                             r 
                           
                         
                         
                           ( 
                           
                             
                               l 
                               f 
                             
                             + 
                             
                               l 
                               r 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                              
                       
                         F 
                         
                           z 
                           ⁢ 
                           r 
                         
                       
                     
                     = 
                     
                       
                         
                           9.8 
                           
                             Ml 
                             f 
                           
                         
                         
                           ( 
                           
                             
                               l 
                               f 
                             
                             + 
                             
                               l 
                               r 
                             
                           
                           ) 
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   4 
                   ) 
                 
               
             
           
         
       
       
         
           
             
               
                 
                   
                     s 
                     xf 
                   
                   = 
                   
                     
                       
                         
                           
                             
                               ω 
                               f 
                             
                             ⁢ 
                             
                               R 
                               tyre 
                             
                           
                           - 
                           
                             v 
                             xf 
                           
                         
                         
                           max 
                           ⁡ 
                           ( 
                           
                             
                               
                                 ω 
                                 f 
                               
                               ⁢ 
                               
                                 R 
                                 tyre 
                               
                             
                             , 
                             
                               v 
                               xf 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                              
                       
                         s 
                         xr 
                       
                     
                     = 
                     
                       
                         
                           
                             
                               ω 
                               r 
                             
                             ⁢ 
                             
                               R 
                               tyre 
                             
                           
                           - 
                           
                             v 
                             xr 
                           
                         
                         
                           max 
                           ⁡ 
                           ( 
                           
                             
                               
                                 ω 
                                 r 
                               
                               ⁢ 
                               
                                 R 
                                 tyre 
                               
                             
                             , 
                             
                               v 
                               xr 
                             
                           
                           ) 
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
           
         
       
     
     In the formulas, R tyre  is the tire radius, ω f  and ω r  represent the rotational angular velocity of the front and rear wheels, respectively, obtained by calculating a linear velocity measured by a wheel velocity sensor, v xf  and v xr  represent the velocities of the front and rear axles along the tire direction, respectively, and v xr =v x , and v xf =v x  cos δ+(v y +l f r l )sin δ. 
     At the same time, the longitudinal and lateral velocities v x  and v y  of the vehicle, and the east and north positions p e  and p n  satisfy the following relationship:
 
 {dot over (p)}   e   =v   e =√{square root over ( v   x   2   +v   y   2 )}sin β {dot over (p)}   n   =v   n =√{square root over ( v   x   2   +v   y   2 )}cos β  (6).
 
     In formula (6), v e  and v n  represent the east and north velocities of the vehicle, respectively, and β represents the azimuth of the vehicle&#39;s motion direction relative to the due north direction, and the following relationship is satisfied: 
     
       
         
           
             β 
             = 
             
               { 
               
                 
                   
                     
                       
                         when 
                         ⁢ 
                             
                         
                           arctan 
                           ⁡ 
                           ( 
                           
                             
                               v 
                               e 
                             
                             / 
                             
                               v 
                               n 
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           v 
                           n 
                         
                         &gt; 
                         0 
                       
                     
                   
                   
                     
                       
                         
                           when 
                           ⁢ 
                               
                           
                             arctan 
                             ⁡ 
                             ( 
                             
                               
                                 v 
                                 e 
                               
                               / 
                               
                                 v 
                                 n 
                               
                             
                             ) 
                           
                         
                         - 
                         π 
                       
                     
                     
                       
                         
                           
                             v 
                             n 
                           
                           &lt; 
                           0 
                         
                         , 
                         
                           
                             v 
                             e 
                           
                           &lt; 
                           0 
                         
                       
                     
                   
                   
                     
                       
                         
                           when 
                           ⁢ 
                               
                           
                             arctan 
                             ⁡ 
                             ( 
                             
                               
                                 v 
                                 e 
                               
                               / 
                               
                                 v 
                                 n 
                               
                             
                             ) 
                           
                         
                         + 
                         π 
                       
                     
                     
                       
                         
                           
                             v 
                             n 
                           
                           &lt; 
                           0 
                         
                         , 
                         
                           
                             v 
                             e 
                           
                           &gt; 
                           0 
                         
                       
                     
                   
                   
                     
                       
                         when 
                         ⁢ 
                             
                         π 
                         / 
                         2 
                       
                     
                     
                       
                         
                           
                             v 
                             n 
                           
                           = 
                           0 
                         
                         , 
                         
                           
                             v 
                             e 
                           
                           &gt; 
                           0 
                         
                       
                     
                   
                   
                     
                       
                         when 
                             
                         - 
                         
                           π 
                           / 
                           2 
                         
                       
                     
                     
                       
                         
                           
                             v 
                             n 
                           
                           = 
                           0 
                         
                         , 
                         
                           
                             v 
                             e 
                           
                           &lt; 
                           
                             0 
                             ⁢ 
                                 
                             and 
                           
                         
                       
                     
                   
                   
                     
                       0 
                     
                     
                       
                         
                           v 
                           n 
                         
                         = 
                         
                           
                             v 
                             e 
                           
                           = 
                           0 
                         
                       
                     
                   
                 
                 . 
               
             
           
         
       
     
     For the intelligent vehicle platoon lane change process, taking a system state vector X=[p e  p n  v x  v y  r l ] T , the superscript T of the matrix in the present invention represents the transposition of the matrix, and T represents the discrete period. According to the dynamics model described by formula (1), the system state equation is established:
 
 X=f ( X,U,W ,γ)  (7)
 
     In formula (7), f( ) is a 5-dimensional vector function, W is the zero-mean system Gaussian white noise, γ is the zero-mean Gaussian white noise corresponding to the external input of the system, U is an external input vector of the system and U=[δ F xf  F xr ] T , where a steering angle of the front wheel is δ=ε l /ρ l , the steering wheel angle ε l  is obtained by a body CAN bus, ρ l  is a transmission ratio of the steering system, and in the present invention, ρ l =10, the tire longitudinal forces F xf  and F xr  are determined by the brush tire model. 
     Step III, vehicle motion state estimation based on an improved unscented Kalman filter algorithm. 
     In order to estimate the vehicle motion state in the process of platoon lane change, the method of filtering recursive estimation can be used, and a more comprehensive and accurate parameter estimation can be achieved by using less systematic observations. To process the nonlinear system state equation described in step I, a nonlinear Kalman filter is used. 
     In the common nonlinear filters, a particle filter has higher computational complexity. If the number of particles is reduced, the estimation accuracy may decrease. The extended Kalman filter algorithm introduces a linearization error, which is easy to reduce the filtering effect for the system with complex model. An Unscented Kalman Filter (UKF) has the same order as an Extended Kalman Filter in computational complexity, but the parameter estimation accuracy is higher than that of the Extended Kalman Filter. Therefore, the UKF algorithm is adopted to perform recursive estimation on the vehicle motion state parameters. 
     First, the present invention selects centimeter-level high-precision satellite positioning systems (such as GPS and Beidou) and inertial measurement units as measurement sensors for vehicle motion, and takes the vehicle&#39;s east position, north position, longitudinal forward velocity and yaw angular velocity as the system observation vectors, the observation equation of the system can be expressed as:
 
 Z ( t )= h ( x ( t ), v ( t ))  (8).
 
     In formula (8), h is the observation equation, t is the time, and the system observation vector Z=[p eg  p ng  v x_m  ω z_m ] T , where p eg  and p ng  represent the observed values of the east and north positions of the vehicle, respectively, and are obtained by converting the latitude and longitude coordinates collected by a centimeter-level high-precision satellite positioning system (such as GPS and Beidou); and v x_m  and ω z_m  represent the longitudinal forward velocity and yaw angular velocity of the vehicle, respectively, and are measured by an inertial measurement unit. 
     In the actual filtering recursion process, it is necessary to adopt a discretized filtering model. To this end discretization is performed on formulas (7) and (8), and the system state equation and observation equation after discretization are respectively: 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               X 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               f 
                               ⁡ 
                               ( 
                               
                                 
                                   X 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   U 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   W 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                                 , 
                                 
                                   γ 
                                   ⁡ 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               ) 
                             
                           
                         
                       
                       
                         
                           
                             
                               Z 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               h 
                               ⁡ 
                               ( 
                               
                                 
                                   X 
                                   ⁡ 
                                   ( 
                                   k 
                                   ) 
                                 
                                 , 
                                 
                                   V 
                                   ⁡ 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
           
         
       
     
     In formula (9), k is the discretization time, and the system process noise W=[w 1  w 2  w 3  w 4  w 5 ] T , where w 1 , w 2 , w 3 , w 4 , and w 5  represent five system Gaussian white noise components, respectively, a Gaussian white noise covariance matrix corresponding to W(k−1) is Q(k−1)=diag[σ w     1     2  σ w     2     2  σ w     3     2  σ w     4     2  σ w     5     2 ], where σ w     1     2 , σ w     2     2 , σ w     3     2 , σ w     4     2 , and σ w     5     2  represent the variances corresponding to Gaussian white noises w 1 , w 2 , w 3 , w 4 , and w 5 , respectively. The system observation noise V=[v 1  v 2  v 3  v 4 ] T , where v 1 , v 2 , v 3 , and v 4  represent four system Gaussian white noise components, respectively, a measured white Gaussian noise covariance matrix corresponding to V(k) is R(k)=diag[σ v     1     2  σ v     2     2  σ v     3     2  σ v     4     2 ], where σ v     1     2 , σ v     2     2 , σ v     3     2 , and σ v     4     2 , respectively represent the variances corresponding to the Gaussian white noises v 1 , v 2 , v 3 , and v 4 , and are determined according to the statistical characteristics of the measurement noise of the sensor&#39;s position, velocity, and yaw angular velocity. The external input noise of the system is γ=[w δ  w F     xf    w F     sr   ] T , where w δ , w F     xf   , and F xr , represent the zero-mean Gaussian white noise components corresponding to δ, F xf , and F sr , respectively. These white noises are implied in the three external inputs of the system of the system state function f. The system state function is: 
     
       
         
           
                              
             
               
                 
                   f 
                   ⁡ 
                   ( 
                   
                     X 
                     , 
                     U 
                     , 
                     W 
                     , 
                     γ 
                   
                   ) 
                 
                 = 
                 
                   [ 
                   
                     
                       
                         
                           
                             f 
                             1 
                           
                           ( 
                           
                             
                               X 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               U 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               W 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             , 
                             
                               γ 
                               ⁡ 
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           
                             f 
                             2 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             3 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             4 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             f 
                             5 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 X 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 U 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 W 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               , 
                               
                                 γ 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   ] 
                 
               
               ⁢ 
               
 
                                
               
                 where 
                 , 
               
             
           
         
       
       
         
           
               
             
               
                 
                   
                     f 
                     1 
                   
                   ( 
                   
                     
                       X 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       U 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       W 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       γ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   ) 
                 
                 = 
                 
                   
                     
                       p 
                       e 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       
                         
                           
                             v 
                             x 
                             2 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         + 
                         
                           
                             v 
                             y 
                             2 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                     
                     ⁢ 
                     sin 
                     ⁢ 
                     
                       β 
                       · 
                       T 
                     
                   
                   + 
                   
                     w 
                     1 
                   
                 
               
               ⁢ 
                 
               
                 
                   
                     f 
                     2 
                   
                   ( 
                   
                     
                       X 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       U 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       W 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       γ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   ) 
                 
                 = 
                 
                   
                     
                       p 
                       n 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       
                         
                           
                             v 
                             x 
                             2 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         + 
                         
                           
                             v 
                             y 
                             2 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                       β 
                       · 
                       T 
                     
                   
                   + 
                   
                     w 
                     2 
                   
                 
               
               ⁢ 
                 
               
                 
                   
                     f 
                     3 
                   
                   ( 
                   
                     
                       X 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       U 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       W 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       γ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   ) 
                 
                 = 
                 
                   
                     
                       v 
                       x 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       T 
                       M 
                     
                     [ 
                     
                       
                         
                           
                             Mv 
                             y 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                         ⁢ 
                         
                           
                             r 
                             l 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       + 
                       
                         2 
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             f 
                           
                         
                         ⁢ 
                         
                           
                             
                               
                                 v 
                                 y 
                               
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             + 
                             
                               
                                 l 
                                 f 
                               
                               ⁢ 
                               
                                 
                                   r 
                                   l 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                           
                             
                               v 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           δ 
                           ⁡ 
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                     
                     ] 
                   
                   + 
                   
                     
                       
                         2 
                         ⁢ 
                         T 
                       
                       M 
                     
                     [ 
                     
                       
                         
                           F 
                           xf 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                       + 
                       
                         
                           F 
                           xf 
                         
                         ( 
                         
                           k 
                           - 
                           1 
                         
                         ) 
                       
                     
                     ] 
                   
                   + 
                   
                     w 
                     3 
                   
                 
               
               ⁢ 
                 
               
                 
                   
                     f 
                     4 
                   
                   ( 
                   
                     
                       X 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       U 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       W 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       γ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   ) 
                 
                 = 
                 
                   
                     
                       v 
                       y 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       T 
                       M 
                     
                     [ 
                     
                       
                         
                           - 
                           
                             
                               Mv 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           
                             r 
                             l 
                           
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       + 
                       
                         2 
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             f 
                           
                         
                         ⁢ 
                         
                           δ 
                           ⁡ 
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       - 
                       
                         2 
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             f 
                           
                         
                         ⁢ 
                         
                           
                             
                               
                                 v 
                                 y 
                               
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             + 
                             
                               
                                 l 
                                 f 
                               
                               ⁢ 
                               
                                 
                                   r 
                                   l 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                           
                             
                               v 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                       + 
                       
                         2 
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             r 
                           
                         
                         ⁢ 
                         
                           
                             
                               
                                 l 
                                 r 
                               
                               ⁢ 
                               
                                 
                                   r 
                                   l 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             - 
                             
                               
                                 v 
                                 y 
                               
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                           
                             
                               v 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                     
                     ] 
                   
                   + 
                   
                     
                       
                         2 
                         ⁢ 
                         T 
                       
                       M 
                     
                     ⁢ 
                     
                       
                         F 
                         xf 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     ⁢ 
                     
                       δ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   + 
                   
                     w 
                     4 
                   
                 
               
               ⁢ 
                 
               
                 
                   
                     f 
                     5 
                   
                   ( 
                   
                     
                       X 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       U 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       W 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     , 
                     
                       γ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   ) 
                 
                 = 
                 
                   
                     
                       r 
                       l 
                     
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       T 
                       
                         I 
                         z 
                       
                     
                     [ 
                     
                       
                         2 
                         ⁢ 
                         
                           l 
                           f 
                         
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             f 
                           
                         
                         ⁢ 
                         
                           δ 
                           ⁡ 
                           ( 
                           
                             k 
                             - 
                             1 
                           
                           ) 
                         
                       
                       - 
                       
                         2 
                         ⁢ 
                         
                           l 
                           f 
                         
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             f 
                           
                         
                         ⁢ 
                         
                           
                             
                               
                                 v 
                                 y 
                               
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                             + 
                             
                               
                                 l 
                                 f 
                               
                               ⁢ 
                               
                                 
                                   r 
                                   l 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                           
                             
                               v 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                       - 
                       
                         2 
                         ⁢ 
                         
                           l 
                           r 
                         
                         ⁢ 
                         
                           C 
                           
                             α 
                             ⁢ 
                             r 
                           
                         
                         ⁢ 
                         
                           
                             
                               
                                 l 
                                 r 
                               
                               ⁢ 
                               
                                 
                                   r 
                                   l 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             - 
                             
                               
                                 v 
                                 y 
                               
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                           
                             
                               v 
                               x 
                             
                             ( 
                             
                               k 
                               - 
                               1 
                             
                             ) 
                           
                         
                       
                     
                     ] 
                   
                   + 
                   
                     
                       
                         2 
                         ⁢ 
                         
                           l 
                           f 
                         
                         ⁢ 
                         T 
                       
                       
                         I 
                         z 
                       
                     
                     ⁢ 
                     
                       
                         F 
                         xf 
                       
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                     ⁢ 
                     
                       δ 
                       ⁡ 
                       ( 
                       
                         k 
                         - 
                         1 
                       
                       ) 
                     
                   
                   + 
                   
                     
                       w 
                       5 
                     
                     . 
                   
                 
               
             
           
         
       
     
     Secondly, according to the system state equation and observation equation described by formula (9), the unscented Kalman filtering recursive process is established, and the filtering recursion is performed by using time update and measurement update. 
     (1) Initialization of the Input Variable and Parameter Calculation 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 X 
                                 ˆ 
                               
                               0 
                             
                             = 
                             
                               E 
                               [ 
                               
                                 X 
                                 0 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               P 
                               0 
                             
                             = 
                             
                               E 
                               [ 
                               
                                 
                                   ( 
                                   
                                     
                                       X 
                                       0 
                                     
                                     - 
                                     
                                       
                                         X 
                                         ˆ 
                                       
                                       0 
                                     
                                   
                                   ) 
                                 
                                 · 
                                 
                                   
                                     ( 
                                     
                                       
                                         X 
                                         0 
                                       
                                       - 
                                       
                                         
                                           X 
                                           ˆ 
                                         
                                         0 
                                       
                                     
                                     ) 
                                   
                                   T 
                                 
                               
                               ] 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
     In formula (10), P 0  is an initial error variance matrix, the variable with the superscript symbol {circumflex over ( )} represents a filtering estimated value of the variable, and {circumflex over (X)} 0  is a filtering estimated value representing the initial value x 0  of the input variable. 
     (2) State Estimation 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   ξ 
                                   i 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                               = 
                               
                                 
                                   X 
                                   ˆ 
                                 
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             , 
                             
                               i 
                               = 
                               0 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ( 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                       ) 
                                     
                                     = 
                                     
                                       
                                         
                                           X 
                                           ˆ 
                                         
                                         ( 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                         ) 
                                       
                                       + 
                                       
                                         
                                           ( 
                                           
                                             
                                               
                                                 ( 
                                                 
                                                   
                                                     x 
                                                     dim 
                                                   
                                                   + 
                                                   λ 
                                                 
                                                 ) 
                                               
                                               ⁢ 
                                               
                                                 P 
                                                 ⁡ 
                                                 ( 
                                                 
                                                   k 
                                                   - 
                                                   1 
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ) 
                                         
                                         i 
                                       
                                     
                                   
                                   , 
                                 
                               
                             
                             
                               
                                 
                                   
                                     i 
                                     = 
                                     1 
                                   
                                   , 
                                   2 
                                   , 
                                   … 
                                       
                                   , 
                                   
                                     x 
                                     dim 
                                   
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ( 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                       ) 
                                     
                                     = 
                                     
                                       
                                         
                                           X 
                                           ˆ 
                                         
                                         ( 
                                         
                                           k 
                                           - 
                                           1 
                                         
                                         ) 
                                       
                                       - 
                                       
                                         
                                           ( 
                                           
                                             
                                               
                                                 ( 
                                                 
                                                   
                                                     x 
                                                     dim 
                                                   
                                                   + 
                                                   λ 
                                                 
                                                 ) 
                                               
                                               ⁢ 
                                               
                                                 P 
                                                 ⁡ 
                                                 ( 
                                                 
                                                   k 
                                                   - 
                                                   1 
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ) 
                                         
                                         i 
                                       
                                     
                                   
                                   , 
                                 
                               
                             
                             
                               
                                 
                                   
                                     i 
                                     = 
                                     
                                       
                                         x 
                                         dim 
                                       
                                       + 
                                       1 
                                     
                                   
                                   , 
                                   
                                     
                                       x 
                                       dim 
                                     
                                     + 
                                     2 
                                   
                                   , 
                                   … 
                                       
                                   , 
                                   
                                     2 
                                     ⁢ 
                                     
                                       x 
                                       dim 
                                     
                                   
                                 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     In formula (11), ξ i (k−1) is the Sigma point, (√{square root over ((x dim +λ)P(k−1))}) i  is the i-th column of a square root of a weighted covariance matrix, and x dim  is the number of dimensions of the state vector, taking x dim =5. 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               ω 
                               0 
                               
                                 ( 
                                 e 
                                 ) 
                               
                             
                             = 
                             
                               λ 
                               
                                 
                                   x 
                                   dim 
                                 
                                 + 
                                 λ 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               0 
                               
                                 ( 
                                 c 
                                 ) 
                               
                             
                             = 
                             
                               
                                 ω 
                                 0 
                                 
                                   ( 
                                   e 
                                   ) 
                                 
                               
                               + 
                               
                                 ( 
                                 
                                   1 
                                   - 
                                   
                                     α 
                                     2 
                                   
                                   + 
                                   ε 
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               i 
                               
                                 ( 
                                 c 
                                 ) 
                               
                             
                             = 
                             
                               
                                 ω 
                                 i 
                                 
                                   ( 
                                   e 
                                   ) 
                                 
                               
                               = 
                               
                                 1 
                                 
                                   2 
                                   ⁢ 
                                   
                                     ( 
                                     
                                       
                                         x 
                                         dim 
                                       
                                       + 
                                       λ 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
           
         
       
     
     In formula (12), λ is a distance parameter, and λ=x dim (α 2 −1), α is a first scale factor, ε is a second scale factor, taking ε=2, ω i   (c)  and ω i   (e)  represent the weight coefficients of the mean and variance, respectively. 
     (3) Time Update
 
ξ i ( k,k− 1)= f [ξ i ( k,k− 1)], i =0,1, . . . ,2 x   dim   (13).
 
 {circumflex over (X)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) ξ i ( k,k− 1)  (14).
 
 P ( k,k− 1)=Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)]·[ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)] T   +Q ( k )  (15).
 
In the formulas, {circumflex over (X)}(k−1) is the optimal estimate at time k−1, and P(k,k−1) is a one-step prediction error variance matrix at time k.
 
     (4) Observation Update
 
χ i ( k,k− 1)= h [ξ i ( k,k− 1)]  (16).
 
 {circumflex over (Z)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) χ i ( k,k− 1)  (17).
 
 P   {tilde over (Z)} =Σ i=0   2x     dim   ω i   (c) [χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   +R ( k )  (18).
 
 P   XZ =Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   (19).
 
     In the formulas, X i  (k,k−1) represents the value of the observation equation transformed from the Sigma point set, {circumflex over (Z)}(k,k−1) represents the one-step predicted observation value at time k recursively from time k−1, P {tilde over (Z)}  is the predicted value covariance, and P XZ  is the covariance of the state value and the measured value. 
     (5) Filter Update
 
 K ( k )= P   XZ   ·P   {tilde over (Z)}   −1   (20).
 
 {circumflex over (X)} ( k )= {circumflex over (X)} ( k,k− 1)+ K ( k )·[ Z ( k )− {circumflex over (Z)} ( k,k− 1)]  (21).
 
 P ( k )= P ( k,k− 1)− K ( k )· P   {tilde over (Z)}   ·K ( k ) T   (22).
 
     In the formulas, K(k) is a filter gain matrix, {circumflex over (X)}(k) is a state quantity estimated value, and P(k) is an estimated error variance matrix. 
     In the actual test process, there are differences in the motion characteristics of different platoon vehicles, so that the noise statistical characteristics of the system cannot be consistent. When dealing with nonlinear problems, although the UKF algorithm has good parameter estimation accuracy, it still has certain shortcomings, that is, the prior statistical characteristics of system observation noise and process noise need to be accurately known. 
     In view of the above problems, the UKF algorithm is improved. Considering that the Adaptive Kalman Filter (AKF) has the advantages of suppressing filter divergence and correcting inaccurate system model parameters, the present invention combines the AKF filter with the UKF filter, and targeted improvement is made to improve the accuracy of system state estimation. Specifically: 
     (1) In order to reduce the influence of the system observation noise and process noise uncertainty on the filtering accuracy, a noise estimator with time-varying performance is introduced. (2) In order to improve the accuracy of the one-step prediction error variance matrix, and formulas (14-15) and formulas (17-18) are replaced with formulas (23-26):
 
 {circumflex over (X)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) ξ i ( k,k− 1)+ {circumflex over (q)} ( k− 1)  (23).
 
 P ( k,k− 1)=τ f Σ i=0   2x     dim   ω i   (c) [ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)][ξ i ( k,k− 1)− {circumflex over (X)} ( k,k− 1)] T   +{circumflex over (Q)} ( k )  (24).
 
 {circumflex over (Z)} ( k,k− 1)=Σ i=0   2x     dim   ω i   (e) χ i ( k,k− 1)+ {circumflex over (r)} ( k )  (25).
 
 P   {tilde over (Z)} =Σ i=0   2x     dim   ω i   (c) [χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)][χ i ( k,k− 1)− {circumflex over (Z)} ( k,k− 1)] T   +{circumflex over (R)} ( k )  (26).
 
where the means and variance matrixes of the observation noise and system noise {circumflex over (r)}(k), {circumflex over (R)}(k), {circumflex over (q)}(k), {circumflex over (Q)}(k), and forgetting factors are:
 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               
                                 q 
                                 ^ 
                               
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               
                                 
                                   
                                     δ 
                                     d 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                                 [ 
                                 
                                   
                                     
                                       X 
                                       ^ 
                                     
                                     ( 
                                     k 
                                     ) 
                                   
                                   - 
                                   
                                     
                                       ∑ 
                                       
                                         i 
                                         = 
                                         0 
                                       
                                       
                                         2 
                                         ⁢ 
                                         
                                           x 
                                           dim 
                                         
                                       
                                     
                                     
                                       
                                         ω 
                                         i 
                                         
                                           ( 
                                           e 
                                           ) 
                                         
                                       
                                       ⁢ 
                                       
                                         
                                           ξ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                                 ] 
                               
                               + 
                               
                                 
                                   [ 
                                   
                                     1 
                                     - 
                                     
                                       
                                         δ 
                                         d 
                                       
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                   ] 
                                 
                                 ⁢ 
                                 
                                   
                                     q 
                                     ^ 
                                   
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 Q 
                                 ^ 
                               
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               
                                 
                                   δ 
                                   d 
                                 
                                 ( 
                                 k 
                                 ) 
                               
                               ⁢ 
                               
                                 { 
                                 
                                   
                                     K 
                                     ⁡ 
                                     ( 
                                     k 
                                     ) 
                                   
                                   [ 
                                   
                                     
                                       Z 
                                       ⁡ 
                                       ( 
                                       k 
                                       ) 
                                     
                                     - 
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           
                                             χ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   [ 
                                   
                                     
                                       Z 
                                       ⁢ 
                                       
                                         ( 
                                         k 
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           χ 
                                           i 
                                         
                                         ⁢ 
                                         
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                                 T 
                               
                               ⁢ 
                               
                                 
                                   K 
                                   T 
                                 
                                 ( 
                                 k 
                                 ) 
                               
                             
                             + 
                           
                         
                       
                       
                         
                           
                             
                               P 
                               ⁡ 
                               ( 
                               k 
                               ) 
                             
                             - 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   0 
                                 
                                 
                                   2 
                                   ⁢ 
                                   
                                     x 
                                     dim 
                                   
                                 
                               
                               
                                 
                                   ω 
                                   i 
                                   
                                     ( 
                                     c 
                                     ) 
                                   
                                 
                                 [ 
                                 
                                   
                                     
                                       ξ 
                                       i 
                                     
                                     ( 
                                     
                                       k 
                                       , 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                   - 
                                   
                                     
                                       X 
                                       ^ 
                                     
                                     ( 
                                     
                                       k 
                                       , 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                 
                                 ] 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   [ 
                                   
                                     
                                       
                                         ξ 
                                         i 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           kk 
                                           - 
                                           1 
                                         
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         X 
                                         ^ 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                   ] 
                                 
                                 T 
                               
                               } 
                             
                             + 
                             
                               
                                 [ 
                                 
                                   1 
                                   - 
                                   
                                     
                                       δ 
                                       d 
                                     
                                     ⁢ 
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                 
                                 ] 
                               
                               ⁢ 
                               
                                 Q 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 r 
                                 ^ 
                               
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               
                                 
                                   [ 
                                   
                                     1 
                                     - 
                                     
                                       
                                         δ 
                                         d 
                                       
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                   ] 
                                 
                                 ⁢ 
                                 
                                   
                                     r 
                                     ^ 
                                   
                                   ( 
                                   
                                     k 
                                     - 
                                     1 
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   
                                     δ 
                                     d 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                                 [ 
                                 
                                   
                                     Z 
                                     ⁡ 
                                     ( 
                                     k 
                                     ) 
                                   
                                   - 
                                   
                                     
                                       ∑ 
                                       
                                         i 
                                         = 
                                         0 
                                       
                                       
                                         2 
                                         ⁢ 
                                         
                                           x 
                                           dim 
                                         
                                       
                                     
                                     
                                       
                                         ω 
                                         i 
                                         
                                           ( 
                                           e 
                                           ) 
                                         
                                       
                                       ⁢ 
                                       
                                         
                                           χ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                                 ] 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 R 
                                 ^ 
                               
                               ( 
                               k 
                               ) 
                             
                             = 
                             
                               
                                 
                                   δ 
                                   d 
                                 
                                 ( 
                                 k 
                                 ) 
                               
                               ⁢ 
                               
                                 { 
                                 
                                   [ 
                                   
                                     
                                       Z 
                                       ⁡ 
                                       ( 
                                       k 
                                       ) 
                                     
                                     - 
                                     
                                       
                                         ∑ 
                                         
                                           i 
                                           = 
                                           0 
                                         
                                         
                                           2 
                                           ⁢ 
                                           
                                             x 
                                             dim 
                                           
                                         
                                       
                                       
                                         
                                           ω 
                                           i 
                                           
                                             ( 
                                             e 
                                             ) 
                                           
                                         
                                         ⁢ 
                                         
                                           
                                             χ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 [ 
                                 
                                   
                                     Z 
                                     ⁢ 
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                   - 
                                   
                                     
                                       ∑ 
                                       
                                         i 
                                         = 
                                         0 
                                       
                                       
                                         2 
                                         ⁢ 
                                         
                                           x 
                                           dim 
                                         
                                       
                                     
                                     
                                       
                                         ω 
                                         i 
                                         
                                           ( 
                                           e 
                                           ) 
                                         
                                       
                                       ⁢ 
                                       
                                         χ 
                                         i 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                                 ] 
                               
                               T 
                             
                             - 
                           
                         
                       
                       
                         
                           
                             
                               ∑ 
                               
                                 i 
                                 = 
                                 0 
                               
                               
                                 2 
                                 ⁢ 
                                 
                                   x 
                                   dim 
                                 
                               
                             
                             
                               
                                 ω 
                                 i 
                                 
                                   ( 
                                   c 
                                   ) 
                                 
                               
                               [ 
                               
                                 
                                   
                                     χ 
                                     i 
                                   
                                   ( 
                                   
                                     k 
                                     , 
                                     
                                       k 
                                       - 
                                       1 
                                     
                                   
                                   ) 
                                 
                                 - 
                                 
                                   
                                     Z 
                                     ^ 
                                   
                                   ( 
                                   
                                     k 
                                     , 
                                     
                                       k 
                                       - 
                                       1 
                                     
                                   
                                   ) 
                                 
                               
                               ] 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   [ 
                                   
                                     
                                       
                                         χ 
                                         i 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         Z 
                                         ^ 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                   ] 
                                 
                                 T 
                               
                               } 
                             
                             + 
                             
                               
                                 [ 
                                 
                                   1 
                                   - 
                                   
                                     
                                       δ 
                                       d 
                                     
                                     ⁢ 
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                 
                                 ] 
                               
                               ⁢ 
                               
                                 R 
                                 ^ 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   27 
                   ) 
                 
               
             
           
         
       
       
         
           
             
               
                 
                   
                     τ 
                     f 
                   
                   = 
                   
                     max 
                     ⁢ 
                     
                       
                         { 
                         
                           1 
                           , 
                           
                             
                               trace 
                               ⁡ 
                               ( 
                               
                                 
                                   P 
                                   
                                     Z 
                                     ~ 
                                   
                                 
                                 , 
                                 
                                   P 
                                   
                                     Z 
                                     ~ 
                                   
                                   T 
                                 
                               
                               ) 
                             
                             
                               trace 
                               ⁢ 
                               
                                 { 
                                 
                                   
                                     
                                       [ 
                                       
                                         
                                           
                                             χ 
                                             i 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                         - 
                                         
                                           
                                             Z 
                                             ^ 
                                           
                                           ( 
                                           
                                             k 
                                             , 
                                             
                                               k 
                                               - 
                                               1 
                                             
                                           
                                           ) 
                                         
                                       
                                       ] 
                                     
                                     [ 
                                     
                                       
                                         
                                           χ 
                                           i 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                       - 
                                       
                                         
                                           Z 
                                           ^ 
                                         
                                         ( 
                                         
                                           k 
                                           , 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                     ] 
                                   
                                   T 
                                 
                                 } 
                               
                             
                           
                         
                         } 
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
           
         
       
     
     In the formulas, δ d (k) represents an adaptive weighting parameter at time k, and δ d (k)=1/k, τ f  is the forgetting factor. 
     In conclusion, formulas (10-13), formula (16), and formulas (19-28) constitute the improved adaptive unscented Kalman filter algorithm. 
     Finally, after the above-mentioned improved filtering recursive calculation, the output state estimate {circumflex over (X)}(k) is the east position, north position, longitudinal velocity, lateral velocity and yaw angular velocity information of the leading vehicle at time k. 
     Similarly, the dynamic model of each following vehicle is established by using the method described in step II and step III, and filtering recursion is performed, to output the motion state parameters of each following vehicle. 
     For the intelligent vehicles in the platoon, the output longitudinal velocity information is V x_j ={v x_j (0), v x_j  (1), . . . , v x_j (k)}, the lateral velocity information is V y_j ={v y_j (0), v y_j  (1), . . . , v y_j (k)}, the yaw angular velocity information is R j ={r l_j (0), r l_j  (1), . . . , r l_j (k)}, and the position information is P j ={(p e_j (0), p n_j (0)), (p e_j (1), p n_j (1)), . . . , (p e_j (k), p n_j (k)), (j=1, 2, 3 . . . , a). 
     The subscript j represents the j-th platoon vehicle in the platoon, for example, j=1 represents that the vehicle is the leading vehicle, and j=2 represents that the vehicle is the first following vehicle, α is the total number of platoon vehicles, p e_j  (k), p n_j (k), v x_j  (k), v y_j (k), and r l_j (k) represent the east position, the north position, the longitudinal velocity, the lateral velocity and the yaw angular velocity of the j-th intelligent vehicle at time k, respectively. 
     Step IV, the evaluation indexes for platoon lane change performance are proposed and quantified. 
     In the existing national standards and test regulations, there is no clear and unified evaluation index for platoon lane change performance. Considering that it is difficult for a single evaluation index to cover all the information of the evaluation object, it cannot meet the needs of comprehensive and reliable platoon lane change performance evaluation. Therefore, the present invention proposes a multi-dimensional platoon lane change performance evaluation index from the perspectives of safety, reliability, comfort and other aspects of the lane change process for the following vehicles in platoon: lane change yaw stability, velocity consistency, safety distance margin and average vehicle clearance. Secondly, based on the position and velocity motion state parameters of the leading vehicle and the following vehicles output in step II, the above performance evaluation indexes are quantified. Specifically: 
     (1) Lane Change Yaw Stability 
     
       
         
           
             
               
                 
                   
                     σ 
                     j 
                   
                   = 
                   
                     
                       
                         
                           1 
                           s 
                         
                         ⁢ 
                         
                           
                             ∑ 
                             
                               k 
                               = 
                               1 
                             
                             s 
                           
                           
                             
                               [ 
                               
                                 
                                   
                                     r 
                                     j 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                                 - 
                                 
                                   
                                     r 
                                     E_j 
                                   
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               ] 
                             
                             2 
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   29 
                   ) 
                 
               
             
           
         
       
     
     In formula (29), σ j  represents the lane change yaw stability of the j-th following vehicle, which reflects the aggressiveness of the following vehicle to change lanes, r E_j (k) is an expected value of the yaw angular velocity at time k, and r E_j (k)=√{square root over (v x_j   2 (k)+v y_j   2 (k))}/R road , the unit is rad/s, R road  is the radius of curvature of the road, calculated from the position and velocity information output in step III, and s is the number of sampling points in the test process. 
     (2) Velocity Consistency 
     
       
         
           
             
               
                 
                   
                     τ 
                     platoon 
                   
                   = 
                   
                     
                       1 
                       
                         j 
                         - 
                         1 
                       
                     
                     ⁢ 
                     
                       ∑ 
                       
                         
                           τ 
                           j 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
           
         
       
     
     In formula (30), τ platoon  represents the velocity consistency in the platoon lane change process, τ j  represents the average relative velocity of the j-th following vehicle and the vehicle ahead in the same direction, and 
                 τ   j     =       1   s     ⁢       ∑     k   =   1     s       [         v     res   ⁢   _   ⁢   j       (   k   )     -       v       res   ⁢   _   ⁢   j     -   1       (   k   )       ]           ,         
v res_j (k) is the combined velocity of the j-th following vehicle at time k, in m/s.
 
     (3) Safety Distance Margin 
                     L   j     =             [         p     e   ⁢   _   ⁢   j       (   k   )     -       p       e   ⁢   _   ⁢   j     -   1       (   k   )       ]     2     +       [         p     n   ⁢   _   ⁢   j       (   k   )     -       p       n   ⁢   _   ⁢   j     -   1       (   k   )       ]     2         -       L   v     .               (   31   )               
In formula (31), L j  represents the safety distance margin between the j-th following vehicle and the vehicle ahead in the same direction, and L v  is the body length of the following vehicle, and the unit is m.
 
     (4) Average Vehicle Clearance 
     
       
         
           
             
               
                 
                   
                     L 
                     avg 
                   
                   = 
                   
                     
                       1 
                       
                         j 
                         - 
                         1 
                       
                     
                     ⁢ 
                     
                       ∑ 
                       
                         L 
                         j 
                       
                     
                   
                 
               
               
                 
                   ( 
                   32 
                   ) 
                 
               
             
           
         
       
     
     In formula (32), L avg  is the average value of the platoon vehicle clearance, and the unit is m. 
     In evaluation of the intelligent vehicle platoon lane change performance, first, in the test scenario constructed in “step I”, “step II” and “step III” are utilized to perform filtering recursion on each motion state parameter of the leading vehicle and the following vehicles. Secondly, according to “step IV”, the quantitative values of the performance evaluation indexes are calculated. Finally, the lane change performance of the platoon vehicles is analyzed by quantitative evaluation. 
     Therefore, a comprehensive, accurate and reliable scientific quantitative evaluation of the intelligent vehicle platoon lane change performance is achieved. 
     In order to check the actual effect of the intelligent vehicle platoon lane change performance evaluation method disclosed in the present invention, an actual road test is carried out, and the basic conditions of the test are described as follows: 
     (1) Composition of an evaluation system: the evaluation system consists of hardware devices and a software system. The Nissan X-Trail test vehicle is used as the leading vehicle for the platoon lane change test, and is equipped with an embedded industrial computer, a high-precision MEMS integrated navigation system, a wireless communication module, an inverter and other hardware devices. The Chery Tiggo test vehicle is used as the following vehicle for the platoon lane change test, and is equipped with an embedded industrial computer, a high-performance integrated navigation system, a wireless communication module, an inverter and other hardware devices. The sensor installation position is close to the center of mass of the test vehicle, and the antenna installation position is located in the center of the roof. 
     (2) Test setup: two test vehicles are used to simulate the process of platoon lane change performance test. The leading vehicle is located in the middle of the test road and drives in a straight line at a set velocity. The following vehicles follow the leading vehicle at a set distance. When the leading vehicle passes through a test starting point, it is started to synchronously collect motion state parameters of the leading vehicle and the following vehicles. Then, when encountering an obstacle, the leading vehicle performs a lane change operation, and the following vehicles perform lane change with a similar motion trajectory. When the leading vehicle reaches an end point, a test is completed. 
     (3) Test environment: the test site is located near the airport expressway in Nanjing, Jiangsu Province, which is a typical expressway scenario in the intelligent vehicle function test scenario. The test road surface is a dry and smooth asphalt road. 
     (4) Test results: the test shows that the platoon lane change performance evaluation method proposed by the present invention has good performance, specifically: 
     1) Through the test method proposed by the present invention and the constructed platoon lane change performance evaluation system, the current problem of lack of quantitative evaluation of platoon lane change performance is solved, and a comprehensive and reliable scientific quantitative evaluation of intelligent vehicle platoon lane change performance is achieved. 
     2) Compared with the evaluation method of virtual simulation, the platoon lane change performance evaluation method based on an actual road test proposed by the present invention can better ensure the safety and reliability of the platoon lane change of the intelligent vehicles, and is more accurate and persuasive. 
     3) The measurement accuracy of vehicle motion state parameters and platoon lane change performance indexes is high, and the position measurement accuracy is less than or equal to 0.0295 m (RMS), and the accuracy can meet the high-precision test requirements of platoon lane change performance. 
     4) The environmental adaptability is high, under complex conditions such as wet and slippery roads, nights and rainy days, the method proposed by the present invention can complete the platoon lane change performance test with the same accuracy as the dry road surface. 
     In order to illustrate the actual effect of the present invention, the test results of a certain intelligent vehicle platoon lane change performance test are given below, and the test result curves are as shown in  FIG.  3    and  FIG.  4   . The dense dotted line in  FIG.  3    is the motion trajectory curve of the leading vehicle and the following vehicles during the test process, and  FIG.  4    is the graph of the change of the safety distance margin of the leading vehicle and the following vehicles over time in the test process. Through the calculation, the platoon lane change performance evaluation index proposed by the present invention can be output, for example, the lane change yaw stability is 0.2782 rad/s, etc., which realizes comprehensive, accurate and reliable scientific quantitative evaluation of the intelligent vehicle platoon lane change performance.