CN111522022A - Dynamic target detection method of robot based on laser radar - Google Patents

Abstract

The invention relates to a dynamic target detection method, in particular to a dynamic target detection method for a robot based on a laser radar, which corrects positioning by using a non-gradient optimization mode, reduces positioning errors, improves detection precision, and can reach centimeter level; the invention uses radar to scan the whole plane for detection, and has wide detection range. The robot dynamic multi-target detection method based on the laser radar is provided to reduce positioning errors and improve detection precision.

Description

Dynamic target detection method for robot based on lidar

technical field

The invention relates to a dynamic target detection method, in particular to a dynamic target detection method for a robot based on a laser radar.

Background technique

In recent years, robots have become a representative strategic target in high-tech fields. The emergence and development of robot technology will not only fundamentally change the traditional industrial production, but also have a profound impact on human social life. Since an autonomous mobile robot can only perform autonomous motion effectively and safely only by accurately knowing its own position, the position of obstacles in the workspace and the motion of the obstacles, the problem of target detection and localization of autonomous mobile robots is particularly important. . A lot of research work has been done in this field at home and abroad. As far as the current research progress is concerned, it mainly collects environmental information and processes it through GPS, cameras, inertial navigation and other sensors to achieve autonomous positioning and target detection. And this also determines the correctness of subsequent decision-making by the performance of the sensor and the accurate and robust description of the environment.

To perform accurate object detection, the robot must first be positioned. When using a robot for target detection in the prior art, there are many uncertain factors in the positioning process. For example, the uncertainty of the robot itself, the accumulation of odometer errors, the noise interference of sensors, and the complexity and unknown of the environment in which the robot is located. Due to the existence of these uncertain factors, the positioning of the robot becomes more complicated, which leads to errors in the description of the environment, and leads to the accumulation of wrong information in the system. lower.

SUMMARY OF THE INVENTION

The purpose of the present invention is to propose a dynamic multi-target detection method for a robot based on lidar, in order to reduce the positioning error and improve the detection accuracy, aiming at the above-mentioned shortcomings of the prior art.

In order to achieve the above purpose, the dynamic target detection method for a robot based on lidar includes the following steps:

(1) Include a laser scanning radar in the center of the robot to collect and generate point cloud sets [β _n , l _1n ], and the robot positioning unit gives the robot positioning pose; where β _n represents the scanning angle of the laser radar, and the angle range is The radar scans the entire plane, l _1n represents the distance between the detected target and the robot at the corresponding angle; the entity radar point cloud data map established with β _n abscissa and l _1n as the ordinate;

(2) establishing a vector scene map according to the scene, representing the obstacle blocks in the scene map and the surrounding edges of the scene with enclosed line segments, and obtaining the coordinates of the end points and starting points of all line segments;

(3) Establish a simulated lidar model, input the pose of the entity lidar, and use the intercept traversal method to obtain the point cloud set [β _n , l _2n ] composed of the intersections of all rays emitted by the simulated lidar and the edges of the map and obstacle blocks , get the radar simulation map;

(4) Use the non-gradient optimization method to correct the positioning pose of the robot, use the corrected pose p _k to replace the previously input simulated entity radar pose, and obtain a new simulated lidar point cloud set and point cloud data map;

(5) Make the difference between the solid lidar point cloud set and the new simulated lidar point cloud set at the same scanning angle, and compare the difference with the set threshold ζ:

将相邻n点的实体激光雷达的距离平均值记为

以实体激光雷达所在位置为极坐标系原点，得到该目标的极坐标为

If the difference l between the distances of n consecutive points in the solid lidar point cloud data graph and the distances of the corresponding points in the simulated lidar point cloud data graph is greater than the set threshold ζ, that is, l>ζ, then the solid lidar The target is detected, and the mean of β _n of adjacent n points is recorded as

The average distance of the physical lidar of adjacent n points is recorded as

Taking the location of the physical lidar as the origin of the polar coordinate system, the polar coordinates of the target are obtained as

Otherwise, no target is detected.

Compared with the prior art of the present invention, it has the following advantages:

1) The present invention uses a non-gradient optimization method to correct the positioning, reduces the positioning error, and improves the detection accuracy, and the detection accuracy can reach the centimeter level;

2) The present invention uses the radar to scan the entire plane for detection, and has a wide detection range.

Description of drawings

Fig. 1 realizes the overall flow chart of the present invention

Fig. 2 is the entity radar point cloud data diagram in the present invention;

Fig. 3 is the actual scene map used in the present invention;

Fig. 4 is the scene vector diagram established according to the actual scene in the present invention;

Fig. 5 is the sub-flow chart of generating simulated radar point cloud data in the present invention;

FIG. 6 is a simulation diagram of the ray emitted by the simulated lidar and the scene edge and the obstacle block respectively intersecting in the present invention;

Fig. 7 is the comparison result diagram of the point cloud data of the physical radar and the simulated radar in the robot positioning point in the present invention;

8 is a sub-flow diagram of the present invention using a non-gradient optimization method to correct the positioning pose of the robot;

Fig. 9 is the point cloud data diagram of simulated radar in the present invention;

FIG. 10 is a comparison result diagram of the point cloud data at the robot positioning point after the correction of the real radar and the simulated radar in the present invention.

Detailed ways

Embodiments and effects of the present invention are described in detail below in conjunction with the accompanying drawings:

Referring to FIG. 1 , a method for dynamic target detection by a lidar-based robot includes the following specific implementation steps:

Step 1: Include a laser scanning radar in the center of the robot to collect and generate point cloud sets [β _n , l _1n ], the robot includes a positioning unit to give the robot positioning pose (x, y, α); where β _n represents the laser The scanning angle of the radar, the angle range is the full plane of the radar scanning, and l _1n represents the distance between the detected target and the robot at the corresponding angle; the entity radar point cloud data map established with β _n abscissa and l _1n as the ordinate;

The laser scanning radar adopts hokuyo's UTM-30LX, the laser scanning radar hokuyo's UTM-30LX has a scanning angle θ of 270 degrees, an angular resolution λ of 0.25 degrees, a scanning range of 0.1-30m, and a scanning frequency of 40HZ, hokuyo's UTM-30LX The point cloud data is (β _n , _ln ), β _n and _ln respectively represent the angle and distance of the nth ray emitted by the solid lidar relative to the polar coordinate system of the lidar body, with β _n as the abscissa, l _n is the vertical coordinate of the laser scanning radar point cloud data map, as shown in Figure 4.

且安装在机器人的中心，则机器人的位姿就是激光扫描雷达的位姿，其通过蒙塔卡洛定位产生，或者由UWB、GPS系统提供，假设位姿为(x,y,α),其中x,y表示实体激光雷达在场景坐标系中的位置，α表示实体激光雷达的中心线与地图坐标系x轴的夹角，本实例使用UWB系统产生机器人的定位位姿。Referring to Figure 4, the range of the angle β _n of the nth ray emitted by the laser scanning radar relative to the polar coordinate system of the laser radar body is:

And installed in the center of the robot, the pose of the robot is the pose of the laser scanning radar, which is generated by Monta Carlo positioning, or provided by UWB and GPS systems, assuming that the pose is (x, y, α), where x, y represent the position of the entity lidar in the scene coordinate system, and α represents the angle between the center line of the entity lidar and the x-axis of the map coordinate system. In this example, the UWB system is used to generate the positioning pose of the robot.

Step 2: Create a vector scene map according to the scene, represent the obstacle blocks in the scene map and the surrounding edges of the scene with the enclosed line segments, and obtain the coordinates of the end points and start points of all line segments;

Referring to Fig. 3, the actual scene map used in the present invention is composed of obstacle blocks and scene edges surrounded by line segments, and the coordinates of the starting point and the end point of all line segments are shown in Table 1:

Table 1: Coordinates of line segments in the map (unit: CM)

According to the endpoint coordinates of the line segment of the table, express it in the x-o-y rectangular coordinate system, and then the vector map of the scene can be obtained, as shown in Figure 4.

