Robot high-precision positioning control method and system based on point cloud road sign

Abstract

The invention relates to a robot high-precision positioning control method and system based on point cloud road signs. The method comprises the following steps that a standard point cloud set of the point cloud road signs arranged on preset point locations is acquired; the method comprises the steps of obtaining laser radar scanning data, converting the data into two-dimensional laser point cloud data, filtering out abnormal points, detecting jump points, obtaining a plurality of point cloud cluster sets, and obtaining a candidate point cloud cluster set through screening; matching the candidate point cloud cluster with the standard point cloud by using an ICP algorithm, and performing matching degree evaluation to obtain a correct matching result of the point cloud road sign of the current frame so as to obtain an accurate real-time pose of the robot; if the point cloud road sign matching of the current frame fails, tracking and updating the accurate real-time pose of the robot by using a wheel type odometer; and according to the accurate real-time pose, the robot is controlled to move to a target point through a state machine in combination with a PID motion control method. Compared with the prior art, high-precision positioning and control of the robot are achieved, and numerous special operation requirements are met.

Description

Technical Field

[0001] The present invention relates to the field of laser point cloud and mobile robot, and in particular to a high-precision positioning control method and system of a robot based on point cloud landmarks. Background Art

[0002] In order to achieve the autonomy and intelligence of mobile robots, mobile robots must have good perception modeling and autonomous control capabilities in any complex dynamic scenarios where they need to be deployed. Among them, the positioning accuracy and control accuracy that the robot can achieve are the key indicators that determine its performance.

[0003] In recent years, algorithms such as EKF and AMCL have become the mainstream positioning technologies for mobile robots that use lidar as the main sensor. These algorithms can meet the real-time and robust posture tracking of robots and can achieve centimeter-level positioning accuracy in general environments. Although this accuracy can meet the requirements of mobile robots to perform most tasks, in some key special mobile tasks, robots often must have more high-precision autonomous positioning capabilities. For example, in autonomous charging tasks, robots need to accurately dock with charging piles to achieve truly unmanned autonomous and continuous operation; in intelligent pickup tasks, robots need to accurately dock next to lockers to complete autonomous pickup; in some workstation tasks that require high-precision operations, robots need to accurately reach the specified posture and complete the corresponding task operations.

[0004] Therefore, the millimeter-level autonomous positioning and control capability with higher precision is an irreplaceable key function of mobile robots. Among the current related technologies, vision-based methods such as QR code recognition are often easily affected by light, resulting in unstable positioning results; while laser-based methods mostly only recognize the shape of fixed charging piles for docking. The auxiliary shape of such methods cannot be adjusted, which has a high degree of constraint on the robot's position and mission purpose, and does not have the flexible application capability of high-precision positioning and control technology. Summary of the invention

[0005] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and to provide a robot high-precision positioning control method and system based on point cloud landmarks, so as to achieve high-precision positioning and control of the robot and meet many special operation requirements.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] A robot high-precision positioning control method based on point cloud landmarks includes the following steps:

[0008] Acquire a set of corner points of point cloud landmarks arranged at preset points as input of a standard point cloud generation function to obtain a standard point cloud set, wherein the point cloud landmarks are of special geometric shapes;

[0009] Acquire laser radar scanning data, convert the laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filter out abnormal points in the two-dimensional laser point cloud data of the current frame; the laser radar scanning data is obtained by sensing the environment through a single-line laser radar;

[0010] Detecting a jump point in the two-dimensional laser point cloud data of the current frame, segmenting the two-dimensional laser point cloud data using the jump point as an endpoint to obtain a plurality of point cloud cluster sets, filtering out invalid point cloud clusters in the point cloud cluster sets by screening, and retaining potential candidate point cloud clusters to obtain a candidate point cloud cluster set;

[0011] The candidate point cloud clusters in the candidate point cloud cluster set are matched with the standard point clouds in the standard point cloud set by using the ICP algorithm, the matching degree of all successfully matched point cloud pairs is evaluated according to the scoring function, the point cloud pair with the highest score is selected as the correct matching result of the point cloud landmark in the current frame, and the accurate real-time posture of the robot is obtained according to the posture transformation relationship between the correct matching result and the point cloud landmark;

[0012] If the point cloud landmark matching in the current frame fails, the wheel odometer is used to track and update the accurate real-time position of the robot;

[0013] According to the accurate real-time posture, the robot is controlled to move to the target point through a state machine combined with a PID motion control method.

[0014] Furthermore, the corner point set is:

[0015] corner={c 1 , …, c k}

[0016] In the formula, corner is the set of corner points, c k is the corner point;

