CN117949966B - Mobile robot tracking control method based on laser radar and odometer - Google Patents

Abstract

The invention belongs to the technical field of mobile robot tracking control, and discloses a mobile robot tracking control method based on a laser radar and an odometer. The mobile robot of the follower acquires laser radar information and converts the laser radar information into point cloud information; the follower mobile robot preprocesses the point cloud information; distinguishing a pilot mobile robot point cloud and an obstacle point cloud by utilizing a relative motion principle; calculating the pose of the navigator mobile robot relative to the follower mobile robot by using the plane geometric relationship; and continuously tracking the point cloud of the pilot mobile robot by using the principle of space position continuity and the historical pose of the pilot mobile robot. The invention realizes the stable tracking of the follower mobile robot to the navigator mobile robot in an unknown environment, and improves the safety and continuous tracking performance of the mobile robot in a complex environment.

Description

A tracking control method for mobile robots based on laser radar and odometer

Technical Field

The present invention belongs to the technical field of mobile robot tracking control, and specifically relates to a mobile robot tracking control method based on laser radar and odometer, which is suitable for realizing stable tracking of the mobile robot in an environment with unknown obstacles.

Background Art

With the development of autonomous control technology, mobile robots need to achieve stable tracking control in various complex environments to ensure continuous tracking performance. Traditional mobile robot tracking control methods face difficulties such as repeated obstacle detection, interference with the motion of the mobile robot itself, and loss of tracking targets and re-establishing tracking. In addition, some sensors may also be interfered by environmental noise. Therefore, it is necessary to propose a mobile robot tracking control method based on laser radar and odometer to improve the autonomous control ability of mobile robots in complex environments through effective data processing and relative motion principles.

Summary of the invention

The purpose of the present invention is to propose a mobile robot tracking control method based on laser radar and odometer, so as to realize stable tracking of a leader mobile robot by a follower mobile robot in an environment with unknown obstacles, thereby significantly improving the continuous tracking performance of the mobile robot in a complex environment.

In order to achieve the above object, the present invention adopts the following technical scheme:

A mobile robot tracking control method based on laser radar and odometer, wherein the mobile robot tracking control system includes a follower mobile robot and a leader mobile robot, and the method comprises the following steps:

Step 1. Define a data structure for describing an obstacle area, use a linked list to store the data structure, and name the linked list as a historical obstacle area, and define a data structure for describing the historical posture state of the navigator mobile robot;

Step 2. Obtain the laser radar sensor data of the follower mobile robot to generate the original point cloud data, while considering the laser radar sensor noise, the sampling density of the laser radar sensor data and the motion interference of the follower mobile robot, and fuse the odometer sensor data of the follower mobile robot to pre-process the original point cloud data to obtain the expected point cloud data;

Step 3. Eliminate the expected point cloud data located in the historical obstacle area to obtain valid point cloud data. At the same time, based on the historical posture state of the leader mobile robot, the follower mobile robot distinguishes the point cloud data of the leader mobile robot from the point cloud data of the obstacle by using the principle of relative motion or the principle of spatial position continuity;

Step 4. If step 3 fails to successfully identify the point cloud data of the navigator mobile robot, clear the historical position state variables of the navigator mobile robot and return to step 2; if step 3 successfully identifies the point cloud data of the navigator mobile robot, continue to step 5;

Step 5. Based on the point cloud data of the navigator mobile robot obtained in step 3, the center position and azimuth of the navigator mobile robot are calculated using the plane geometric relationship, and the historical position and posture state of the navigator mobile robot is updated. Then, the odometer sensor data of the follower mobile robot is used to perform coordinate transformation on the center position and azimuth of the navigator mobile robot, and the position and posture of the navigator mobile robot relative to the follower mobile robot are obtained, so that the follower mobile robot can track the point cloud of the navigator mobile robot;

Step 6. Based on the obstacle point cloud data obtained in step 3, the center position and size range of the obstacle are calculated, and the buffer area size is specified to obtain the obstacle area, and then the obstacle area is added to the historical obstacle area, and then return to step 2;

Among them, both the follower mobile robot and the leader mobile robot are equipped with laser radar and odometer sensors.

The present invention has the following advantages:

As described above, the present invention relates to a mobile robot tracking control method based on laser radar and odometer, which is suitable for environments with unknown obstacles. The system targeted by the method of the present invention is composed of a follower mobile robot and a navigator mobile robot equipped with a two-dimensional laser radar and an odometer sensor, wherein the follower mobile robot obtains laser radar information and converts it into point cloud information; the follower mobile robot performs data preprocessing on the point cloud information, including outlier removal, voxel grid filtering downsampling, reference coordinate system transformation and Euclidean clustering segmentation; the principle of relative motion is used to distinguish the point cloud of the navigator mobile robot from the point cloud of obstacles; the plane geometric relationship is used to calculate the position and posture of the navigator mobile robot relative to the follower mobile robot; the principle of spatial position continuity and the historical position and posture of the navigator mobile robot are used to continuously track the point cloud of the navigator mobile robot; an obstacle buffer area is set, and the obstacle area is calibrated and recorded. Even if the follower mobile robot loses its view of the navigator mobile robot due to special circumstances such as obstacle occlusion, when the navigator mobile robot appears in the follower mobile robot's field of view again, the follower mobile robot can still locate and continuously track the navigator mobile robot's point cloud based on the relative motion principle and historical obstacle area information. The present invention realizes the stable tracking of the navigator mobile robot's point cloud by the follower mobile robot in an unknown environment through sensor data processing, relative motion principle, spatial position continuity principle and plane geometric relationship calculation, thereby improving the safety and continuous tracking performance of the mobile robot in a complex environment.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a mobile robot tracking control method based on a laser radar and an odometer in an embodiment of the present invention.

FIG2 is a schematic diagram of a posture model of a mobile robot in an embodiment of the present invention;

FIG3 is a schematic diagram of a mobile robot tracking system according to an embodiment of the present invention;

FIG4 is a schematic diagram of point cloud data segmentation and intermediate segmentation and segment center point calculation for each cluster according to an embodiment of the present invention;

FIG5 is a schematic diagram of obtaining desired point cloud data after point cloud data preprocessing in an embodiment of the present invention;

FIG6 is a schematic diagram of the application of the position continuity principle in an embodiment of the present invention;

7 is a schematic diagram of the relative distance between the leader mobile robot and the follower mobile robot in an embodiment of the present invention;

FIG8 is a schematic diagram of the direction angle of the leader mobile robot relative to the follower mobile robot in an embodiment of the present invention.

DETAILED DESCRIPTION

The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

Example

In view of the complexity of mobile robot tracking in an environment with unknown obstacles and the data processing challenges of lidar and odometer sensors, the present invention proposes a mobile robot tracking control method based on lidar and odometer to achieve stable tracking of a leader mobile robot by a follower mobile robot in an environment with unknown obstacles, thereby improving the safety and continuous tracking performance of the mobile robot in a complex environment.

The mobile robot tracking control system targeted by the present invention comprises a follower mobile robot and a navigator mobile robot, wherein the follower mobile robot and the navigator mobile robot are both equipped with a two-dimensional laser radar and an odometer sensor.

As shown in FIG1 , the mobile robot tracking control method based on laser radar and odometer includes the following steps:

Step 1. Define a data structure for describing an obstacle region, use a linked list to store the data structure, and name the linked list as a historical obstacle region, where an obstacle region is defined as a rectangular region. At the same time, define a data structure for describing the historical position and posture state of the navigator mobile robot.

The data structure used to describe the obstacle area contains four double-precision floating-point numbers, which represent the horizontal and vertical coordinates of the center point of the obstacle area, the length of the obstacle area, and the width of the obstacle area, respectively.

A linked list is used to store the data structure for describing the obstacle area. The geometric meaning of the linked list represents multiple rectangular obstacle areas, so the linked list is named historical obstacle area.

The data structure used to describe the historical position and posture state of the navigator mobile robot contains three double-precision floating-point numbers, which respectively represent the horizontal and vertical coordinates of the center point of the navigator mobile robot and the direction angle of the navigator mobile robot.

Step 2. Obtain the follower mobile robot's lidar sensor data to generate raw point cloud data, while considering the lidar sensor noise, lidar sensor data sampling density, and follower mobile robot motion interference, and fuse the follower mobile robot's odometer sensor data to preprocess the raw point cloud data and obtain the expected point cloud data.

As shown in Figure 3, the scanning results of the laser radar sensor of the follower mobile robot are obtained to generate surrounding environment data, which are stored in the computer in the form of an array. The surrounding environment data includes distance information, laser radar scanning range and angular resolution. The surrounding environment data is converted from the laser radar polar coordinate system to the laser radar rectangular coordinate system coordinates to generate raw cloud data. The raw point cloud data is defined as point cloud data No. 1.

Among them, the conversion formula from the laser radar polar coordinate system to the laser radar rectangular coordinate system is as follows:

Where r is the distance from the lidar scanning point to the lidar sensor, θ is the direction of the laser beam relative to the lidar sensor position, and x and y are the positions of the point cloud in the lidar rectangular coordinate system.

The laser radar rectangular coordinate system is the laser radar sensor coordinate system hereinafter.

Step 2.1. Considering the lidar sensor noise and lidar sensor data sampling density, the original point cloud data is filtered and downsampled using a statistical outlier removal algorithm and a voxel grid filter to generate point cloud data No. 2.

Step 2.2. Consider the motion interference of the follower mobile robot, obtain the transformation information between the follower mobile robot odometer sensor data and the follower mobile robot rigid body, calculate the transformation matrix from the follower mobile robot lidar sensor coordinate system to the odometer sensor coordinate system, and use the coordinate transformation to transform the reference coordinate system of the No. 2 point cloud data from the follower mobile robot lidar sensor coordinate system to the odometer sensor coordinate system to generate the No. 3 point cloud data.

The odometer sensor data of the follower mobile robot records the position and direction of the follower mobile robot's body coordinate system base_link in the odometer sensor coordinate system odom, specifically including the three-dimensional coordinates (x, y, z) used to describe the position information of the follower mobile robot and the quaternion [a, b, c, d] ^T used to describe the direction of the follower mobile robot.

FIG2 shows a schematic diagram of the pose model of the mobile robot in an embodiment of the present invention. In the figure, _Yodom and _Xodom respectively represent the horizontal and vertical coordinate systems of the odometer sensor coordinate system, _Oodom represents the coordinate origin of the odometer sensor coordinate system, _OR is both the position of the mobile robot in the odometer sensor coordinate system and the coordinate origin of the mobile robot body coordinate system, and _YR and _XR respectively represent the horizontal and vertical coordinate systems of the mobile robot body coordinate system.

The calculation formula of the transformation matrix and the process of obtaining the No. 3 point cloud data are as follows:

Where R is the rotation matrix, which represents the posture information of the follower mobile robot body coordinate system relative to the odometer sensor coordinate system; B is the translation vector, which represents the position information of the follower mobile robot body coordinate system relative to the odometer sensor coordinate system, and the parameters (x, y, z) represent the three-dimensional coordinates of the follower mobile robot position information.

The rotation matrix R and the translation vector B are combined to form a 4×4 transformation matrix T ₁ , which represents the position information of the follower mobile robot body coordinate system relative to the odometer sensor coordinate system; where 0 is a 1×3 row vector, [a, b, c, d] ^T is the quaternion representation of the rotation matrix R, which is used to describe the posture of the follower mobile robot.

The transformation information between the rigid bodies of the follower mobile robot is obtained, and the transformation matrix from the laser radar sensor coordinate system front_laser of the follower mobile robot to the coordinate system of the follower mobile robot body is obtained. Since the laser radar sensor is rigidly connected to the follower mobile robot, the relative position and posture between the two are fixed, so the transformation matrix mentioned here is a constant matrix, denoted as T ₂ .

Considering the motion interference of the follower mobile robot, the reference coordinate system of the No. 2 point cloud data is transformed from the follower mobile robot's lidar sensor coordinate system to the odometer sensor coordinate system using coordinate transformation to generate No. 3 point cloud data.

P ₃ =(T ₁ ×T ₂ )·P ₂ (2)

Where _P2 is the second point cloud data, and _P3 is the third point cloud data.

Step 2.3. Use the Euclidean clustering method to segment the point cloud data No. 3 to obtain the point cloud data No. 4 containing K clusters; perform intermediate segmentation and segment center point calculation on each cluster in the point cloud data No. 4 to obtain two subsets after each cluster segmentation and the center point coordinates of each subset, where K is a natural number.

The coordinates of the central points are all stored in an array, and the desired point cloud data is obtained.

Specifically, the Euclidean clustering method is first used to segment the point cloud data No. 3, and the point cloud data No. 4 containing K clusters is obtained as shown in Figure 4:

First, a distance threshold ε is set, and for each pair of points (x _i , y _i ) and (x _j , y _j ) in the No. 3 point cloud data, the Euclidean distance is calculated:

Then, the points whose Euclidean distance d _ij is less than the threshold ε are divided into the same cluster, and four point cloud data containing K clusters are generated according to the number of separated objects in the surrounding space.

As shown in FIG5 , the middle segmentation of the point cloud and the calculation of the segment center point are performed on each cluster in the No. 4 point cloud data to obtain two subsets after each cluster segmentation and the coordinates of the center point of each subset.

Specifically, the coordinates of the center points of each subset are shown in the following formula:

Where, _{Zi_1} and _{Zi_2} are the center point coordinates of the two subsets of the i-th cluster point cloud data, i = 1, 2, ..., K; N is the length of the i-th cluster point cloud data, symbol Represents floor operation; it is used to convert a real number into the integer that is closest to and not greater than the real number. _xj and _yj are the horizontal and vertical coordinates of the point cloud data, and j = 1, 2, ..., N.

The above center point coordinates are stored in an array in the original order to obtain the expected point cloud data.

Step 3. Eliminate the expected point cloud data located in the historical obstacle area to obtain valid point cloud data. At the same time, based on the historical posture state of the leader mobile robot and using the principle of relative motion or the principle of spatial position continuity, the follower mobile robot distinguishes the point cloud data of the leader mobile robot from the obstacle point cloud data.

Based on the expected point cloud data obtained in step 2, calculate the center point coordinates of each cluster of the expected point cloud data:

Where ^o M _i is the coordinate of the center point of each cluster of point cloud data, which represents the coordinate of the center point of each cluster of point cloud data in the coordinate system of the follower mobile robot odometer sensor, where i = 1, 2, ..., K.

The specific process of excluding clusters located in historical obstacle areas in the desired point cloud data is as follows:

Input: ^o _Mi is the center point coordinates of each cluster of the expected point cloud data and the historical obstacle area list LIST;

