A method for detecting the path tracking accuracy of an autonomous sightseeing vehicle

Abstract

The present invention discloses a method for detecting the path tracking accuracy of an autonomous sightseeing vehicle. By sequentially obtaining the tracking errors of the autonomous sightseeing vehicle for each target point and based on the maximum value of the tracking errors of all target points, the path tracking accuracy of the autonomous sightseeing vehicle is obtained. At the same time, when obtaining the tracking errors of each target point, through the setting of an adaptive coefficient switching function, the adaptive setting of the Kalman filter algorithm coefficients is realized, and further, during the optimal estimation process of the state vector of the Kalman filter algorithm, the dynamic adaptive adjustment of the weights of the predicted value and the observed value is realized, overcoming the problem of Kalman gain divergence during the recursive process.

Description

Technical Field

[0001] The present invention relates to a method for detecting path tracking accuracy, and particularly to a method for detecting path tracking accuracy of an autonomous sightseeing vehicle. Background Art

[0002] Autonomous sightseeing vehicles have great advantages in improving active safety, traffic efficiency, and reducing energy consumption. Therefore, autonomous driving technology has become a research hotspot in the current industry. Path tracking is one of the key functions of autonomous sightseeing vehicles. Achieving accurate path tracking is the basis for the autonomous driving of sightseeing vehicles, which can ensure driving safety and improve vehicle stability. Therefore, path tracking accuracy is a key indicator for measuring the performance of autonomous sightseeing vehicles. In order to detect the path tracking accuracy of autonomous sightseeing vehicles, it is necessary to estimate the position of the autonomous sightseeing vehicle in real time to calculate the lateral or longitudinal error between the actual driving path and the target path of the autonomous sightseeing vehicle.

[0003] Currently, the positioning of sightseeing vehicles is mainly achieved by the Global Positioning System (GPS). However, GPS has certain errors, and the update frequency is only about 10 Hz. The sightseeing vehicle is prone to GPS signal loss in an environment with signal occlusion. Therefore, using only GPS cannot meet the positioning requirements of autonomous sightseeing vehicles.

[0004] The Inertial Navigation System (INS) has an update frequency of up to 100 Hz and is not easily affected by external interference. However, its positioning principle determines that there is a problem of error accumulation in INS.

[0005] The current mainstream positioning method is to use the Kalman filter algorithm to fuse GPS and INS. The output of INS is used to predict the current position of the autonomous sightseeing vehicle, and the observed information output by GPS is used to update the predicted position to achieve the optimal estimation of the position of the autonomous sightseeing vehicle. The optimal estimation of the state vector by the Kalman filter algorithm is a weighted processing of the predicted value and the observed value. As the recursive process continues, the calculation of the Kalman gain, that is, the weight value, will become more and more inaccurate. Therefore, it is necessary to improve the Kalman filter algorithm.

[0006] Therefore, providing a new technical solution to improve the above problems and improve the accuracy of path tracking inspection of autonomous sightseeing vehicles is an urgent problem for those skilled in the art. Summary of the Invention

[0007] Aiming at the problems existing in the prior art, the present invention provides a method for detecting path tracking accuracy of an autonomous sightseeing vehicle, which adaptively adjusts the relevant coefficients in the Kalman filter algorithm to improve the positioning accuracy of the autonomous sightseeing vehicle, and further improves the accuracy of path tracking inspection of the autonomous sightseeing vehicle.

[0008] The method for detecting the path tracking accuracy of an autonomous sightseeing vehicle according to the present invention includes the following steps;

[0009] Collect the coordinate data of several target points on the planned path;

[0010] During the automatic driving of the autonomous sightseeing vehicle along the said path, sequentially obtain the tracking errors of the autonomous sightseeing vehicle for each target point;

[0011] Among multiple tracking errors, the one with the largest error value is the path tracking accuracy of the autonomous sightseeing vehicle;

[0012] It is characterized in that: the method for obtaining the tracking error of the autonomous sightseeing vehicle for a certain target point includes the following steps;

[0013] Step S1;

[0014] During the process of the autonomous sightseeing vehicle approaching and departing from the target point, obtain the optimal estimated value of the current position of the autonomous sightseeing vehicle at every predetermined time interval;

[0015] Step S2;

[0016] According to the optimal estimated value of the current position of the autonomous sightseeing vehicle obtained in Step S1, calculate the distance between it and the target point;