[0017] The standard point cloud set is:

[0018]

[0019] In the formula, PC ref is the standard point cloud set, res is the interpolation resolution, and j is the standard point cloud interpolation step.

[0020] Furthermore, the step of filtering out abnormal points includes:

[0021] Traversing each point in the two-dimensional laser point cloud data of the current frame to obtain three relative distances between each point and its adjacent points;

[0022] The outliers are obtained by measuring the relationship between the three relative distances and the outlier threshold, and the outliers are taken as abnormal points, and the abnormal points are filtered out by mean filtering, as shown in the following formula:

[0023]

[0024] In the formula, pt′ i For pt i is a point in the two-dimensional laser point cloud data, l 1 , l 2 , l 3 is the relative distance of the three items, l out is the outlier threshold;

[0025] The expressions of the three relative distances are as follows:

[0026] l 1 =||pt i+1 -pt i || 2

[0027] l 2 =||pt i+1 -pt i-1 || 2

[0028] l 3 =||pt i+2 -pt i+1 || 2

[0029] In the formula, pt i+1 For pt i The next adjacent point, pt i-1 For pt i The previous adjacent point, pt i+2 For pt i+1 The next adjacent point of .

[0030] Furthermore, the jump point extraction process includes: traversing each point in the two-dimensional laser point cloud data after filtering out the abnormal point, calculating the relative distance between each point and its next adjacent point, and if the relative distance is greater than the jump threshold, the current point is the jump point.

[0031] Furthermore, the ICP algorithm uses the pose obtained according to the point cloud data of the previous frame as the initial estimated pose of the current frame, and classifies the number of iterations to accelerate the calculation. The number of iterations is shown in the following formula:

[0032]

[0033] Among them, n iteris the number of iterations, n max is the maximum number of iterations, and the candidate point cloud cluster is matched with the standard point cloud by ICP. If the iteration converges, the match is considered successful, and the pose transformation relationship between the point cloud pairs is obtained.

[0034] Furthermore, the score function is:

[0035]

[0036] In the formula, score i is the score function, ef i is the nearest neighbor error between the candidate point cloud cluster after pose transformation and the standard point cloud, er i is the nearest neighbor error between the standard point cloud and the candidate point cloud cluster after inverse pose transformation, is the pose rotation angle between the point cloud pairs, Δd i is the length difference between the point cloud cluster and the standard point cloud.

[0037] Furthermore, the nearest neighbor error between the candidate point cloud cluster after the pose transformation and the standard point cloud is:

[0038]

[0039] Where n is the number of laser points in the candidate point cloud cluster, p k For each point in the candidate point cloud cluster after pose transformation, q k is the midpoint p of the standard point cloud k The nearest neighbor of ;

[0040] The nearest neighbor error between the standard point cloud and the candidate point cloud cluster after the inverse transformation of the pose is:

[0041]

[0042] Where m is the number of laser points in the standard point cloud, p l For each point in the standard point cloud after inverse transformation of pose, q l is the point p in the candidate point cloud cluster l The nearest neighbor of .

[0043] Furthermore, the specific calculation steps of the accurate real-time posture include:

[0044] If the point cloud landmark in the current frame is matched successfully, the pose transformation matrix from the robot to the point cloud landmark is calculated synchronously by the ICP algorithm, and the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system is obtained by accurate measurement, and then the transformation matrix of the robot coordinate system relative to the world coordinate system is obtained, as shown in the following formula:

[0045]

[0046] In the formula, is the transformation matrix of the robot coordinate system relative to the world coordinate system, T l w is the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system, is the pose transformation matrix from the robot to the point cloud landmark;

[0047] The calculation formula of the accurate real-time posture is as follows:

[0048]

[0049] In the formula, p w* is the element value at the row and column corresponding to the position of the robot in the world coordinate system, is the transformation matrix of the robot coordinate system relative to the world coordinate system The position of the robot in the world coordinate system corresponding to the element values ​​at the rows and columns is as follows:

[0050]

[0051] In the formula, p w is the position of the robot in the world coordinate system.

[0052] Furthermore, the motion state of the state machine includes an in-situ rotation state, an arc-shaped movement state and a linear movement state;

[0053] In the in-situ rotating state, the robot selects a rotation direction that consumes less time according to the difference between the desired orientation and the actual orientation, and rotates to the desired orientation at the maximum rotation speed;

[0054] In the arc-shaped moving state, the direction is used as the controlled variable of the PID controller, the difference between the expected direction and the actual direction is used as the input error, the angular velocity of the mobile robot is calculated using the PID control law, and the output linear velocity of the mobile robot is obtained according to the angular velocity;

[0055] In the linear movement state, when the distance between the robot and the target point is less than a set value, the robot docks with the target point using a gradually decaying linear speed.

[0056] According to another aspect of the present invention, a robot high-precision positioning control system based on point cloud landmarks is provided, comprising:

[0057] A standard point cloud set acquisition module is used to acquire a set of corner points of point cloud landmarks arranged at preset points as an input of a standard point cloud generation function to obtain a standard point cloud set;

[0058] An abnormal point filtering module is used to obtain laser radar scanning data, convert the obtained laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filter out abnormal points in the two-dimensional laser point cloud data of the current frame;

[0059] A jump point detection module is used to detect the jump points in the two-dimensional laser point cloud data of the current frame, and use the jump points as endpoints to segment the two-dimensional laser point cloud data to obtain a plurality of point cloud cluster sets, and filter out invalid point cloud clusters in the point cloud cluster sets by screening, and retain potential candidate point cloud clusters to obtain a candidate point cloud cluster set;

[0060] The accurate real-time pose acquisition module uses the ICP algorithm to match the candidate point cloud clusters in the candidate point cloud cluster set with the standard point cloud in the standard point cloud set, evaluates the matching degree of all successfully matched point cloud pairs according to the scoring function, selects the point cloud pair with the highest score as the correct matching result of the point cloud landmark in the current frame, and obtains the accurate real-time pose of the robot according to the pose transformation relationship between the correct matching result and the point cloud landmark; if the point cloud landmark in the current frame fails to match, the wheel odometer is used to track and update the accurate real-time pose of the robot;

[0061] The mobile control module is used to control the robot to move to a target point through a state machine combined with a PID motion control method according to the accurate real-time posture.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] 1. The present invention uses point cloud landmarks of special geometric shapes for auxiliary positioning, generates a standard point cloud set based on the point cloud landmarks, uses laser radar for environmental perception, and sequentially performs abnormal point filtering and jump point detection on the current frame laser point cloud data, and designs screening conditions to extract a number of potential candidate point cloud clusters to obtain a candidate point cloud cluster set; uses the ICP algorithm to match each candidate point cloud cluster with the standard point cloud to obtain the correct matching result of the current frame point cloud landmark, and then obtains the accurate real-time posture of the robot; uses a state machine combined with a PID motion control method to achieve millimeter-level high-precision positioning and control of the robot in key operating areas, providing a guarantee for the mobile robot to complete high-precision operating tasks.

[0064] 2. The present invention combines laser data information and the speed information of the wheel odometer to perform robot positioning, avoiding the accumulation and increase of detection errors, and can obtain a more accurate robot posture. The combination of the two data allows the robot to continue to position and control even when one of the information is lost, greatly increasing the stability of the control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1A schematic flow chart of a robot high-precision positioning control method based on point cloud landmarks proposed by the present invention;

[0066] Figure 2 This is a schematic diagram of the appearance of a V-shaped point cloud road sign;

[0067] Figure 3 This is a schematic diagram of the candidate point cloud cluster extraction results for a frame of laser point cloud data;

[0068] Figure 4 This is a schematic diagram of the robot motion control process;

[0069] Figure 5 This is the robot angular velocity PID control flow chart;

[0070] Figure 6 Schematic diagram of the robot's motion path. DETAILED DESCRIPTION

[0071] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0072] Abbreviations involved:

[0073] Iterative Closest Point, ICP

[0074] Proportional Integral Derivative: Proportional Integral Derivative, PID

[0075] Example 1

[0076] This embodiment provides a robot high-precision positioning control method based on point cloud landmarks, such as Figure 1 As shown, the following steps are included:

[0077] S1. Obtain a set of corner points of point cloud landmarks arranged at preset points as input of a standard point cloud generation function to obtain a standard point cloud set.

[0078] Any variety of point cloud landmarks can be arranged in the robot's operating environment, but only one of each type of landmark can be arranged. The set point position needs to be accurately measured in advance to obtain the position transformation relationship between the point position and the robot's map coordinate system or the task target point coordinate system.

[0079] The special geometric shape is obviously different from the general shape of objects in the robot's environment. In this embodiment, a V-shaped object with a fixed angle is selected as a landmark, such as Figure 2As shown. The signpost is 20 cm long, 5 cm wide, with a V-shaped angle of 126.9°, and its height is at the same level as the robot's laser radar. In this embodiment, the V-shaped signpost is set 0.5 m in front of the robot as the working point that the robot needs to reach accurately, and a rangefinder is used to accurately measure, place and fix the V-shaped signpost, thereby ensuring the positioning accuracy of the robot.