Output: Boolean value indicating whether the vehicle is within the historical obstacle area.

The specific process of judging whether a point is within the historical obstacle area is as follows:

For each obstacle area in LIST:

Obstacle center point_x = x coordinate of the center point of the obstacle area;

Obstacle center point_y = y coordinate of the center point of the obstacle area;

Obstacle length = length of obstacle area;

Obstacle width = width of the obstacle area.

Calculate the bounds of the rectangle:

Right boundary = (obstacle center point_y-obstacle width)/2;

Left boundary = (obstacle center point_y + obstacle width)/2;

Lower boundary = (obstacle center point_x-obstacle length)/2;

Upper boundary = (obstacle center point_x + obstacle length)/2.

If ^o M _i is within the rectangular range enclosed by the above four boundary values, return yes; return no.

The remaining expected point cloud data clusters are stored in the array in the original order to form valid point cloud data.

In order to solve the positioning problem of the navigator mobile robot in the initial stage, the tracking and positioning problem of the navigator mobile robot in the normal operation stage, and the re-positioning problem of the navigator mobile robot in the tracking loss stage, the present invention designs three different positioning methods, namely initial positioning, tracking positioning and re-positioning, and corresponding methods are run in different stages.

The initial stage refers to the period after the program is started and before the point cloud of the leader mobile robot is located.

After the initialization phase successfully locates the point cloud of the navigator mobile robot, it enters the normal operation phase.

Due to the obstruction of the navigator mobile robot by obstacles, the point cloud of the navigator mobile robot cannot be located during the normal operation stage, and the robot enters the tracking loss stage.

After re-identifying the point cloud of the leader mobile robot during the tracking loss phase, it returns to the normal operation phase.

Initial positioning method:

When the program enters the initial positioning method less than C times continuously, the valid point cloud data is recorded in the array.

When the program enters the initial positioning method for C consecutive times, the valid point cloud data is recorded in an array, and the average value algorithm is used to calculate the average value of the recorded C groups of valid point cloud data, which is called the initial valid point cloud data.

Formula (4) is used to calculate the coordinates of the center points of each cluster of initial valid point cloud data and each cluster of valid point cloud data, which are called the initial position of the point cloud and the real-time position of the point cloud, respectively.

The above-mentioned clusters of initial valid point cloud data and clusters of valid point cloud data are connected into line segments, and the inverse tangent function is used to calculate the angle between the line segment and the positive direction of the horizontal axis of the follower mobile robot odometer sensor coordinate system, which are called the point cloud initial angle and the point cloud real-time angle respectively.

When the navigator mobile robot undergoes translational motion, resulting in the Euclidean distance between the initial position of the point cloud and the real-time position of the point cloud being greater than a threshold σ, the point cloud data of the navigator mobile robot is distinguished from the valid point cloud data.

Or, when the navigator mobile robot rotates, causing the difference between the initial angle of the point cloud and the real-time angle of the point cloud to be greater than a threshold η, the point cloud data of the navigator mobile robot is distinguished from the valid point cloud data.

Tracking and positioning method:

Formula (4) is used to calculate the center point coordinates of each cluster of valid point cloud data, which is called the real-time position of the point cloud.

The historical posture state variables of the navigator mobile robot are obtained, and a circular area with a radius of R is made with the center point of the navigator mobile robot in the historical posture state variables of the navigator mobile robot as the center of the circle, as shown in Figure 6. The circular area is called the circular boundary condition, and the valid point cloud data cluster whose real-time position of the point cloud is located within the circular boundary condition is considered to be the point cloud data of the navigator mobile robot, thereby distinguishing the point cloud data of the navigator mobile robot from the valid point cloud data.

Repositioning method:

When the program enters the re-positioning method for no more than C consecutive times, the valid point cloud data is recorded into an array, and the average value of the recorded valid point cloud data is calculated, which is called the real-time initial valid point cloud data.

Formula (4) is used to calculate the coordinates of the center points of each cluster of the initial effective point cloud data and each cluster of the effective point cloud data, which are called the real-time point cloud initial position and point cloud real-time position respectively.

The above-mentioned clusters of initial valid real-time point cloud data and clusters of valid point cloud data are connected into line segments, and the inverse tangent function is used to calculate the angle between the line segment and the positive direction of the horizontal axis of the follower mobile robot odometer sensor coordinate system, which are called the real-time point cloud initial angle and point cloud real-time angle respectively.

When the navigator mobile robot undergoes translational motion, resulting in the Euclidean distance between the initial position of the real-time point cloud and the real-time position of the point cloud being greater than a threshold σ, the point cloud data of the navigator mobile robot is distinguished from the valid point cloud data.

Or, when the navigator mobile robot rotates, causing the difference between the real-time point cloud initial angle and the point cloud real-time angle to be greater than a threshold η, the navigator mobile robot point cloud data is distinguished from the valid point cloud data.

