WO2022061799A1

WO2022061799A1 - Pose estimation method and device

Info

Publication number: WO2022061799A1
Application number: PCT/CN2020/118035
Authority: WO
Inventors: 潘志琛; 李延召
Original assignee: 深圳市大疆创新科技有限公司
Priority date: 2020-09-27
Filing date: 2020-09-27
Publication date: 2022-03-31
Also published as: CN114585879A

Abstract

Embodiments of the present application provide a pose estimation method and device, which are beneficial to improve the accuracy and robustness of pose estimation of a laser radar. The method comprises: after completion of initialization of inertial measurement unit (IMU) data outputted by an IMU, performing pose estimation of the laser radar according to an integration result of the IMU data and laser point cloud data outputted by the laser radar so as to obtain a motion acceleration of the laser radar, the IMU data comprising gyroscope data and accelerometer data; obtaining an estimation value of a gravitational acceleration according to the motion acceleration of the laser radar and the accelerometer data; determining, according to the degree of proximity between the estimation value of the gravitational acceleration and a reference value of the gravitational acceleration, whether to update the estimation value of the gravitational acceleration; and when the estimation value of the gravitational acceleration is determined not to be updated, outputting a result of the pose estimation.

Description

Method and apparatus for pose estimation

Copyright notice

The disclosure of this patent document contains material that is subject to copyright protection. This copyright belongs to the copyright owner. The copyright owner has no objection to the reproduction by anyone of the patent document or the patent disclosure as it exists in the official records and archives of the Patent and Trademark Office.

technical field

The present application relates to the field of lidar, and more particularly, to a method and apparatus for pose estimation.

Background technique

For lidar, by continuously completing point cloud matching of adjacent frames, the pose of lidar can be estimated, so that lidar has the ability to perceive its own motion state. However, for the non-repetitive scanning radar, due to the large difference between the point clouds of adjacent frames, the matching error of the point cloud will increase and the pose estimation ability of the lidar will be reduced.

The related art proposes to fuse the inertial measurement unit (IMU) data with point cloud data to improve the pose estimation capability of lidar.

However, in the process of IMU data initialization, the initial value cannot avoid the existence of errors, especially the estimation result of the gravitational acceleration, which seriously affects the integration accuracy of the accelerometer, which has a great impact on the accuracy and stability of the pose estimation. .

SUMMARY OF THE INVENTION

The embodiments of the present application provide a method and apparatus for pose estimation, which can improve the accuracy and robustness of pose estimation, especially the manner of non-repetitive scanning of point clouds.

In a first aspect, a method for estimating pose and attitude is provided. The method includes: after the initialization of IMU data output by an inertial measurement unit (IMU) is completed, performing the process according to the integration result of the IMU data and the laser point cloud data output by the lidar. The position and attitude of the lidar is estimated to obtain the motion acceleration of the lidar, and the IMU data includes gyroscope data and accelerometer data; according to the motion acceleration of the lidar and the accelerometer data, the estimated value of the gravitational acceleration is obtained; according to the gravity The closeness between the estimated value of the acceleration and the reference value of the gravitational acceleration determines whether to update the estimated value of the gravitational acceleration; when it is determined not to update the estimated value of the gravitational acceleration, output the result of the pose estimation.

In a second aspect, an apparatus for pose estimation is provided, including a processor for performing the operations in the method of the first aspect.

In a third aspect, a computer system is provided, including: a memory for storing computer-executable instructions; a processor for accessing the memory and executing the computer-executable instructions to perform the method in the first aspect above. operate.

In a fourth aspect, a computer storage medium is provided, and program codes are stored in the computer storage medium, and the program codes can be used to instruct to execute the method of the above-mentioned first aspect.

In a fifth aspect, a computer program product is provided, the program product includes program code, and the program code can be used to instruct to execute the method of the above-mentioned first aspect.

Therefore, in this embodiment of the present application, at the end of the pose estimation, it is further judged whether the estimated value of the gravitational acceleration obtained according to the result of the pose estimation is close to the reference value of the gravitational acceleration, and if it is close to the reference value of the gravitational acceleration, Only output the result of the pose estimation, which is beneficial to improve the accuracy and robustness of the pose estimation.

Description of drawings

In order to illustrate the technical solutions of the embodiments of the present application more clearly, the following briefly introduces the drawings that need to be used in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some of the drawings in the present application. In the embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.

FIG. 1 is a schematic block diagram of a lidar apparatus according to an embodiment of the present application.

FIG. 2 is another schematic block diagram of a lidar apparatus according to an embodiment of the present application.

FIG. 3 is a scanning pattern of the laser radar according to the embodiment of the present application in a non-repetitive scanning manner.

FIG. 4 is a schematic block diagram of a method for pose estimation according to an embodiment of the present application.

FIG. 5 is a schematic flowchart of a method for pose estimation according to an embodiment of the present application.

FIG. 6 is a schematic block diagram of an apparatus for pose estimation according to an embodiment of the present application.

detailed description

The technical solutions in the embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

Unless otherwise specified, all technical and scientific terms used in the embodiments of the present application have the same meaning as commonly understood by those skilled in the technical field of the present application. The terminology used in this application is for the purpose of describing specific embodiments only and is not intended to limit the scope of the application.

Light Detection and Ranging (LiDAR), referred to as LiDAR, can be applied to mobile platforms, and LiDAR can be installed on the platform body of the mobile platform. The mobile platform with lidar can measure the external environment, for example, measure the distance between the mobile platform and obstacles for obstacle avoidance and other purposes, and perform two-dimensional or three-dimensional mapping of the external environment. In some embodiments, the mobile platform includes at least one of an unmanned aerial vehicle, a car, a remote control car, a robot, and a camera. When the lidar is applied to the unmanned aerial vehicle, the platform body is the fuselage of the unmanned aerial vehicle. When the lidar is applied to the car, the platform body is the body of the car. The vehicle may be an autonomous driving vehicle or a semi-autonomous driving vehicle, which is not limited herein. When the lidar is applied to the RC car, the platform body is the body of the RC car. When the lidar is applied to the robot, the platform body is the robot. When the lidar is applied to the camera, the platform body is the camera itself. In an implementation manner, the lidar is used to sense external environmental information, such as distance information, orientation information, reflection intensity information, speed information, and the like of environmental targets.

For lidar, the pose of lidar can usually be estimated by completing point cloud matching of adjacent frames, so that lidar has the ability to perceive its own motion state. The point cloud data obtained by the lidar may be composed of multiple point cloud points, and one point cloud point may include at least one of the external environment information measured by the lidar.

For ease of understanding, the lidar generating the point cloud data mentioned herein will be described by way of example first with reference to the lidar device 100 shown in FIG. 1 .

As shown in FIG. 1 , the lidar apparatus 100 may include a transmitting circuit 110 , a receiving circuit 120 , a sampling circuit 130 and an arithmetic circuit 140 .

The transmit circuit 110 may transmit a sequence of optical pulses (eg, a sequence of laser pulses). The receiving circuit 120 can receive the optical pulse sequence reflected by the detected object, and perform photoelectric conversion on the optical pulse sequence to obtain an electrical signal, which can be output to the sampling circuit 130 after processing the electrical signal. The sampling circuit 130 may sample the electrical signal to obtain a sampling result. The arithmetic circuit 140 may determine the distance between the lidar device 100 and the detected object based on the sampling result of the sampling circuit 130 .

Optionally, the lidar device 100 may further include a control circuit 150, which may control other circuits, for example, may control the working time of each circuit and/or set parameters for each circuit.

It should be understood that although the lidar device shown in FIG. 1 includes a transmitting circuit, a receiving circuit, a sampling circuit and an arithmetic circuit for emitting a beam of light for detection, the embodiment of the present application is not limited to this, the transmitting circuit The number of any one of the receiving circuits, sampling circuits, and arithmetic circuits may also be at least two, for emitting at least two light beams in the same direction or in different directions respectively; wherein, the at least two light beam paths can be simultaneously The ejection can also be ejected at different times. In one example, the light-emitting chips in the at least two emission circuits are packaged in the same module. For example, each emitting circuit includes one laser emitting chip, and the dies in the laser emitting chips in the at least two emitting circuits are packaged together and accommodated in the same packaging space.

In some implementations, in addition to the circuit shown in FIG. 1 , the lidar device 100 may further include a scanning module 160 for changing the propagation direction of at least one laser pulse sequence emitted from the transmitting circuit.

Wherein, the module including the transmitting circuit 110, the receiving circuit 120, the sampling circuit 130 and the operation circuit 140, or the module including the transmitting circuit 110, the receiving circuit 120, the sampling circuit 130, the operation circuit 140 and the control circuit 150 may be referred to as the measuring circuit The ranging module 150 can be independent of other modules, for example, the scanning module 160 .

A coaxial optical path can be used in the laser radar device, that is, the light beam emitted by the laser radar device and the reflected light beam share at least part of the optical path in the ranging device. For example, after at least one laser pulse sequence emitted by the transmitting circuit changes its propagation direction through the scanning module, the laser pulse sequence reflected by the detection object passes through the scanning module and then enters the receiving circuit. Alternatively, the laser radar device can also adopt an off-axis optical path, that is, the light beam emitted by the laser radar device and the reflected light beam are transmitted along different optical paths in the laser radar device respectively. FIG. 2 shows a schematic diagram of an embodiment in which the laser radar device of the present application adopts a coaxial optical path.

The lidar device 200 includes a ranging module 201, and the ranging module 201 includes a transmitter 203 (which may include the above-mentioned transmitting circuit), a collimating element 204, a detector 205 (which may include the above-mentioned receiving circuit, sampling circuit and arithmetic circuit) and Optical path changing element 206 . The ranging module 201 is used for emitting a light beam, receiving the returning light, and converting the returning light into an electrical signal. Among them, the transmitter 203 can be used to transmit a sequence of optical pulses. In one embodiment, the transmitter 203 may emit a sequence of laser pulses. Optionally, the laser beam emitted by the transmitter 203 is a narrow bandwidth beam with a wavelength outside the visible light range. The collimating element 204 is disposed on the outgoing light path of the transmitter, and is used for collimating the light beam emitted from the transmitter 203, and collimating the light beam emitted by the transmitter 203 into parallel light and outputting to the scanning module. The collimating element 204 also serves to converge at least a portion of the return light reflected by the probe. The collimating element 204 may be a collimating lens or other elements capable of collimating light beams.

In one embodiment, as shown in FIG. 2 , the lidar device 200 may further include a time-of-flight (TOF) element 207 , which may be used to measure light propagation between the lidar device 200 and the detected object. time to detect the distance from the detected object to the lidar device 200 . Alternatively, the lidar device 200 can also detect the distance from the detected object to the lidar device 200 through other techniques, such as a ranging method based on phase shift measurement, or a ranging method based on frequency shift measurement , which is not limited here.

In the embodiment shown in FIG. 2 , the transmitting optical path and the receiving optical path in the lidar device are combined by the optical path changing element 206 before the collimating element 204, so that the transmitting optical path and the receiving optical path can share the same collimating element, so that the optical path can share the same collimating element. more compact. In some other implementations, the emitter 203 and the detector 205 may use respective collimating elements, and the optical path changing element 206 may be arranged on the optical path behind the collimating element.

In the embodiment shown in FIG. 2 , since the beam aperture of the beam emitted by the transmitter 203 is small, and the beam aperture of the return light received by the ranging device is relatively large, the optical path changing element can use a small-area reflective mirror to The transmit light path and the receive light path are combined. In some other implementations, the optical path changing element may also use a reflector with a through hole, wherein the through hole is used to transmit the outgoing light of the emitter 203 , and the reflector is used to reflect the return light to the detector 205 . In this way, in the case of using a small reflector, the occlusion of the return light by the support of the small reflector can be reduced.

