Positioning method and system based on four-wheel steering bus in intelligent driving scene

Abstract

The invention discloses a positioning method and system based on a four-wheel steering bus in an intelligent driving scene. The positioning method comprises the steps that the initial pose of the bus is obtained through a satellite positioning system and initialized; carrying out registration on the initial pose and a pre-constructed high-precision map to obtain a measurement pose of the bus; the distortion information of the bus is calculated through a gyro odometer; the two-dimensional vehicle dynamics model is used as the prediction model through the extended Kalman filtering algorithm, and the optimal pose is obtained according to the measurement pose and distortion information of the bus, so that the positioning accuracy is improved, and the use experience is improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation, and in particular to a positioning method and system for a four-wheel steering minibus in an intelligent driving scenario. Background Art

[0002] In modern intelligent transportation, precise positioning is one of the key technologies for autonomous vehicles to operate safely and efficiently. The positioning system must have high precision and high real-time performance to ensure that the vehicle can accurately perceive the surrounding environment and perform path planning and control. When the minibus is driving inside the park, due to the narrow roads inside the park, a high-precision positioning method is required to improve the user experience and safety.

[0003] In the existing positioning system, minibuses are usually positioned by obtaining satellite data, but this method has low accuracy and affects the user experience. Summary of the invention

[0004] To achieve the above-mentioned purpose and other related purposes, the present invention discloses a positioning method for a four-wheel steering minibus in an intelligent driving scenario, comprising: Obtain the initial position of the minibus through the satellite positioning system and initialize it; The initial pose is registered with the pre-built high-precision map to obtain the measured pose of the minibus; The distortion information of the minibus is obtained by calculating the gyroscopic odometer; The two-dimensional vehicle dynamics model is used as a prediction model through the extended Kalman filter algorithm, and the optimal posture is obtained according to the measured posture and distortion information of the minibus.

[0005] Furthermore, the registering of the initial posture with the pre-built high-precision map includes: The normal distribution transformation method is used for registration.

[0006] Furthermore, the distortion information of the minibus calculated by the gyro odometer includes: Record the longitudinal velocity information and IMU information transmitted by the vehicle chassis, perform time filtering and coordinate system conversion on the data, and calculate the vehicle speed and angular velocity at each moment, including: Angular Update: Location Updates: The initial posture (yaw angle) of the vehicle is , the angular velocity around the z-axis obtained from the IMU is , then at each time step Inside, you can get the next angle; The position is updated by the vehicle's linear velocity The fused angle update result (obtained from the vehicle's speed sensor or wheel speedometer) is used to update the vehicle's position in the world coordinate system, where x and y represent the position in the world coordinate system.

[0007] Furthermore, a time delay compensation mechanism is added to the prediction model, including: in is the total delay time, is the current time, is the measured time, is the additional delay parameter, dT is the time step of the extended Kalman filter, is the number of time delay steps to compensate, For Round down.

[0008] Furthermore, a bias correction mechanism is added to the prediction model, including: For the extended Kalman filter, the yaw angle deviation in the initial prediction state covariance matrix is ​​set to: Where x is the x coordinate in the world coordinate system, y is the y coordinate in the world coordinate system, yaw is the yaw angle, indicating the direction of the vehicle, and yawb is the deviation of yaw. is the linear speed of the vehicle, is the yaw rate, indicating the speed at which the vehicle rotates, Var represents variance, and Cov represents covariance.

[0009] Furthermore, a smooth update mechanism is added to the prediction model, including: The pose covariance matrix is: *pose_smoothing_step In the above formula, x, y, z are the x, y, z coordinates in the world coordinate system, yaw is the Euler angle of the pose, Var is the variance, Cov is the covariance, and pose_smoothing_step is a configurable pose smoothing update parameter; The Twist covariance matrix is: *twist_smoothing_step In the above formula is the linear speed of the vehicle control, is the z-axis angular velocity collected by the IMU, Var is the variance, Cov is the covariance, and twist_smoothing_step is a configurable pose smoothing update parameter.