Step 4. If the obstacle blocks the navigator mobile robot and the point cloud data of the navigator mobile robot cannot be distinguished from the valid point cloud data during the normal operation phase of step 3, clear the historical pose state variables of the navigator mobile robot and return to step 2. Otherwise, if step 3 successfully identifies the point cloud data of the navigator mobile robot, continue to step 5.

Step 5. Based on the point cloud data of the navigator mobile robot obtained in step 3, the center position and azimuth of the navigator mobile robot are calculated using plane geometric relationships, and the odometer sensor data of the follower mobile robot is used to perform coordinate transformation on the center position and azimuth of the navigator mobile robot to obtain the position and posture of the navigator mobile robot relative to the follower mobile robot, thereby enabling the follower mobile robot to track the point cloud of the navigator mobile robot.

Step 5.1. Based on the point cloud data of the navigator mobile robot obtained in step 3, use the average value algorithm to calculate the coordinates of the center point of the point cloud data of the navigator mobile robot. According to the fixed position relationship between the point cloud data center point and the center point of the navigator mobile robot, the center point position of the navigator mobile robot, that is, the center position, is obtained, denoted as ^o M _l .

Step 5.2. Based on the point cloud data of the navigator mobile robot obtained in step 3, connect the point cloud data of the navigator mobile robot into a line segment, calculate the angle between the line segment and the positive direction of the horizontal axis of the odometer sensor coordinate system, and obtain the angle of the navigator mobile robot, that is, the azimuth angle, recorded as ^o θ _l .

Step 5.3. Use the center position obtained in step 5.1 and the azimuth information obtained in step 5.2 to update the historical posture state variables of the leader mobile robot.

Step 5.4. Obtain the transformation information between the odometer sensor data of the follower mobile robot and the rigid body of the follower mobile robot, and calculate the transformation matrix from the odometer sensor coordinate system to the chassis coordinate system of the follower mobile robot.

First, the odometer sensor data of the follower mobile robot is obtained, and the rotation matrix and transformation matrix from the coordinate system of the follower mobile robot to the odometer sensor coordinate system are calculated. The two are inverted to obtain the rotation matrix and transformation matrix from the odometer sensor coordinate system to the coordinate system of the follower mobile robot, which are recorded as R ^-1 and T ₃ respectively.

The transformation information between the rigid bodies of the follower mobile robot is obtained, and the transformation matrix from the coordinate system of the follower mobile robot to the coordinate system of the chassis of the follower mobile robot chassis_link is obtained. Since the relative position and posture of the coordinate system of the follower mobile robot body and the coordinate system of the chassis of the follower mobile robot are fixed, the transformation matrix here is a constant matrix, denoted as T ₄ .

Step 5.5. Using the transformation matrix obtained in step 5.4, transform the reference coordinate system of the center position obtained in step 5.1 and the azimuth angle obtained in step 5.2 from the odometer sensor coordinate system to the chassis coordinate system of the follower mobile robot, as shown in formula (5), and obtain the position and posture of the leader mobile robot relative to the follower mobile robot, as shown in Figure 7. In the figure, _OF represents the coordinate origin of the body coordinate system of the follower mobile robot, and _YF and _XF represent the horizontal and vertical coordinate systems of the body coordinate system of the mobile robot, respectively. The relative position calculation formula of the leader mobile robot relative to the follower mobile robot is as follows:

^f M _l = (T ₃ ×T ₄ )· ^o M _l (5)

Where ^fMl is the relative position, which represents the coordinate of the center point of the leader mobile robot in the chassis coordinate system _of the follower mobile robot.

Using the rotation matrix, the reference coordinate system of the azimuth angle is transformed from the odometer sensor coordinate system to the chassis coordinate system of the follower mobile robot to obtain the angle of the leader mobile robot relative to the follower mobile robot, as shown in Figure 8:

Where ^fRl is the rotation matrix, _which represents the rotation relationship from the positive direction of the horizontal axis of the mobile robot chassis coordinate system to the positive direction of the leader mobile robot.

From the above rotation matrix ^f R _l , the angle ^f θ _l between the positive direction of the leader mobile robot and the positive direction of the horizontal axis of the mobile robot chassis coordinate system is extracted.

At this point, the position and posture of the leader mobile robot relative to the follower mobile robot are obtained, so that the follower mobile robot can track the point cloud of the leader mobile robot.

Step 6. Based on the obstacle point cloud data obtained in step 3, the center position and size range of the obstacle are calculated, and the buffer area size is specified to obtain the obstacle area, and then the obstacle area is added to the historical obstacle area, and then return to step 2.

Based on the obstacle point cloud data obtained in step 3, the rectangular area that can enclose all the obstacle point cloud data is calculated using the plane geometric relationship. The specific calculation formula is as follows:

Where Z _{j_mid} is the coordinate of the center point of the obstacle point cloud data;

and are the center point coordinates of the two subsets of obstacle point cloud data, _lj is the length of the rectangular area, _wj is the width of the rectangular area, where j = 1, 2, ..., K-1.

Considering the circumscribed circle radius and the minimum turning radius of the mobile robot, a buffer zone is created around the rectangular area, and the size of the buffer zone is specified. The rectangular area with the buffer zone is named the obstacle area.

Calculate the center point coordinates, length and width of the obstacle area. The specific formula is as follows:

Where _Mj represents the coordinates of the center point of the obstacle area, _lengthj represents the length of the obstacle area, _widthj represents the width of the obstacle area, and buffer represents the specified buffer area size, where j=1, 2, ..., K-1.

The center point coordinates, length and width of the obstacle area are stored in the data structure used to describe the obstacle area, and then added to the historical obstacle area linked list, and then return to step 2.

The method of the present invention realizes the stable tracking of the point cloud of the leader mobile robot by the follower mobile robot in an unknown environment through sensor data processing, relative motion principle, spatial position continuity principle and plane geometric relationship calculation, thereby improving the safety and continuous tracking performance of the mobile robot in complex environments.

Of course, the above description is only a preferred embodiment of the present invention, and the present invention is not limited to the above embodiments. It should be noted that all equivalent substitutions and obvious deformation forms made by any technician familiar with the field under the guidance of this specification fall within the essential scope of this specification and should be protected by the present invention.

Claims (5)

1. The mobile robot tracking control method based on the laser radar and the odometer aims at that a mobile robot tracking control system comprises a follower mobile robot and a navigator mobile robot and is characterized by comprising the following steps:

Step 1, defining a data structure for describing an obstacle area, storing the data structure by using a linked list, respectively naming the linked list as a history obstacle area, and defining the data structure for describing the history pose state of the pilot mobile robot;

Step 2, acquiring data of a laser radar sensor of a mobile robot of a follower to generate original point cloud data, simultaneously considering laser radar sensor noise, laser radar sensor data sampling density and motion interference of the mobile robot of the follower, fusing data of an odometer sensor of the mobile robot of the follower, and preprocessing the original point cloud data to obtain expected point cloud data;

in the step2, the process of acquiring the desired point cloud data is as follows:

Step 2.1, filtering and downsampling the original point cloud data by using a statistical outlier rejection algorithm and a voxel grid filter to generate second point cloud data;

Step 2.2, acquiring data of a follower mobile robot odometer sensor and transformation information between rigid bodies of the follower mobile robot, calculating to obtain a transformation matrix from a coordinate system of the follower mobile robot laser radar sensor to a coordinate system of the odometer sensor, transforming a reference coordinate system of second point cloud data from the coordinate system of the follower mobile robot laser radar sensor to the coordinate system of the odometer sensor by using coordinate transformation, and generating third point cloud data;

Step 2.3, dividing the third point cloud data by using a Euclidean clustering method to obtain fourth point cloud data containing K clusters; performing intermediate segmentation and segmentation center point calculation of the point cloud on each cluster in the fourth point cloud data to obtain two subsets after segmentation of each cluster and center point coordinates of each subset, wherein K is a natural number;

storing all the coordinates of the central point into an array to obtain expected point cloud data;

In the step 2.2, the calculation formula of the transformation matrix and the acquisition process of the third point cloud data are as follows:

Wherein R is a rotation matrix and represents the attitude information of a body coordinate system of the follower mobile robot relative to a coordinate system of an odometer sensor; b is a translation vector, which represents the position information of the body coordinate system of the follower mobile robot relative to the coordinate system of the odometer sensor, and the parameters (x, y, z) represent the three-dimensional coordinates of the position information of the follower mobile robot;

Combining the rotation matrix R and the translation vector B to form a4 multiplied by 4 transformation matrix T ₁, and representing pose information of a body coordinate system of the follower mobile robot relative to a coordinate system of an odometer sensor; wherein 0 is a row vector of 1×3, and the parameters [ a, b, c, d ] ^T are quaternion representations of the rotation matrix R for describing the pose of the follower mobile robot;

obtaining transformation information between rigid bodies of the follower mobile robot to obtain a transformation matrix from a laser radar sensor coordinate system of the follower mobile robot to a body coordinate system of the follower mobile robot, wherein the transformation matrix is a constant matrix and is marked as T ₂;

transforming a reference coordinate system of the second point cloud data from a follower mobile robot laser radar sensor coordinate system to an odometer sensor coordinate system by using coordinate transformation, and generating third point cloud data:

P₃＝(T₁×T₂)·P₂ (2)

Wherein P ₂ is the second point cloud data, and P ₃ is the third point cloud data;

in the step 2.3, the coordinates of the central points of the subsets are shown in the following formula:

Wherein Z _i1 and Z _i2 are center point coordinates of two subsets of i-th cluster point cloud data, i=1, 2,., K; n is the length of the ith cluster point cloud data and the symbol Representing round-down, x _j and y _j are the abscissas of the point cloud data, j=1, 2,..n;

Step 3, eliminating expected point cloud data in a history obstacle area to obtain effective point cloud data, and distinguishing the pilot mobile robot point cloud data and the obstacle point cloud data by the follower mobile robot based on the pilot mobile robot history pose state and by utilizing a relative motion principle or a spatial position continuity principle;

in the step 3, the process of excluding the desired point cloud data located in the history obstacle region is as follows:

based on the expected point cloud data obtained in the step 2, calculating the center point coordinates of each cluster of the expected point cloud data:

wherein ^oM_i is the center point coordinate of each cluster of the point cloud data, and represents the center point coordinate of each cluster of the point cloud data under the coordinate system of the mobile robot odometer sensor of the follower, wherein i=1, 2;

Z _{i_1} and Z _{i_2} are center point coordinates of two subsets of i-th cluster point cloud data;

Given ^oM_i, namely the center point coordinates of each cluster of the expected point cloud data and the historical obstacle region linked LIST, the specific process of excluding the clusters in the historical obstacle region of the expected point cloud data is as follows:

For each obstacle region in LIST:

Obstacle center point_x=x coordinate of obstacle region center point;

Obstacle center point_y=the y coordinate of the obstacle region center point;

barrier length = length of barrier region;

Barrier width = width of barrier region;

calculating rectangular boundaries:

right boundary= (obstacle center point_y-obstacle width)/2;

left boundary= (obstacle center point_y+obstacle width)/2;

Lower boundary= (obstacle center point_x-obstacle length)/2;

upper boundary= (obstacle center point_x+obstacle length)/2;

If ^oM_i is located within the rectangular range enclosed by the four boundary values, the expected point cloud data is in the historical obstacle region, otherwise, the expected point cloud data is not in the historical obstacle region;

the rest expected point cloud data clusters are sequentially stored in an array according to the original sequence to form effective point cloud data;

The step3 specifically comprises the following steps:

Aiming at the positioning problem of the pilot mobile robot in the initial stage, the tracking positioning problem of the pilot mobile robot in the normal operation stage and the repositioning problem of the pilot mobile robot in the tracking loss stage, three different positioning methods of initial positioning, tracking positioning and repositioning are respectively designed, and corresponding methods are respectively operated in different stages;

the initial positioning method comprises the following steps:

when the number of times of the program continuously entering the initial positioning method is less than C, recording effective point cloud data, wherein C is a natural number;

When the number of times of the program continuously entering the initial positioning method is equal to C, recording effective point cloud data, and calculating the average value of the recorded C groups of effective point cloud data, namely the initial point cloud position;

When the number of times of continuous program entering into the initial positioning method is not less than C, based on the initial position of the point cloud, when the pilot mobile robot translates or rotates, distinguishing pilot mobile robot point cloud data from the effective point cloud data by utilizing a relative motion principle;

the tracking and positioning method comprises the following steps:

Acquiring a historical pose state variable of a pilot mobile robot, taking a central point of the pilot mobile robot in the historical pose state variable of the pilot mobile robot as a circle center, making a circular area with a radius of R, and considering the circular area as a circular boundary condition, wherein the effective point cloud data appearing in the circular boundary condition at the current moment is considered as pilot mobile robot point cloud data, so that the pilot mobile robot point cloud data is distinguished from the effective point cloud data;

the repositioning method comprises the following steps:

Recording effective point cloud data when the number of times of the program continuously entering the repositioning method is not more than C, and calculating the average value of the recorded effective point cloud data, namely the real-time initial position of the point cloud;

When the program enters a repositioning method, based on the real-time initial position of the point cloud, when the navigator mobile robot translates or rotates, distinguishing navigator mobile robot point cloud data from effective point cloud data by utilizing a relative motion principle;

Step 4, if the step 3 cannot successfully identify the point cloud data of the mobile robot of the navigator, emptying the historical pose state variables of the mobile robot of the navigator, and returning to the step 2; if the step 3 successfully identifies the mobile robot point cloud data of the navigator, the step 5 is continuously executed;

Step 5, calculating the central position and azimuth angle of the pilot mobile robot based on the pilot mobile robot point cloud data obtained in the step 3 by utilizing a plane geometric relationship, updating the historical pose state of the pilot mobile robot, and then carrying out coordinate transformation on the central position and azimuth angle of the pilot mobile robot by using the follower mobile robot odometer sensor data to obtain the pose of the pilot mobile robot relative to the follower mobile robot, so as to realize the tracking of the pilot mobile robot point cloud by the follower mobile robot;

Step 6, calculating the center position and the size range of the obstacle based on the obstacle point cloud data obtained in the step 3, designating the size of a buffer area to obtain an obstacle area, adding the obstacle area into a historical obstacle area, and returning to the step 2;

Wherein, the follower mobile robot and the navigator mobile robot are both carried with a laser radar and an odometer sensor.

2. The method for controlling tracking of a mobile robot based on a lidar and an odometer according to claim 1, wherein in step 1, the obstacle area is defined as a rectangular area;

The data structure for describing the obstacle region comprises four double-precision floating point numbers which sequentially represent the abscissa and the ordinate of the center point of the obstacle region, the length of the obstacle region and the width of the obstacle region;