Step 3: Establish a simulated lidar model, input the pose of the physical lidar, and use the intercept traversal method to obtain the point cloud set [β _n , l _2n ] composed of the intersection of all rays emitted by the simulated lidar and the edge of the map and obstacle blocks , get the radar simulation map;

3.1) Establish polar coordinates with the origin of vector map coordinates and the X-axis;

3.2) Assuming that the coordinates of the physical radar are (x ₀ , y ₀ ), the angle between the center line of the physical radar and the x-axis direction is α degrees, the scanning range of the radar is θ degrees, and the angular resolution of the radar is λ degrees, the simulated radar emits a total of n Rays:

的范围，这些射线最后与障碍块或者场景四周所在的线段相交，这些交点即为仿真雷达扫描得到的点云坐标(x_i,y_i)；These rays are emitted from the radar coordinate points, at λ degree intervals, covering the α-centred

The range of these rays finally intersects with the obstacle block or the line segment around the scene, and these intersection points are the point cloud coordinates (x _i , y _i ) obtained by the simulated radar scanning;

3.3) Referring to Figure 5, find each intersection point by the vector method:

3.3.1) Convert the ray and the obstacle block or the edge contour around the scene into a vector, and the angle γ between the i-th ray and the horizontal direction satisfies

3.3.2) Find the intersection (x _i , y _i ) of the ray and the edge contour line in the two-dimensional space through the constraint relationship of the intersection of the two straight lines:

According to the result of 3.3.1), the direction vector of the i-th ray is (cosγ, sinγ), and the coordinates of the starting point of the ray are (x ₀ , y ₀ ), then (x ₀ +cosγ, y ₀ +sinγ) is also A point on the ray, let the start and end points of the edge contour be (x _s , y _s ) and (x _e , y _e ) respectively,

Then the point of intersection satisfies both line equations:

The intersection coordinates (x _i , y _i ) are calculated from this equation system, then the distance from the simulated radar coordinate point to the obstacle

3.4) Record the γ and l _β of all n rays to obtain a simulated lidar point cloud set, the simulation diagram is shown in Figure 6.

Step 4: Use the non-gradient optimization method to revise the robot positioning pose, use the revised pose p _k to replace the previously input simulated entity radar pose, and obtain a new simulated lidar point cloud set and point cloud data map;

After the above steps, the point cloud data of the physical radar and the simulated radar at the robot positioning point have been obtained. However, since there will be a certain deviation between the point cloud data of the two, the deviation will cause false detection, as shown in Figure 7. ;

In Figure 7, the first line A represents the real lidar point cloud, the second line B represents the simulated lidar point cloud obtained in step 3, and the third line C represents the difference between the two. , the point cloud of the simulated lidar and the physical lidar are not completely matched. The black arrow is the obstacle detected by the solid lidar, and the gray arrow is the position where the deviation between the two is the largest; the source of the deviation is from two aspects. It is caused by the difference between the actual scene map and the simulated scene map. This difference can be reduced by minimizing the error between the simulated map and the actual scene map when establishing the simulated map; the second is due to the inaccurate positioning results of the physical robot. Yes, the difference between the positioning pose used by the simulated radar and the real pose where the lidar is located will cause the overall offset of the point cloud, which is reflected on the graph as the offset of line B and line A in the box in Figure 7. The offset generated in this case will be reflected in the entire point cloud, especially at the inflection point of the curve, the deviation will be large, and it is easy to cause false detection of the target. In this regard, this example uses a non-gradient optimization method to eliminate the offset. To correct the robot positioning pose;

Referring to Figure 8, the specific implementation of this step is as follows:

为无偏率，其中，L为实体激光雷达点云集，L’为仿真激光雷达点云集，N_t表示激光雷达点云的总点数，在本实例中，N_t＝1080，N_u为满足距离之差l≤ζ的所有点数，ζ为设定的阈值；4.1) Assuming that the real pose of the radar in the map coordinate system is (x ₀ , y ₀ , α ₀ ), the pose is unknown but real, and the pose obtained by the robot through the positioning unit or Monte Carlo self-positioning is (x ₁ , y ₁ , α ₁ ), due to the noise of the sensor, the pose and the real pose of the robot will deviate more or less, the definition

is the unbiased rate, where L is the solid lidar point cloud set, L' is the simulated lidar point cloud set, and N _t represents the total number of points of the lidar point cloud. In this example, N _t =1080, and Nu _is the satisfying distance All points where the difference l≤ζ, ζ is the set threshold;

4.2) Initialize the unbiased rate C=-1, the pose obtained by the robot at a certain moment is: p ₁ =(x ₁ , y ₁ , α ₁ ), where x ₁ , y ₁ represent the physical lidar in the scene coordinates The position in the system, α ₁ is the angle between the radar center line and the x-axis direction at this time, the point cloud set scanned by the solid lidar is L _1n =[β _n ,l _1n ], the pose is taken as the initial value, the calculation is based on The point cloud L _2n = [β _n , l _2n ]=f(x ₁ , y ₁ , α ₁ ) of the simulated radar at point p ₁ , and the unbiased rate C ₁ =f _C is calculated according to the formula in 4.1). (L _1n , L _2n );

该点云集对应三个方向上的无偏率为

4.3) Set the step amount to be represented by step, and step and offset x ₁ , y ₁ , and α ₁ respectively, and obtain the expressions of the simulated lidar point clouds in three directions after the offset:

The point cloud set corresponds to the unbiased rate in the three directions

4.4) Define dx, dy, and dα as offsets respectively, then dx=C ^x -C ₁ , dy=C ^y -C ₁ , dα=C ^α -C ₁ , and a new positioning pose p ₂ =(x ₂ , y ₂ , α ₂ )=(x ₁ +dx, y ₁ +dy, α ₁ +dα);

4.5) Repeat (4.3) and (4.4) a total of 200 times, and record the maximum unbiased rate C _max and _the corresponding pose p _k = (x _k , y _k , α _k ), where x _k , y _k In order to obtain the position of the simulated radar in the scene coordinate system when _Cmax is obtained, α _k is the angle between the radar centerline and the x-axis direction at this time, and this p _k is the point closest to the real pose of the robot;

Step 5: Use the corrected pose p _k to replace the previously input simulated entity radar pose to obtain a new simulated lidar point cloud set.

Input the positioning pose p _k obtained in step 4, which is closest to the real pose of the robot, into the simulated lidar model, and generate simulated radar point cloud data based on this pose according to step 3, and the generated new simulated radar point cloud data, as shown in the figure 9 shown.

The real radar and simulated radar point cloud data are compared at p _k , and the results are shown in Figure 10, where line A represents the real lidar point cloud, the second line B represents the new simulated radar point cloud, and the third line C It represents the difference between the two. It can be seen from Figure 10 that the offset between the two has been greatly reduced at this time, indicating that the robot pose has been corrected at this time.

Step 6: Determine whether there is a target according to the difference between the point cloud set of the entity lidar and the point cloud of the new simulated lidar at the same scanning angle.

6.1) Make the difference between the solid lidar point cloud set and the new simulated lidar point cloud set at the same scanning angle, as shown by line C in Figure 10, and compare the difference with the set threshold ζ:

If the difference l between the distances of n consecutive points in the solid lidar point cloud data graph and the distances of the corresponding points in the simulated lidar point cloud data graph is greater than the set threshold ζ, as shown by the arrow mark in Figure 10 , then the entity lidar detects the target, and executes 6.2);

Otherwise, no target is detected;

将相邻n点的距离平均值记为

以实体激光雷达所在位置为极坐标系原点，得到该目标的极坐标为

6.2) Record the mean value of β _n of adjacent n points as

The average distance between adjacent n points is recorded as

Taking the location of the physical lidar as the origin of the polar coordinate system, the polar coordinates of the target are obtained as

The above description is only a specific example of the present invention, and does not constitute any limitation to the present invention. Obviously, for those skilled in the art, after understanding the content and principles of the present invention, they may not deviate from the principles and structures of the present invention. Under the circumstance of the present invention, various modifications and changes in form and details are made, but these modifications and changes based on the idea of the present invention are still within the scope of protection of the claims of the present invention.

Claims (7)

1. A method for dynamic target detection based on a lidar-based robot, characterized in that it comprises the following specific implementation steps: Step 1: Include a laser scanning radar in the center of the robot to collect and generate point cloud sets [β _n , l _1n ], the robot includes a positioning unit to give the robot positioning pose (x, y, α); where β _n represents the laser The scanning angle of the radar, the angle range is the full plane of the radar scanning, and l _1n represents the distance between the detected target and the robot at the corresponding angle; the entity radar point cloud data map established with β _n abscissa and l _1n as the ordinate; Step 2: Create a vector scene map according to the scene, represent the obstacle blocks in the scene map and the surrounding edges of the scene with the enclosed line segments, and obtain the coordinates of the end points and start points of all line segments; Step 3: Establish a simulated lidar model, input the pose of the physical lidar, and use the intercept traversal method to obtain the point cloud set [β _n , l _2n ] composed of the intersection of all rays emitted by the simulated lidar and the edge of the map and obstacle blocks , get the radar simulation map; Step 4: Use the non-gradient optimization method to revise the robot positioning pose, use the revised pose p _k to replace the previously input simulated entity radar pose, and obtain a new simulated lidar point cloud set and point cloud data map; Step 5: Use the corrected pose p _k to replace the previously input simulated entity radar pose to obtain a new simulated lidar point cloud set; Step 6: Determine whether there is a target according to the difference between the point cloud set of the entity lidar and the point cloud of the new simulated lidar at the same scanning angle. 2. the robot based on laser radar according to claim 1 carries out dynamic target detection method, it is characterized in that: the laser scanning radar that described step 1 uses adopts the UTM-30LX of hokuyo, the UTM-30LX of laser scanning radar hokuyo The scanning angle θ is 270 degrees, the angular resolution λ is 0.25 degrees, the scanning range is 0.1-30 m, and the scanning frequency is 40 Hz. 3. the robot based on lidar according to claim 2 carries out dynamic target detection method, it is characterized in that: the point cloud data of the UTM-30LX of hokuyo is (β _n , 1 _n ), β _n and 1 _n represent entities respectively The angle and distance of the nth ray emitted by the lidar relative to the polar coordinate system of the lidar body; the angle β _n of the nth ray emitted by the lidar relative to the polar coordinate system of the lidar body is in the range of

And installed in the center of the robot, the pose of the robot is the pose of the laser scanning radar, which is generated by Monta Carlo positioning, or provided by UWB and GPS systems, assuming that the pose is (x, y, α), where x, y represent the position of the physical lidar in the scene coordinate system, and α represents the angle between the center line of the physical lidar and the x-axis of the map coordinate system. 4. The method for detecting a dynamic target based on a lidar-based robot according to claim 1, wherein the step 3 specifically comprises the following steps: 3.1) Establish polar coordinates with the origin of vector map coordinates and the X-axis; 3.2) Assuming that the coordinates of the physical radar are (x ₀ , y ₀ ), the angle between the center line of the physical radar and the x-axis direction is α degrees, the scanning range of the radar is θ degrees, and the angular resolution of the radar is λ degrees, the simulated radar emits a total of n Rays:

These rays are emitted from the radar coordinate points, at λ degree intervals, covering the α-centred

The range of these rays finally intersects with the obstacle block or the line segment around the scene, and these intersection points are the point cloud coordinates (x _i , y _i ) obtained by the simulated radar scanning; 3.3) Find each intersection by the vector method: 3.3.1) Convert the ray and the obstacle block or the edge contour around the scene into a vector, and the angle γ between the i-th ray and the horizontal direction satisfies

3.3.2) Find the intersection (x _i , y _i ) of the ray and the edge contour line in the two-dimensional space through the constraint relationship of the intersection of the two straight lines: According to the result of 3.3.1), the direction vector of the i-th ray is (cosγ, sinγ), and the coordinates of the starting point of the ray are (x ₀ , y ₀ ), then (x ₀ +cosγ, y ₀ +sinγ) is also A point on the ray, let the start and end points of the edge contour be (x _s , y _s ) and (x _e , y _e ) respectively, Then the point of intersection satisfies both line equations:

The intersection coordinates (x _i , y _i ) are calculated from this equation system, then the distance from the simulated radar coordinate point to the obstacle

3.4) Record the γ and l _β of all n rays to obtain a simulated lidar point cloud set. 5. The method for detecting a dynamic target based on a lidar-based robot according to claim 1, wherein the step 4 specifically comprises the following steps: 4.1) Assuming that the real pose of the radar in the map coordinate system is (x ₀ , y ₀ , α ₀ ), the pose is unknown but real, and the pose obtained by the robot through the positioning unit or Monte Carlo self-positioning is (x ₁ , y ₁ , α ₁ ), due to the noise of the sensor, the pose and the real pose of the robot will deviate more or less, the definition

is the unbiased rate, where L is the solid lidar point cloud set, L' is the simulated lidar point cloud set, and N _t represents the total number of points of the lidar point cloud. In this example, N _t =1080, and Nu _is the satisfying distance All points where the difference l≤ζ, ζ is the set threshold; 4.2) Initialize the unbiased rate C=-1, the pose obtained by the robot at a certain moment is: p ₁ =(x ₁ , y ₁ , α ₁ ), where x ₁ , y ₁ represent the physical lidar in the scene coordinates The position in the system, α ₁ is the angle between the radar center line and the x-axis direction at this time, the point cloud set scanned by the solid lidar is L _1n =[β _n ,l _1n ], the pose is taken as the initial value, the calculation is based on The point cloud L _2n = [β _n , l _2n ]=f(x ₁ , y ₁ , α ₁ ) of the simulated radar at point p ₁ , and the unbiased rate C ₁ =f _C is calculated according to the formula in 4.1). (L _1n , L _2n ); 4.3) Set the step amount to be represented by step, and step and offset x ₁ , y ₁ , and α ₁ respectively, and obtain the expressions of the simulated lidar point clouds in three directions after the offset:

The point cloud set corresponds to the unbiased rate in the three directions

4.4) Define dx, dy, and dα as offsets respectively, then dx=C ^x -C ₁ , dy=C ^y -C ₁ , dα=C ^α -C ₁ , and a new positioning pose p ₂ =(x ₂ , y ₂ , α ₂ )=(x ₁ +dx, y ₁ +dy, α ₁ +dα); 4.5) Repeat (4.3) and (4.4) a total of 200 times, and record the maximum unbiased rate C _max and _the corresponding pose p _k = (x _k , y _k , α _k ), where x _k , y _k In order to obtain the position of the simulated radar in the scene coordinate system when _Cmax is obtained, α _k is the angle between the radar centerline and the x-axis direction at this time, and the p _k is the point closest to the real pose of the robot. 6. The method for detecting a dynamic target based on a lidar-based robot according to claim 1, wherein the step 5 specifically comprises the following further steps: Input the positioning pose p _k that is closest to the real pose of the robot obtained in step 4 into the simulated lidar model, and generate simulated radar point cloud data based on this pose according to step 3, and generate new simulated radar point cloud data; Compare the real radar with the simulated radar point cloud data at p _k , where line A represents the real lidar point cloud, the second line B represents the new simulated radar point cloud, and the third line C represents the difference between the two. The pose is corrected. 7. The lidar-based robot according to claim 1 carries out a dynamic target detection method, wherein the step 6 specifically comprises the following further steps: 6.1) Make the difference between the solid lidar point cloud set and the new simulated lidar point cloud set at the same scanning angle, and compare the difference with the set threshold ζ: If the difference l between the distances of n consecutive points in the lidar point cloud data graph and the distances of the corresponding points in the simulated lidar point cloud data graph is greater than the set threshold ζ, the lidar scanning radar detects the target and executes 6.2); Otherwise, no target is detected; 6.2) Record the mean value of β _n of adjacent n points as

The average distance between adjacent n points is recorded as

Taking the location of the physical lidar as the origin of the polar coordinate system, the polar coordinates of the target are obtained as

Images