[0010] Furthermore, the prediction model includes: The state prediction model is: The extended Kalman filter algorithm state prediction model is updated for the four-wheel steering chassis, and the improvement results are as follows: The state transition model is: Where x and y are the x and y coordinates in the world coordinate system, is the current yaw angle of the vehicle body, is the rear wheel turning angle, is the time step, is the angle deviation, is the current linear velocity of the vehicle, is the angular velocity, Predicted vehicle yaw angle with angle deviation optimization.

[0011] On the other hand, the present invention provides a positioning system based on a four-wheel steering minibus in an intelligent driving scenario, comprising: The initialization module is used to obtain the initial position of the minibus through the satellite positioning system and initialize it; The registration module is used to register the initial pose with the pre-built high-precision map to obtain the measured pose of the minibus; A distortion information calculation module, used to calculate the distortion information of the minibus through a gyro odometer; The optimal posture acquisition module is used to use the two-dimensional vehicle dynamics model as a prediction model through the extended Kalman filter algorithm to obtain the optimal posture according to the measured posture and distortion information of the minibus.

[0012] By adopting the above technical solution, the position and posture of the minibus are accurately positioned in real time through normal distribution transformation and extended Kalman filter algorithm, and time delay compensation mechanism, deviation correction mechanism and smooth update mechanism are added thereto, and the extended Kalman filter algorithm is updated to ensure high-precision positioning of the autonomous driving minibus in complex campus environments, thereby improving the user experience and safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] The above and other features, advantages and aspects of the embodiments of the present disclosure will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present disclosure. In the accompanying drawings, the same or similar reference numerals represent the same or similar elements, among which:

[0014] Figure 1 The figure is a flow chart of a positioning method for a four-wheel steering minibus in an intelligent driving scenario. DETAILED DESCRIPTION

[0015] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0016] Reference Figure 1 The embodiment of the present invention provides a positioning method for a four-wheel steering minibus in an intelligent driving scenario, comprising the following steps: S1: Obtain the initial position of the minibus through the satellite positioning system and initialize it.

[0017] Specifically, the initial position is obtained through GPS or Beidou navigation system. S2: Align the initial pose with the pre-built high-precision map to obtain the measured pose of the minibus.

[0018] As mentioned above, the normal distribution transformation method is used to align the initial pose with the pre-built high-precision map. The normal distribution transformation has good robustness. It represents point cloud data through Gaussian distribution, which can handle noise and incomplete data to a certain extent, making it perform well in complex environments. At the same time, the normal distribution transformation is efficient. Since the voxel grid discretization is used to represent the point cloud data, the normal distribution transformation greatly reduces the amount of matching calculations and improves the efficiency of registration. Compared with other registration algorithms such as ICP (Iterative Closest Point), the normal distribution transformation has lower requirements for the estimation of the initial pose and is easier to obtain convergence results. The normal distribution transformation has global optimality. Since the normal distribution transformation considers the global information of the point cloud (i.e., Gaussian distribution), compared with the ICP algorithm that only considers local information, the normal distribution transformation is easier to find the globally optimal pose transformation. For processing dynamic changes, the normal distribution transformation can effectively handle dynamic changes in the point cloud, such as the point cloud data of a moving vehicle, and is suitable for autonomous driving scenarios with high real-time requirements.

[0019] Normal distribution transformation is an efficient algorithm for point cloud registration, which is mainly used to align point cloud data at different positions and different perspectives. The detailed registration process is as follows: Discretize the point cloud, discretize the target point cloud and the reference point cloud into voxel grids. The point cloud in each voxel grid is represented by Gaussian distribution, where the mean is the average position of the point cloud in the grid, and the covariance matrix is ​​the covariance of the point cloud position in the grid. Initialize, set the initial pose transformation parameters (usually the unit matrix or set according to a rough estimate). Calculate the corresponding position of each point in the initial pose. Calculate the registration error, for each point in the target point cloud, find the voxel grid where it is located. Using the Gaussian distribution of the grid, calculate the probability density value of the point in the reference point cloud. Accumulate the probability density values ​​of all points to get the total registration error. Optimize the pose transformation parameters, use the optimization algorithm (gradient descent method) to adjust the pose transformation parameters to minimize the registration error. The specific optimization steps include calculating the gradient of the error to the pose transformation parameters and updating the pose transformation parameters. Iteration, repeat the above steps until the registration error converges or reaches the preset number of iterations. The pose transformation parameters finally obtained are the optimal alignment pose of the target point cloud relative to the reference point cloud.

