Dynamic and static estimation method and equipment for low-speed or static object and mobile device

Abstract

The invention discloses a dynamic and static estimation method and device for a low-speed or static object and a mobile device, and the method comprises the steps: S1, obtaining original point clouds of a front detection frame and a rear detection frame through a laser radar sensor; s2, acquiring an obstacle target frame obtained after detection, and finding out a rigid body point cloud cluster of a related point cloud connected domain as an obstacle by taking a point cloud in an envelope frame as a seed point; s3, eliminating the observation error of the own vehicle through coordinate transformation, and transforming the point cloud at the next moment to the coordinate system by taking the coordinate system of the other vehicle at the previous moment as a reference; s4, calculating the optimal transformation of the grid clusters of the front and back frames; and S5, judging whether the obstacle moves or not based on the optimal transformation result obtained in the step S4. According to the method, the motion state of the obstacle is estimated very accurately, the resolving result of the static obstacle is close to 0, and on the contrary, as long as the obstacle moves, even if the obstacle is very tiny, the result can be reflected; meanwhile, the resolving result, namely the movement displacement and orientation of the moving target, can also be used as an input source for calculating the differential speed.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method, equipment and mobile device for dynamic and static estimation of low-speed or stationary objects. Background Art

[0002] In the field of autonomous driving, the detection and motion estimation of traffic participants (vehicles, pedestrians, non-motor vehicles, etc., hereinafter referred to as obstacles) around the ego vehicle is crucial and is necessary information for many subsequent decisions. Obstacle detection and recognition require the help of some sensors, such as cameras and lidar, to identify the location, category, posture and other information of obstacles through the images or point clouds obtained by them. The motion estimation of obstacles requires certain time series information to associate the same targets of different time series (target tracking), and on this basis, calculate the speed, acceleration and other motion parameters through the position to determine whether there will be interaction with the ego vehicle's motion. At the same time, due to the influence of the ego vehicle's motion, the relative motion information obtained by detection and tracking calculations needs to be transformed into an inertial coordinate system to obtain the objective motion information of the obstacle. In particular, the impact on the ego vehicle is particularly critical in the scenes where the key obstacles around the ego vehicle are unclear in terms of dynamic and static, such as turning around, slowing down, starting, slowing down and parking, and the frequent switching between dynamic and static.

[0003] In the prior art, the motion characteristics of obstacles are generally obtained by the following methods: (1) differentiating the position information of the previous and next frames to obtain the position difference and thus calculate the speed and acceleration at the current moment; (2) directly inferring the motion parameters by training the model.

[0004] Generally, the judgment of dynamic and static is mainly determined by the output speed. When the speed is zero or less than the disturbance speed, the obstacle is considered to be stationary, otherwise it is considered to be moving.

[0005] For the differential calculation method of speed, it is necessary to use the regular 3D detection envelope rectangle obtained by the detection of the previous and next frames, and obtain it by differentiating the world coordinate position of the center point and dividing it by the time difference. For the direct model output solution, the speed truth value of the labeled training data needs to be obtained by annotating the obstacle rectangular envelope and the unique ID in the time series, and then obtaining it by the aforementioned method of differentiating the center point position of the previous and next frames.

[0006] However, the appearance information (size, orientation, etc.) output by the detection model is unstable, and the differential calculation speed will produce deviations. Especially when the obstacle moves at a low speed (less than or close to 1m / s), the dynamic and static judgments are inaccurate due to the jumping of the bounding box. In addition, due to the long-tail effect of model training, there is less labeled data for low-speed scenes, and it is difficult to obtain the true value, so the output effect of the motion state of low-speed (less than or close to 1m / s) and stationary obstacles is poor. Summary of the invention

[0007] In order to solve the problem that the position and size of low-speed target detection fluctuate and the dynamic and static estimation is inaccurate, an embodiment of the present invention provides a method for dynamic and static estimation of obstacles, which is applied to a mobile device equipped with a laser radar sensor.

[0008] In order to achieve the above-mentioned purpose, the technical solution of the present invention provides a method for dynamic and static estimation of low-speed or stationary objects, which includes the following steps: S1: obtaining the original point clouds of the front and rear detection frames through a laser radar sensor; S2: obtaining the obstacle target frame obtained after detection, taking the point cloud in the envelope frame as the seed point, and finding the connected domain of the relevant point cloud as the rigid point cloud cluster of the obstacle; S3: eliminating the observation error of the vehicle through coordinate transformation, taking the coordinate system of the other vehicle at the previous moment as the reference, and transforming the point cloud at the next moment to the coordinate system; S4: calculating the optimal transformation of the grid clusters of the front and rear frames; S5: judging whether the obstacle moves based on the optimal transformation result obtained in step S4.

