CN111461023B - Method for quadruped robot to automatically follow pilot based on three-dimensional laser radar - Google Patents

Abstract

A method for a quadruped robot autonomously following a navigator based on 3D lidar, the specific steps are as follows: (1) use 3D lidar to obtain the original 3D point cloud data of the environment; (2) preprocess the original point cloud; (3) ) Carry out segmentation and clustering based on Euclidean distance in the extracted non-ground point cloud data to obtain a list of point cloud clusters of objects on the ground; (4) screen out candidate object point cloud clusters for the segmented point cloud clusters, The set laser reflection intensity threshold determines the target position of the navigator wearing the reflective label; (5) After obtaining the coordinates of the navigator's target position at each moment, output the instructions of the traveling speed and direction to the quadruped robot to realize autonomous following pilot. The invention adopts the omnidirectional three-dimensional laser point cloud data obtained by the laser radar to contain rich environmental information and is not easily disturbed by light. In the wild environment, the navigator can be accurately identified and the quadruped robot can follow it autonomously. The real-time performance is high and the misjudgment rate is high. Low.

Description

A method for autonomously following a pilot by a quadruped robot based on three-dimensional laser radar

Technical Field

The present invention provides a method for realizing autonomous following of a navigator by a quadruped robot, which belongs to the technical field of mobile robot environment perception, and the main technology involves processing of laser point cloud data.

Background Art

Mobile robots can be divided into three categories from a structural perspective: wheeled robots, legged robots, and tracked robots. Among them, legged robots only need discrete footholds during movement, and are more adaptable to rugged unstructured environments than mobile robots of other structures, and have better movement flexibility and environmental adaptability.

As the most common legged robot, the walking structure of the quadruped robot is similar to that of most mammals in nature. Based on its structural characteristics, many scientific research institutions have conducted research on the motion control of quadruped robots. However, with the expansion of application scenarios, further research has also focused on the environmental perception and autonomous behavior of quadruped robots. Large quadruped robots are often used for military purposes. In complex field environments, quadruped robots that perform load-bearing tasks can reduce the burden of operator control and the requirements of the robot's own path planning by identifying and tracking the navigator, thereby completing the task of avoiding obstacles and reaching the designated destination.

Regarding the selection of sensors, infrared cameras, depth cameras, ToF and other optical cameras can all be used as the main equipment for robot environmental perception. Among them, the rise of three-dimensional optical ranging systems represented by laser radar has brought many new research possibilities to the field of robotics. Currently, the common omnidirectional three-dimensional laser radar on the market has the characteristics of strong anti-interference ability, small size and low power consumption, and can obtain rich three-dimensional point cloud information through high-frequency scanning.

The premise for a quadruped robot to perform autonomous behavior, including autonomous following of a navigator, is that it can effectively and in real time identify objects in a dynamically changing environment. Generally, sensors based on visual image processing can also obtain environmental information, but the images obtained by such devices are usually easily affected by changes in light and can hardly be used in dark scenes.

Summary of the invention

In view of the shortcomings of the prior art, the present invention provides a method for a quadruped robot to autonomously follow a navigator based on a three-dimensional laser radar, so that the robot can completely autonomously follow a designated navigator to avoid obstacles and unstructured environments such as trenches. The present invention uses a 16-line three-dimensional laser radar as a data acquisition device, processes the laser point cloud to accurately identify the navigator and sends motion control instructions to the quadruped robot so that it can autonomously follow the navigator's target safely and smoothly.

The method of the present invention for a quadruped robot to autonomously follow a navigator based on a three-dimensional laser radar comprises the following steps:

(1) Use a 16-line 3D laser radar as a sensor to obtain the original 3D point cloud data of the environment;

(2) Preprocessing the original point cloud using filtering operations, and the preprocessing process also includes removing ground point clouds to retain non-ground point cloud data;

(3) Perform segmentation and clustering based on Euclidean distance in the extracted non-ground point cloud data to obtain a point cloud cluster list of objects on the ground;

(4) Using the appearance classifier to select the candidate human target point cloud clusters from the segmented point cloud clusters, the target position of the navigator wearing the reflective sticker is determined according to the set laser reflection intensity threshold;

(5) Use Kalman filtering to associate moving objects at adjacent moments to maintain accurate tracking of the navigator; after obtaining the coordinates of the navigator's target position at each moment, output instructions on the speed and direction of movement to the quadruped robot to achieve autonomous following of the navigator.

In step (2), the specific process of preprocessing the point cloud data is:

的点，Ω_F是滤波后的点云簇，

是P_i的邻点，d_f是从

到P_i的距离，K_f是给定的系数；对于点云簇中的任意点，如果原始强反射值是

则其新反射强度值

将通过

计算得到；Assuming that the point cloud cluster _DT satisfies the normal distribution of N( _μf , _σf ), it needs to be traversed twice in the filtering stage. For any point, the first iteration calculates its average distribution distance _μf and variance _σf , and the second iteration filters out those that do not satisfy

points, Ω _F is the filtered point cloud cluster,

is the neighbor of _Pi , and _df is from

The distance to _Pi , _Kf is a given coefficient; for any point in the point cloud cluster, if the original strong reflection value is

The new reflection intensity value is

Will pass

Calculated;

求解法线n。After filtering, the noise is removed. The result is segmented. The point with the average value of the lowest height value in the segmented point cloud set is selected as the lowest point representative (LPR). It is used as the initial seed for plane model estimation. The simplest linear model ax+by+cz+d=0 is used for plane estimation, and the covariance matrix calculated by the seed point set is used.

Solve for the normal n.

The specific process of point cloud clustering segmentation based on Euclidean distance in step (3) is:

If the clustering threshold is set to a very small value, the actual object will be divided into multiple smaller objects; if the clustering threshold is set to a large value, then the actual multiple objects will be divided into one object;

其中a，b和c是调整参数，它们的值通常根据激光雷达的角度精度而变化，角度精度越高，它们的值应该越小，Sd将随着数据点的坐标而变化；换句话说，每当计算数据点或聚类与其相邻点或聚类之间的欧几里得距离时，Sd就会根据点或聚类的中心坐标来调整其值。Set an adaptive distance threshold as

Where a, b, and c are adjustment parameters. Their values usually vary according to the angular accuracy of the lidar. The higher the angular accuracy, the smaller their values should be. Sd will vary with the coordinates of the data points; in other words, whenever the Euclidean distance between a data point or cluster and its neighboring points or clusters is calculated, Sd will adjust its value according to the center coordinates of the point or cluster.

The specific implementation process of the appearance classifier in step (4) is:

The appearance-based classifier assigns the object of interest to one of two categories: human or non-human P. To this end, the non-ground point cloud cluster _Ck after removing the storefront point cloud is first converted into a compact six-dimensional feature vector: _xk = [ _hk , _ωk , _dk , _νk , _λk , _ρk ] ^T ;

① Human body contour feature extraction:

This part will extract three features related to the human body contour, including point cloud cluster cluster height, aspect ratio, and distribution symmetry; after screening the calculation results of the three features, a point cloud cluster with human body contour characteristics is obtained.

确定；When the navigator moves, the human body is partially blocked or the sensor is disturbed, which causes the navigator cluster in the collected point cloud to be suspended above the ground. The height _hk of the point cloud cluster _Ck is given by

Sure;

引入偏斜度γ来描述分布的不对称性，其中μ和σ分别是点云簇C_k分布的均值和方差；The aspect ratio of the point cloud cluster r _k = h _k / ω _k is used as one of the features for identifying the pilot. However, in real scenes, there may be interference from non-ground objects such as fire hydrants. Using the formula

The skewness γ is introduced to describe the asymmetry of the distribution, where μ and σ are the mean and variance of the distribution of the point cloud cluster C _k, respectively;

② Principal component analysis method to estimate posture:

利用C＝V∧V^T对协方差矩阵C进行特征值分解。得到的特征值λ和特征向量ν通过排序，最小的特征值λ_min对应的特征向量ν_min即为求解点p的点法向量。In the previous step, the human target point cloud cluster was preliminarily determined by the new height calculation method, aspect ratio and skewness. It should also be noted that the navigator always remained upright when walking, so the point cloud cluster generated an almost vertical main feature vector. The principal component analysis (PCA) method was used to estimate the posture of the target point cloud cluster, so as to more accurately screen the navigator target point cloud cluster. The covariance matrix C of the point cloud p and its neighborhood points is calculated as follows:

The covariance matrix C is decomposed by eigenvalue using C= ^V∧VT . The obtained eigenvalues λ and eigenvectors ν are sorted, and the eigenvector _νmin corresponding to the smallest eigenvalue _λmin is the point normal vector of the solution point p.

The appearance-based classifier can basically extract the accurate navigator target, but due to environmental interference and sensor measurement noise, there may still be a few misjudged point cloud clusters at the end of this step. To this end, the navigator wears a reflective marker on his waist, and the laser intensity returned by the sensor is used to select the cluster with the maximum intensity among the targets greater than the threshold range to obtain the accurate navigator position.

The laser radar used in the present invention has the advantages of small size and low power consumption, and is easy to be integrated into a quadruped robot platform. The omnidirectional three-dimensional laser point cloud data acquired contains rich environmental information and is not easily disturbed by light. Using the above solution in a field environment can accurately identify the navigator and allow the quadruped robot to follow autonomously, with high real-time performance and low misjudgment rate.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a method for a quadruped robot to autonomously follow a navigator based on three-dimensional laser radar according to the present invention.

FIG. 2 is a flow chart of point cloud clustering for identifying human targets in the present invention.

DETAILED DESCRIPTION

In the method of autonomously following a navigator by a quadruped robot based on three-dimensional laser radar, the VLP16-line laser radar of the laser radar sensor with three-dimensional coordinate information is selected as the point cloud acquisition device, and on this basis, a method for autonomously following a navigator suitable for a quadruped robot platform is provided. In the research on robot environmental perception based on laser scanning data, some institutions use multiple 32-line or 64-line laser radars to obtain dense and rich point cloud data for target recognition and thus determine the motion control strategy. However, high-speed movement scenes do not appear on the application platform of the quadruped robot, so the selection of 16-line laser radar can fully meet the real-time requirements and reduce the cost of the environmental perception system.

The 16-line laser radar is installed on the head of the quadruped robot at a height of about 1 meter from the ground. The navigator wears a reflective sticker on his waist and walks in front of the robot. The point cloud input module obtains the original point cloud data after coordinate conversion and parameter calibration. The point cloud preprocessing stage filters the point cloud and extracts the ground point cloud segmentation. The target recognition and tracking stage clusters the non-ground point cloud and extracts features to identify the accurate target position of the navigator. According to the target position, motion control commands are issued to enable the quadruped robot to follow the navigator autonomously and smoothly and safely, and to perform combat missions in the field environment.

Combined with the flowchart shown in Figure 1, the method of the present invention is mainly divided into four modules: point cloud input module, point cloud preprocessing module, target recognition and tracking module, and robot motion control module. The technical solution in the specific embodiment of the present invention will be described clearly and completely below.

1. Point cloud data acquisition process

The laser radar system consists of a laser ranging and scanning unit and a positioning system (POS) including an integrated differential global positioning system (DGPS) and an inertial navigation system (INS). The laser ranging unit estimates the distance from the sensor to the measured surface by measuring the time delay between the transmission of the laser pulse and its detection. Although the 16-line laser radar selected by the present invention does not have as many laser beams as the 32-line and 64-line laser radars, its resolution and sampling frequency can fully meet the real-time performance of the present method, and its cost is much lower than other laser radars.

External parameter calibration is required before the raw point cloud data obtained by the laser radar. According to the field of view requirements of the experimental platform for the sensor, the installation posture of the laser radar is that the head of the quadruped robot is about 1 meter above the ground. External parameter calibration requires obtaining the translation and rotation matrix from the laser radar coordinate system to the robot coordinate system. According to the installation position of the radar, the offset in each direction can be directly measured within the allowable error range, and the rotation matrix is obtained through the standard orthogonal matrix of the three-dimensional space posture.

2. Point cloud preprocessing process

的点，Ω_F是滤波后的点云簇，

是P_i的邻点，d_f是从

到P_i的距离，K_f是给定的系数。Point cloud filtering is the basis of point cloud preprocessing. In the present invention, important reflection intensity information required for subsequent data processing is retained based on statistical filtering. Assume that the point cloud cluster _DT satisfies the normal distribution of N( _μf , _σf ). Two iterations are required in the filtering stage. For any point, the average distribution distance _μf and variance _σf are calculated for the first time. The second iteration filters out points that do not meet the normal distribution.

points, Ω _F is the filtered point cloud cluster,

is the neighbor of _Pi , and _df is from

The distance to _Pi , _Kf is the given coefficient.

则其新反射强度值

将通过

计算得到。For the filtering scenario in the present invention, not only the spatial position of each data point needs to be considered, but also the reflection intensity. Here, a bilateral filtering algorithm is used. For any point in the point cloud cluster, if the original strong reflection value is

The new reflection intensity value is

Will pass

Calculated.

求解法线n。After filtering, the laser point cloud with noise removed but still retaining most of the environmental information and reflection intensity information is obtained. The result is segmented and the point with the average value of the lowest height value in the segmented point cloud set is selected as the lowest point representative (LPR). It is used as the initial seed for plane model estimation. The simplest linear model ax+by+cz+d=0 is used for plane estimation, and the covariance matrix calculated by the seed point set is used.

Solve for the normal n.

3. Target recognition and tracking

Only after the ground point cloud is eliminated can the influence on the clustering of non-ground point clouds be eliminated. The work of target recognition is carried out on this basis. As shown in Figure 2, the specific process of target recognition is shown. The first is point cloud clustering. In this step of point cloud clustering, a clustering method based on Euclidean distance is used.

If the traditional Euclidean distance clustering algorithm does not correctly select the Euclidean distance threshold, it will often lead to neighborhood detection errors or far-field detection omissions. If the clustering threshold is set to a very small value, the actual object will be divided into multiple smaller objects; if the clustering threshold is set to a large value, then multiple actual objects will be divided into one object.

其中a，b和c是调整参数，它们的值通常根据激光雷达的角度精度而变化。角度精度越高，它们的值应该越小，Sd将随着数据点的坐标而变化。换句话说，每当计算数据点或聚类与其相邻点或聚类之间的欧几里得距离时，Sd就会根据点或聚类的中心坐标来调整其值。In the present invention, in order to improve the clustering rate, reduce the misjudgment rate, and save computing resources, an adaptive distance threshold is set as

Where a, b, and c are adjustment parameters whose values usually vary according to the angular accuracy of the LiDAR. The higher the angular accuracy, the smaller their values should be, and Sd will vary with the coordinates of the data points. In other words, whenever the Euclidean distance between a data point or cluster and its neighboring points or clusters is calculated, Sd will adjust its value according to the center coordinates of the point or cluster.

After obtaining the point cloud clustering results, the appearance-based classifier and enhanced reflection intensity features are used to screen out the correct human targets in the point cloud cluster list.

The appearance-based classifier assigns the object of interest to one of two categories: human or non-human P. To this end, the non-ground point cloud cluster _Ck after removing the storefront point cloud is first converted into a compact six-dimensional feature vector: _xk = [ _hk , _ωk , _dk , _νk , _λk , _ρk ] ^T .

First, human body contour feature extraction is performed. This part will extract three features related to human body contour, including point cloud cluster cluster height, aspect ratio, and distribution symmetry. After screening the calculation results of the three features, point cloud clusters with human body contour characteristics are obtained.

确定，这样的计算高度的方式使悬浮的人体点云最大高度定义为为对整个人的扫描的高度。When the navigator moves, the human torso is partially blocked or the sensor is disturbed, which causes the navigator cluster in the collected point cloud to be suspended above the ground. The previous calculation method uses the height of the bounding box to represent the point cloud cluster height, which causes the floating torso height to be halved, making it far from the height of a person. In the present invention, the height _hk of cluster _Ck is given by

It is determined that such a method of calculating the height enables the maximum height of the suspended human body point cloud to be defined as the height of the scan of the entire person.

引入偏斜度γ来描述分布的不对称性，其中μ和σ分别是点云簇C_k分布的均值和方差。The aspect ratio of the point cloud cluster r _k = h _k /ω _k is used as one of the features for identifying the pilot, but in real scenes, there may be interference from non-ground objects such as fire hydrants. Using the formula

The skewness γ is introduced to describe the asymmetry of the distribution, where μ and σ are the mean and variance of the distribution of the point cloud cluster C _k, respectively.

利用C＝V∧V^T对协方差矩阵C进行特征值分解。得到的特征值λ和特征向量ν通过排序，最小的特征值λ_min对应的特征向量ν_min即为求解点p的点法向量。In the previous step, the human target point cloud cluster was preliminarily determined by the new height calculation method, aspect ratio and skewness. It should also be noted that the navigator always remained upright when walking, so the point cloud cluster generated an almost vertical main feature vector. The principal component analysis (PCA) method was used to estimate the posture of the target point cloud cluster, so as to more accurately screen the navigator target point cloud cluster. The covariance matrix C of the point cloud p and its neighborhood points is calculated as follows:

The covariance matrix C is decomposed by eigenvalue using C= ^V∧VT . The obtained eigenvalues λ and eigenvectors ν are sorted, and the eigenvector _νmin corresponding to the smallest eigenvalue _λmin is the point normal vector of the solution point p.

The appearance-based classifier can basically extract the accurate navigator target, but due to environmental interference and sensor measurement noise, there may still be a few misjudged point cloud clusters at the end of this step. The solution to this problem is that the navigator wears a reflective marker on his waist, and through the laser intensity returned by the sensor, select the cluster with the maximum intensity from the targets within the threshold range to obtain the accurate navigator position.

4. Robot motion control

Kalman filtering is used to associate moving objects at adjacent moments to keep accurate tracking of the navigator. After obtaining the coordinates of the navigator's target position at each moment, the speed and direction instructions are output to the quadruped robot to achieve autonomous following of the navigator. Before the control instruction is output, a simple motion strategy judgment is performed. If the navigator's position suddenly changes, it is easy to cause the quadruped robot to turn rapidly, affecting smooth driving. In addition, when the distance between the navigator's position and the robot is less than the set safety threshold, a control instruction with a speed of 0 is issued to avoid collision.

The present invention accurately identifies and tracks the pilot target based on the laser point cloud data obtained by the laser radar, and enables the quadruped robot to effectively and smoothly follow the pilot autonomously. The present invention can meet the real-time and accuracy requirements of the robot autonomously following the personnel.

Claims (3)

1. A method for a quadruped robot to autonomously follow a pilot based on a three-dimensional laser radar is characterized by comprising the following specific steps:

(1) Using a 16-line three-dimensional laser radar as a sensor to obtain original environment three-dimensional point cloud data;

(2) Using a filtering operation to preprocess the original point cloud, wherein the preprocessing process also comprises the step of rejecting ground point cloud so as to keep non-ground point cloud data;

(3) Carrying out segmentation clustering based on Euclidean distance in the extracted non-ground point cloud data to obtain a point cloud cluster list of objects on the ground;

(4) Screening candidate human body target point cloud clusters from the segmented point cloud clusters by using an appearance classifier, and judging the target position of a pilot wearing a reflective label according to a set laser reflection intensity threshold;

(5) Using Kalman filtering to correlate the moving objects at adjacent moments so as to keep accurate tracking of a pilot; after the coordinates of the target position of the pilot at each moment are obtained, instructions of the traveling speed and the traveling direction are output to the quadruped robot, so that the pilot can be autonomously followed;

the specific implementation process of the appearance classifier in the step (4) is as follows:

appearance-based classifiers assign an object of interest to one of two categories: human or non-human P; for this purpose, firstly, the non-ground point cloud cluster C after the ground point cloud is removed is processed _k Conversion to a compact six-dimensional feature vector: x is the number of _k ＝[h _k ,ω _k ,d _k ,ν _k ,λ _k ,ρ _k ] ^T ；

(1) Human body contour feature extraction:

three characteristics related to the human body contour are extracted from the part, including the clustering height, the height-to-width ratio and the distribution symmetry of the point cloud clusters, and the point cloud clusters with the human body contour characteristics are obtained after the calculation results of the three characteristics are screened;

in the process of movement of a pilot, when the trunk of a human body is partially shielded or a sensor is interfered and other factors, an object cluster such as the pilot in the collected point cloud is suspended above the ground, and a point cloud cluster C _k Height h of _k By

Determining;

aspect ratio r of point cloud cluster _k ＝h _k /ω _k As one of the characteristics for identifying the pilot, a formula is used

Skewness gamma is introduced to describe the asymmetry of the distribution, where mu and sigma are point cloud clusters C, respectively _k Mean and variance of the distribution;

(2) Estimating the attitude by a principal component analysis method:

the method comprises the following steps that in the previous step, a human body target point cloud cluster is preliminarily judged through a new height calculation mode, the height-width ratio and the introduced skewness, and attention should be paid to that a pilot always keeps an upright state when walking, so that a main characteristic vector in an almost vertical direction is generated by the point cloud cluster; performing attitude estimation on the target point cloud cluster by using a principal component analysis method, so that the target point cloud cluster of the pilot is more accurately screened out; the calculation mode of the covariance matrix C of the point cloud p and the neighborhood points is

Using C = V ^ V ^T The covariance matrix C is subjected to eigenvalue decomposition, the obtained eigenvalue lambda and eigenvector v are sorted, and the minimum eigenvalue lambda is _min Corresponding feature vector v _min Namely, the point normal vector of the solution point p is obtained;

the navigator wears a reflection sign at the waist, and the accurate position of the navigator is obtained by selecting the clustering cluster with the maximum intensity from the targets in the range larger than the threshold value through the laser intensity returned by the sensor.

2. The method for the quadruped robot to autonomously follow the pilot based on the three-dimensional laser radar as claimed in claim 1, wherein the specific process of point cloud data preprocessing in the step (2) is as follows:

hypothesis Point cloud Cluster D _T Satisfies N (mu) _f ,σ _f ) The normal distribution of (2) needs to be traversed twice in the filtering stage, and for any point, the average distribution distance mu is calculated for the first time _f Sum variance σ _f Second iteration filters out unsatisfied

Point of (a), omega _F Is a filtered point cloud cluster, P _i ⁿ Is P _i Adjacent point of (a), d _f Is from P _i ⁿ To P _i Distance of, K _f Is a given coefficient; for any point in a point cloud cluster, if the original strong reflection value is +>

Then its new reflection intensity value->

Will pass through

Calculating to obtain;

removing noise after filtering processing, adopting a segmentation method for the result, selecting a point of the average value of the lowest height value points in a segmented point cloud set as the lowest point representative, taking the point as the initial seed of plane model estimation, using the simplest linear model ax + by + cz + d =0 for plane estimation, and calculating a covariance matrix through a seed point set

The normal n is solved.

3. The method for the quadruped robot to autonomously follow the pilot based on the three-dimensional laser radar as claimed in claim 1, wherein the specific process of the Euclidean distance-based point cloud clustering in the step (3) is as follows:

setting an adaptive distance threshold of

Where a, b and c are adjustment parameters, their values typically vary depending on the angular accuracy of the lidar; the higher the angular accuracy, the smaller their value should be, sd will vary with the coordinates of the data points. />

Images