[0017] Step S3;

[0018] Among the distances between multiple optimal estimated values and the target point obtained from Step S1 and Step S2, the smallest distance is the tracking error of the autonomous sightseeing vehicle for this target point;

[0019] In Step S1, at a certain k moment, the method for obtaining the optimal estimated value of the current position of the autonomous sightseeing vehicle includes the following steps;

[0020] Step S11;

[0021] Establish a state vector and an observation vector according to the state parameters of the autonomous sightseeing vehicle. The state vector includes the position, speed and heading angle parameters of the autonomous sightseeing vehicle. The state vector and the observation vector are expressed as follows;

[0022] +

[0023]

[0024] Predict kPrior state vector of the autonomous sightseeing vehicle at a moment and prior covariance estimate , the prediction formula is as follows;

[0025]

[0026]

[0027] where is the state transition matrix of the autonomous sightseeing vehicle from the k -1 moment to the k moment, is the k posterior state vector of the autonomous sightseeing vehicle at the -1 moment, is the k control matrix of the autonomous sightseeing vehicle at the moment, is the k control input vector of the autonomous sightseeing vehicle at the moment, T( s ) is the adaptive coefficient switching function, is the k posterior estimation covariance of the autonomous sightseeing vehicle at the -1 moment, is the transpose matrix of, is the k moment, a system noise vector subject to a normal distribution with a mean of 0

[0028] , where, T (s) is the adaptive coefficient switching function, as follows;

[0029] T (s) =

[0030] In the formula s is the adaptive coefficient, the hyperparameter s 0 is the adaptive coefficient threshold greater than 1;

[0031] Step S12;

[0032] Update the prior state vector and prior skew estimate variance obtained in step S11 according to the observation value, and the update formula is as follows;

[0033]

[0034]

[0035] where is the k posterior state vector of the autonomous sightseeing vehicle at the moment, is the Kalman gain, is the observed value, is k the conversion matrix from the state variable to the observed variable at time is k the posterior estimation covariance at time

[0036] wherein, -1

[0037] in the formula, is Gaussian white noise subject to a normal distribution with a mean of 0;

[0038] The posterior state vector is k the current optimal estimation vector of the state vector of the autonomous sightseeing vehicle at time , which includes the optimal estimated value of the current position of the autonomous sightseeing vehicle.

[0039] Furthermore, in the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle of the present invention, the adaptive coefficient s in step S11 is calculated by the following formula;

[0040]

[0041] wherein, N is a hyperparameter, indicating the width of the time window before the taken time k ;

[0042] is the k th deviation between the observed vector z k and the predicted value , and the calculation formula is as follows;

[0043]

[0044] is transpose of;

[0045] Tr() is the matrix trace calculation function, and the subscript j corresponds to the serial numbers of each marked point passed by the autonomous sightseeing vehicle No. 0 to N -1;

[0046] When s ≥ s 0, the update step S12 will increase the deviation of the prior state vector of the autonomous sightseeing vehicle, thereby increasing the weight of the observed information in the update stage.

[0047] Furthermore, in the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle of the present invention, the target point is collected by an RTK high-precision positioning device.

[0048] Furthermore, in the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle of the present invention, a GPS / INS integrated navigation system is installed on the autonomous sightseeing vehicle, and the observed values are obtained by the GPS / INS integrated navigation system.

[0049] Furthermore, in the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle of the present invention, the prior state vector and the posterior state vector include the position, speed, and attitude parameters of the autonomous sightseeing vehicle.

[0050] The advantage of this method for detecting the path tracking accuracy of the autonomous sightseeing vehicle is that it obtains the tracking errors of the autonomous sightseeing vehicle for each target point in sequence, and obtains the path tracking accuracy of the autonomous sightseeing vehicle according to the maximum value of the tracking errors of all target points. At the same time, when obtaining the tracking errors of each target point, through the setting of the adaptive coefficient switching function, the adaptive setting of the Kalman filter algorithm coefficients is realized, and then the dynamic adaptive adjustment of the weights of the predicted value and the observed value is realized in the optimal estimation process of the state vector of the Kalman filter algorithm, overcoming the problem of Kalman gain divergence in the recursive process.

[0051] The above description is only an overview of the technical solution of the present invention. In order to understand the technical means of the present invention more clearly and implement it specifically according to the content of the specification, the following takes the embodiments of the present invention to describe it in detail. Description of the Drawings

[0052] Figure 1 is a flowchart of the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle of the present invention.

[0053] Figure 2 is a schematic diagram of the target path and target points that the autonomous sightseeing vehicle needs to track.

[0054] In the figure, path 1; path centerline 2; target point 3. Detailed Embodiments

[0055] The following combines the drawings and embodiments to further describe the detailed embodiments of the present invention in detail. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0056] Refer to Figures 1 to 2 , the method for detecting the path tracking accuracy of the autonomous sightseeing vehicle in this embodiment includes the following steps;

[0057] Plan the path 1 that the autonomous sightseeing vehicle needs to track;

[0058] Collect the coordinate information of M target points 3 on the path center line 2 by using an RTK high-precision positioning device. The M target points are numbered as No. 1, No. 2, …, No. M respectively. The starting point is the No. 1 target point, and the ending point is the No. M target point. Input the coordinate information of the M target points into the automatic driving control system of the automatic driving sightseeing vehicle;

[0059] Install a GPS / INS integrated navigation system on the vehicle. The GPS / INS integrated navigation system uses an adaptive Kalman filtering algorithm to achieve high-precision positioning;

[0060] Drive the automatic driving sightseeing vehicle to the No. 1 target point. The adaptive Kalman filtering algorithm uses the data output by the GPS / INS integrated navigation system to make an optimal estimate of the current position of the automatic driving sightseeing vehicle. The system establishes a state vector based on the position, speed, and heading angle of the automatic driving sightseeing vehicle. The system state space expression is as follows;

[0061] +

[0062]

[0063] In the formula, , are the state vectors of the automatic driving sightseeing vehicle at the k th moment and the k-1 th moment respectively, is the state transition matrix from the k-1 th moment to the k th moment, is the control matrix, is the control input vector at the k th moment, is the system noise vector at the k-1 th moment. The noise follows a normal distribution with a mean of 0, and the covariance matrix is expressed as , is the observation vector at the k th moment, is the conversion matrix from the state variable to the observation variable at the k th moment, is the observation noise vector at the k th moment. It is Gaussian white noise and follows a normal distribution with a mean of 0. The covariance matrix is expressed as .

[0064] The optimal estimate of the position of the automatic driving sightseeing vehicle by the Kalman filtering algorithm is divided into two steps: prediction and update. In the prediction step, predict the prior state vector k and the prior covariance estimate of the automatic driving sightseeing vehicle at the th moment. The prediction formula is as shown in the formula.

[0065]

[0066]

[0067] Among them, is the state transition matrix of the autonomous sightseeing vehicle from the k -1 moment to the k moment, is k the posterior state vector of the autonomous sightseeing vehicle at the is k the control matrix of the autonomous sightseeing vehicle at the is k the control input vector of the autonomous sightseeing vehicle at the s moment, T( is k the posterior estimation covariance of the autonomous sightseeing vehicle at the is the transpose matrix of is k the moment, and

[0068] is the system noise vector that follows a normal distribution with a mean of 0 (s) where T

[0069] T (s) =

[0070] In the formula s is the adaptive coefficient, and the hyperparameter s 0 is the adaptive coefficient threshold greater than 1;

[0071] In the update step, the system observes the pseudo-range and Doppler shift values of each satellite of the autonomous sightseeing vehicle based on the satellite position, speed, etc. obtained from the GPS satellite ephemeris, and updates the prior estimation vector according to the deviation between the observed value and the predicted value, as shown in the formula;

[0072]

[0073]

[0074] Among them, is k the posterior state vector of the autonomous sightseeing vehicle at the is the Kalman gain, is the observed value, is kThe conversion matrix from the moment state variable to the observation variable, is k the posterior estimation covariance after the moment;

[0075] Among them, -1 ;

[0076] In the formula, is Gaussian white noise that follows a normal distribution with a mean of 0;

[0077] Among them, the posterior state vector is k the current optimal estimation vector of the state vector of the autonomous sightseeing vehicle at the moment, which includes the optimal estimated value of the current position of the autonomous sightseeing vehicle.

[0078] The system calculates the shortest distance between the autonomous sightseeing vehicle and the No. 1 target point according to the above optimal estimated value, and this shortest distance is the tracking error value of the autonomous sightseeing vehicle for the first target point;

[0079] Next, the autonomous sightseeing vehicle automatically follows the target path. The adaptive Kalman filtering algorithm optimally estimates the current driving position in real time and calculates the distance between the optimal estimated value and the next target point, that is, the No. 2 target point. As the autonomous sightseeing vehicle approaches and then moves away from the No. 2 target point, the distance between the autonomous sightseeing vehicle and the No. 2 target point first decreases and then increases. The system automatically records the minimum distance between the autonomous sightseeing vehicle and the No. 2 target point, which is the tracking error value of the autonomous sightseeing vehicle for the second target point.

[0080] The adaptive Kalman filtering algorithm optimally estimates the current position of the autonomous sightseeing vehicle and calculates the distance between the current position and the next target point, such as N the No. N target point. As the autonomous sightseeing vehicle approaches and then moves away from N the No. N target point, the distance between the autonomous sightseeing vehicle and n the No.

[0081] The system can evaluate the path tracking ability of the autonomous sightseeing vehicle according to the tracking errors of the autonomous sightseeing vehicle for the above M coordinate points. The maximum value of the M tracking errors is the maximum error value of the path tracking of the autonomous sightseeing vehicle, that is, its tracking accuracy.

[0082] The above adaptive coefficient s is calculated through the following formula;

[0083]

[0084] Among them,N is a hyperparameter representing the width of the time window before the k time instant;

[0085] is the k observation vector z at the k time instant and the deviation from the predicted value is calculated as follows;

[0086]

[0087] is the transpose;

[0088] Tr() is the matrix trace calculation function, and the subscript j corresponds to the sequence numbers of the respective marked points passed by the N autonomous sightseeing vehicle No. 0 to

[0089] When s ≥ s 0, the update step S12 will increase the deviation of the prior state vector of the autonomous sightseeing vehicle, thereby increasing the weight of the observation information in the update stage.

[0090] The state vector of the autonomous sightseeing vehicle, including the prior state vector and the posterior state vector, is established based on the position, speed, and attitude of the autonomous sightseeing vehicle. The state vector includes 10 dimensions, including 3 position parameters, 3 speed parameters, and quaternions.

[0091] Above, the RTK high-precision positioning device is a positioning device using carrier phase differential technology, and the GPS / INS integrated navigation system is an integrated system combining the GPS global positioning system and the inertial navigation system, both of which are existing commercially available products.

[0092] The method for detecting the path tracking accuracy of the autonomous sightseeing vehicle in this embodiment obtains the tracking error of the autonomous sightseeing vehicle for each target point in sequence, and obtains the path tracking accuracy of the autonomous sightseeing vehicle according to the maximum value of all target point tracking errors. At the same time, when obtaining the tracking error of each target point, through the setting of the adaptive coefficient switching function, the adaptive setting of the Kalman filter algorithm coefficients is realized, and then the dynamic adaptive adjustment of the weights of the predicted value and the observed value in the optimal estimation process of the Kalman filter algorithm state vector is realized, overcoming the problem of Kalman gain divergence in the recursive process.

[0093] Among them, the state transition matrix , the control matrix , and the control input vector are determined by the inherent attributes of the autonomous sightseeing vehicle, is the observed value, and the transformation matrix Determined by the self-driving sightseeing vehicle and the reference system of the observed values, the specific calculation methods of the above variables or parameter values are conventional techniques in this field and will not be elaborated here. For details, reference can be made to relevant literature, such as "Kalman Filter and Its Real-Time Applications" (authors: Dai Hongde, Li Juan, Dai Shaowu, etc., publisher: Tsinghua University Press, 2018). Hyperparameters Is a preset adaptive coefficient threshold, which is set by the operator according to experience.

[0094] The above are only the preferred embodiments of the present invention, which are used to assist those skilled in the art to implement the corresponding technical solutions, rather than to limit the protection scope of the present invention. The protection scope of the present invention is defined by the appended claims. It should be noted that for those of ordinary skill in the art, based on the technical solutions of the present invention, several equivalent improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention. At the same time, it should be understood that although this specification is described according to the above embodiments, not every embodiment only contains an independent technical solution. This narrative way of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions of each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for detecting the path tracking accuracy of an autonomous sightseeing vehicle, comprising the following steps; Collect coordinate data of several target points on the planned path; During the automatic driving of the autonomous sightseeing vehicle along the path, sequentially obtain the tracking errors of the autonomous sightseeing vehicle for each target point; Among multiple tracking errors, the one with the largest error value is the path tracking accuracy of the autonomous sightseeing vehicle; It is characterized in that: the method for obtaining the tracking error of the autonomous sightseeing vehicle for a certain target point comprises the following steps; Step S1; During the process of the autonomous sightseeing vehicle approaching and leaving the target point, obtain the optimal estimated value of the current position of the autonomous sightseeing vehicle at every predetermined time interval; Step S2; According to the optimal estimated value of the current position of the autonomous sightseeing vehicle obtained in Step S1, calculate the distance between it and the target point; Step S3; Among the distances between multiple optimal estimated values obtained in Step S1 and the target point and the target point, the one with the smallest distance is the tracking error of the autonomous sightseeing vehicle for the target point; In step S1, a certain k method for obtaining the optimal estimated value of the current position of the autonomous sightseeing vehicle at a moment includes the following steps; Step S11; Establish a state vector based on the state parameters of the autonomous sightseeing vehicle and an observation vector , the state vector includes the position, speed, and heading angle parameters of the autonomous sightseeing vehicle, and the state vector and the observation vector are expressed as follows; ; ; Prediction k Prior state vector of the autonomous sightseeing vehicle at a certain moment and prior covariance estimation variance , the prediction formula is as follows; ; ; Among them, is the state transition matrix of the autonomous sightseeing vehicle from the k -1 moment to the k moment, is k the posterior state vector of the autonomous sightseeing vehicle at the -1 moment, k is the control matrix of the autonomous sightseeing vehicle at the moment, k is the control input vector of the autonomous sightseeing vehicle at the s moment, T( ) is the adaptive coefficient switching function, k is the posterior estimation covariance of the autonomous sightseeing vehicle at the -1 moment, is the transpose matrix of is k the system noise vector that follows a normal distribution with a mean of 0 at the moment, where T (s) is the adaptive coefficient switching function, as shown below; ; where s is an adaptive coefficient, a hyperparameter s 0 is a threshold of the adaptive coefficient greater than 1; Step S12; update the prior state vector obtained in step S11 according to the observation value and the prior skew estimation variance , and the update formula is as follows; ; ; Among them, is k the posterior state vector of the autonomous sightseeing vehicle at time is the Kalman gain, is the observation value, is k the conversion matrix from the state variable to the observation variable at time is k the posterior estimation covariance at time Among them, ; In the formula, is Gaussian white noise that follows a normal distribution with a mean of 0; The posterior state vector is k The current optimal estimation vector of the state vector of the autonomous sightseeing vehicle at a moment, which includes the optimal estimated value of the current position of the autonomous sightseeing vehicle; Among them, is the observation noise vector at k moment, and the noise follows a normal distribution with a mean of 0. is k-1 the system noise vector at moment, which is Gaussian white noise and follows a normal distribution with a mean of 0.

2. The method for detecting the path tracking accuracy of an autonomous sightseeing vehicle according to claim 1, characterized in that: The adaptive coefficient in step S11 s is calculated by the following formula; ; Among them, N is a hyperparameter, representing the k time window width before the selected moment; is the k observation vector at the moment, and the deviation from the predicted value is as follows; ; is the transpose of; Tr() is a matrix trace calculation function, and the subscript j corresponds to the serial numbers of each marked point passed by the driverless sightseeing vehicle No. N starting from 0 to -1; When s ≥ s is 0 or greater, update step S12 increases the deviation of the prior state vector of the autonomous sightseeing vehicle, thereby increasing the weight of the observation information in the update phase.

3. The method for detecting the path tracking accuracy of the autonomous sightseeing vehicle according to claim 1, characterized in that: The target point is collected by an RTK high-precision positioning device.

4. The method for detecting the path tracking accuracy of the autonomous sightseeing vehicle according to claim 1, characterized in that: A GPS / INS integrated navigation system is installed on the autonomous sightseeing vehicle, and the observed values are obtained by the GPS / INS integrated navigation system.

5. The method for detecting the path tracking accuracy of an autonomous sightseeing vehicle according to claim 1, characterized in that: The prior state vector and the posterior state vector include the position, speed and attitude parameters of the autonomous sightseeing vehicle.

Images