In the embodiment shown in FIG. 2 , the optical path altering element is offset from the optical axis of the collimating element 204 . In some other implementations, the optical path altering element may also be located on the optical axis of the collimating element 204 .

The lidar device 200 also includes a scanning module 202 . The scanning module 202 is placed on the outgoing light path of the ranging module 201 , and the scanning module 202 is used to change the transmission direction of the collimated beam 219 emitted by the collimating element 204 and project it to the external environment, and project the return light to the collimating element 204 . The returned light is focused on the detector 205 via the collimating element 204.

In one embodiment, the scanning module 202 can include at least one optical element for changing the propagation path of the light beam, wherein the optical element can change the propagation path of the light beam by reflecting, refracting, diffracting the light beam, or the like. For example, the scanning module 202 includes lenses, mirrors, prisms, galvanometers, gratings, liquid crystals, optical phased arrays (Optical Phased Array) or any combination of the above optical elements. In one example, at least part of the optical elements are moving, for example, the at least part of the optical elements are driven to move by a driving module, and the moving optical elements can reflect, refract or diffract the light beam to different directions at different times. In some embodiments, the multiple optical elements of the scanning module 202 may be rotated or oscillated about a common axis 209, each rotating or oscillating optical element being used to continuously change the propagation direction of the incident beam. In one embodiment, the plurality of optical elements of the scanning module 202 may rotate at different rotational speeds, or vibrate at different speeds. In another embodiment, at least some of the optical elements of scan module 202 may rotate at substantially the same rotational speed. In some embodiments, the plurality of optical elements of the scanning module may also be rotated about different axes. In some embodiments, the plurality of optical elements of the scanning module may also rotate in the same direction, or rotate in different directions; or vibrate in the same direction, or vibrate in different directions, which are not limited herein.

In one embodiment, the scanning module 202 includes a first optical element 214 and a driver 216 connected to the first optical element 214, and the driver 216 is used to drive the first optical element 214 to rotate around the rotation axis 209, so that the first optical element 214 changes The direction of the collimated beam 219. The first optical element 214 projects the collimated beam 219 in different directions. In one embodiment, the angle between the direction of the collimated light beam 219 changed by the first optical element and the rotation axis 209 changes with the rotation of the first optical element 214 . In one embodiment, the first optical element 214 includes a pair of opposing non-parallel surfaces through which the collimated beam 219 passes. In one embodiment, the first optical element 214 includes a prism whose thickness varies along at least one radial direction. In one embodiment, the first optical element 214 includes a wedge prism that refracts the collimated light beam 219 .

In one embodiment, the scanning module 202 further includes a second optical element 215 , the second optical element 215 rotates around the rotation axis 209 , and the rotation speed of the second optical element 215 is different from the rotation speed of the first optical element 214 . The second optical element 215 is used to change the direction of the light beam projected by the first optical element 214 . In one embodiment, the second optical element 215 is connected to another driver 217, and the driver 217 drives the second optical element 215 to rotate. The first optical element 214 and the second optical element 215 may be driven by the same or different drivers, so that the rotational speed and/or steering of the first optical element 214 and the second optical element 215 are different, thereby projecting the collimated beam 219 into the external space Different directions can scan a larger spatial range. In one embodiment, the controller 218 controls the

drivers

216 and 217 to drive the first optical element 214 and the second optical element 215, respectively. The rotational speeds of the first optical element 214 and the second optical element 215 may be determined according to the area and pattern expected to be scanned in practical applications.

Drives

216 and 217 may include motors or other drives.

In one embodiment, the second optical element 215 includes a pair of opposing non-parallel surfaces through which the light beam passes. In one embodiment, the second optical element 215 comprises a prism whose thickness varies along at least one radial direction. In one embodiment, the second optical element 215 comprises a wedge prism.

In one embodiment, the scanning module 202 further includes a third optical element (not shown) and a driver for driving the movement of the third optical element. Optionally, the third optical element includes a pair of opposing non-parallel surfaces through which the light beam passes. In one embodiment, the third optical element comprises a prism of varying thickness along at least one radial direction. In one embodiment, the third optical element comprises a wedge prism. At least two of the first, second and third optical elements rotate at different rotational speeds and/or rotations.

The rotation of each optical element in the scanning module 202 can project light in different directions, such as

directions

211 and 213 , thus scanning the space around the laser measurement device 200 .

The laser radar in the embodiment of the present application can not only acquire point cloud data in a traditional repeated scanning manner, but also acquire point cloud data in a non-repetitive scanning manner. Compared with the former, the scanning paths corresponding to two adjacent point cloud frames output by lidar are different, which has the advantages of high point cloud coverage, low cost and high reliability. FIG. 3 shows a schematic diagram of the scanning pattern of the non-repetitive scanning lidar.

Since the laser scanning position of the non-repetitive scanning radar is always changing, there will be a large difference even between two adjacent frames of point clouds. Therefore, when the pose estimation method based on point cloud matching faces non-repetitive scanning point clouds, the accuracy and stability of pose estimation are relatively poor. One of the typical performances is that even if the radar is in a stationary state, the result of pose estimation It will show that the radar is in a jittering or even drifting motion state, which reduces the pose estimation ability of the lidar.

The embodiments of the present application propose to use the IMU and lidar point cloud fusion method to perform pose estimation, which is beneficial to improve the point cloud matching effect and obtain a radar pose estimation result with higher accuracy and stability.

