CN114357794B - Bus arrival and stop evaluation method based on improved Kalman filtering - Google Patents
Bus arrival and stop evaluation method based on improved Kalman filtering Download PDFInfo
- Publication number
- CN114357794B CN114357794B CN202210033540.4A CN202210033540A CN114357794B CN 114357794 B CN114357794 B CN 114357794B CN 202210033540 A CN202210033540 A CN 202210033540A CN 114357794 B CN114357794 B CN 114357794B
- Authority
- CN
- China
- Prior art keywords
- bus
- point
- vehicle
- yaw rate
- lateral acceleration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000011156 evaluation Methods 0.000 title claims abstract description 60
- 238000001914 filtration Methods 0.000 title claims abstract description 23
- 230000001133 acceleration Effects 0.000 claims abstract description 54
- 238000012545 processing Methods 0.000 claims abstract description 34
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 238000000034 method Methods 0.000 claims description 73
- 230000008569 process Effects 0.000 claims description 46
- 239000011159 matrix material Substances 0.000 claims description 24
- 240000004282 Grewia occidentalis Species 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 6
- 238000012360 testing method Methods 0.000 claims description 5
- 230000003137 locomotive effect Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000013097 stability assessment Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001052 transient effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 3
- 238000013461 design Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/02—Reliability analysis or reliability optimisation; Failure analysis, e.g. worst case scenario performance, failure mode and effects analysis [FMEA]
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Software Systems (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Computing Systems (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a bus arrival and stop evaluation method based on improved Kalman filtering. Pushing the measured track of a certain point on the bus to four corner tracks by adopting a relative position calculation method, and carrying out bus arrival completion evaluation; and processing the yaw rate and lateral acceleration data by adopting improved Kalman filtering, and evaluating the bus arrival stability. The invention is irrelevant to the realization principle and technology of the bus stop, and carries out the evaluation from the perspective of a third party, and has the characteristics of multiple evaluation indexes and accurate evaluation results.
Description
Technical Field
The invention relates to an evaluation method for bus arrival and parking application, in particular to an evaluation method for bus arrival and parking based on improved Kalman filtering, and belongs to the field of intelligent driving evaluation.
Background
According to the definition of "automatic classification of automobile driving" formally implemented on 1 month 1 year 2021, the existing automatic driving buses (Self-drivingbus) are classified into L3 (the system can continuously perform all dynamic driving tasks in its design operating conditions) and L4 (the system can continuously perform all dynamic driving tasks and perform dynamic driving task takeover in its design operating conditions); in a driving task of an automatic driving Bus, bus Stop (BS) is a typical application applied to an actual traffic scene. The automatic driving bus is driven to a platform or a parking space in the process of entering the station and stopping, and certain safety standards are met; after the vehicle is completely stopped, certain standards are met, but a complete evaluation system or evaluation equipment does not exist at present.
The method aims at providing an evaluation method for the application. Firstly, basic parameter data required by the evaluation of a bus to be tested are acquired, then known position data of a station is combined, and according to related standard requirements, the evaluation result is finally obtained through analysis on the basis of high-precision data processing. The evaluation content mainly comprises two aspects related to the degree of completion and the stability, and the evaluation items in the degree of completion comprise: the included angle between the axis of the vehicle and the lateral line of the parking space when the entering parking is completed, the maximum distance between the side surface of the tested vehicle and the platform when the entering parking is completed, and the maximum distance between the headstock and the front mark line when the entering parking is completed are evaluated; the evaluation items in terms of process stability include: maximum value of vehicle yaw rate and lateral acceleration.
Disclosure of Invention
The invention aims to provide a bus arrival and stop evaluation method based on improved Kalman filtering, which is characterized by comprising the following steps of: basic parameter information of a tested bus in the driving process is obtained by utilizing the integrated navigation system, data processing is carried out by adopting improved Kalman filtering, and the evaluation of indexes such as the distance between the side surface of the bus and a platform when the entering parking is finished, the included angle between the axis of the vehicle and the lateral line of a parking space when the entering parking is finished, the maximum distance between the head of the bus and a front mark line when the entering parking is finished, the stability of the entering process of the tested bus and the like is realized, wherein the specific steps comprise:
step one: basic parameter acquisition
And installing a double-antenna integrated navigation system for the bus to be tested, wherein the integrated navigation system time takes the Beidou time as a reference standard. The basic parameter information required for evaluation includes: longitude and latitude coordinates, speed, yaw rate, lateral acceleration, course angle and the like of the bus to be detected in the process of entering the bus stop; parameters of the bus to be tested, namely the width and length of the bus, and the distance from the rear main antenna to the head and the side of the bus; the longitude and latitude coordinates of the station side line are represented by coordinates of four points.
Step two: data processing
On the basis of acquiring basic parameters required by evaluation, the data processing mainly comprises two parts, wherein one part is to calculate the motion trail of four corner points of the vehicle according to the motion trail of a main antenna at the rear side of the vehicle and the vehicle parameters, and the main purpose of the data processing of the part is to evaluate the completion degree of the bus entering and stopping; the other part is to consider that the yaw rate and the lateral acceleration data are greatly fluctuated due to the influence of sensor noise, and the two groups of data need to be subjected to improved Kalman filtering processing, and the main purpose of the data processing is to evaluate the stability of the bus entering and stopping process.
Sub-step 1: defining and calculating four-corner point motion trail of detected bus
The method comprises the steps of taking the left front corner of a vehicle as a starting point, defining an A point, sequentially defining a point A, a point B, a point C and a point D aiming at four corner points of a tested bus; the main antenna at the rear side of the vehicle is defined as E point; the middle points of the vehicle head and the vehicle tail are respectively defined as M points and N points; defining the distance from the main antenna to the head of a vehicle as Lf, the distance from the main antenna to the side of the vehicle as Ll, the width of the vehicle as W and the length as L so as to facilitate calculation; the course angle obtained by reading is defined as Dir;
when the running track of the bus to be tested is acquired, because the acquired track data is longitude and latitude information output by combined navigation, gaussian projection transformation is needed to be carried out on the track data, so that the track data is unitized into meters, and the evaluation index item is convenient to calculate:
wherein:
ex (x, y) is an abscissa transform function, ey (x, y) is an ordinate transform function, x is latitude, and y is longitude;
Calculating the angle (radian system) required by data processing:
The four angular point motion trail of the bus to be tested can be calculated according to the trigonometric function as follows:
Wherein Ax (x, y) is an A-point abscissa transform function, ay (x, y) is an A-point ordinate transform function, bx (x, y) is a B-point abscissa transform function, by (x, y) is a B-point ordinate transform function, cx (x, y) is a C-point abscissa transform function, cy (x, y) is a C-point ordinate transform function, dx (x, y) is a D-point abscissa transform function, dy (x, y) is a D-point ordinate transform function, x is latitude, and y is longitude;
Sub-step 2: improved Kalman filter processing of yaw rate and lateral acceleration
The yaw rate and the lateral acceleration can represent the stability of the vehicle in the motion process, but because the yaw rate and the lateral acceleration are influenced by the noise of the sensor, the data fluctuation of the yaw rate and the lateral acceleration which are directly acquired is larger, and the data fluctuation is directly used for judging that the stability can generate larger errors, so that the filtering process is needed; the method for improving the Kalman filter is adopted, and the processing procedure is as follows:
(1) From the yaw acceleration versus lateral acceleration, it is possible to:
Wherein, Representing the calculated lateral acceleration at time n-1,Represents the calculated yaw rate at time n-1,For a priori lateral acceleration at time n,The yaw rate is a priori for time n,And establishing a relation between the two, wherein V is the running speed of the vehicle.
(2) Calculating a priori estimated covariance matrix at the moment k:
Wherein the method comprises the steps of For the prior estimated covariance matrix at time n, P n-1 is the posterior estimated covariance matrix at time n-1, and Q is the process error matrix.
(3) Calculating Kalman gain:
Where H is the state variable to measured state transition matrix, where the value is R is a measurement error matrix.
(4) Calculating a posterior estimated value:
Wherein the method comprises the steps of For the posterior lateral acceleration at time n,For the prior yaw rate at time n, the two are the obtained filtered results, and Z n is an observation matrix which is used as an input part of the filtering.
(5) Updating the covariance matrix:
(6) Introducing forgetting factors:
In the process of processing the detected bus yaw rate and lateral acceleration data, considering the transient of vehicle motion state change, new data can reflect parameter change conditions more than old data, so forgetting factors are introduced, and the weight of the new and old data in the recursion process is modified:
When gamma is smaller, forgetting is faster; when γ is larger, forgetting is slower; here, in order to avoid that the new data cannot be corrected for the estimated value, γ is taken in (0.95,0.995);
Step three: bus stop-in evaluation
Sub-step 1: completion evaluation
(1) Evaluation of included angle between vehicle axis and outside side line of parking space when entering parking is completed
First, a classification discussion is required for the range of heading angles:
and then integrating and calculating with the coordinates of the side line outside the station:
Wherein (X1, Y1), (X2, Y2) are coordinates of an O point and a P point of a station outside edge line subjected to Gaussian projection coordinate conversion respectively;
when the included angle is larger than 0, the vehicle head faces the outer side of the station, and when the included angle is smaller than 0, the vehicle tail faces the outer side of the station; when the included angle is more than or equal to-3 degrees and less than or equal to theta <3 degrees, the entering stop meets the standard.
(2) Distance assessment for testing vehicle side surface from platform when entering station and stopping
The distance of the vehicle side from the platform can be calculated by the triangle area:
the three sides are respectively:
from the equation of halen:
wherein (X3, Y3), (X4, Y4) are coordinates of an S point and a T point of a station inner side line subjected to Gaussian projection coordinate conversion respectively;
from this, the distance function ADis (x, y) of the corner a from the platform can be obtained, and the distance functions BDis (x, y), CDis (x, y), DDis (x, y) of the other three corner B, C, D from the platform can be obtained by analogy, and the distance between the vehicle corner and the platform can be determined by combining the included angle conditions: taking minimum values of BDis (x, y) and CDis (x, y) when theta is less than or equal to 0, taking minimum values of ADis (x, y) and DDis (x, y) when theta is more than or equal to 0, and taking the obtained value d 1,d2 as the required distance, wherein when d 1 is more than or equal to 0.05m and d 2 is more than or equal to 0.25m, the inbound parking meets the requirements.
(3) Maximum distance evaluation of locomotive and front mark line of car when entering station and stopping
The distance between the headstock and the front side line of the car is calculated through the area of the triangle:
the three sides are respectively:
from the equation of halen:
Wherein (X2, Y2), (X3, Y3) are coordinates of a station boundary P point and an S point, respectively, which have undergone Gaussian projection coordinate conversion;
The distance function AFDis (x, y) of the corner point A from the front line of the car can be obtained, and the distance function BFDis (x, y) of the corner point B from the front line of the car can be obtained by analogy, wherein the maximum value in AFDis (x, y) or BFDis (x, y) can be selected as the maximum distance M e according to practical situations, and when M e is more than or equal to 0.05M and less than 0.25M, the requirement of parking in the stop is met.
Sub-step 2: stability assessment of inbound Process
The stability of the bus in-process is one of the key indexes for measuring the in-process parking function, and the maximum value of the lateral acceleration and the yaw velocity in the in-process is usually required to be smaller than a certain threshold value, namely
Wherein,Representing the lateral acceleration value of the modified kalman filter process, a y_threshold is the lateral acceleration threshold,The yaw-rate value, ω z_threshold, representing the modified kalman filter process is the yaw-rate threshold. And in the whole process, the yaw rate and the lateral acceleration are not larger than the threshold value, and the inbound parking stability meets the standard.
The invention has the advantages and remarkable effects that:
(1) The evaluation method provided by the invention can calculate the motion trail of the four corner points by only measuring the position information of a certain point on the tested bus, is irrelevant to the principle and technology of bus stop-in and stop, and is developed from the perspective of a third party, and the test items are comprehensive.
(2) The invention adopts the improved Kalman filtering to process the yaw rate and lateral acceleration information, thereby ensuring the accuracy and reliability of the evaluation result.
Drawings
FIG. 1 is a schematic illustration of a bus stop-in evaluation application;
FIG. 2 is a schematic diagram of an evaluation flow;
FIG. 3 is a data processing flow diagram;
fig. 4 is a diagram showing the actual effect of the improved kalman filter.
Detailed Description
According to the definition of "automatic classification of automobile driving" formally implemented on 1 month 1 of 2021, an existing automatic driving bus (Self-driving bus) is classified into L3 (the system can continuously perform all dynamic driving tasks in its design operation condition) and L4 (the system can continuously perform all dynamic driving tasks and perform dynamic driving task takeover in its design operation condition); in a driving task of an automatic driving Bus, bus Stop (BS) is a typical application applied to an actual traffic scene. The automatic driving bus is driven to a platform or a parking space in the process of entering the station and stopping, and certain safety standards are met; after the bus is completely stopped, the bus meets certain standard, and the driver cannot directly observe the bus in the process, so that certain auxiliary measures are needed to achieve the purposes of assisting the driver in avoiding or relieving side collision and improving the safety of the bus in the process of entering the bus stop. The schematic diagram is shown in fig. 1.
At present, a complete evaluation system or evaluation equipment does not exist for the application, and the method of the invention aims to provide an evaluation method for the application. Firstly, basic parameter data required by the evaluation of a bus to be tested are acquired, then known position data of a station is combined, and according to related standard requirements, the evaluation result is finally obtained through analysis on the basis of high-precision data processing. The evaluation content mainly comprises two aspects related to the degree of completion and the stability, and the evaluation items in the degree of completion comprise: the included angle between the axis of the vehicle and the lateral line of the parking space when the entering parking is completed, the maximum distance between the side surface of the tested vehicle and the platform when the entering parking is completed, and the maximum distance between the headstock and the front mark line when the entering parking is completed are evaluated; the evaluation items of the process stability comprise: maximum value of vehicle yaw rate and lateral acceleration. The evaluation flow is shown in fig. 2.
The invention aims to provide a bus arrival and stop evaluation method based on an improved Kalman filter, which is characterized by comprising the following steps of: basic parameter information of a tested bus in the driving process is obtained by utilizing the integrated navigation system, data processing is carried out by adopting improved Kalman filtering, and the evaluation of indexes such as the distance between the side surface of the bus and a platform when the entering parking is finished, the included angle between the axis of the vehicle and the lateral line of a parking space when the entering parking is finished, the maximum distance between the head of the bus and a front mark line when the entering parking is finished, the stability of the entering process of the tested bus and the like is realized, wherein the specific steps comprise:
step one: basic parameter acquisition
And installing a double-antenna integrated navigation system for the bus to be tested, wherein the integrated navigation system time takes the Beidou time as a reference standard. The basic parameter information required for evaluation includes: longitude and latitude coordinates, speed, yaw rate, lateral acceleration, course angle and the like of the bus to be detected in the process of entering the bus stop; parameters of the bus to be tested, namely the width and length of the bus, and the distance from the rear main antenna to the head and the side of the bus; the longitude and latitude coordinates of the station side line are represented by coordinates of four points.
Step two: data processing
On the basis of acquiring basic parameters required by evaluation, the data processing mainly comprises two parts, wherein one part is to calculate the motion trail of four corner points of the vehicle according to the motion trail of a main antenna at the rear side of the vehicle and the vehicle parameters, the calculation flow is shown in fig. 3, and the main purpose of the data processing of the part is to evaluate the completion degree of bus entering and stopping; the other part is to consider that the yaw rate and the lateral acceleration data are greatly fluctuated due to the influence of sensor noise, and the two groups of data need to be subjected to improved Kalman filtering processing, and the main purpose of the data processing is to evaluate the stability of the bus entering and stopping process.
Sub-step 1: defining and calculating four-corner point motion trail of detected bus
The method comprises the steps of taking the left front corner of a vehicle as a starting point, defining an A point, sequentially defining a point A, a point B, a point C and a point D aiming at four corner points of a tested bus; the main antenna at the rear side of the vehicle is defined as E point; the middle points of the vehicle head and the vehicle tail are respectively defined as M points and N points; defining the distance from the main antenna to the head of a vehicle as Lf, the distance from the main antenna to the side of the vehicle as Ll, the width of the vehicle as W and the length as L so as to facilitate calculation; the course angle obtained by reading is defined as Dir; when the running track of the bus to be tested is acquired, because the acquired track data is longitude and latitude information output by combined navigation, gaussian projection transformation is needed to be carried out on the track data, so that the track data is unitized into meters, and the evaluation index item is convenient to calculate:
wherein:
ex (x, y) is an abscissa transform function, ey (x, y) is an ordinate transform function, x is latitude, and y is longitude;
Calculating the angle (radian system) required by data processing:
The four angular point motion trail of the bus to be tested can be calculated according to the trigonometric function as follows:
Wherein Ax (x, y) is an A-point abscissa transform function, ay (x, y) is an A-point ordinate transform function, bx (x, y) is a B-point abscissa transform function, by (x, y) is a B-point ordinate transform function, cx (x, y) is a C-point abscissa transform function, cy (x, y) is a C-point ordinate transform function, dx (x, y) is a D-point abscissa transform function, dy (x, y) is a D-point ordinate transform function, x is latitude, and y is longitude;
Sub-step 2: improved Kalman filter processing of yaw rate and lateral acceleration
The yaw rate and the lateral acceleration can represent the stability of the vehicle in the motion process, but because the yaw rate and the lateral acceleration are influenced by the noise of the sensor, the data fluctuation of the yaw rate and the lateral acceleration which are directly acquired is larger, and the data fluctuation is directly used for judging that the stability can generate larger errors, so that the filtering process is needed; the method for improving the Kalman filter is adopted, and the processing procedure is as follows:
(1) From the yaw acceleration versus lateral acceleration, it is possible to:
Wherein, Representing the calculated lateral acceleration at time n-1,Represents the calculated yaw rate at time n-1,For a priori lateral acceleration at time n,The yaw rate is a priori for time n,And establishing a relation between the two, wherein V is the running speed of the vehicle.
(2) Calculating a priori estimated covariance matrix at the moment k:
Wherein the method comprises the steps of For the prior estimated covariance matrix at time n, P n-1 is the posterior estimated covariance matrix at time n-1, and Q is the process error matrix.
(3) Calculating Kalman gain:
Where H is the state variable to measured state transition matrix, where the value is R is a measurement error matrix.
(4) Calculating a posterior estimated value:
Wherein the method comprises the steps of For the posterior lateral acceleration at time n,For the prior yaw rate at time n, the two are the obtained filtered results, and Z n is an observation matrix which is used as an input part of the filtering.
(5) Updating the covariance matrix:
(6) Introducing forgetting factors:
In the process of processing the detected bus yaw rate and lateral acceleration data, considering the transient of vehicle motion state change, new data can reflect parameter change conditions more than old data, so forgetting factors are introduced, and the weight of the new and old data in the recursion process is modified:
When gamma is smaller, forgetting is faster; when γ is larger, forgetting is slower; here, γ is taken in (0.95,0.995) in order to avoid that the new data cannot be corrected for the estimate.
The filtered effects of yaw rate and lateral acceleration are shown in fig. 4.
Step three: bus stop-in evaluation
Sub-step 1: completion evaluation
(1) Evaluation of included angle between vehicle axis and outside side line of parking space when entering parking is completed
First, a classification discussion is required for the range of heading angles:
and then integrating and calculating with the coordinates of the side line outside the station:
Wherein (X1, Y1), (X2, Y2) are coordinates of an O point and a P point of a station outside edge line subjected to Gaussian projection coordinate conversion respectively;
when the included angle is larger than 0, the vehicle head faces the outer side of the station, and when the included angle is smaller than 0, the vehicle tail faces the outer side of the station; when the included angle is more than or equal to-3 degrees and less than or equal to theta <3 degrees, the entering stop meets the standard.
(2) Distance assessment for testing vehicle side surface from platform when entering station and stopping
The distance of the vehicle side from the platform can be calculated by the triangle area:
the three sides are respectively:
from the equation of halen:
wherein (X3, Y3), (X4, Y4) are coordinates of an S point and a T point of a station inner side line subjected to Gaussian projection coordinate conversion respectively;
from this, the distance function ADis (x, y) of the corner a from the platform can be obtained, and the distance functions BDis (x, y), CDis (x, y), DDis (x, y) of the other three corner B, C, D from the platform can be obtained by analogy, and the distance between the vehicle corner and the platform can be determined by combining the included angle conditions: taking minimum values of BDis (x, y) and CDis (x, y) when theta is less than or equal to 0, taking minimum values of ADis (x, y) and DDis (x, y) when theta is more than or equal to 0, and taking the obtained value d 1,d2 as the required distance, wherein when d 1 is more than or equal to 0.05m and d 2 is more than or equal to 0.25m, the inbound parking meets the requirements.
(3) Maximum distance evaluation of locomotive and front mark line of car when entering station and stopping
The distance between the headstock and the front side line of the car is calculated through the area of the triangle:
the three sides are respectively:
from the equation of halen:
Wherein (X2, Y2), (X3, Y3) are coordinates of a station boundary P point and an S point, respectively, which have undergone Gaussian projection coordinate conversion;
The distance function AFDis (x, y) of the corner point A from the front line of the car can be obtained, and the distance function BFDis (x, y) of the corner point B from the front line of the car can be obtained by analogy, wherein the maximum value in AFDis (x, y) or BFDis (x, y) can be selected as the maximum distance M e according to practical situations, and when M e is more than or equal to 0.05M and less than 0.25M, the requirement of parking in the stop is met.
Sub-step 2: stability assessment of inbound Process
The stability of the bus in-process is one of the key indexes for measuring the in-process parking function, and the maximum value of the lateral acceleration and the yaw velocity in the in-process is usually required to be smaller than a certain threshold value, namely
Wherein,Representing the lateral acceleration value of the modified kalman filter process, a y_threshold is the lateral acceleration threshold,The yaw-rate value, ω z_threshold, representing the modified kalman filter process is the yaw-rate threshold. And in the whole process, the yaw rate and the lateral acceleration are not larger than the threshold value, and the inbound parking stability meets the standard.
The evaluation method provided by the invention can calculate the motion trail of the four corner points by only measuring the position information of a certain point on the tested bus, is irrelevant to the principle and technology of bus stop-in and stop, and is developed from the perspective of a third party, and the test items are comprehensive. And the improved Kalman filtering is adopted to process the yaw rate and lateral acceleration information, so that the accuracy and reliability of the evaluation result are ensured.
Claims (1)
1. A bus arrival and stop evaluation method based on improved Kalman filtering is characterized in that: basic parameter information of a tested bus in the driving process is obtained by utilizing the integrated navigation system, data processing is carried out by adopting relative position calculation and improved Kalman filtering, and the evaluation of indexes such as the distance between the side surface of the bus and a platform when the entering parking is finished, the included angle between the axis of the vehicle and the lateral line of a parking space when the entering parking is finished, the maximum distance between the head of the bus and a front mark when the entering parking is finished, and the stability of the entering process of the tested bus is realized, wherein the specific steps comprise:
step one: basic parameter acquisition
The method comprises the steps that a dual-antenna integrated navigation system is installed for a bus to be tested, the system time of integrated navigation is based on Beidou time, and basic parameter information required by evaluation comprises: longitude and latitude coordinates, speed, yaw rate, lateral acceleration and course angle of the bus to be measured in the process of entering a station; parameters of the bus to be tested, namely the width and length of the bus, and the distance from the rear main antenna to the head and the side of the bus; the longitude and latitude coordinates of the station side line represent the station side line by coordinates of four points;
Step two: data processing
On the basis of acquiring basic parameters required by evaluation, the data processing comprises two parts, wherein one part is to calculate the motion trail of four corner points of the vehicle according to the motion trail of a main antenna at the rear side of the vehicle and the vehicle parameters, and the aim of the data processing is to evaluate the completion degree of the bus entering stop; the other part considers that the yaw rate and the lateral acceleration data are greatly fluctuated due to the influence of sensor noise, and needs to carry out improved Kalman filtering processing on the two groups of data, and the purpose of the data processing is to evaluate the stability of the bus entering and stopping process;
Sub-step 1: defining and calculating four-corner point motion trail of detected bus
The method comprises the steps of taking the left front corner of a vehicle as a starting point, defining an A point, sequentially defining a point A, a point B, a point C and a point D aiming at four corner points of a tested bus; the main antenna at the rear side of the vehicle is defined as E point; the middle points of the vehicle head and the vehicle tail are respectively defined as M points and N points; defining the distance from the main antenna to the head of a vehicle as Lf, the distance from the main antenna to the side of the vehicle as Ll, the width of the vehicle as W and the length as L so as to facilitate calculation; the course angle obtained by reading is defined as Dir; when the running track of the bus to be tested is acquired, because the acquired track data is longitude and latitude information output by combined navigation, gaussian projection transformation is needed to be carried out on the track data, so that the track data is unitized into meters, and the evaluation index item is convenient to calculate:
wherein:
ex (x, y) is an abscissa transform function, ey (x, y) is an ordinate transform function, x is latitude, and y is longitude;
calculating the angle required by data processing by adopting an radian system:
The four angular point motion trail of the bus to be tested can be calculated according to the trigonometric function as follows:
Wherein Ax (x, y) is an A-point abscissa transform function, ay (x, y) is an A-point ordinate transform function, bx (x, y) is a B-point abscissa transform function, by (x, y) is a B-point ordinate transform function, cx (x, y) is a C-point abscissa transform function, cy (x, y) is a C-point ordinate transform function, dx (x, y) is a D-point abscissa transform function, dy (x, y) is a D-point ordinate transform function, x is latitude, and y is longitude;
Sub-step 2: improved Kalman filter processing of yaw rate and lateral acceleration
The yaw rate and the lateral acceleration can represent the stability of the vehicle in the motion process, but because the yaw rate and the lateral acceleration are influenced by the noise of the sensor, the data fluctuation of the yaw rate and the lateral acceleration which are directly acquired is larger, and the data fluctuation is directly used for judging that the stability can generate larger errors, so that the filtering process is needed; the method for improving the Kalman filter is adopted, and the processing procedure is as follows:
(1) From the yaw acceleration versus lateral acceleration, it is possible to:
Wherein, Representing the calculated lateral acceleration at time n-1,Represents the calculated yaw rate at time n-1,For a priori lateral acceleration at time n,The yaw rate is a priori for time n,Establishing a relation between the two, wherein V is the running speed of the vehicle;
(2) Calculating a priori estimated covariance matrix at the moment k:
Wherein the method comprises the steps of For the prior estimated covariance matrix at time n, P n-1 is the posterior estimated covariance matrix at time n-1, and Q is the process error matrix;
(3) Calculating Kalman gain:
Where H is the state variable to measured state transition matrix, where the value is R is a measurement error matrix;
(4) Calculating a posterior estimated value:
Wherein the method comprises the steps of For the posterior lateral acceleration at time n,For the priori yaw rate at the moment n, the yaw rate and the priori yaw rate are obtained after filtering, and Z n is an observation value matrix and is used as an input part of filtering;
(5) Updating the covariance matrix:
(6) Introducing forgetting factors:
In the process of processing the detected bus yaw rate and lateral acceleration data, considering the transient of vehicle motion state change, new data can reflect parameter change conditions more than old data, so forgetting factors are introduced, and the weight of the new and old data in the recursion process is modified:
When gamma is smaller, forgetting is faster; when γ is larger, forgetting is slower; here, in order to avoid that the new data cannot be corrected for the estimated value, γ is taken in (0.95,0.995);
Step three: bus stop-in evaluation
Sub-step 1: completion evaluation
(1) Evaluation of included angle between vehicle axis and outside side line of parking space when entering parking is completed
First, a classification discussion is required for the range of heading angles:
and then integrating and calculating with the coordinates of the side line outside the station:
Wherein (X1, Y1), (X2, Y2) are coordinates of an O point and a P point of a station outside edge line subjected to Gaussian projection coordinate conversion respectively;
When the included angle is larger than 0, the vehicle head faces the outer side of the station, and when the included angle is smaller than 0, the vehicle tail faces the outer side of the station; when the included angle is more than or equal to-3 degrees and less than or equal to theta and less than 3 degrees, entering a stop to meet the standard;
(2) Distance assessment for testing vehicle side surface from platform when entering station and stopping
The distance of the vehicle side from the platform can be calculated by the triangle area:
the three sides are respectively:
from the equation of halen:
Wherein (X3, Y3), (X4, Y4) are coordinates of an S point and a T point of a station inner side line subjected to Gaussian projection coordinate conversion respectively;
From this, the distance function ADis (x, y) of the corner a from the platform can be obtained, and the distance functions BDis (x, y), CDis (x, y), DDis (x, y) of the other three corner B, C, D from the platform can be obtained by analogy, and the distance between the vehicle corner and the platform can be determined by combining the included angle conditions: taking minimum values of BDis (x, y) and CDis (x, y) when theta is less than or equal to 0, taking minimum values of ADis (x, y) and DDis (x, y) when theta is more than or equal to 0, wherein the obtained value d 1,d2 is the required distance, and the inbound parking meets the requirements when d 1 is more than or equal to 0.05m and less than or equal to 0.25m and d 2 is more than or equal to 0.05 m;
(3) Maximum distance evaluation of locomotive and front mark line of car when entering station and stopping
The distance between the headstock and the front side line of the car is calculated through the area of the triangle:
the three sides are respectively:
from the equation of halen:
wherein (X2, Y2), (X3, Y3) are coordinates of a station boundary P point and an S point, respectively, which have undergone Gaussian projection coordinate conversion;
the distance function AFDis (x, y) of the corner point A from the front line of the automobile can be obtained, and the distance function BFDis (x, y) of the corner point B from the front line of the automobile can be obtained by analogy, wherein the maximum value in AFDis (x, y) or BFDis (x, y) can be selected as the maximum distance M e according to actual conditions, and when M e is more than or equal to 0.05M and less than 0.25M, the requirement of parking in the station is met;
sub-step 2: stability assessment of inbound Process
The stability of the bus in-process is one of the key indexes for measuring the in-process parking function, and the maximum value of the lateral acceleration and the yaw rate in the in-process parking is required to be smaller than a certain threshold value, namely
Wherein,Representing the lateral acceleration value of the modified kalman filter process, a y_threshold is the lateral acceleration threshold,And (3) representing a yaw rate value subjected to improved Kalman filtering, wherein omega z_threshold is a yaw rate threshold value, and if the yaw rate and the lateral acceleration are not greater than the threshold value in the whole process, the inbound parking stability meets the standard.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210033540.4A CN114357794B (en) | 2022-01-12 | 2022-01-12 | Bus arrival and stop evaluation method based on improved Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210033540.4A CN114357794B (en) | 2022-01-12 | 2022-01-12 | Bus arrival and stop evaluation method based on improved Kalman filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114357794A CN114357794A (en) | 2022-04-15 |
| CN114357794B true CN114357794B (en) | 2024-07-05 |
Family
ID=81109936
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210033540.4A Active CN114357794B (en) | 2022-01-12 | 2022-01-12 | Bus arrival and stop evaluation method based on improved Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114357794B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114578832A (en) * | 2022-05-05 | 2022-06-03 | 魔门塔(苏州)科技有限公司 | Evaluation device, system and method for autonomous passenger-riding parking function and electronic equipment |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109211233A (en) * | 2018-09-25 | 2019-01-15 | 常熟理工学院 | Elevator motion detection and abnormal position parking judgement based on acceleration transducer |
| CN110940344A (en) * | 2019-11-25 | 2020-03-31 | 奥特酷智能科技(南京)有限公司 | Low-cost sensor combination positioning method for automatic driving |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9606240B2 (en) * | 2007-11-27 | 2017-03-28 | General Electric Company | Vehicle determination system and method using a kalman filter and critical milepost data |
| US10234292B2 (en) * | 2014-08-08 | 2019-03-19 | Stmicroelefctronics S.R.L. | Positioning apparatus and global navigation satellite system, method of detecting satellite signals |
| CN110987463B (en) * | 2019-11-08 | 2020-12-01 | 东南大学 | Multi-scene-oriented autonomous lane change performance test method for intelligent driving |
| CN111645699B (en) * | 2020-06-10 | 2021-08-31 | 北京理工大学 | Model-adaptive lateral velocity estimation method based on multi-sensor information fusion |
-
2022
- 2022-01-12 CN CN202210033540.4A patent/CN114357794B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109211233A (en) * | 2018-09-25 | 2019-01-15 | 常熟理工学院 | Elevator motion detection and abnormal position parking judgement based on acceleration transducer |
| CN110940344A (en) * | 2019-11-25 | 2020-03-31 | 奥特酷智能科技(南京)有限公司 | Low-cost sensor combination positioning method for automatic driving |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114357794A (en) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113819914B (en) | Map construction method and device | |
| CN110987463B (en) | Multi-scene-oriented autonomous lane change performance test method for intelligent driving | |
| US6522956B2 (en) | Method and device for estimating a transverse acceleration at an axle of a semitrailer or a trailer of a vehicle combination | |
| CN113147738A (en) | Automatic parking positioning method and device | |
| CN113109783B (en) | Course angle acquisition method, device, equipment and storage medium | |
| CN105718710A (en) | Driving behavior analysis method and equipment | |
| CN105716617B (en) | The system and method for driving locus is drawn based on vehicle data | |
| CN104678415A (en) | Car Navition System And Method Merged Gnss With Dr | |
| CN113819905B (en) | Mileage metering method and device based on multi-sensor fusion | |
| CN109115231B (en) | Vehicle positioning method and device and automatic driving vehicle | |
| CN109297486B (en) | Inertia and multi-odometer information-assisted vehicle motion state determination method and system | |
| RU2626424C1 (en) | Device and method for evaluation of vehicle attitude angle and position | |
| CN109859528B (en) | V2X Internet of vehicles-based method for classifying positions of vehicles at curves | |
| CN114252277B (en) | Automatic parking high-precision evaluation method based on wavelet transformation denoising | |
| CN109992884B (en) | A functional evaluation method for autonomous vehicles based on worst-case scenario search | |
| CN113252051B (en) | A method and device for constructing a map | |
| WO2023071442A1 (en) | Data processing method and apparatus | |
| CN104464375A (en) | Method for recognizing vehicle high-speed turning | |
| CN114357794B (en) | Bus arrival and stop evaluation method based on improved Kalman filtering | |
| CN111504584B (en) | Vibration evaluation method, device and system of sensor support and movable equipment | |
| CN109204318B (en) | Method and terminal equipment for judging rapid lane change of vehicle | |
| Liu et al. | Intelligent vehicle sideslip angle estimation considering measurement signals delay | |
| CN110264741A (en) | Road conditions detection method, device, equipment and medium based on motion sensor | |
| CN114323143B (en) | Vehicle data detection method and device, computer equipment and storage medium | |
| CN114639267A (en) | Vehicle collision avoidance early warning method in vehicle-road cooperative environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |