阅读说明：本技术 一种基于改进icp的点云测速方法 (Point cloud speed measurement method based on improved ICP (inductively coupled plasma) ) 是由 杜雪 闫泽博 胡俊生 管凤旭 孙岩 张勋 陈涛 郑岩 于 2021-08-09 设计创作，主要内容包括：本发明公开了一种基于改进ICP的点云测速方法,包括：点云预处理：去除点云数据中的噪声点,通过聚类及检测手段,移除点云数据中的动态物体；点云的匹配：获得采集当前一帧点云时载体的位姿,并以此位姿为基础,利用改进的ICP方法,进一步优化当前一帧点云所对应的载体位姿；局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；计算速度：利用角轴法对载体经历的位姿变化进行表示,然后计算出载体目前的速度。本发明的三维点云预处理可去除初始点云数据中的离群点和毛刺点,大大减轻噪声点对后续匹配步骤的影响。采用的小区域平面投影至大区域平面,极大的使投影效果贴近真实情况,使得配准结果更为准确。(The invention discloses a point cloud speed measurement method based on improved ICP (inductively coupled plasma), which comprises the following steps: point cloud pretreatment: removing noise points in the point cloud data, and removing dynamic objects in the point cloud data through clustering and detection means; matching point clouds: acquiring the pose of the carrier when the current frame of point cloud is acquired, and further optimizing the pose of the carrier corresponding to the current frame of point cloud by using an improved ICP method on the basis of the pose; local graph optimization: performing local graph optimization on all poses in a given time interval including the pose obtained by the latest calculation; calculating the speed: and (3) representing the pose change experienced by the carrier by using an angular axis method, and then calculating the current speed of the carrier. The three-dimensional point cloud preprocessing of the invention can remove outliers and burr points in the initial point cloud data, and greatly reduce the influence of noise points on the subsequent matching steps. The adopted small area plane is projected to the large area plane, so that the projection effect is greatly close to the real situation, and the registration result is more accurate.)

1. a point cloud speed measurement method based on improved ICP is characterized by comprising the following steps:

step 1, point cloud pretreatment: removing noise points in the point cloud data, and removing dynamic objects in the point cloud data through clustering and detection means;

step 2: matching point clouds: acquiring the pose of the carrier when the current frame of point cloud is acquired, and further optimizing the pose of the carrier corresponding to the current frame of point cloud by using an improved ICP method on the basis of the pose;

and step 3: local graph optimization: performing local graph optimization on all poses in a given time interval including the pose obtained by the latest calculation;

and 4, step 4: calculating the speed: and (3) representing the pose change experienced by the carrier by using an angular axis method, and then calculating the current speed of the carrier.

2. The point cloud speed measurement method based on the improved ICP as recited in claim 1, wherein: the point cloud pretreatment in the step 1 comprises the following specific steps:

let k be in carrier coordinate system L_kThe next collected three-dimensional point cloud data isWhereinRepresenting the ith point in the point cloud data,respectively representing the ith point in the point cloud data in a carrier coordinate system L_kCoordinates of the lower part;

in thatCalculating m points nearest to each point and the distance from the point to the m points nearest to the point by a kd-tree algorithm;

for the ith pointM points and points nearest to itThe distances between are respectively: d_i1，d_i2，…，d_im；

Calculate the average d of the m distances_i-average，d_i-averageIs the local mean distance;

finding pointsLocal average distance d of m adjacent points_i1-average，d_i2-average，…，d_im-averageFrom d_i1-average，d_i2-average，…，d_im-averageTo select the minimum value d_imin-averageThen compare d_i-averageAnd d_imin-averageThe size of (d); if d is_i-average＞Kd_imin-averageThen, it is determinedIs a noise point; wherein K is a given parameter;

and recording the three-dimensional point cloud data obtained after pretreatment as follows:

3. a point cloud velocity measurement method based on improved ICP as claimed in claim 1 or 2, wherein: step 2, the point cloud matching specifically comprises the following steps:

obtaining a carrier coordinate system L using a speedometer or RANSAC method_kInitial estimated pose T under world system W_k-start；

Setting the last sampling time of the point cloud sampling time k as o;

recording the collected point cloud data

Recording the preprocessed point cloud data as

The carrier coordinate system corresponding to the time o is L_o；

The pose of the carrier under the world system is T_o；

Using improved ICP method pairAndprocessing to obtain optimized T_k-startIs marked as T_k；

The improved ICP method is specifically as follows:

pose T with initial estimation_k-startAnd T_oWill be provided withTransforming the point cloud data into a world coordinate system W, and representing the corresponding point cloud data as

FromSelecting points on the plane;

the selected points are uniformly distributed by using a non-maximum value inhibition method;

the resulting set of standard points is recorded as

In thatSearch for distance inThe nearest lambda point to each point in;

from the lambda points, the distance is screened againWherein the corresponding point is smaller than a given distance D₁To obtain a point setWherein l' represents the number of point sets after the screening operation;

fromTo select the distance pointThe nearest lambda point, denotedThen fromTo select the distance pointLess than a given distance D₁Point set ofWherein γ i representsThe number of midpoints;

wherein the distance isThe nearest point is denoted as p_nst-nborComparison of p_nst-nborAndnormal vectors at two points, if the angle between two normal vectors is in the interval [ theta ]_t,180°-θ_t]Then will beFromIn which is cut off, wherein theta_tIs a given value;

computingAll points in (1) and (p)_nst-nborIf there is a distance less than a given distance R₁In the case of (1), thenFromRemoving the intermediate product;

computingWhether each point in the image is located on a plane;

if not, the corresponding point set is removed

To obtainWherein l' representsThe number of sets of the midpoint;

for theEach point set in (2), calculating the points in the point setsIf the nearest neighbor is not located on the plane, and the nearest neighbor distanceExceeds a given distance R₂Then the corresponding point set is selected fromRemoving;

and R is₂＞R₁；

To obtain

The corresponding nearest neighbor set is recorded as

The normal vector for each point is noted

Then will bePoint in (3) is projected to a set of standard pointsTo obtain a projection point set

ComputingAndthe distance of the corresponding point is obtained to obtain an error function error (T)_k-start)；

Error (T) pair by nonlinear optimization method_k-start) Optimizing to obtain delta T;

for T_k-startUpdate, i.e. T_k-start＝ΔT·T_k-start；

Wherein, the delta T is the updating amount;

then continue to aim at new T_k-startPerforming iterative optimization when | | delta T | | non-woven phosphor₂＜T_minOr has reached the number of iterations n_tThen the iteration is stopped and the resulting T is_k-startIs marked as T_kWherein, T_minAnd n_tIs a given value.

4. The point cloud speed measurement method based on the improved ICP as recited in claim 3, wherein: the judging method on the plane is specifically as follows:

for the point set P, take one point P_iCalculating by p_iCentered on R₃All points contained by a sphere of radius

Wherein R is₃＞R₂；

Calculating mu_i、Σ_i；

Wherein the content of the first and second substances,

for sigma_iPerforming singular value decomposition to obtain

The curvature is defined as:

if alpha < alpha_thresholdAnd λ₁＜δλ₂Then, consider p_iLying on a plane, α_thresholdA given curvature threshold;

otherwise, consider p_iDo not lie on a plane;

wherein, delta is more than 0 and less than 1.

5. The point cloud speed measurement method based on the improved ICP as recited in claim 3, wherein: the device is toPoint in (3) is projected to a set of standard pointsThe corresponding points of (a) are specifically:

for theSet of ith points ofFromTake out the jth point

In thatThe nearest neighbor of (1) is Is a normal vector of

ComputingDistance between two adjacent platesDistance of plane formed by point neighborhood

To obtainIn thatProjection in

Said distanceDistance of plane formed by point neighborhood:

is provided withSet of points of neighborhood

Then

Wherein:

6. the point cloud speed measurement method based on the improved ICP as recited in claim 3, wherein: the error function error (T)_k-start) Satisfies the following conditions:

for theWhich corresponds to an error portion of

Then

7. A point cloud velocity measurement method based on improved ICP as claimed in claim 1 or 2, wherein: step 4, the calculation speed is specifically as follows:

through T_kAnd T_oCan find from L_oTo L_kIs transformed by the transformation matrix T_k←o；

If angular axis method and translation vector t are used_k←oTo represent T_k←oThen there is a rotation axis n_k←oAngle of rotation theta_k←o；

Setting the time interval between two frames of point cloud data as delta tau;

then the rotation speed is:

translation speed:

Technical Field

The invention belongs to the technical field of navigation, and particularly relates to a point cloud speed measurement method based on improved ICP (inductively coupled plasma).

Background

With the rapid development of sensor technologies such as laser radar and the like, the application of point cloud data in the fields of mapping, navigation, obstacle avoidance and the like becomes one of research hotspots, and if the point cloud data is used for measuring the carrier speed, the point cloud information can be more fully utilized. For a carrier only provided with a sensor for acquiring point cloud information, the speed measurement of the carrier by using point cloud data can be the only speed measurement means. Meanwhile, in state estimation, better estimation of the estimated state by the navigation system is facilitated by providing more information. In addition, ICP (iterative close point) has a relatively important meaning in the traditional point cloud registration method, but the common ICP method has the problem of introducing overlarge errors, so that the common ICP method is improved.

Disclosure of Invention

Aiming at the prior art, the invention aims to provide a point cloud speed measuring method based on improved ICP, which has more accurate registration result and is used for providing a speed measuring method for a carrier provided with a point cloud data acquisition sensor.

In order to solve the technical problem, the invention provides a point cloud speed measuring method based on improved ICP, which comprises the following steps:

step 1, point cloud pretreatment: removing noise points in the point cloud data, and removing dynamic objects in the point cloud data through clustering and detection means;

step 2: matching point clouds: acquiring the pose of the carrier when the current frame of point cloud is acquired, and further optimizing the pose of the carrier corresponding to the current frame of point cloud by using an improved ICP method on the basis of the pose;

and step 3: local graph optimization: performing local graph optimization on all poses in a given time interval including the pose obtained by the latest calculation;

and 4, step 4: calculating the speed: and (3) representing the pose change experienced by the carrier by using an angular axis method, and then calculating the current speed of the carrier.

The invention also includes:

1. the point cloud pretreatment in the step 1 specifically comprises the following steps:

let k be in carrier coordinate system L_kThe next collected three-dimensional point cloud data isWhereinRepresenting the ith point in the point cloud data,respectively representing the ith point in the point cloud data in a carrier coordinate system L_kCoordinates of the lower part;

in thatCalculating m points nearest to each point and the distance from the point to the m points nearest to the point by a kd-tree algorithm;

for the ith pointM points and points nearest to itThe distances between are respectively: d_i1，d_i2，…，d_im；

Calculate the average d of the m distances_i-average，d_i-averageIs the local mean distance;

finding pointsLocal average distance d of m adjacent points_i1-average，d_i2-average，…，d_im-averageFrom d_i1-average，d_i2-average，…，d_im-averageTo select the minimum value d_imin-averageThen compare d_i-averageAnd d_imin-averageThe size of (d); if d is_i-average＞Kd_imin-averageThen, it is determinedIs a noise point; wherein K is a given parameter;

and recording the three-dimensional point cloud data obtained after pretreatment as follows:

2. the point cloud matching in the step 2 specifically comprises the following steps:

obtaining a carrier coordinate system L using a speedometer or RANSAC method_kInitial estimated pose T under world system W_k-start；

Setting the last sampling time of the point cloud sampling time k as o;

recording the collected point cloud data

Recording the preprocessed point cloud data as

The carrier coordinate system corresponding to the time o is L_o；

The pose of the carrier under the world system is T_o；

Using improved ICP method pairAndprocessing to obtain optimized T_k-startIs marked as T_k；

The improved ICP method is specifically as follows:

pose T with initial estimation_k-startAnd T_oWill be provided withTransforming the point cloud data into a world coordinate system W, and representing the corresponding point cloud data as

FromSelecting points on the plane;

the selected points are uniformly distributed by using a non-maximum value inhibition method;

the resulting set of standard points is recorded as

In thatSearch for distance inThe nearest lambda point to each point in;

from the lambda points, the distance is screened againWherein the corresponding point is smaller than a given distance D₁To obtain a point setWherein l' represents the number of point sets after the screening operation;

fromTo select the distance pointThe nearest lambda point, denoted

Then fromTo select the distance pointLess than a given distance D₁Point set ofWherein γ i representsThe number of midpoints;

wherein the distance isThe nearest point is denoted as p_nst-nborComparison of p_nst-nborAndnormal vectors at two points, if the angle between two normal vectors is in the interval [ theta ]_t,180°-θ_t]Then will beFromIn which is cut off, wherein theta_tIs a given value;

computingAll points in (1) and (p)_nst-nborIf there is a distance less than a given distance R₁In the case of (1), thenFromRemoving the intermediate product;

computingWhether each point in the image is located on a plane;

if not, the corresponding point set is removed

To obtainWherein l' representsThe number of sets of the midpoint;

for theEach point set in (2), calculating the points in the point setsIf the nearest neighbor is not located on the plane, and the nearest neighbor distanceExceeds a given distance R₂Then the corresponding point set is selected fromRemoving;

and R is₂＞R₁；

To obtain

The corresponding nearest neighbor set is recorded as

The normal vector for each point is noted

Then will bePoint in (3) is projected to a set of standard pointsTo obtain a projection point set

ComputingAndthe distance of the corresponding point is obtained to obtain an error function error (T)_k-start)；

Error (T) pair by nonlinear optimization method_k-start) Optimizing to obtain delta T;

for T_k-startUpdate, i.e. T_k-start＝ΔT·T_k-start；

Wherein, the delta T is the updating amount;

then continue to aim at new T_k-startPerforming iterative optimization when | | delta T | | non-woven phosphor₂＜T_minOr has reached the number of iterations n_tThen go to restStopping iteration and obtaining the T finally_k-startIs marked as T_kWherein, T_minAnd n_tIs a given value.

3. The method for judging the position on the plane specifically comprises the following steps:

for the point set P, take one point P_iCalculating by p_iCentered on R₃All points contained by a sphere of radius

Wherein R is₃＞R₂；

Calculating mu_i、Σ_i；

Wherein the content of the first and second substances,

for sigma_iPerforming singular value decomposition to obtain

The curvature is defined as:

if alpha < alpha_thresholdAnd λ₁＜δλ₂Then, consider p_iLying on a plane, α_thresholdA given curvature threshold;

otherwise, consider p_iDo not lie on a plane;

wherein, delta is more than 0 and less than 1.

4. Will be provided withPoint in (3) is projected to a set of standard pointsThe corresponding points of (a) are specifically:

for theSet of ith points ofFromTake out the jth point

In thatThe nearest neighbor of (1) isIs a normal vector of

ComputingDistance between two adjacent platesDistance of plane formed by point neighborhood

To obtainIn thatProjection in

Said distanceDistance of plane formed by point neighborhood:

is provided withSet of points of neighborhood

Then

Wherein:

5. error function error (T)_k-start) Satisfies the following conditions:

for theWhich corresponds to an error portion of

Then

6. The calculation speed in the step 4 is specifically as follows:

through T_kAnd T_oCan find from L_oTo L_kIs transformed by the transformation matrix T_k←o；

If angular axis method and translation vector t are used_k←oTo represent T_k←oThen there is a rotation axis n_k←oAnd rotateAngle theta_k←o；

Setting the time interval between two frames of point cloud data as delta tau;

then the rotation speed is:

translation speed:

the invention has the beneficial effects that: compared with the prior art, the invention has the following beneficial effects:

1. the three-dimensional point cloud preprocessing of the invention can effectively remove outliers and burr points in the initial point cloud data, and greatly reduce the influence of noise points on the subsequent matching steps.

2. The small area plane adopted by the invention is projected to the large area plane, so that the projection effect is greatly close to the real situation, and the registration result is more accurate.

3. The invention estimates the speed of the carrier by using the point cloud data.

Drawings

FIG. 1 is a flow chart of a method for measuring speed according to the present invention;

fig. 2 is an introduction to outliers and burrs.

Detailed Description

The invention is further described with reference to the drawings and the detailed description.

The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.

As shown in fig. 1, a point cloud velocity measurement method based on improved ICP includes the following steps:

point cloud preprocessing: removing noise points in the point cloud data, and removing dynamic objects in the point cloud data through clustering and detection means;

point cloud matching: the pose of the carrier is preliminarily obtained by other means when the current frame of point cloud is acquired, and the pose of the carrier corresponding to the current frame of point cloud is further optimized by utilizing an improved ICP method on the basis of the pose;

and (3) local graph optimization: optimizing all poses within a certain time interval including the pose obtained by the latest calculation;

and a speed calculating step: and (3) representing the pose change experienced by the carrier by using an angular axis method, and then calculating the current speed of the carrier.

Example 1: preprocessing step of three-dimensional point cloud

Let k be in carrier coordinate system L_kThe next collected three-dimensional point cloud data isWhereinRepresenting the ith point in the point cloud data,respectively representing the ith point in the point cloud data in a carrier coordinate system L_kCoordinates of the lower part;

in thatCalculating m points nearest to each point and the distance from the point to the m points nearest to the point by a kd-tree algorithm;

for the ith pointM points and points nearest to itThe distances between are respectively: d_i1，d_i2，…，d_im；

Calculate the average d of the m distances_i-averageIt is possible to call d_i-averageIs the local mean distance;

in the same way, the point can be calculatedLocal average distance d of m adjacent points_i1-average，d_i2-average，…，d_im-average. From d_i1-average，d_i2-average，…，d_im-averageTo select the minimum value d_imin-averageThen compare d_i-averageAnd d_imin-averageThe size of (d); if d is_i-average＞Kd_imin-averageThen, it is determinedIs a noise point;

wherein K is a parameter to be adjusted;

and recording the three-dimensional point cloud data obtained after pretreatment as follows:

example 2: matching of three-dimensional point clouds

Obtaining a carrier coordinate system L using a speedometer or RANSAC method_kInitial estimated pose T under world system W_k-start；

Setting the last sampling time of the point cloud sampling time k as o;

recording the collected point cloud data

Recording the preprocessed point cloud data as

The carrier coordinate system corresponding to the time o is L_o；

The pose of the carrier under the world system is T_o；

Using improved ICP method pairAndprocessing to obtain optimized T_k-startIs marked as T_k。

Example 3: improved ICP

Obtaining a carrier coordinate system L using a speedometer or RANSAC method_kInitial estimated pose T under world system W_k-start；

Setting the last sampling time of the point cloud sampling time k as o;

recording the collected point cloud data

Recording the preprocessed point cloud data as

The carrier coordinate system corresponding to the time o is L_o；

The pose of the carrier under the world system is T_o；

By T_k-startAnd T_oWill be provided withTransforming the point cloud data into a world coordinate system W, and representing the corresponding point cloud data as

FromSelecting points on the plane;