The IMU is generally composed of gyroscopes and accelerometers in three directions of x, y, and z, which can output high-frequency gyroscope data and accelerometer data, and usually the short-term accuracy of the IMU data is very high. The gyroscope can measure the angular velocity of the body, and the rotation angle can be obtained by integrating it once. The accelerometer can measure the magnitude of acceleration, the velocity can be obtained by one integration, and the displacement can be obtained by the second integration. This means that the integral result of the IMU can provide high-quality initial pose values for the point cloud matching of lidar. Moreover, if the IMU integration is introduced into the optimization process of point cloud matching, additional pose constraints can be added to the point cloud matching, and the accuracy of the point cloud matching can be improved, thereby making the radar pose estimation more robust.

Specifically, the methods of merging IMU and lidar for pose estimation are mainly divided into two categories: loose coupling and tight coupling. The loosely coupled method mainly performs distortion correction for the point cloud through the IMU integration result, provides the initial value of point cloud matching, and filters and fuses the IMU integration result and the point cloud matching result. The tightly coupled pose estimation method has higher accuracy and stability, but it needs to initialize the IMU parameters first. Orientation estimation, etc. Then, when the point cloud matching is performed to solve the radar pose, the integral result of the IMU is added as a pose constraint to the solution process, which can more effectively suppress the point cloud matching error and make the pose estimation result more accurate.

At the same time, when the IMU and lidar are tightly coupled for pose estimation, initializing the IMU parameters is a critical step. However, due to the unstable matching accuracy of pure point cloud of non-repetitive scanning lidar, it is easy to cause large initialization errors of IMU parameters, especially the acceleration of gravity.

The estimation result of , seriously affects the integration accuracy of the gravitational accelerometer, which in turn has a great impact on the accuracy and stability of the entire tightly coupled pose estimation.

Therefore, an embodiment of the present application provides a method for estimating a pose. When the pose estimation ends, it is further judged whether the estimated value of the gravitational acceleration obtained from the pose estimation is close to the reference value of the gravitational acceleration. Only in the case of the reference value, the result of the pose estimation is output, which is beneficial to improve the accuracy and robustness of the pose estimation.

FIG. 4 shows a schematic block diagram of a method 300 for pose estimation according to an embodiment of the present application. As shown in Figure 4, the method 300 includes some or all of the following:

S310, after the initialization of the IMU data output by the inertial measurement unit IMU is completed, perform pose estimation of the lidar according to the integration result of the IMU data and the laser point cloud data output by the lidar, and obtain the motion acceleration of the lidar, the IMU data includes gyroscope data and accelerometer data;

S320, obtain an estimated value of gravitational acceleration according to the motion acceleration of the lidar and the accelerometer data;

S330, according to the closeness between the estimated value of the gravitational acceleration and the reference value of the gravitational acceleration, determine whether to update the estimated value of the gravitational acceleration;

S340, when it is determined not to update the estimated value of the gravitational acceleration, output the result of the pose estimation.

Gravitational acceleration refers to the acceleration of objects near the ground falling in a vacuum under the action of the earth's gravity, and its direction is always vertically downward, usually recorded as

In order to facilitate the calculation, its approximate standard value is usually taken as 9.8 m/s2 ⁽ for the convenience of description, the unit is omitted later), that is, the reference value of the acceleration of gravity mentioned in this article can be 9.8, but it should be understood that due to the acceleration of gravity It is related to the position of the object on the earth, and the reference value of the gravitational acceleration in the embodiment of the present application is not constant as 9.8.

It should be understood that since the IMU is mounted with the lidar, when the lidar is stationary, the accelerometer data measured by the IMU is the gravitational acceleration. When the lidar is moving, the accelerometer data measured by the IMU is the acceleration vector synthesized by the acceleration of gravity and the motion acceleration of the lidar. Therefore, by closely coupling the IMU and the lidar for pose estimation, the motion acceleration of the lidar can be obtained, and then combined with the accelerometer data output by the IMU, the estimated value of the gravitational acceleration can be obtained. Further, it can be judged whether the obtained estimated value of gravitational acceleration is close to the reference value of gravitational acceleration, if it is close to the reference value of gravitational acceleration, it means that the obtained estimated value of gravitational acceleration has a small error, and the pose estimation obtained through it The results are also more accurate, that is to say, the accuracy of the pose estimation is relatively high.

It should also be understood that the scanning mode of the laser radar in the embodiment of the present application may be not only repeated scanning, but also non-repetitive scanning. Improve the pose estimation accuracy and robustness of lidar.

It should be noted that if the obtained estimated value of gravitational acceleration is not close to the reference value of gravitational acceleration, the estimated value of gravitational acceleration can be updated, and the updated estimated value of gravitational acceleration is used for pose estimation. In this way, the pose estimation method 300 provided by the embodiment of the present application may be executed cyclically until the recently obtained estimated value of the gravitational acceleration is close to the reference value of the gravitational acceleration.

By correcting the gravitational acceleration to suppress the initialization error of the IMU, the robustness and accuracy of the tightly coupled pose estimation between the lidar and the IMU can be significantly improved.

In one embodiment, determining whether to update the estimated value of gravitational acceleration according to the closeness between the estimated value of gravitational acceleration and the reference value of gravitational acceleration may include: comparing the estimated value of gravitational acceleration obtained this time with the Whether the change amount (also called the update amount) of the estimated value of gravitational acceleration obtained at one time is less than a certain threshold (such as the first threshold), if the estimated value of gravitational acceleration obtained this time is the same as the estimated value of gravitational acceleration obtained last time If the variation of the gravitational acceleration is less than the first threshold, the pose estimation result obtained here can be output, if the variation between the estimated value of gravitational acceleration obtained this time and the estimated value of gravitational acceleration obtained last time is greater than or equal to the first threshold , the estimated value of the gravitational acceleration can be updated, and the pose estimation can be performed again based on the updated estimated value of the gravitational acceleration.