[0009] Furthermore, in step S1, the acquired original point cloud is downsampled to generate a placeholder grid cluster to obtain a downsampled point cloud for subsequent calculations.

[0010] Furthermore, in step S1, generating a placeholder grid cluster specifically includes: intercepting a point cloud within a certain range around the vehicle as a boundary, and dividing it into resolution_x*resolution_y*resolution_z grids according to the computational requirements; if there is a laser point in the grid, the grid is considered to be occupied, where resolution_x, resolution_y and resolution_z are the resolutions of the three-dimensional grid in the x, y and z directions respectively.

[0011] Further, in step S3, the other vehicle coordinate system is defined as a coordinate system with the center of the envelope as the origin and the direction of the obstacle as the x-axis, wherein the envelope rectangle BBOX of the target obstacle has been calculated by the upstream detection t1 BBOX t2 , the corresponding calculated point cloud cluster sets are and

[0012] At time t1, the point cloud cluster coordinates P of the obstacle t1 :

[0013]

[0014] in, l1, l2, θ represent the position and orientation of the obstacle observed by the vehicle relative to the vehicle coordinate system.

[0015] At time t2, the point cloud cluster coordinates P of the obstacle t2 :

[0016]

[0017] Among them, Pose t1 and Pose t2 They represent the posture of the vehicle relative to the world coordinate system at each moment.

[0018] Further, in step S4,

[0019] Compute the optimal transformation of mesh clusters between previous and next frames:

[0020]

[0021] Solving Transformations Matrix; when the number of points is large (greater than 500), P t1 ,P t2 Build kdtree matching point pairs.

[0022] Further, in step S5, when When it is less than the threshold thres11, it is considered stationary and the speed is 0; When it is greater than the threshold thres1, it is considered motion; when When it is greater than the threshold thres2, it is considered that the solution fails and the targets of the previous and next frames are inconsistent; Represented by the matrix The solved Euler angles.

[0023] Further, in step S4, the transformation is solved by using the iterative closest point algorithm ICP algorithm or the Jacobi matrix singular value decomposition JacobiSVD method. matrix.

[0024] The technical solution of the present invention also provides a dynamic and static estimation device for low-speed or stationary objects, and the device includes the following modules: an original point cloud acquisition module, which is used to obtain the original point clouds of the front and rear detection frames through a laser radar sensor; an obstacle target frame acquisition module, which is used to obtain the obstacle target frame obtained after detection, and use the point cloud in the envelope frame as a seed point to find the rigid point cloud cluster of the relevant point cloud connected domain as the obstacle; a coordinate system transformation module, which is used to eliminate the observation error of the vehicle through coordinate transformation, and transform the point cloud at the next moment into the coordinate system based on the coordinate system of the other vehicle at the previous moment; an optimal transformation calculation module, which is used to calculate the optimal transformation of the grid clusters of the front and rear frames; an obstacle motion judgment module, which is used to judge whether the obstacle moves based on the optimal transformation result obtained by the optimal transformation calculation module.

[0025] The technical solution of the present invention also provides a mobile device equipped with a laser radar sensor, which uses the dynamic and static estimation method of low-speed or stationary objects as described above, or is loaded with the dynamic and static estimation device of low-speed or stationary objects as described above.

[0026] The technical solution of the present invention also provides a computer-readable storage medium containing a computer program. When the computer program is executed by one or more processors, the dynamic and static estimation method of the low-speed or stationary object as described above is implemented.

[0027] The technical solution of the present invention can estimate the motion state of obstacles very accurately. The solution result of a stationary obstacle is close to 0. On the contrary, as long as there is movement, even if it is very small, it will be reflected in the result. At the same time, the solution result, that is, the motion displacement and direction of the moving target, can also be used as an input source for calculating the differential velocity. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0029] Figure 1 It is a flow chart of a method for estimating the dynamic and static state of a low-speed or stationary object of the present invention;

[0030] Figure 2 It is a coordinate transformation schematic diagram of the present invention. DETAILED DESCRIPTION

[0031] 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 ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0032] The technical solution of the present invention mainly includes: obtaining the original point clouds of the front and rear detection frames through the laser radar sensor; obtaining the obstacle target frame obtained after the detection; using the point cloud in the envelope frame as the seed point, finding the connected domain of the relevant point cloud as the rigid point cloud cluster of the obstacle; downsampling the point cloud to generate an occupant grid cluster; eliminating the observation error of the vehicle through a certain coordinate transformation; calculating the optimal transformation of the grid clusters of the front and rear frames; and judging whether the obstacle has moved.

[0033] See also Figure 1The obstacle dynamic and static estimation method of the present invention comprises the following steps:

[0034] 1. Obtain the original point cloud of the front and back detection frames through the LiDAR sensor.

[0035] For the sake of real-time performance and memory space, the point cloud is downsampled to generate a placeholder grid cluster: the point cloud within a certain range around the vehicle is intercepted as the boundary, such as [-x_limit, x_limit], [-y_limit, y_limit], [-z_limit, z_limit] in the vehicle coordinate system, and divided into resolution_x*resolution_y*resolution_z grids according to the computational requirements, where resolution_x, resolution_y and resolution_z are the resolutions of the three-dimensional grid in the x, y and z directions respectively. If there is a laser point in the grid, the grid is considered to be occupied. Since the point cloud of the high beam is dense, this downsampling method can effectively reduce the amount of calculation while retaining more obstacle shape information. The placeholder grid is the point cloud after downsampling, hereinafter referred to as the point cloud.

[0036] Alternatively, some offline solution schemes can use the original point cloud information instead of the down-sampling result, which can obtain more detailed results, but the real-time performance is poor and cannot be calculated in real time on the vehicle side. Therefore, the present invention preferably uses the above down-sampling result.

[0037] 2. Get the obstacle target frame after detection:

[0038] Take the point cloud in the envelope as the seed point, and find the connected domain of the related point cloud as the rigid body point cloud cluster of the obstacle: Since the boundary of the envelope output from the upstream is jumping, the point cloud in the box cannot be considered to represent the situation of the entire obstacle. For example, a large trailer truck (extended part), a car with an open door, etc. Use the principle of connected domain to search the surrounding occupant network, consider the entire rigid body as part of the obstacle, and retain the shape information of the original obstacle.

[0039] 3. Eliminate the ego-vehicle observation error through certain coordinate transformation:

[0040] So far, the coordinates of the point cloud are in the self-vehicle coordinate system at each moment. If they are transformed into the world coordinate system to calculate the motion of the target, especially when the self-vehicle moves and rotates, the observation error will be amplified. Therefore, the coordinate system of the other vehicle at the previous moment must be used as the reference to transform the point cloud at the next moment to this coordinate system. The coordinate system of the other vehicle can be simply defined as a coordinate system with the center of the envelope as the origin and the direction of the obstacle as the x-axis. The envelope rectangle BBOX of the target obstacle has been calculated by the upstream detection. t1 BBOX t2, the corresponding calculated point cloud cluster sets are and .

[0041] At time t1, the point cloud cluster coordinates P of the obstacle t1 :

[0042]

[0043] in, l1, l2, θ represent the position and orientation of the obstacle observed by the vehicle at this time relative to the vehicle coordinate system.

[0044] At time t2, the point cloud cluster coordinates P of the obstacle t2 :

[0045]

[0046] Among them, Pose t1 and Pose t2 They represent the posture of the vehicle relative to the world coordinate system at each moment.

[0047] 4. Calculate the optimal transformation of the mesh clusters of the previous and next frames:

[0048]

[0049] Here, the ICP (Iterative Closest Point) algorithm is used to solve the transformation matrix;

[0050] When the number of points is large (greater than 500), t1 ,P t2 Build kdtree matching point pairs.

[0051] It is understandable that ICP is not the only solution when calculating the optimal transformation. For example, the JacobiSVD (singular value decomposition of the Jacobi matrix) solution can also be used to directly solve the least squares solution of the equation.

[0052] 5. Determine whether the obstacle has moved:

[0053] when When it is less than the threshold thres11, it is considered stationary and the speed is 0;

[0054] when When it is greater than the threshold thres1, it is considered to be in motion;

[0055] when When it is greater than the threshold thres2, the solution is considered to have failed, and the targets of the previous and next frames are inconsistent. Represented by the matrix The solved Euler angle (in radians).

[0056] In an embodiment of the present invention, a method for dynamic and static estimation of a low-speed or stationary object is provided, which includes the following steps: S1: obtaining the original point cloud of the front and rear detection frames through a laser radar sensor; S2: obtaining the obstacle target frame obtained after detection, taking the point cloud in the envelope frame as the seed point, and finding the connected domain of the relevant point cloud as the rigid point cloud cluster of the obstacle; S3: eliminating the observation error of the vehicle through coordinate transformation, taking the coordinate system of the other vehicle at the previous moment as the reference, and transforming the point cloud at the next moment to the coordinate system; S4: calculating the optimal transformation of the grid clusters of the front and rear frames; S5: judging whether the obstacle moves based on the optimal transformation result obtained in step S4.

[0057] Furthermore, in step S1, the acquired original point cloud is downsampled to generate a placeholder grid cluster to obtain a downsampled point cloud for subsequent calculations.

[0058] Furthermore, in step S1, generating a placeholder grid cluster specifically includes: intercepting a point cloud within a certain range around the vehicle as a boundary, and dividing it into resolution_x*resolution_y*resolution_z grids according to the computational requirements; if there is a laser point in the grid, the grid is considered to be occupied, where resolution_x, resolution_y and resolution_z are the resolutions of the three-dimensional grid in the x, y and z directions respectively.

[0059] Further, in step S3, the other vehicle coordinate system is defined as a coordinate system with the center of the envelope as the origin and the direction of the obstacle as the x-axis, wherein the envelope rectangle BBOX of the target obstacle has been calculated by the upstream detection t1 BBOX t2 , the corresponding calculated point cloud cluster sets are and

[0060] At time t1, the point cloud cluster coordinates P of the obstacle t1 :

[0061]

[0062] in, l1, l2, θ represent the position and orientation of the obstacle observed by the vehicle relative to the vehicle coordinate system.

[0063] At time t2, the point cloud cluster coordinates P of the obstacle t2 :

[0064]

[0065] Among them, Pose t1 and Pose t2 They represent the posture of the vehicle relative to the world coordinate system at each moment.

[0066] Further, in step S4,

[0067] Compute the optimal transformation of mesh clusters between previous and next frames:

[0068]

[0069] Solving Transformations Matrix; when the number of points is large, P t1 ,P t2 Build kdtree matching point pairs.

[0070] Further, in step S5, when When it is less than the threshold thres11, it is considered stationary and the speed is 0; When it is greater than the threshold thres1, it is considered motion; when When it is greater than the threshold thres2, the solution is considered to have failed, and the targets of the previous and next frames are inconsistent. Represented by the matrix The solved Euler angles.

[0071] Further, in step S4, the transformation is solved by using the iterative closest point algorithm ICP algorithm or the Jacobi matrix singular value decomposition JacobiSVD method. matrix.

[0072] In another embodiment of the present invention, a dynamic and static estimation device for low-speed or stationary objects is also provided, and the device includes the following modules: an original point cloud acquisition module, which is used to acquire the original point clouds of the front and rear detection frames through a laser radar sensor; an obstacle target frame acquisition module, which is used to acquire the obstacle target frame obtained after detection, and use the point cloud in the envelope frame as the seed point to find the connected domain of the relevant point cloud as a rigid point cloud cluster of the obstacle; a coordinate system transformation module, which is used to eliminate the observation error of the vehicle through coordinate transformation, and transform the point cloud at the next moment into the coordinate system based on the coordinate system of the other vehicle at the previous moment; an optimal transformation calculation module, which is used to calculate the optimal transformation of the grid clusters of the front and rear frames; an obstacle motion judgment module, which is used to judge whether the obstacle is moving based on the optimal transformation result obtained by the optimal transformation calculation module.

[0073] In another embodiment of the present invention, a mobile device is provided, which is equipped with a laser radar sensor. The mobile device uses the dynamic and static estimation method of low-speed or stationary objects as described above, or is loaded with the dynamic and static estimation device of low-speed or stationary objects as described above.

[0074] In other embodiments of the present invention, a computer-readable storage medium containing a computer program is further provided. When the computer program is executed by one or more processors, the dynamic and static estimation method of a low-speed or stationary object as described above is implemented.

[0075] Beneficial effects of the technical solution of the present invention:

[0076] The method of the present invention is very accurate in estimating the motion state of obstacles. The solution result of a stationary obstacle is close to 0. On the contrary, as long as there is movement, even if it is very small, it will be reflected in the result. At the same time, the solution result, that is, the motion displacement and direction of the moving target, can also be used as an input source for calculating the differential velocity.

[0077] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A method for dynamic and static estimation of a low-speed or stationary object, characterized in that: The steps include: S1: Obtain the original point cloud of the front and back detection frames through the LiDAR sensor; S2: Obtain the obstacle target frame obtained after detection, use the point cloud in the envelope frame as the seed point, and find the connected domain of the relevant point cloud as the rigid point cloud cluster of the obstacle; S3: Eliminate the observation error of the self-vehicle through coordinate transformation. Take the coordinate system of the other vehicle at the previous moment as the reference and transform the point cloud at the next moment to the coordinate system; S4: Calculate the optimal transformation of the grid clusters of the previous and next frames; S5: Based on the optimal transformation result obtained in step S4, determine whether the obstacle is moving.

2. The method according to claim 1, characterized in that In step S1, the acquired original point cloud is downsampled to generate an occupancy grid cluster to obtain a downsampled point cloud for subsequent calculations.

3. The method according to claim 2, characterized in that In step S1, generating a placeholder grid cluster specifically includes: intercepting a point cloud within a certain range around the vehicle as a boundary, and dividing it into resolution_x*resolution_y*resolution_z grids according to the computational requirements; if there is a laser point in the grid, the grid is considered to be occupied, where resolution_x, resolution_y and resolution_z are the resolutions of the three-dimensional grid in the x, y and z directions respectively.

4. The method according to claim 1, characterized in that: In step S3, the other vehicle coordinate system is defined as a coordinate system with the center of the envelope as the origin and the obstacle direction as the x-axis, wherein the envelope rectangle BBOX of the target obstacle has been calculated by upstream detection t1 BBOX t2 , the corresponding calculated point cloud cluster sets are and At time t1, the point cloud cluster coordinates P of the obstacle t1 : in, l1, l2, θ represent the position and orientation of the obstacle observed by the vehicle relative to the vehicle coordinate system. At time t2, the point cloud cluster coordinates P of the obstacle t2 : Among them, Pose t1 and Pose t2 They represent the posture of the vehicle relative to the world coordinate system at each moment.

5. The method according to claim 4, characterized in that In step S4, Compute the optimal transformation of mesh clusters between previous and next frames: Solving Transformations Matrix; when the number of points is large, P t1 , P t2 Build kdtree matching point pairs.

6. The method according to claim 5, characterized in that In step S5, when When it is less than the threshold thres11, it is considered stationary and the speed is 0; when When it is greater than the threshold thres1, it is considered to be in motion; when When it is greater than the threshold thres2, it is considered that the solution fails and the targets of the previous and next frames are inconsistent; Represented by the matrix The solved Euler angles.

7. The method according to claim 5, characterized in that In step S4, the transformation is solved by using the iterative closest point algorithm | CP algorithm or Jacobi matrix singular value decomposition JacobiSVD method. matrix.

8. A device for estimating the dynamic and static state of a low-speed or stationary object, characterized in that: The device comprises the following modules: The original point cloud acquisition module is used to obtain the original point clouds of the front and rear detection frames through the laser radar sensor; The obstacle target frame acquisition module is used to obtain the obstacle target frame obtained after detection, take the point cloud in the envelope frame as the seed point, and find the connected domain of the relevant point cloud as the rigid point cloud cluster of the obstacle; The coordinate system transformation module is used to eliminate the observation error of the self-vehicle through coordinate transformation. The coordinate system of the other vehicle at the previous moment is used as the reference, and the point cloud at the next moment is transformed to the coordinate system; An optimal transformation calculation module, used to calculate the optimal transformation of the grid clusters of the previous and next frames; The obstacle motion judgment module is used to judge whether the obstacle is moving based on the optimal transformation result obtained by the optimal transformation calculation module.

9. A mobile device, characterized in that: Equipped with a laser radar sensor, the mobile device uses the dynamic and static estimation method of a low-speed or stationary object as described in any one of claims 1 to 7, or is loaded with the dynamic and static estimation device of a low-speed or stationary object as described in claim 8.

10. A computer-readable storage medium containing a computer program, characterized in that: When the computer program is executed by one or more processors, the dynamic and static estimation method of a low-speed or stationary object according to any one of claims 1 to 7 is implemented.

Images