Positioning method and device based on multi-sensor fusion

Patent metadata

Field	Value
Publication number	US-2022018962-A1
Application number	US-202117378484-A
Country	US
Kind code	A1
Filing date	Jul 16, 2021
Priority date	Jul 16, 2020
Publication date	Jan 20, 2022
Grant date	—

Abstract

A positioning method and device based on multi-sensor fusion are provided and relates to object positioning field. The method includes obtaining sensor data collected by various sensors on a movable object in real time; temporally and spatially synchronizing sensor data collected by the sensors to form various temporally and spatially synchronized sensor data; performing data preprocessing and correlation on the temporally and spatially synchronized sensor data to form to-be-jointly-optimized sensor data; obtaining state information at each time point before a current time point in a preset sliding window; and determining a current pose state of the movable object by performing a joint optimization according to the to-be-jointly-optimized sensor data and the state information at each time point before the current time point in the sliding window. The movable object can be accurately positioned in scenarios where GPS signals are lost, jumping exists, or LiDAR observation is degraded seriously.

First claim

1 - 13 . (canceled)

14 . A positioning method based on multi-sensor fusion, applied to a movable object equipped with various sensors, the method comprising:
obtaining sensor data collected by the various sensors equipped on the movable object in real time; temporally and spatially synchronizing the sensor data collected by the various sensors to form various temporally and spatially synchronized sensor data; performing data preprocessing and data correlation on the various temporally and spatially synchronized sensor data to form to-be-jointly-optimized sensor data; obtaining state information at each time point before a current time point in a preset sliding window; and determining a current pose state of the movable object by performing a joint optimization solution according to the to-be-jointly-optimized sensor data and the state information at each time point before the current time point in the preset sliding window.

15 . The method of claim 14 , wherein the various sensors comprise an inertial measurement unit (IMU), a wheel speedometer, a light detection and ranging (LiDAR); and
wherein the IMU comprises an accelerometer and a gyroscope.

16 . The method of claim 15 , wherein the state information comprises state variables, and each of the state variables comprises a translation vector, a velocity vector, a rotation matrix, an IMU accelerometer bias, and an IMU gyroscope bias; and
wherein the obtaining the state information at each time point before the current time point in the preset sliding window comprises: determining, in real time, whether data in the preset sliding window meets an initialization condition in response to the to-be-jointly-optimized sensor data being not initialized, wherein the initialization condition comprises that a number of data observation time points in the preset sliding window is greater than or equal to a preset number threshold, and IMU data in the preset sliding window is fully stimulated; initializing the state variables to form initialized state variables in response to the number of the data observation time points in the preset sliding window being greater than or equal to the preset number threshold and the IMU data in the preset sliding window is fully stimulated; and in response to initializing the state variables and in response to the joint optimization solution at a previous time point before the current time point, obtaining a jointly optimized current state variable at each time point before the current time point.

17 . The method of claim 16 , wherein the obtaining sensor data collected by the various sensors equipped on the movable object in real time comprises:
obtaining the IMU data measured by the IMU, wheel speed data of the movable object measured by the wheel speedometer, and point cloud data measured by the LiDAR in real time.

18 . The method of claim 17 , wherein the temporally and spatially synchronizing the sensor data collected by the various sensors to form the various temporally and spatially synchronized sensor data comprises:
transforming the wheel speed data of the movable object and the point cloud data into an IMU coordinate system according to pre-calibrated external parameters of the IMU, the wheel speedometer, and the LiDAR, and aligning collection time of each frame of the wheel speed data of the movable object and the point cloud data to a time stamp of the IMU according to respective frame rates of data collection of the IMU, the wheel speedometer, and the LiDAR so that the time stamps of the IMU data, the wheel speed data of the movable object, and the point cloud data in the IMU coordinate system are aligned to each other.

19 . The method of claim 17 , wherein the performing the data preprocessing and the data correlation on the various temporally and spatially synchronized sensor data to form the to-be-jointly-optimized sensor data comprises:
propagating IMU data between two consecutive frames of LiDAR time stamps in temporally and spatially synchronized IMU data using a preset integration algorithm, and processing the IMU data between the two consecutive frames of LiDAR time stamps in the temporally and spatially synchronized IMU data using a preset pre-integration method to obtain an offset pre-integration amount, a velocity pre-integration amount, and a rotation increment pre-integration amount; predicting a state variable at a next time point according to a jointly optimized state variable at the previous time point before the current time point in the preset sliding window and the temporally and spatially synchronized IMU data, to obtain predicted data of the state variable at the next time point; performing de-distortion processing on temporally and spatially synchronized point cloud data using the predicted data of the state variable at the next time point; performing a line feature extraction and a surface feature extraction on the de-distortion processed point cloud data to obtain line feature data and surface feature data; obtaining line feature constraint relationship data and surface feature constraint relationship data by performing a registration on the line feature data and pre-generated line features in a feature map, and a registration on the surface feature data and pre-generated surface features in the feature map; and determining a linear velocity in a forward direction, a linear velocity in a horizontal direction, and a yaw angular velocity in the IMU coordinate system by inputting temporally and spatially synchronized wheel speed data of the movable object into a preset vehicle dynamics model.

20 . The method of claim 19 , further comprises:
performing a tightly coupled inter-frame optimization on the point cloud data and the wheel speed data of the movable object in a current sliding window to obtain an initialized LiDAR pose at a current time point in the current sliding window, in response to the number of the data observation time points in the preset sliding window being less than the preset threshold number and the IMU data in the preset sliding window being fully stimulated, wherein the initialized LiDAR pose includes a LiDAR rotation matrix and a LiDAR translation vector; and wherein the initializing the state variables to form the initialized state variables comprises: adopting consistency constraints of LiDAR rotation change between adjacent frames and a LiDAR rotation increment pre-integration amount between the adjacent frames according to the initialized LiDAR poses at the current time point and at each time point before the current time point, to accumulate the consistency constraints of consecutive multiple frames to obtain an initialized gyroscope bias; obtaining gravitational acceleration in a LiDAR coordinate system according to the initialized LiDAR poses in the current sliding window and the consistency constraints of the offset pre-integration amount, and converting the gravitational acceleration in the LiDAR coordinate system into a world coordinate system to obtain an initialized gravitational acceleration; and determining an initialized velocity by estimating an initial velocity vector according to the initialized gyroscope bias and the initialized gravitational acceleration.

21 . The method of claim 20 , wherein the determining the current pose state of the movable object by performing the joint optimization solution according to the to-be-jointly-optimized sensor data and the state information at each time point before the current time point in the preset sliding window comprises:
determining the state variable at each time point in the preset sliding window by adopting a joint probability density estimation according to the initialized state variable at each time point before the curre

Assignees

• Beijing Tusen Weilai Tech Co Ltd

Inventors

• Pan Liangliang
• Zhao Ji
• Ji Kaijin

Classifications

• G01C21/005 Primary

  with correlation of navigation data from several sources, e.g. map or contour matching (G01C21/30 takes precedence) · CPC title

• G01S17/86 Primary

  Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders · CPC title

• G01C21/165

  combined with non-inertial navigation instruments · CPC title

• G01S17/42

  Simultaneous measurement of distance and other co-ordinates (indirect measurement G01S17/46) · CPC title

• G01C21/28

  with correlation of data from several navigational instruments · CPC title