[0080] The standard point cloud generation function takes the corner point set of the point cloud landmark as input and fits an accurate standard point cloud through linear interpolation. In this embodiment, the interpolation resolution is 1mm. The standard point cloud set stores both the point cloud set of each shape and the position and posture of the shape in the environment.

[0081] The set of corner points of the point cloud landmark is:

[0082] corner={c 1 , …, c k}

[0083] In the formula, corner is the set of corner points, c k is the corner point;

[0084] The standard point cloud collection is:

[0085]

[0086] In the formula, PC ref is the standard point cloud set, res is the interpolation resolution, and j is the standard point cloud interpolation step.

[0087] S2. Obtain laser radar scanning data, convert the laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filter out abnormal points in the two-dimensional laser point cloud data of the current frame.

[0088] The point cloud conversion function first deletes all out-of-limit and invalid values ​​in the current frame laser radar scan data to obtain a valid laser data set, and then uses the following formula for conversion:

[0089] PC in ={(r i cos(σ start +iΔσ), r i sin(σ start +iΔσ))|i∈[0,n]}

[0090] In the formula, PC in is the converted two-dimensional laser point cloud data, σ start is the scanning starting angle of the laser radar, Δσ is the angular resolution of the laser radar, and the angular resolution Δσ of the laser radar used in this embodiment is 0.5°.

[0091] When filtering out abnormal points, traverse each point in the point cloud and measure the three relative distances l 1 , l 2 , l 3 and the outlier threshold l out The relationship between them is that all outliers are found to be abnormal points, as shown in the following formula:

[0092]

[0093] In the formula, pt′ i For pt i is a point in the two-dimensional laser point cloud data, l 1 , l 2 , l 3 is the relative distance of the three items, l out is the outlier threshold; in this embodiment, out The value of is 4cm, and all detected abnormal points are filtered out by mean filtering to obtain an input point cloud without interference noise.

[0094] The expressions of the three relative distances are as follows:

[0095] l 1 =||pt i+1 -pt i || 2

[0096] l 2 =||pt i+1 -pt i-1 || 2

[0097] l 3 =||pt i+2 -pt i+1 || 2

[0098] In the formula, pt i+1 For pt i The next adjacent point, pt i-1 For pt i The previous adjacent point, pt i+2 For pt i+1 The next adjacent point of .

[0099] S3. Detect the jump points in the two-dimensional laser point cloud data of the current frame, and use the jump points as endpoints to segment the two-dimensional laser point cloud data to obtain several point cloud cluster sets. Filter out invalid point cloud clusters in the point cloud cluster sets by screening, and retain potential candidate point cloud clusters to obtain candidate point cloud cluster sets.

[0100] The process of extracting the jump point includes: traversing each point in the two-dimensional laser point cloud data after the abnormal points are filtered out, calculating the relative distance between each point and its next adjacent point, and if the relative distance is greater than the jump threshold, the current point is the jump point. The calculation expression of the relative distance is:

[0101] l′ 1 =||pt′ i+1 -pt′ i || 2

[0102] Where l′1 is the relative distance between adjacent points.

[0103] In this embodiment, jump The value of is 3cm. Taking all the jump points as endpoints, the point cloud can be divided into several point cloud cluster sets.

[0104] The screening conditions comprehensively consider the length limit of the clustered point cloud cluster and the distance limit from the clustered point cloud cluster to the laser radar. In this embodiment, the length of the standard point cloud is 20cm. If the length of the point cloud cluster differs from it by more than 10cm, then the point cloud cluster will not be a potential candidate point cloud; if the distance from the point cloud cluster to the laser radar is less than 5cm or greater than 1.5m, then the point cloud cluster will not be considered either, because when the distance is too close, the laser radar cannot scan and perceive the entire object, and when the distance is too far, the laser point cloud is relatively sparse and cannot be accurately perceived. After screening, the invalid point cloud clusters in the original point cloud cluster set are deleted, and the potential candidate point cloud clusters are retained to form a new candidate point cloud cluster set.

[0105] The candidate point cloud cluster extraction results in this embodiment are as follows Figure 3 As shown in Figure 2, a total of 6 candidate point cloud clusters are extracted. At the same time, the correct point cloud landmark point cloud cluster pcs 4 It is successfully segmented and included in the candidate point cloud cluster set.

[0106] S4. Use the ICP algorithm to match the candidate point cloud clusters in the candidate point cloud cluster set with the standard point cloud in the standard point cloud set, evaluate the matching degree of all successfully matched point cloud pairs according to the scoring function, select the point cloud pair with the highest score as the correct matching result of the current frame point cloud landmark, and obtain the accurate real-time posture of the robot based on the correct matching result and the posture transformation relationship of the point cloud landmark.