the data structure for describing the historical pose state of the pilot mobile robot comprises three double-precision floating point numbers which sequentially represent the abscissa and the ordinate of the center point of the pilot mobile robot and the direction angle of the pilot mobile robot.

3. The mobile robot tracking control method based on the laser radar and the odometer according to claim 1, wherein the step 5 is specifically:

Step 5.1, calculating the coordinates of a point cloud data center point of the mobile robot of the navigator by using an average algorithm based on the point cloud data of the mobile robot of the navigator obtained in the step 3, and obtaining the position of the center point of the mobile robot of the navigator, namely the center position, according to the fixed position relation between the point cloud data center point of the mobile robot of the navigator and the center point of the mobile robot of the navigator;

Step 5.2, based on the pilot mobile robot point cloud data obtained in the step 3, connecting the pilot mobile robot point cloud data into a line segment, and calculating an included angle between the line segment and the positive direction of the horizontal axis of the odometer sensor coordinate system to obtain the angle, namely the azimuth angle, of the pilot mobile robot;

Step 5.3, updating the historical pose state variable of the mobile robot of the pilot by using the central position obtained in the step 5.1 and the azimuth information obtained in the step 5.2;

Step 5.4, acquiring sensor data of an odometer of the mobile robot of the follower and transformation information between rigid bodies of the mobile robot of the follower, and calculating to obtain a transformation matrix from an odometer sensor coordinate system to a chassis coordinate system of the mobile robot of the follower;

Step 5.5, using the transformation matrix obtained in the step 5.4, transforming the central position obtained in the step 5.1 and the reference coordinate system of the azimuth angle obtained in the step 5.2 from the odometer sensor coordinate system to the chassis coordinate system of the follower mobile robot, and obtaining the pose of the navigator mobile robot relative to the follower mobile robot;

Therefore, the tracking of the mobile robot point cloud of the navigator by the mobile robot of the follower is realized.

4. The method for tracking and controlling a mobile robot based on a laser radar and an odometer according to claim 3, wherein in the step 5.4, the odometer sensor data of the mobile robot of the follower is obtained, the rotation matrix and the transformation matrix from the body coordinate system of the mobile robot of the follower to the odometer sensor coordinate system are calculated, and the rotation matrix and the transformation matrix from the odometer sensor coordinate system to the body coordinate system of the mobile robot of the follower are obtained by inverting the rotation matrix and the transformation matrix respectively, which are respectively denoted as R ^-1 and T ₃;

Obtaining transformation information between rigid bodies of the follower mobile robot, and obtaining a transformation matrix from a body coordinate system of the follower mobile robot to a chassis coordinate system of the follower mobile robot, wherein the transformation matrix is a constant matrix and is marked as T ₄;

In the step 5.5, the calculation formula of the relative position of the pilot mobile robot with respect to the follower mobile robot is as follows:

^fM_l＝(T₃×T₄)·^oM_l (5)

Wherein ^fM_l is a relative position, which represents the coordinates of the center point of the navigator mobile robot on the chassis coordinate system of the follower mobile robot; ^oM_l The center position obtained in the step 5.1;

Transforming a reference coordinate system of the azimuth angle from an odometer sensor coordinate system to a follower mobile robot chassis coordinate system by using a rotation matrix to obtain an angle of the navigator mobile robot relative to the follower mobile robot:

Wherein ^fR_l is a rotation matrix, which represents the rotation relation from the forward direction of the horizontal axis of the chassis coordinate system of the mobile robot to the forward direction of the mobile robot of the navigator; ^oθ_l The azimuth angle obtained in the step 5.2;

Extracting an included angle ^fθ_l between the forward direction of the navigator mobile robot and the forward direction of the horizontal axis of the mobile robot chassis coordinate system from the rotation matrix ^fR_l;

thus, the pose of the navigator mobile robot relative to the follower mobile robot is obtained.

5. The mobile robot tracking control method based on the laser radar and the odometer according to claim 1, wherein the step 6 is specifically:

based on the obstacle point cloud data obtained in the step 3, a rectangular area which can surround all obstacle point cloud data is calculated by utilizing a plane geometric relationship, and a specific calculation formula is as follows:

Wherein Z _{j_mid} is the center point coordinate of the obstacle point cloud data;

And (3) with For the center point coordinates of two subsets of obstacle point cloud data, l _j is the rectangular area length, w _j is the rectangular area width, where j=1, 2,..;

Creating a buffer area around the rectangular area in consideration of the circumscribed circle radius and the minimum turning radius of the mobile robot, designating the size of the buffer area, and naming the rectangular area with the buffer area as an obstacle area;

The coordinates, length and width of the center point of the obstacle region are calculated as follows:

Where M _j represents the center point coordinates of the obstacle region, length _j represents the length of the obstacle region, width _j represents the width of the obstacle region, buffer represents the designated buffer region size, where j=1, 2, K-1;

The coordinates of the center point, the length and the width of the obstacle area are stored in the data structure for describing the obstacle area, then added to the historical obstacle area linked list, and the step returns to the step 2.