[0020] S3: Obtain the distortion information of the minibus through the gyro odometer calculation; The gyro odometer calculates distortion information by fusing IMU data and vehicle speed, which is used as one of the measurement update inputs of the extended Kalman filter algorithm. The specific method includes recording the longitudinal speed information and IMU information transmitted by the vehicle chassis, performing time filtering and coordinate system conversion on the data, calculating the vehicle speed and angular velocity at each moment, and ensuring the accuracy and real-time nature of the data, including:

[0021] Angular Update: Location Updates: The initial posture (yaw angle) of the vehicle is , the angular velocity around the z-axis obtained from the IMU is , then at each time step Inside, you can get the next angle; The position is updated by the vehicle's linear velocity The fused angle update result (obtained from the vehicle's speed sensor or wheel speedometer) is used to update the vehicle's position in the world coordinate system, where x and y represent the position in the world coordinate system.

[0022] S4: The two-dimensional vehicle dynamics model is used as a prediction model through the extended Kalman filter algorithm, and the optimal posture is obtained according to the measured posture and distortion information of the minibus.

[0023] The extended Kalman filter algorithm uses the two-dimensional vehicle dynamics model as a prediction model and the input normally distributed transformed posture and distortion messages as measurement update values ​​to estimate the vehicle posture and distortion values ​​with high robustness and less noise. For nonlinear dynamic models involved in vehicles, the extended Kalman filter algorithm can handle the state estimation problem of nonlinear systems. At the same time, the extended Kalman filter algorithm has low computational complexity and can complete state estimation in a shorter time, making it suitable for autonomous driving systems with high real-time control requirements.

[0024] Specifically, the state prediction model is: The EKF state prediction model is updated for the four-wheel steering chassis, and the improvement results are as follows: The state transition model is: Where x and y are the x and y coordinates in the world coordinate system, is the current yaw angle of the vehicle body, is the rear wheel turning angle, is the time step, is the angle deviation, is the current linear velocity of the vehicle, is the angular velocity, Predicted vehicle yaw angle with angle deviation optimization.

[0025] Time delay compensation for input messages has been added for self-driving scenarios to ensure that input information with different time delays is correctly fused and filtered. The reason for the time delay may be due to differences in sensor sampling frequencies. Different sensors have different sampling frequencies. For example, a lidar may sample 10 times per second, while an IMU may sample 100 times per second. This frequency difference will cause the time between the data to be out of sync. Transmission delay: When data is transmitted from the sensor to the processing unit, it will experience a certain transmission delay. This delay may be caused by the internal processing time of the sensor, the transmission time of the communication network, and the overhead of the data transmission protocol. Data processing delay: After receiving the data, the processing unit needs to perform preprocessing, filtering, and fusion. These steps will introduce a certain processing delay. System clock asynchrony: If the sensors and processing units do not share a unified system clock, the timestamps between the data may be inconsistent, resulting in time asynchrony.

[0026] According to the time difference between the time measured by the sensor and the time difference between the pose estimation system, the time difference is introduced into the pose prediction model to participate in the calculation to solve the delay problem. An additional time delay is provided to further compensate for the delay on the basis of compensating the timestamp of the actual sensor data to ensure that the Kalman filter can accurately align the data and state estimation. Finally, the actual delay time is converted into the number of delay steps to achieve the purpose of historical state alignment. The formula is as follows:

[0027] in is the total delay time, is the current time, is the measured time, is an additional delay parameter, which can be set by technicians in this field according to actual conditions. dT is the time step of the extended Kalman filter. is the number of time delay steps to compensate, For Round down.

[0028] In addition, the embodiment of the present invention adds a deviation correction mechanism to the prediction model, including: For the extended Kalman filter, the yaw angle deviation in the initial prediction state covariance matrix is ​​set to: Where x is the x coordinate in the world coordinate system, y is the y coordinate in the world coordinate system, yaw is the yaw angle, indicating the direction of the vehicle, and yawb is the deviation of yaw. is the linear speed of the vehicle, is the yaw angular velocity, indicating the speed at which the vehicle rotates, Var represents the variance, and Cov represents the covariance. After modification, the angle error caused by the sensor installation angle error can be prevented, thereby improving the estimation accuracy.

[0029] In addition, this embodiment adds a smooth update mechanism to the prediction model, including: The pose covariance matrix is: *pose_smoothing_step In the above formula, x, y, z are the x, y, z coordinates in the world coordinate system, yaw is the Euler angle of the pose, Var is the variance, Cov is the covariance, and pose_smoothing_step is a configurable pose smoothing update parameter; The Twist covariance matrix is: *twist_smoothing_step In the above formula is the linear speed of the vehicle control, is the z-axis angular velocity collected by the IMU, Var is the variance, Cov is the covariance, and twist_smoothing_step is a configurable pose smoothing update parameter.

[0030] Kalman filter measurement updates are usually performed when the measurement is obtained, but for low-frequency measurements, this may cause large changes in the estimate. Since the algorithm can take into account the measurement time, the measurement data can be divided into multiple segments and smoothly fused while maintaining consistency. Adjust the pose smoothing order and twist smoothing order to smooth the covariance matrix during the pose prediction process. This can reduce the fluctuations in the estimate. Directly using low-frequency measurements for updates may cause large fluctuations in the estimate and affect the stability of the system. Through smooth updates, these fluctuations can be reduced and the estimate can be more stable. Improve estimation accuracy. Smooth updates can make full use of historical measurement data and improve estimation accuracy. By dividing the measurement data into multiple segments and fusing them, the real state of the system can be more accurately reflected. Improve real-time performance. Compared with waiting for direct updates of low-frequency measurements, smooth updates can make use of measurement data in a more timely manner and provide more real-time estimation results.

[0031] An embodiment of the present invention provides a positioning method for a four-wheel steering minibus in an intelligent driving scenario, which accurately locates the position of the minibus in real time through normal distribution transformation and extended Kalman filter algorithm, adds a time delay compensation mechanism, a deviation correction mechanism, and a smooth update mechanism, and updates the extended Kalman filter algorithm to ensure high-precision positioning of the autonomous driving minibus in a complex campus environment, thereby improving the user experience and safety.

[0032] An embodiment of the present invention further provides a positioning system based on a four-wheel steering minibus in an intelligent driving scenario, comprising: The initialization module is used to obtain the initial position of the minibus through the satellite positioning system and initialize it; The registration module is used to register the initial pose with the pre-built high-precision map to obtain the measured pose of the minibus; A distortion information calculation module, used to calculate the distortion information of the minibus through a gyro odometer; The optimal posture acquisition module is used to use the two-dimensional vehicle dynamics model as a prediction model through the extended Kalman filter algorithm to obtain the optimal posture according to the measured posture and distortion information of the minibus.

[0033] Those skilled in the art will appreciate that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with the meanings in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless specifically defined.

[0034] For the method embodiments, for the sake of simplicity, they are all described as a series of action combinations, but those skilled in the art should know that the embodiments of the present invention are not limited by the order of the actions described, because according to the embodiments of the present invention, some steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also know that the embodiments described in the specification are all preferred embodiments, and the actions involved are not necessarily required by the embodiments of the present invention.

[0035] It can be known from the description of the above implementation modes that those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application can be essentially or partly contributed to the prior art in the form of a software product, which can be stored in a storage medium such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes several instructions for enabling a computer device (which can be a personal computer, a server or a network device, etc.) to execute the methods described in the various implementation modes of the present application or certain parts of the implementation modes.

[0036] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning method for a four-wheel steering minibus in an intelligent driving scenario, characterized in that: include: Obtain the initial position of the minibus through the satellite positioning system and initialize it; The initial pose is registered with the pre-built high-precision map to obtain the measured pose of the minibus; The distortion information of the minibus is obtained by calculating the gyroscopic odometer; The two-dimensional vehicle dynamics model is used as a prediction model through the extended Kalman filter algorithm, and the optimal posture is obtained according to the measured posture and distortion information of the minibus.

2. The method according to claim 1, characterized in that The registering of the initial pose with the pre-built high-precision map includes: The normal distribution transformation method is used for registration.

3. The method according to claim 1, characterized in that The distortion information of the minibus calculated by the gyro odometer includes: Record the longitudinal velocity information and IMU information transmitted by the vehicle chassis, perform time filtering and coordinate system conversion on the data, and calculate the vehicle speed and angular velocity at each moment, including: Angular Update: Location Updates: The initial posture (yaw angle) of the vehicle is , the angular velocity around the z-axis obtained from the IMU is , then at each time step Inside, you can get the next angle; The position is updated by the vehicle's linear velocity The fused angle update result (obtained from the vehicle's speed sensor or wheel speedometer) is used to update the vehicle's position in the world coordinate system, where x and y represent the position in the world coordinate system.

4. The method according to claim 1, characterized in that Add time delay compensation mechanism to the prediction model, including: in is the total delay time, is the current time, is the measured time, is the additional delay parameter, dT is the time step of the extended Kalman filter, is the number of time delay steps to compensate, For Round down.

5. The method according to claim 1, characterized in that: Add bias correction mechanisms to the prediction model, including: For the extended Kalman filter, the yaw angle deviation in the initial prediction state covariance matrix is ​​set to: Where x is the x coordinate in the world coordinate system, y is the y coordinate in the world coordinate system, yaw is the yaw angle, indicating the direction of the vehicle, and yawb is the deviation of yaw. is the linear speed of the vehicle, is the yaw rate, indicating the speed at which the vehicle rotates, Var represents variance, and Cov represents covariance.

6. The method according to claim 3, characterized in that Add a smooth update mechanism to the prediction model, including: The pose covariance matrix is: *pose_smoothing_step In the above formula, x, y, z are the x, y, z coordinates in the world coordinate system, yaw is the Euler angle of the pose, Var is the variance, Cov is the covariance, and pose_smoothing_step is a configurable pose smoothing update parameter; The Twist covariance matrix is: *twist_smoothing_step In the above formula is the linear speed of the vehicle control, is the z-axis angular velocity collected by the IMU, Var is the variance, Cov is the covariance, and twist_smoothing_step is a configurable pose smoothing update parameter.

7. The method according to claim 1, characterized in that The prediction model includes: The state prediction model is: The extended Kalman filter algorithm state prediction model is updated for the four-wheel steering chassis, and the improvement results are as follows: The state transition model is: Where x and y are the x and y coordinates in the world coordinate system, is the current yaw angle of the vehicle body, is the rear wheel turning angle, is the time step, is the angle deviation, is the current linear velocity of the vehicle, is the angular velocity, Predicted vehicle yaw angle with angle deviation optimization.

8. A positioning system based on a four-wheel steering minibus in an intelligent driving scenario, characterized in that: include: The initialization module is used to obtain the initial position of the minibus through the satellite positioning system and initialize it; The registration module is used to register the initial pose with the pre-built high-precision map to obtain the measured pose of the minibus; A distortion information calculation module, used to calculate the distortion information of the minibus through a gyro odometer; The optimal posture acquisition module is used to use the two-dimensional vehicle dynamics model as a prediction model through the extended Kalman filter algorithm to obtain the optimal posture according to the measured posture and distortion information of the minibus.

Images