[0107] The ICP algorithm is an iterative closest point algorithm. The ICP algorithm uses the pose obtained based on the point cloud data of the previous frame as the initial estimated pose of the current frame, and classifies the number of iterations to speed up the calculation. The number of iterations is shown in the following formula:

[0108]

[0109] Among them, n iteris the number of iterations, n max is the maximum number of iterations, and the candidate point cloud cluster is ICP matched with the standard point cloud. A larger number of iterations is used when the ICP matching of the previous frame fails, and a smaller number of iterations is used when the ICP matching of the previous frame succeeds. In this embodiment, n max The value is 10. Use each candidate point cloud cluster in the point cloud cluster set to perform ICP matching with the standard point cloud. If the iterative result converges, the match is considered successful, and the pose transformation relationship T between the point cloud pairs is obtained. i .

[0110] The score function is determined by the following four values: (1) the nearest neighbor error ef between the transformed candidate point cloud cluster and the standard point cloud i (2) The nearest neighbor error er between the inverse transformed standard point cloud and the candidate point cloud cluster i (3) Pose rotation angle between point cloud pairs (4) Point cloud cluster pcs i The difference between the length and the standard length (20cm) is Δd i .

[0111] Combining the above four parts, the score function score i The expression is as follows:

[0112]

[0113] In the formula, score i is the score function, ef i is the nearest neighbor error between the candidate point cloud cluster after pose transformation and the standard point cloud, er i is the nearest neighbor error between the standard point cloud and the candidate point cloud cluster after inverse pose transformation, is the pose rotation angle between the point cloud pairs, Δd i is the length difference between the point cloud cluster and the standard point cloud.

[0114] The nearest neighbor error between the candidate point cloud cluster after pose transformation and the standard point cloud is:

[0115]

[0116] Where n is the number of laser points in the candidate point cloud cluster, p k For each point in the candidate point cloud cluster after pose transformation, q k is the midpoint p of the standard point cloud k The nearest neighbor of ;

[0117] The nearest neighbor error between the standard point cloud and the candidate point cloud cluster after inverse pose transformation is:

[0118]

[0119] Where m is the number of laser points in the standard point cloud, p l For each point in the standard point cloud after inverse transformation of pose, q l is the point p in the candidate point cloud cluster l The nearest neighbor of .

[0120] The scores of all successfully matched point cloud pairs are calculated according to the scoring function. Figure 3 The scores of the candidate point cloud clusters in are shown in Table 1:

[0121] Table 1 Matching score table of candidate point cloud clusters

[0122]

[0123]

[0124] The highest score is selected from all matching scores and compared with the score threshold. The score threshold in this embodiment is set to 60. As can be seen from Table 1, the candidate point cloud cluster pcs 4 The matching score of is the highest and exceeds the score threshold, so the matching of the point cloud landmark is successful.

[0125] After confirming the pcs 4 After the correct point cloud landmark is obtained, the pose transformation relationship between the robot and the point cloud landmark is shown. It has been calculated synchronously by the ICP algorithm. In addition, the accurate pose relationship of the point cloud landmarks in the world coordinate system is fixed. l w It has been accurately measured, so the accurate real-time position and posture of the robot can be obtained.

[0126] The specific calculation steps for accurate real-time pose include:

[0127] If the point cloud landmark of the current frame is matched successfully, the pose transformation matrix from the robot to the point cloud landmark is calculated synchronously by the ICP algorithm, and the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system is obtained by accurate measurement, and then the transformation matrix of the robot coordinate system relative to the world coordinate system is obtained, as shown in the following formula:

[0128]

[0129] In the formula, is the transformation matrix of the robot coordinate system relative to the world coordinate system, is the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system, is the pose transformation matrix from the robot to the point cloud landmark;

[0130] The calculation formula for accurate real-time pose is as follows:

[0131]

[0132] In the formula, p w* is the element value at the row and column corresponding to the position of the robot in the world coordinate system, is the transformation matrix of the robot coordinate system relative to the world coordinate system The position of the robot in the world coordinate system is as follows for the element values ​​at the row and column:

[0133]

[0134] In the formula, p w is the position of the robot in the world coordinate system.

[0135] S5. If the current frame point cloud landmark matching fails, the wheel odometer is used to track and update the accurate real-time position of the robot.

[0136] If the detection and matching of the point cloud landmark in the current frame fails, the wheel odometer is used to track and update the robot's posture until the next new laser point cloud is successfully matched to the point cloud landmark.