In another embodiment, determining whether to update the estimated value of the gravitational acceleration according to the closeness between the estimated value of the gravitational acceleration and the reference value of the gravitational acceleration may include: according to the estimated value of the gravitational acceleration obtained this time and the gravitational acceleration Whether the error between the reference values of the The estimated value of the gravitational acceleration is updated, and the pose estimation is further performed based on the updated estimated value of the gravitational acceleration.

In other embodiments, the reference value of the gravitational acceleration may also be an interval. If the obtained estimated value of the gravitational acceleration is within the interval, the pose estimation result can be output. If the obtained estimated value of the gravitational acceleration is not in this interval Within the range, the estimated value of the gravitational acceleration can be updated, and the pose estimation can be performed again based on the updated estimated value of the gravitational acceleration.

It should be understood that how to update the estimated value of the gravitational acceleration includes but is not limited to the above-mentioned various embodiments.

Optionally, in this embodiment of the present application, updating the estimated value of the gravitational acceleration may include: integrating all IMU data between the (i+1)th moment and the ith moment to obtain the (i+1) The angle change amount, the speed change amount and the displacement change amount between the time and the i-th time; determine the error of the angle change between the (i+1) time and the i-th time, and the speed change The (i+1)th error function vector constituted by the error of the quantity and the error of the displacement change; according to the (i+1)th error function vector and the first From the error function vector to the ith error function vector, the estimated value of the gravitational acceleration is updated, where i is a positive integer.

Specifically, an error function vector can be defined, which is composed of the error of the angle variation, the error of the velocity variation, and the error of the displacement variation. Among them, the angle change amount may be the angle change amount between two frames of point cloud data obtained continuously, the speed change amount may be the speed change amount between the two frames of point cloud data obtained continuously, and the displacement change amount may be obtained two frames of point cloud data continuously. The amount of displacement change between cloud data. That is to say, there is an error function vector between every two consecutive frames of point cloud data. Denoted as the first error function vector, the second error function vector..., and the i-th error function vector. When the estimated value of gravitational acceleration needs to be updated, the (i+1)th error function vector can be obtained, and based on the first error function vector, the second error function vector..., the ith error function vector and the (i+1)th error function vector to update the estimated value of gravitational acceleration.

Among them, the angle change in the ith error function vector is obtained by integrating all the IMU data between the (i-1)th moment and the ith moment. That is, the angle change is obtained by integrating the gyroscope data once, the velocity change is obtained by integrating the accelerometer data once, and the displacement change is obtained by integrating the accelerometer data twice.

Optionally, in this embodiment of the present application, the method 300 further includes: initializing the IMU data.

In an embodiment, the initialization processing of the IMU data may be performed in a traditional initialization manner, that is, a step-by-step manner. For example, the gyroscope bias can be initialized first, then the accelerometer bias can be initialized, and then the magnitude and direction of the gravitational acceleration can be initialized. However, the estimation errors of each step of this initialization method cannot be unified, and it is easy to cause inaccurate estimation of the initial values of parameters in some steps, which leads to the phenomenon of initialization failure.

In another embodiment, all parameters in the IMU data can be unified into one optimization framework for synchronous solution, which can balance the estimation errors of different parameters, improve the solution success rate, and effectively prevent the problem of initialization failure that is easy to occur in the step-by-step initialization method. . Specifically, the initialization vector can be solved to obtain the initial value of the IMU data, and the initialization vector can be composed of gyroscope data, accelerometer data and gravitational acceleration vector.

Optionally, in this embodiment of the present application, before the initialization of the IMU data is completed, the pose estimation may be performed by loosely coupling the IMU data and the lidar point cloud. Further, according to the loosely coupled pose estimation result, the IMU data is processed. For example, before the initialization of the IMU data is completed, pure point cloud matching can be performed based on the laser point cloud data, and the integration result of the IMU data and the point cloud matching result are filtered and fused, and the initialization vector can be solved according to the result of the filtering and fusion.

Optionally, in this embodiment of the present application, solving the initialization vector according to the result of the filtering and fusion may include: obtaining the absolute pose information of the lidar at two adjacent moments according to the result of the filtering and fusion, and the adjacent The two moments are the receiving moments of two adjacent frames of laser point cloud data, and further all the IMU data between the two adjacent moments can be integrated to obtain the relative pose information of the two adjacent frames of laser point cloud data; The initialization vector is solved according to the absolute pose information and the relative pose information. The absolute pose information may include angle, velocity, and displacement, and the relative pose information may include angle change, speed change, and displacement change. The angle variation, velocity variation and displacement variation here can refer to the above description.

Optionally, in this embodiment of the present application, before performing pose estimation by tightly coupling the IMU data with the lidar point cloud, distortion correction may also be performed on the laser point cloud data obtained by the lidar. The distorted point cloud correction may include point cloud rotation distortion correction and/or point cloud translation distortion correction.

Specifically, point cloud rotation distortion correction is accomplished by integrating the gyroscope data within the receiving time of the two frames of laser point cloud data once to obtain the relative angular change of the two frames of laser point cloud data, and then through linear interpolation. Since the gyroscope is not affected by the gravitational acceleration, the point cloud rotation distortion correction can be performed before the initialization of the IMU data, while the point cloud translation distortion correction is performed by the two frames of the accelerometer data during the receiving time of the laser point cloud data. Sub-integration to obtain the displacement change of the connected laser point cloud data, and then complete it through linear interpolation. Since the accelerometer is affected by the gravitational acceleration, the point cloud translation distortion correction needs to be performed after the IMU data is initialized.

A schematic block diagram of a method 400 for pose estimation according to an embodiment of the present application will be described in detail below with reference to FIG. 5 . As shown in Figure 5, the method 400 includes:

S410, read sensor data. It is mainly responsible for receiving IMU data and laser point cloud data, completing the time synchronization of different data, and jumping to S420. The IMU data mainly includes gyroscope data and accelerometer data.

S420, point cloud rotation distortion correction. Since the gyroscope is not affected by the gravitational acceleration, it is possible to directly perform a time integration on the gyroscope data within the receiving time of the two frames of point clouds to obtain the relative angle change of the two frames of point clouds. The cloud is corrected for rotational distortion.

S430, determine whether the initialization of the IMU data is completed, if not, then jump to step S440.

S440, the pose estimation is performed by loosely coupling the gyroscope data and the lidar point cloud. In this step, point cloud matching is performed using the point cloud with the rotation distortion removed. At the same time, the integration result of the gyroscope data can provide the point cloud matching with a high-precision and fast-response initial rotation value, which can improve the accuracy and efficiency of point cloud matching. Success rate. After the point cloud matching is completed, the matching results and the integration results of the gyroscope data are filtered and fused (such as Kalman filtering), which can further improve the accuracy of pose estimation.

S450, initialize the IMU data according to the result outputted in S440. Using graph optimization theory, a fast and stable parameter optimization framework is proposed, which can better complete the initial value estimation of IMU parameters. IMU parameters mainly include gyroscope bias (δb ^g ), accelerometer bias (δb ^a ) and the gravitational acceleration vector

Therefore, the variable to be solved (X) in the optimization process can be defined as:

Assuming that the timestamps of two consecutive frames of laser point cloud data are i and j, the error function vector f _i,j (X) is defined as follows:

Let the angle, velocity, and displacement of the lidar pose at the _i -th time be Ri, _Vi , and _Pi , respectively. The time stamp is the angle change, velocity change, and displacement change calculated by integration of all IMU data between i and j, respectively:

as well as

but

as well as

They are defined as follows:

Finally, the value of the variable to be solved (X) can be expressed as:

Here, the variable to be solved (X) is the initialization vector described in the method 300 .

The embodiment of the present application unifies all parameters into the same optimization framework and solves synchronously, which can balance the estimation errors of different parameters, improve the solution success rate, and effectively prevent the initialization failure problem that is easy to occur in the step-by-step initialization method.

S460, after the initialization of the IMU data is completed, the accelerometer data after initialization can be time-integrated to obtain the displacement increment between the laser point cloud data of adjacent frames, and then the laser point cloud data can be calculated by the linear difference method. The translational distortion of the data is corrected. At this time, the original laser point cloud data output by the lidar has completed point cloud rotation distortion correction and point cloud translation distortion correction at the same time.

In S470, the tightly coupled pose optimization is performed on the integrated result of the corrected laser point cloud data and the IMU data, so that a radar pose estimation result with higher accuracy and stronger robustness can be obtained. Specifically, assuming that the receiving times of two adjacent frames of laser point cloud data are i and j respectively, the variables to be solved in this problem are defined as:

T _j =[R _i V _i P _i b ^g b ^a ] ^T

The definition of the error function vector f _i,j (T _j ) is the same as formulas (1)-(4), and defines the reprojection error function between the laser point cloud data i and j:

e _i,j (T _j )=F _i -ΔT _i,j F _j

Among them, F _i and F _j represent the correspondence between point clouds i and j, which can be the distance from point to point, the distance from point to plane, the distance from point to straight line, etc. ΔT _i,j is the relative pose between point cloud i and j, which can be obtained by:

ΔT _i,j =T _i ^-1 T _j

Calculated. Then the result of the variable to be solved can be expressed as:

S480, the estimated value of the gravitational acceleration can be obtained according to the b ^a obtained in S470 and the accelerometer data in the IMU data. And judge whether the estimated value of gravitational acceleration is close to the reference value of gravitational acceleration, if not, then recalculate the estimated value of gravitational acceleration, that is, correct the gravitational acceleration, and bring the corrected result to step S470 Perform pose estimation again. If it is close, the correction of the gravitational acceleration is stopped, and the result of the pose estimation is output.

The correction of the gravitational acceleration may adopt the method in step S450 or step S470. Specifically, the band solution variable can be defined as

Error function vector

The definition of is the same as formulas (1)-(4). according to

Whether the update amount of is less than a certain threshold is used to determine whether to output the pose estimation result. i.e. if

The update amount of is less than a certain threshold, indicating that the result is converged, and the gravitational acceleration vector is ended.

correction process.

For non-repetitive scanning radar, it is often not very accurate to rely solely on point cloud matching, or only loosely couple the radar and the IMU to estimate the radar pose. This will introduce a large error to the initialization of the IMU parameters, resulting in poor initialization accuracy of the IMU parameters, thus affecting the accuracy and stability of the tightly coupled pose optimization. especially the acceleration of gravity

The initialization result of , seriously affects the integration accuracy of the accelerometer, which in turn has a great impact on the accuracy and stability of the entire tightly coupled pose estimation. By correcting the gravitational acceleration in the embodiments of the present application, the initial error can be suppressed, and the accuracy and robustness of the tightly coupled pose estimation can be improved.

The method for estimating the pose according to the embodiment of the present application has been described above, and the apparatus for implementing the pose estimation according to the embodiment of the present application will be described below.

FIG. 6 shows a schematic block diagram of an apparatus 500 for pose estimation according to an embodiment of the present application.

As shown in FIG. 6 , the apparatus 500 may include a processor 510 , and may further include a memory 520 .

It should be understood that the apparatus 500 may further include other components, such as input and output devices, communication interfaces, etc., which are not limited in this embodiment of the present application.

Memory 520 is used to store computer-executable instructions.

The memory 520 may be various types of memory, for example, may include a high-speed random access memory (Random Access Memory, RAM), and may also include a non-volatile memory (non-volatile memory), such as at least one disk memory, which is implemented in this application. The example is not limited to this.

The processor 510 is configured to access the memory 520 and execute the computer-executable instructions, so as to perform the operations in the above-mentioned method for estimating the pose of the embodiments of the present application.

