Multi-sensor fusion in-tunnel vehicle positioning method

Abstract

The invention discloses an in-tunnel vehicle positioning method based on multi-sensor fusion, and the method comprises the following steps: S1, collecting the initial position information of a vehicle through a first sensor and a second sensor before the vehicle enters a tunnel; s2, after the vehicle enters the tunnel, the distance of surrounding obstacles is detected through a third sensor on the vehicle; and S3, integrating the data obtained in the step S1 and the step S2, and calculating the accurate position of the vehicle by using an inertia derivation algorithm. According to the scheme, no new hardware cost is added, the operation and maintenance cost is reduced, the refitting of the vehicle is reduced to zero, the cost is moderate, deployment is easy, the positioning problem can still be efficiently solved in the environment with weak GPS signals or without GPS signals, and the application scene and effectiveness of the intelligent driving training system are greatly expanded.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and in particular to a method for positioning a vehicle in a tunnel. Background Art

[0002] In intelligent driving training, accurate vehicle location information is essential for automated teaching. However, the traditional global positioning system (GPS) signal will be severely interfered or even lost in tunnels, indoors and other environments, resulting in inaccurate positioning. This limits the application scenarios of intelligent robot training vehicles, because in these environments, the vehicle cannot determine its own position, and thus cannot determine the subject the trainee is practicing or evaluate the correctness or errors of the trainee's practice.

[0003] To solve this problem, there are currently many technical solutions that attempt to solve such problems, such as using inertial measurement units (IMUs), multi-line lidars, wheel encoders, or ground base stations to enhance the stability and accuracy of GPS positioning. Although these solutions can provide high-precision location information, they are costly, complex to deploy, or still cannot provide sufficient positioning accuracy in certain environments. For cost-sensitive use scenarios such as driving schools, they are almost impossible to apply because the cost far exceeds the income of the driving school and is difficult to deploy in practice. Summary of the invention

[0004] In order to solve one of the problems existing in the prior art, the present invention provides a multi-sensor fusion vehicle positioning method in a tunnel, comprising the following steps:

[0005] S1: Before the vehicle enters the tunnel, the first sensor and the second sensor are used to collect the initial position information of the vehicle;

[0006] S2: After the vehicle enters the tunnel, the distance of surrounding obstacles is detected by a third sensor on the vehicle;

[0007] S3: Integrate the data obtained in steps S1 and S2 and use the inertial derivation algorithm to calculate the precise position of the vehicle.

[0008] Furthermore, the first sensor is a GPS sensor.

[0009] Furthermore, the second sensor is an IMU sensor and / or a wheel speed sensor.

[0010] Furthermore, the third sensor is an ultrasonic sensor and / or a lidar sensor.

[0011] Furthermore, in step S1, the first sensor obtains the initial posture of the vehicle, and the initial posture includes the following variables: x, y, z, θ, where x represents the lateral position coordinate of the vehicle on the horizontal plane, y represents the longitudinal position coordinate of the vehicle on the horizontal plane, z represents the altitude coordinate of the vehicle, and θ represents the rotation angle of the vehicle on the horizontal plane relative to a reference direction.

[0012] Furthermore, the specific steps of the inertia derivation algorithm in step S3 are:

[0013] S31: Set the current state of the vehicle X k =[x,y,θ,v,w,acc]; where v is the speed of the vehicle, w is the angular velocity of the vehicle, and acc is the acceleration of the vehicle;

[0014] S32: Calculate the state quantity at the next moment in integral form:

[0015]

[0016] Among them, t represents the time variable;

[0017] S33: Simplify the state equation and obtain the final state equation:

[0018]

[0019] S34: Calculate the partial derivative of each state variable (x, y, θ, v, w, acc) to obtain the Jacobian matrix F_;

[0020] S35: Covariance matrix of the prediction process:

[0021] P_=F_*P_*F_.transpose()+Q_;wherein P_ on the right side of the equal sign is the state quantity covariance matrix, and its initial value is set by the system; F_.transpose() is the transpose of the Jacobian matrix F_ obtained in step S34, and Q_ is the noise error matrix;

[0022] S36: Calculate the Kalman gain matrix: K = P_*C.transpose()*(C*P_*C.transpose()+R.inverse(); where C is the state quantity, R.inverse() is the measurement error matrix, C.transpose() is the transpose of C, and the values ​​of C and R.inverse() are set by the system;

[0023] S37: X = X + K * error, where error refers to the error, which is obtained by subtracting the predicted data from the observed data;

[0024] S38: Covariance update P_=P_-K*C*P_.

[0025] The method proposed in the present invention does not require any additional hardware costs, reduces the work of integrating new sensors with existing equipment, and further reduces operation and maintenance costs, reducing vehicle modification to zero. Therefore, the invention is a vehicle positioning solution with moderate cost, easy deployment, and can still work efficiently in an environment with weak or no GPS signals, which greatly expands the application scenarios and utility of the intelligent driving training system. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the technical solutions in the specific implementation manner of the present invention, the drawings required for use in the specific implementation manner or the description of the prior art will be briefly introduced below.

[0027] Figure 1 It is a flow chart of the method disclosed in the embodiment of the first aspect of the present invention.

[0028] Figure 2 It is a specific flow chart of the method disclosed in the embodiment of the first aspect of the present invention. DETAILED DESCRIPTION

[0029] The structure and operation of the present invention will be further described in detail below in conjunction with the accompanying drawings. Obviously, the accompanying drawings are provided only for a better understanding of the present invention and should not be construed as limiting the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0030] In the description of the present invention, it should be noted that, if the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, if the terms "first" and "second" appear, they are only used for descriptive purposes and cannot be understood as indicating or implying relative importance.

[0031] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected" and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0032] Example 1

[0033] Combination Figure 1 to Figure 2 This embodiment discloses a multi-sensor fusion vehicle positioning method in a tunnel, comprising the following steps:

[0034] S1: Before the vehicle enters the tunnel, the first sensor and the second sensor are used to collect the initial position information of the vehicle;

[0035] S2: After the vehicle enters the tunnel, the distance of surrounding obstacles is detected by a third sensor on the vehicle;

[0036] S3: Integrate the data obtained in steps S1 and S2 and use the inertial derivation algorithm to calculate the precise position of the vehicle.

[0037] Specifically, the first sensor is a GPS sensor.

[0038] Specifically, the second sensor is an IMU sensor and / or a wheel speed sensor.

[0039] Specifically, the third sensor is an ultrasonic sensor and / or a lidar sensor.

[0040] It is understandable that after a vehicle enters a tunnel, the GPS signal is prone to weaken or be lost. At this time, activating other sensors on the vehicle can compensate for this data in order to accurately locate the vehicle.

[0041] Specifically, in step S1, the first sensor obtains the initial posture of the vehicle, and the initial posture includes the following variables: x, y, z, θ, where x represents the lateral position coordinate of the vehicle on the horizontal plane, y represents the longitudinal position coordinate of the vehicle on the horizontal plane, z represents the altitude coordinate of the vehicle, and θ represents the rotation angle of the vehicle on the horizontal plane relative to a reference direction.

[0042] Specifically, the specific steps of the inertia derivation algorithm in step S3 are:

[0043] S31: Set the current state of the vehicle X k =[x,y,θ,v,w,acc]; where v is the speed of the vehicle, w is the angular velocity of the vehicle, and acc is the acceleration of the vehicle;

[0044] S32: Calculate the state quantity at the next moment in integral form:

[0045]

[0046] Among them, t represents the time variable;

[0047] S33: Simplify the state equation and obtain the final state equation:

[0048]

[0049] S34: Calculate the partial derivative of each state variable (x, y, θ, v, w, acc) to obtain the Jacobian matrix F_;

[0050] S35: Covariance matrix of the prediction process:

[0051] P_=F_*P_*F_.transpose()+Q_; wherein P_ on the right side of the equal sign is the state quantity covariance matrix, and its initial value is set by the system; F_.transpose() is the transpose of the Jacobian matrix F_ obtained in step S34, and Q_ is the noise error matrix, which can be obtained by experimental measurement or theoretical derivation in the prior art;

[0052] S36: Calculate the Kalman gain matrix: K = P_*C.transpose()*(C*P_*C.transpose()+R.inverse(); where C is the state quantity, R.inverse() is the measurement error matrix, C.transpose() is the transpose of C, and the values ​​of C and R.inverse() are set by the system;

[0053] S37: X = X + K * error, where error refers to the error, which can be obtained by subtracting the predicted data from the observed data;

[0054] S38: Covariance update P_=P_-K*C*P_.

[0055] The algorithm adopted in the above method is a track deduction model based on a constant turning rate and acceleration model.

[0056] The present invention can achieve the following technical effects:

[0057] The method for positioning a vehicle in a tunnel provided by the present invention does not require any additional hardware costs, reduces the work of integrating new sensors with existing equipment, and further reduces operation and maintenance costs, reducing vehicle modification to zero. Therefore, the invention is a vehicle positioning solution with moderate cost, easy deployment, and can still work efficiently in an environment with weak or no GPS signals, which greatly expands the application scenarios and utility of intelligent driving training systems.

[0058] The above embodiments are only used to illustrate the present invention, wherein the structure, connection method and manufacturing process of each component may be changed. Any equivalent transformations and improvements based on this technical solution should not be excluded from the protection scope of the present invention.

Claims

1. A multi-sensor fusion method for vehicle positioning in tunnels. Features: The steps include: S1: Before the vehicle enters the tunnel, the first sensor and the second sensor are used to collect the initial position information of the vehicle; S2: After the vehicle enters the tunnel, the distance of surrounding obstacles is detected by a third sensor on the vehicle; S3: Integrate the data obtained in steps S1 and S2 and use the inertial derivation algorithm to calculate the precise position of the vehicle.

2. The method according to claim 1, Features: The first sensor is a GPS sensor.

3. The method according to claim 1, Features: The second sensor is an IMU sensor and / or a wheel speed sensor.

4. The method according to claim 1, Features: The third sensor is an ultrasonic sensor and / or a lidar sensor.

5. The method according to claim 1, Features: In step S1, the first sensor obtains the initial posture of the vehicle, and the initial posture includes the following variables: x, y, z, θ, where x represents the lateral position coordinate of the vehicle on the horizontal plane, y represents the longitudinal position coordinate of the vehicle on the horizontal plane, z represents the altitude coordinate of the vehicle, and θ represents the rotation angle of the vehicle on the horizontal plane relative to a reference direction.

6. The method according to claim 5, Features: The specific steps of the inertia derivation algorithm in step S3 are: S31: Set the current state of the vehicle X k =[x,y,θ,v,w,acc]; where v is the speed of the vehicle, w is the angular velocity of the vehicle, and acc is the acceleration of the vehicle; S32: Calculate the state quantity at the next moment in integral form: Among them, t represents the time variable; S33: Simplify the state equation and obtain the final state equation: S34: Calculate the partial derivative of each state variable (x, y, θ, v, w, acc) to obtain the Jacobian matrix F_; S35: Covariance matrix of the prediction process: P_=F_*P_*F_.transpose()+Q_;wherein P_ on the right side of the equal sign is the state quantity covariance matrix, and its initial value is set by the system; F_.transpose() is the transpose of the Jacobian matrix F_ obtained in step S34, and Q_ is the noise error matrix; S36: Calculate the Kalman gain matrix: K = P_*C.transpose()*(C*P_*C.transpose()+R.inverse(); where C is the state quantity, R.inverse() is the measurement error matrix, C.transpose() is the transpose of C, and the values ​​of C and R.inverse() are set by the system; S37: X = X + K * error, where error refers to the error, which is obtained by subtracting the predicted data from the observed data; S38: Covariance update P_=P_-K*C*P_.

Images