[0137] Point cloud landmark matching fails when one of the following conditions exists: (1) No valid candidate point cloud cluster is segmented and retained, that is, the candidate point cloud cluster set is empty. (2) All ICP matching results have not converged successfully. (3) The matching scores of all matching point cloud pairs are i None exceeded the scoring threshold.

[0138] In this embodiment, the encoder data of the mobile robot is used as the wheel odometer information, and the robot posture at the current moment is estimated and updated according to the robot posture result at the previous moment and the current wheel odometer speed value.

[0139] S6. According to the accurate real-time posture, the robot is controlled to move to the target point through the state machine combined with the PID motion control method.

[0140] The robot motion control state machine includes three defined states: rotation in place, arc movement, and linear movement. After the robot receives the command to go to the specified target point, the state machine first enters the rotation in place state to control the robot to face the desired direction; after the orientation error is less than the threshold, the state machine enters the arc movement state to control the robot to reach a temporary point a certain distance in front of the target point with an approximate arc trajectory to ensure the smoothness and fluency of the robot's movement; after reaching the temporary point, the state machine enters the linear movement state to control the robot to go straight to the target point. This state is conducive to more accurate estimation of the posture and ensure the end positioning accuracy of the robot.

[0141] When rotating in place, the robot chooses the direction that takes less time based on the difference between the expected direction and the actual direction, and rotates at the maximum rotation speed ω max Rotate to the desired orientation. In this embodiment, ω max The value is lrad / s.

[0142] PID motion control is used when the robot moves in an arc shape. The control process is as follows: Figure 4 As shown. Based on the robot’s accurate real-time position (x t ,y t ,θ u，t ), select the orientation of the mobile robot as the controlled variable, design and calculate the expected orientation at each moment, use the difference between the expected orientation and the actual orientation as the input error, and use the PID method to control the angular velocity of the mobile robot, such as Figure 5 As shown, the linear velocity is calculated based on the angular velocity. The specific steps include:

[0143] In order to make the robot reach the target point coherently and directly, the expected orientation at each time t needs to be larger than the angle between the robot and the target point. Therefore, the expected orientation at each time is set to a times the angle. The designed expected orientation expression is as follows:

[0144]

[0145] In the formula, is the angle from the robot to the target point, θ r，t is the desired orientation, and in this embodiment, a=1.5 is taken. Input error θ is defined as e，t is the desired direction θ r，t and the actual orientation θ u，t The difference between them is expressed as:

[0146] θ e，t =θ r，t -θ u，t

[0147] In the formula, θ e，t is the input error, θ r，t is the desired orientation, θ u，t The actual direction.

[0148] The PID method is used to control the angular velocity of the mobile robot. The control law of the PID method to control the angular velocity of the mobile robot is:

[0149]

[0150] In the formula, θ e，t is the input error. In this embodiment, K is set p =1.5, K i =0, Kd =0.5.

[0151] Calculate the output linear velocity of the mobile robot based on the robot's angular velocity. Design a piecewise function and give a piecewise threshold. The linear velocity should decrease as the absolute value of the angular velocity increases, and the minimum value should not be less than 0, and the maximum value should not exceed the given maximum linear velocity. The formula for calculating the output linear velocity is:

[0152]

[0153] In the formula, w t is the robot angular velocity, v t is the output linear velocity, σ is the segmentation threshold, v max is the maximum linear velocity. In this embodiment, σ is set to 0.2 m / s.

[0154] In the straight line moving state, when the distance between the robot and the target point is less than ∈ When the robot is connected to the target point, a gradually decaying linear velocity is used to ensure the end positioning accuracy of the robot. The calculation formula for the gradually decaying linear velocity is:

[0155]

[0156] In the formula, l norm is the distance between the robot and the target point, v t is the linear velocity, v max is the maximum linear velocity, and in this embodiment, ∈ is set to 0.1m.

[0157] By combining the state machine in this embodiment with the PID motion control method, the robot can quickly move to the target point along a smooth trajectory. The robot's motion trajectory is as follows: Figure 6 shown.

[0158] Example 2

[0159] This embodiment provides a robot high-precision positioning control system based on point cloud landmarks, including:

[0160] The standard point cloud collection module is used to design point cloud landmarks of various special geometric shapes, and arrange the point cloud landmarks at preset points. The corner point set of the point cloud landmarks is used as the input of the standard point cloud generation function to obtain a standard point cloud collection.

[0161] The abnormal point filtering module senses the environment through a single-line laser radar, obtains laser radar scanning data, converts the laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filters out abnormal points in the two-dimensional laser point cloud data of the current frame;

[0162] The jump point detection module detects the jump points in the two-dimensional laser point cloud data of the current frame, and uses the jump points as endpoints to segment the two-dimensional laser point cloud data to obtain a number of point cloud cluster sets. The invalid point cloud clusters in the point cloud cluster set are filtered out by screening, and potential candidate point cloud clusters are retained to obtain a candidate point cloud cluster set.

[0163] The accurate real-time pose acquisition module uses the ICP algorithm to match the candidate point cloud clusters in the candidate point cloud cluster set with the standard point cloud in the standard point cloud set, evaluates the matching degree of all successfully matched point cloud pairs according to the scoring function, selects the point cloud pair with the highest score as the correct matching result of the current frame point cloud landmark, and obtains the accurate real-time pose of the robot based on the pose transformation relationship between the correct matching result and the point cloud landmark; if the current frame point cloud landmark fails to match, the wheel odometer is used to track and update the accurate real-time pose of the robot;

[0164] The mobile control module is used to control the robot to move to the target point based on the accurate real-time posture through the state machine combined with the PID motion control method.

[0165] The rest is the same as in Example 1.

[0166] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A robot high-precision positioning control method based on point cloud landmarks, characterized in that: The following steps are involved: Acquire a set of corner points of point cloud landmarks arranged at preset points as input of a standard point cloud generation function to obtain a standard point cloud set, wherein the point cloud landmarks are of special geometric shapes; Acquire laser radar scanning data, convert the laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filter out abnormal points in the two-dimensional laser point cloud data of the current frame; the laser radar scanning data is obtained by sensing the environment through a single-line laser radar; Detecting a jump point in the two-dimensional laser point cloud data of the current frame, segmenting the two-dimensional laser point cloud data using the jump point as an endpoint to obtain a plurality of point cloud cluster sets, filtering out invalid point cloud clusters in the point cloud cluster sets by screening, and retaining potential candidate point cloud clusters to obtain a candidate point cloud cluster set; The candidate point cloud clusters in the candidate point cloud cluster set are matched with the standard point clouds in the standard point cloud set by using the ICP algorithm, the matching degree of all successfully matched point cloud pairs is evaluated according to the scoring function, the point cloud pair with the highest score is selected as the correct matching result of the point cloud landmark in the current frame, and the accurate real-time posture of the robot is obtained according to the posture transformation relationship between the correct matching result and the point cloud landmark; If the point cloud landmark matching in the current frame fails, the wheel odometer is used to track and update the accurate real-time position of the robot; According to the accurate real-time posture, the robot is controlled to move to the target point through a state machine combined with a PID motion control method.

2. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The corner point set is: corner={c1,…,c k } In the formula, corner is the set of corner points, c k is the corner point; The standard point cloud set is: In the formula, PC ref is the standard point cloud set, res is the interpolation resolution, and j is the standard point cloud interpolation step.

3. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The step of filtering out abnormal points includes: Traversing each point in the two-dimensional laser point cloud data of the current frame to obtain three relative distances between each point and its adjacent points; The outliers are obtained by measuring the relationship between the three relative distances and the outlier threshold, and the outliers are taken as abnormal points, and the abnormal points are filtered out by mean filtering, as shown in the following formula: In the formula, pt′ i For pt i is a point in the two-dimensional laser point cloud data, l1, l2, l3 are the three relative distances, l out is the outlier threshold; The expressions of the three relative distances are as follows: l1=‖pt i+1 -pt i ‖2 <h2 style=";text-align:left;direction:ltr">l2=‖pt<h2 style=";text-align:left;direction:ltr"> i+1 <h2 style=";text-align:left;direction:ltr"> -pt<h2 style=";text-align:left;direction:ltr"> i-1 <h2 style=";text-align:left;direction:ltr"> ‖2 l3=‖pt i+2 -Fri i+1 ‖2 In the formula, pt i+1 For pt i The next adjacent point, pt i-1 For pt i The previous adjacent point, pt i+2 For pt i+1 The next adjacent point of .

4. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The jump point extraction process includes: traversing each point in the two-dimensional laser point cloud data after filtering out the abnormal point, calculating the relative distance between each point and its next adjacent point, and if the relative distance is greater than the jump threshold, the current point is a jump point.

5. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The ICP algorithm uses the pose obtained from the point cloud data of the previous frame as the initial estimated pose of the current frame, and classifies the number of iterations to speed up the calculation. The number of iterations is shown in the following formula: Among them, n iter is the number of iterations, n max is the maximum number of iterations, and the candidate point cloud cluster is matched with the standard point cloud by ICP. If the iteration converges, the match is considered successful, and the pose transformation relationship between the point cloud pairs is obtained.

6. The robot high-precision positioning control method based on point cloud landmarks according to claim 5 is characterized in that: The score function is: In the formula, score i is the score function, ef i is the nearest neighbor error between the candidate point cloud cluster after pose transformation and the standard point cloud, er i is the nearest neighbor error between the standard point cloud and the candidate point cloud cluster after inverse pose transformation, is the pose rotation angle between the point cloud pairs, Δd i is the length difference between the point cloud cluster and the standard point cloud.

7. The robot high-precision positioning control method based on point cloud landmarks according to claim 6 is characterized in that: The nearest neighbor error between the candidate point cloud cluster after the pose transformation and the standard point cloud is: Where n is the number of laser points in the candidate point cloud cluster, p k For each point in the candidate point cloud cluster after pose transformation, q k is the midpoint p of the standard point cloud k The nearest neighbor of ; The nearest neighbor error between the standard point cloud and the candidate point cloud cluster after the inverse transformation of the pose is: Where m is the number of laser points in the standard point cloud, p l For each point in the standard point cloud after inverse transformation of pose, q l is the point p in the candidate point cloud cluster l The nearest neighbor of .

8. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The specific calculation steps of the accurate real-time posture include: If the point cloud landmark in the current frame is matched successfully, the pose transformation matrix from the robot to the point cloud landmark is calculated synchronously by the ICP algorithm, and the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system is obtained by accurate measurement, and then the transformation matrix of the robot coordinate system relative to the world coordinate system is obtained, as shown in the following formula: In the formula, is the transformation matrix of the robot coordinate system relative to the world coordinate system, is the accurate pose transformation matrix of the point cloud landmark fixed in the world coordinate system, is the pose transformation matrix from the robot to the point cloud landmark; The calculation formula of the accurate real-time posture is as follows: In the formula, p w* is the element value at the row and column corresponding to the position of the robot in the world coordinate system, is the transformation matrix of the robot coordinate system relative to the world coordinate system The position of the robot in the world coordinate system corresponding to the element values ​​at the rows and columns is as follows: In the formula, p w is the position of the robot in the world coordinate system.

9. The robot high-precision positioning control method based on point cloud landmarks according to claim 1 is characterized in that: The motion states of the state machine include an in-situ rotation state, an arc-shaped movement state, and a linear movement state; In the in-situ rotating state, the robot selects a rotation direction that consumes less time according to the difference between the desired orientation and the actual orientation, and rotates to the desired orientation at the maximum rotation speed; In the arc-shaped moving state, the direction is used as the controlled variable of the PID controller, the difference between the expected direction and the actual direction is used as the input error, the angular velocity of the mobile robot is calculated using the PID control law, and the output linear velocity of the mobile robot is obtained according to the angular velocity; In the linear movement state, when the distance between the robot and the target point is less than a set value, the robot docks with the target point using a gradually decaying linear speed.

10. A robot high-precision positioning control system based on point cloud landmarks, characterized in that: include: A standard point cloud set acquisition module is used to acquire a set of corner points of point cloud landmarks arranged at preset points as an input of a standard point cloud generation function to obtain a standard point cloud set; An abnormal point filtering module is used to obtain laser radar scanning data, convert the obtained laser radar scanning data into two-dimensional laser point cloud data using a point cloud conversion function, and filter out abnormal points in the two-dimensional laser point cloud data of the current frame; A jump point detection module is used to detect the jump points in the two-dimensional laser point cloud data of the current frame, and use the jump points as endpoints to segment the two-dimensional laser point cloud data to obtain a plurality of point cloud cluster sets, and filter out invalid point cloud clusters in the point cloud cluster sets by screening, and retain potential candidate point cloud clusters to obtain a candidate point cloud cluster set; The accurate real-time pose acquisition module uses the ICP algorithm to match the candidate point cloud clusters in the candidate point cloud cluster set with the standard point cloud in the standard point cloud set, evaluates the matching degree of all successfully matched point cloud pairs according to the scoring function, selects the point cloud pair with the highest score as the correct matching result of the point cloud landmark in the current frame, and obtains the accurate real-time pose of the robot according to the pose transformation relationship between the correct matching result and the point cloud landmark; if the point cloud landmark in the current frame fails to match, the wheel odometer is used to track and update the accurate real-time pose of the robot; The mobile control module is used to control the robot to move to a target point through a state machine combined with a PID motion control method according to the accurate real-time posture.

Images