The processor 210 may include a microprocessor, a field-programmable gate array (Field-Programmable Gate Array, FPGA), a central processing unit (Central Processing unit, CPU), a graphics processing unit (Graphics Processing Unit, GPU), etc., implemented in this application The example is not limited to this.

The apparatus 500 for pose estimation in this embodiment of the present application may correspond to the execution body of the method for pose estimation in this embodiment of the present application, and the above and other operations and/or functions of the modules of the apparatus 500 for pose estimation are for the purpose of realizing For the sake of brevity, the corresponding processes of the foregoing methods are not repeated here.

An embodiment of the present application further provides an electronic device, and the electronic device may include the apparatus for estimating the pose according to the various embodiments of the present application.

An embodiment of the present application further provides a computer storage medium, where a program code is stored in the computer storage medium, and the program code can be used to instruct to execute the video processing method of the above embodiment of the present application.

It should be understood that, in this embodiment of the present application, the term "and/or" is only an association relationship for describing associated objects, indicating that there may be three kinds of relationships. For example, A and/or B can mean that A exists alone, A and B exist at the same time, and B exists alone. In addition, the character "/" in this document generally indicates that the related objects are an "or" relationship.

Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. Interchangeability, the above description has generally described the components and steps of each example in terms of function. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of this application.

Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the system, device and unit described above may refer to the corresponding process in the foregoing method embodiments, and will not be repeated here.

In the several embodiments provided in this application, it should be understood that the disclosed system, apparatus and method may be implemented in other manners. For example, the apparatus embodiments described above are only illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components may be combined or Can be integrated into another system, or some features can be ignored, or not implemented. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be indirect coupling or communication connection through some interfaces, devices or units, and may also be electrical, mechanical or other forms of connection.

The units described as separate components may or may not be physically separated, and components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solutions of the embodiments of the present application.

In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated unit, if implemented in the form of a software functional unit and sold or used as an independent product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solutions of the present application are essentially or part of contributions to the prior art, or all or part of the technical solutions can be embodied in the form of software products, and the computer software products are stored in a storage medium , including several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), magnetic disk or optical disk and other media that can store program codes .

The above are only specific embodiments of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art can easily think of various equivalents within the technical scope disclosed in the present application. Modifications or substitutions shall be covered by the protection scope of this application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.

Claims

A method for pose estimation, comprising:

After the initialization of the IMU data output by the inertial measurement unit IMU is completed, the pose estimation of the lidar is performed according to the integration result of the IMU data and the laser point cloud data output by the lidar, and the motion acceleration of the lidar is obtained, The IMU data includes gyroscope data and accelerometer data;

Obtain an estimated value of gravitational acceleration according to the motion acceleration of the lidar and the accelerometer data;

determining whether to update the estimated value of gravitational acceleration according to the closeness between the estimated value of gravitational acceleration and the reference value of gravitational acceleration;

When it is determined not to update the estimated value of the gravitational acceleration, the result of the pose estimation is output.
The method according to claim 1, wherein the method further comprises:

When it is determined to update the estimated value of the gravitational acceleration, update the estimated value of the gravitational acceleration;

According to the updated estimated value of the gravitational acceleration, the pose estimation of the lidar is performed again.
The method according to claim 2, wherein the updating the estimated value of the gravitational acceleration comprises:

Integrate all IMU data between the (i+1) time and the i time to obtain the angle change, velocity change and displacement change between the (i+1) time and the i time ;

Determine the (i+1)th error function vector formed by the error of the angle change, the error of the speed change and the error of the displacement change between the (i+1)th moment and the ith moment;

The estimated value of the gravitational acceleration is updated according to the (i+1)th error function vector and the 1st error function vector to the ith error function vector determined when the estimated value of the gravitational acceleration is obtained, wherein , i is a positive integer.
The method according to any one of claims 1 to 3, wherein, determining whether to update the gravitational acceleration according to the closeness between the estimated value of the gravitational acceleration and the reference value of the gravitational acceleration estimates, including:

Whether the estimated value of gravitational acceleration is updated is determined according to whether the amount of change between the estimated value of gravitational acceleration and the estimated value of gravitational acceleration determined by the previous pose estimation is smaller than a first threshold.
The method according to any one of claims 1 to 3, wherein, determining whether to update the gravitational acceleration according to the closeness between the estimated value of the gravitational acceleration and the reference value of the gravitational acceleration estimates, including:

Whether or not to update the estimated value of gravitational acceleration is determined according to whether the error between the estimated value of gravitational acceleration and the reference value of gravitational acceleration is smaller than a second threshold.
The method according to any one of claims 1 to 5, wherein the method further comprises:

Initialize the IMU data.
The method according to claim 6, wherein the initializing the IMU data comprises:

The initialization vector is solved to obtain the initial value of the IMU data, and the initialization vector is composed of the gyroscope data, the accelerometer data and the gravitational acceleration vector.
The method according to claim 6 or 7, wherein the method further comprises:

Before the initialization of the IMU data is completed, point cloud matching is performed based on the laser point cloud data;

Filter and fuse the integration result of the IMU data and the point cloud matching result;

The solution initialization vector includes:

According to the result of the filtering and fusion, the initialization vector is solved.
The method according to claim 8, wherein, calculating the initialization vector according to the result of the filtering and fusion, comprising:

According to the result of the filtering and fusion, obtain the absolute pose information of the lidar at two adjacent moments respectively, and the two adjacent moments are the receiving moments of the two adjacent frames of laser point cloud data;

Integrate all the IMU data between the two adjacent moments to obtain the relative pose information of the two adjacent frames of laser point cloud data;

The initialization vector is solved according to the absolute pose information and the relative pose information.
The method according to claim 9, wherein the absolute pose information includes angle, velocity and displacement, and the relative pose information includes angle change, velocity change and displacement change.
The method according to any one of claims 1 to 10, wherein before performing the pose estimation of the lidar, the method further comprises:

Distortion correction is performed on the laser point cloud data.
The method according to claim 11, wherein the performing distortion correction on the laser point cloud data comprises:

Perform point cloud rotation distortion correction and point cloud translation distortion correction on the laser point cloud data.
The method according to claim 12, wherein the performing point cloud rotation distortion correction on the laser point cloud data comprises:

Integrate the gyroscope data between the receiving moments of two adjacent frames of laser point cloud data to obtain the angular variation of the two adjacent frames of laser point cloud data;

The point cloud rotation distortion correction of the laser point cloud data is performed in the manner of linear interpolation for the angle change.
The method according to claim 12 or 13, wherein the performing point cloud translation and distortion correction on the laser point cloud data comprises:

Integrate the accelerometer data after initialization twice to obtain the displacement change of two adjacent frames of laser point cloud data;

The point cloud translation and distortion correction of the laser point cloud data is performed on the displacement variation in the manner of linear interpolation.
The method according to any one of claims 1 to 14, wherein the laser point cloud data is obtained by the lidar in a non-repetitive scanning manner, wherein the non-repetitive scanning manner refers to Scanning paths corresponding to two adjacent point cloud frames output by the lidar are different.
A device for pose estimation, comprising a processor for:

After the initialization of the IMU data output by the inertial measurement unit IMU is completed, the pose estimation of the lidar is performed according to the integration result of the IMU data and the laser point cloud data output by the lidar, and the motion acceleration of the lidar is obtained, The IMU data includes gyroscope data and accelerometer data,

According to the motion acceleration of the lidar and the accelerometer data, the estimated value of the gravitational acceleration is obtained,

determining whether to update the estimated value of gravitational acceleration according to the closeness between the estimated value of gravitational acceleration and the reference value of gravitational acceleration, and

When it is determined not to update the estimated value of the gravitational acceleration, the result of the pose estimation is output.
The apparatus of claim 16, wherein the processor is further configured to:

When it is determined to update the estimated value of the acceleration of gravity, update the estimated value of the acceleration of gravity;

According to the updated estimated value of the gravitational acceleration, the pose estimation of the lidar is performed again.
The apparatus according to claim 17, wherein the processor is specifically configured to:

Integrate all IMU data between the (i+1) time and the i time to obtain the angle change, velocity change and displacement change between the (i+1) time and the i time ;

Determine the (i+1)th error function vector formed by the error of the angle change, the error of the speed change and the error of the displacement change between the (i+1)th moment and the ith moment;

The estimated value of the gravitational acceleration is updated according to the (i+1)th error function vector and the 1st error function vector to the ith error function vector determined when the estimated value of the gravitational acceleration is obtained, wherein , i is a positive integer.
The apparatus according to any one of claims 16 to 18, wherein the processor is specifically configured to:

Whether the estimated value of gravitational acceleration is updated is determined according to whether the amount of change between the estimated value of gravitational acceleration and the estimated value of gravitational acceleration determined by the previous pose estimation is smaller than a first threshold.
The apparatus according to any one of claims 16 to 18, wherein the processor is specifically configured to:

Whether or not to update the estimated value of gravitational acceleration is determined according to whether the error between the estimated value of gravitational acceleration and the reference value of gravitational acceleration is smaller than a second threshold.
The apparatus according to any one of claims 16 to 20, wherein the processor is further configured to:

Initialize the IMU data.
The apparatus according to claim 21, wherein the processor is specifically configured to:

The initialization vector is solved to obtain the initial value of the IMU data, and the initialization vector is composed of the gyroscope data, the accelerometer data and the gravitational acceleration vector.
The apparatus according to claim 21 or 22, wherein the processor is further configured to:

Before the initialization of the IMU data is completed, point cloud matching is performed based on the laser point cloud data;

Filter and fuse the integration result of the IMU data and the point cloud matching result;

The processor solves the initialization vector, including:

According to the result of the filtering and fusion, the initialization vector is solved.
The apparatus according to claim 23, wherein the processor is specifically configured to:

According to the result of the filtering and fusion, obtain the absolute pose information of the lidar at two adjacent moments respectively, and the two adjacent moments are the receiving moments of the two adjacent frames of laser point cloud data;

Integrate all the IMU data between the two adjacent moments to obtain the relative pose information of the two adjacent frames of laser point cloud data;

The initialization vector is solved according to the absolute pose information and the relative pose information.
The device according to claim 24, wherein the absolute pose information includes angle, velocity and displacement, and the relative pose information includes angle change, velocity change and displacement change.
The apparatus according to any one of claims 16 to 25, wherein before performing the pose estimation of the lidar, the processor is further configured to:

Distortion correction is performed on the laser point cloud data.
The apparatus according to claim 26, wherein the processor is specifically configured to:

Perform point cloud rotation distortion correction and point cloud translation distortion correction on the laser point cloud data.
The device according to claim 27, wherein the processor performs point cloud rotation distortion correction on the laser point cloud data, comprising:

Integrate the gyroscope data between the receiving moments of two adjacent frames of laser point cloud data to obtain the angular variation of the two adjacent frames of laser point cloud data;

The point cloud rotation distortion correction of the laser point cloud data is performed in the manner of linear interpolation for the angle change.
The device according to claim 27 or 28, wherein the processor performs point cloud translation and distortion correction on the laser point cloud data, comprising:

Integrate the accelerometer data after initialization twice to obtain the displacement change of two adjacent frames of laser point cloud data;

The point cloud translation and distortion correction of the laser point cloud data is performed on the displacement variation in the manner of linear interpolation.
The device according to any one of claims 16 to 29, wherein the laser point cloud data is obtained by the lidar in a non-repetitive scanning manner, wherein the non-repetitive scanning manner refers to Scanning paths corresponding to two adjacent point cloud frames output by the lidar are different.
A computer storage medium, characterized in that the computer storage medium is used for storing program codes, and the program codes are used for instructing to execute the method according to any one of claims 1 to 15 .