the selected points are uniformly distributed by using a non-maximum value inhibition method;

the resulting set of standard points is recorded as

In thatIn a search operation, forAt any point p in_jIn aFind the distance p_jThe nearest lambda point;

from this lambda point, the distance p is again screened_jIs less than D₁A point of (a);

after the two screening operations, a point set is obtained

Wherein l' represents the number of point sets after the screening operation;

fromTo select the distance pointThe nearest lambda point, denoted

Then fromTo select the distance pointIs less than D₁Point set of

Wherein γ i representsThe number of midpoints;

wherein the distance isThe nearest point is denoted as p_nst-nborComparison of p_nst-nborAndthe normal vectors at two points are in [ theta ] if the included angle between the two normal vectors is_t,180°-θ_t]In between, then willFromIn which is cut off, wherein theta_tIs a given value of theta_tCan take 20 degrees;

computingAll points in (1) and (p)_nst-nborIf there is a distance less than R₁In the case of (1), thenFromRemoving the intermediate product;

computingWhether each point in the image is located on a plane;

if not, the corresponding point set is removed

To obtain

Wherein l' representsThe number of sets of the midpoint;

for theShould also calculate the point in each of these point setsIf the nearest neighbor is not located on the plane, and the nearest neighbor distanceThe distance of the corresponding point in the middle exceeds R₂Then the corresponding point set is selected fromRemoving;

and R is₂＞R₁；

To obtain

The corresponding nearest neighbor set is recorded as

The normal vector for each point is noted

Then will bePoint in (3) is projected to a set of standard pointsTo obtain a projection point set

ComputingAndthe distance of the corresponding point is obtained to obtain an error function error (T)_k-start)；

Error (T) pair by nonlinear optimization method_k-start) Optimizing to obtain delta T;

for T_k-startUpdate, i.e. T_k-start＝ΔT·T_k-start；

Wherein, the delta T is the updating amount;

then continue to aim at new T_k-startCarrying out iterative optimization by utilizing an improved ICP method;

when | | delta T | non-woven phosphor₂＜T_minOr has reached the number of iterations n_tThen the iteration is stopped and the resulting T is_k-startIs marked as T_k；

Wherein, T_minAnd n_tIs a given value.

Example 4: on a plane

For the point set P, take one point P_iCalculating by p_iCentered on R₃All points contained by a sphere of radius

Wherein R is₃＞R₂；

Calculating mu_i、Σ_i；

Wherein the content of the first and second substances,

for sigma_iPerforming singular value decomposition to obtainThe curvature is defined as:

if alpha < alpha_thresholdAnd λ₁＜δλ₂Then, consider p_iIs positioned on a plane;

otherwise, consider p_iDo not lie on a plane;

wherein, delta is more than 0 and less than 1.

Example 5: the points are projected to a set of standard points

For theSet of ith points ofFromTake out the jth point

In thatThe nearest neighbor of (1) isIs a normal vector of

ComputingDistance between two adjacent platesDistance of plane formed by point neighborhood

To obtainIn thatProjection in

Said distanceDistance of plane formed by point neighborhood:

is provided withSet of points of neighborhood

Then

Wherein:

example 6: error function

For theWhich corresponds to an error portion of

Then

Example 7: local graph optimization

For rho frame effective laser data from k to the front, rho poses T can be obtained_k-ρ+1,...,T_k；

For T_k-ρ+1,...,T_kEstablishing a pose graph;

optimizing the pose graph to obtain optimized T_k-ρ+1,...,T_k。

Example 8: calculating speed

Through T_kAnd T_oCan find from L_oTo L_kIs transformed by the transformation matrix T_k←o；

If angular axis method and translation vector t are used_k←oTo represent T_k←oThen there is a rotation axis n_k←oAngle of rotation theta_k←o；

Setting the time interval between two frames of point cloud data as delta tau;

then the rotation speed is:

translation speed: