A robot high-precision positioning control method and system based on point cloud landmarks

Abstract

The present invention relates to a high-precision positioning control method and system for a robot based on point cloud landmarks. The method comprises the following steps: obtaining a standard point cloud set of point cloud landmarks arranged at preset locations; acquiring laser radar scanning data, converting it into two-dimensional laser point cloud data, filtering outliers, detecting transition points, obtaining a number of point cloud cluster sets, and screening them to obtain a candidate point cloud cluster set; using an ICP algorithm to match the candidate point cloud clusters with the standard point cloud and evaluate the degree of matching to obtain a correct matching result for the current frame point cloud landmarks, thereby obtaining the accurate real-time position and posture of the robot; if the current frame point cloud landmark matching fails, using a wheeled odometry to track and update the accurate real-time position and posture of the robot; and based on the accurate real-time position and posture, controlling the robot to move to the target point using a state machine combined with a PID motion control method. Compared with existing technologies, the present invention achieves high-precision positioning and control of the robot, meeting the needs of many special operations.

Description

Technical Field

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

[0002] To achieve autonomy and intelligence, mobile robots must have good perception modeling and autonomous control capabilities in any complex dynamic scenarios where they need to be deployed. Among these, the positioning accuracy and control accuracy that the robot can achieve are 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 using lidar as their primary sensor. These algorithms can meet the requirements of real-time, robust robot pose tracking and achieve centimeter-level positioning accuracy in general environments. Although this accuracy can meet the requirements of mobile robots to perform most tasks, robots often need to have even higher-precision autonomous positioning capabilities for some critical and special mobile tasks. 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; and in some workstation tasks that require high-precision operations, robots need to accurately reach the specified pose and complete the corresponding task operations.

[0004] Therefore, the ability to achieve highly precise, millimeter-level autonomous positioning and control is a critical, irreplaceable function for mobile robots. Current technologies, such as vision-based methods like QR code recognition, are often susceptible to lighting effects, resulting in unstable positioning. Laser-based methods, on the other hand, often rely solely on the shape of fixed charging stations for docking. These methods, with their non-adjustable auxiliary shapes, place significant constraints on the robot's position and mission objectives, and lack the flexibility to apply high-precision positioning and control technologies. 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 the needs of many special operations.

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

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

[0008] Obtaining a set of corner points of a point cloud landmark arranged at a preset point position as an input of a standard point cloud generation function to obtain a standard point cloud set, wherein the point cloud landmark is a special geometric shape;

[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, and retaining potential candidate point cloud clusters to obtain a candidate point cloud cluster set;

[0011] Using the ICP algorithm, candidate point cloud clusters in the candidate point cloud cluster set are matched with standard point clouds in the standard point cloud set. 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. The accurate real-time pose of the robot is obtained based on the pose 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 odometry is used to track and update the robot's accurate real-time pose;

[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={c1,…,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] Where, PC ref is the standard point cloud set, res is the interpolation resolution, and j is the standard point cloud interpolation step size.

[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 regarded as abnormal points. The abnormal points are filtered out by the mean filtering method, as shown in the following formula:

[0023]

[0024] Where 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;

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

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

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

[0028] l3=||pt i+2 -pt i+1 ||2

[0029] Where, 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 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 accelerate the calculation. The number of iterations is shown in the following formula:

[0032]

[0033] 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 matching. If the iteration converges, the matching is considered successful, and the pose transformation relationship between the point cloud pairs is obtained.

[0034] Furthermore, the scoring 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 after the inverse transformation of the pose and the candidate point cloud cluster 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 Point p is the center point of the candidate point cloud cluster l The nearest neighbor of .

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

[0044] If the point cloud landmark in the current frame is successfully matched, 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] Where, is the transformation matrix of the robot coordinate system relative to the world coordinate system, T l wThe 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 pose is as follows:

[0048]

[0049] Where 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] Where p w is the position of the robot in the world coordinate system.

[0052] Furthermore, the motion states of the state machine include 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 orientation is used as the controlled variable of the PID controller, the difference between the desired orientation and the actual orientation 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 obtain a set of corner points of point cloud landmarks arranged at preset points as input to a standard point cloud generation function to obtain a standard point cloud set;

[0058] An outlier 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 outliers in the two-dimensional laser point cloud data of the current frame;

[0059] a transition point detection module, configured to detect transition points in the two-dimensional laser point cloud data of the current frame, segment the two-dimensional laser point cloud data using the transition points as endpoints to obtain a plurality of 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 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 clouds in the standard point cloud set. 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. The accurate real-time pose of the robot is obtained based on the pose 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 odometry 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 the 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 with special geometric shapes for auxiliary positioning, generates a standard point cloud set based on the point cloud landmarks, uses lidar for environmental perception, and sequentially filters out abnormal points and detects jump points in the current frame laser point cloud data. Filtering conditions are designed to extract several potential candidate point cloud clusters to obtain a candidate point cloud cluster set. The ICP algorithm is used 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 the accurate real-time position and posture of the robot are obtained. A state machine is used in combination with the 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 wheel odometer speed information to perform robot positioning, avoiding the accumulation and increase of detection errors and obtaining a more accurate robot posture. The combination of the two data allows the robot to continue positioning and control even if one of the information is lost, greatly increasing the stability of the control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a flow chart of a high-precision positioning control method for a robot 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 signpost;

[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 flow chart of the robot angular velocity PID control;

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

[0071] The present invention is described in detail below with reference to 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 high-precision positioning control method for a robot based on point cloud landmarks. Figure 1 As shown, the following steps are included:

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

[0078] Any number of point cloud landmarks can be placed in the robot's operating environment, but only one of each type of landmark can be placed. The set point position needs to be accurately measured in advance to obtain the pose 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 2 As shown. The signpost is 20 cm long and 5 cm wide, with a V-shaped angle of 126.9°, and is level with the robot's lidar. In this embodiment, the robot's precise positioning point is set 0.5 m in front of the V-shaped signpost. A rangefinder is used to precisely measure, position, and secure the V-shaped signpost, ensuring the robot's positioning accuracy.

[0080] The standard point cloud generation function takes the corner point set of the point cloud landmarks as input and uses linear interpolation to fit an accurate standard point cloud. In this embodiment, the interpolation resolution is set to 1mm. The standard point cloud collection stores both the point cloud set of each shape and the pose of the shape in the environment.

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

[0082] corner={c1,…,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] Where, PC ref is the standard point cloud set, res is the interpolation resolution, and j is the standard point cloud interpolation step size.

[0087] S2. Obtain the laser radar scanning data, convert the laser radar scanning data into two-dimensional laser point cloud data using the 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 of 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] Where, 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, each point in the point cloud is traversed and the three relative distances l1, l2, l3 are measured against the outlier threshold l out The relationship between them is to find all outliers, which are abnormal points, as shown in the following formula:

[0092]

[0093] Where pt′ i For pt i is a point in the two-dimensional laser point cloud data, l1, l2, l3 are three relative distances, l out is the outlier threshold; in this embodiment, out The value of is 4cm, and all abnormal points detected 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] l1=||pt i+1 -pt i ||2

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

[0097] l3=||pt i+2 -pt i+1 ||2

[0098] Where, 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, use the jump points as endpoints to segment the two-dimensional laser point cloud data, and obtain several point cloud cluster sets. Filter out invalid point cloud clusters in the point cloud cluster sets through screening, and retain potential candidate point cloud clusters to obtain candidate point cloud cluster sets.

[0100] The process of extracting the transition point includes: traversing each point in the two-dimensional laser point cloud data after filtering out abnormal points, calculating the relative distance between each point and its next adjacent point, and if the relative distance is greater than the transition threshold, the current point is a transition 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 criteria comprehensively consider the length limit of the clustered point cloud and the distance limit from the clustered point cloud to the LiDAR. In this example, the standard point cloud length is 20 cm. If the length of the point cloud cluster differs by more than 10 cm from this, it will not be considered as a potential candidate point cloud. If the distance from the point cloud cluster to the LiDAR is less than 5 cm or greater than 1.5 m, the point cloud cluster will also be disregarded. This is because when the distance is too close, the LiDAR cannot scan and perceive the entire object. When the distance is too far, the laser point cloud is sparse and cannot be accurately perceived. After screening, invalid point cloud clusters in the original point cloud cluster set are deleted, and 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 the figure, a total of 6 candidate point cloud clusters are extracted. At the same time, the correct point cloud landmark point cloud cluster pcs4 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 posture transformation relationship between the correct matching result and the point cloud landmark.

[0107] The ICP algorithm is an iterative closest point algorithm. The ICP algorithm uses the pose obtained based on the previous frame of point cloud data 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 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 matching. 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 Δd between the length and the standard length (20cm) 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 Point p is the center point of the candidate point cloud cluster l The nearest neighbor of .

[0120] Calculate the scores of all successfully matched point cloud pairs 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. In this embodiment, the score threshold is set to 60. As shown in Table 1, the matching score of candidate point cloud cluster pcs4 is the highest and exceeds the score threshold. Therefore, the matching of the point cloud landmark is successful.

[0125] After confirming that pcs4 is the correct point cloud landmark, the pose transformation relationship between the robot and the point cloud landmark It has been calculated synchronously by the ICP algorithm. In addition, the accurate pose relationship T of the point cloud landmark is fixed in the world coordinate system. l w It has been accurately measured, so the accurate real-time position 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] Where, is the transformation matrix of the robot coordinate system relative to the world coordinate system, 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] Where 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 row and column is as follows:

[0133]

[0134] Where 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 odometry 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 odometry 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 score of all matching point cloud pairs is 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. Based on 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's motion control state machine includes three defined states: in-place rotation, arc movement, and linear movement. Upon receiving a command to travel to a specified target point, the robot first enters the in-place rotation state, controlling the robot to face the desired orientation. Once the orientation error falls below a threshold, the state machine enters the arc movement state, controlling the robot to follow a roughly arc-shaped trajectory to a temporary point a certain distance in front of the target point, ensuring smooth and fluid movement. Upon reaching the temporary point, the state machine enters the linear movement state, controlling the robot to proceed straight to the target point. This state facilitates more accurate pose estimation and ensures the robot's end-position positioning accuracy.

[0141] When rotating in place, the robot selects the rotation 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 pose (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 for the robot to reach the target point consistently and directly, the desired orientation at each moment t needs to be larger than the angle between the robot and the target point. Therefore, the desired orientation at each moment is set to a times the angle. The designed desired orientation expression is as follows:

[0144]

[0145] Where, is the angle from the robot to the target point, θ r，t is the desired orientation, in this embodiment, a=1.5. Define the input error θ 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] Where θ 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 is as follows:

[0149]

[0150] Where θ e，t is the input error, and this embodiment sets K p =1.5, K i =0,K d =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 segmentation 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] Where w t is the robot angular velocity, v t is the output linear velocity, σ is the segmentation threshold, v max The maximum linear velocity is set to 0.2 m / s in this embodiment.

[0154] In the straight line movement state, when the distance between the robot and the target point is less than ∈When the robot is docked at 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] Where, 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 high-precision positioning control system for a robot 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 them at preset locations. 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 uses a single-line laser radar to perceive the environment, 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 current frame of two-dimensional laser point cloud data;

[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 several point cloud cluster sets. The invalid point cloud clusters in the point cloud cluster sets are filtered out, 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 candidate point cloud clusters in the candidate point cloud cluster set with the standard point cloud in the standard point cloud set. The matching degree of all successfully matched point cloud pairs is evaluated according to the scoring function, and the point cloud pair with the highest score is selected as the correct matching result of the current frame point cloud landmark. The accurate real-time pose of the robot is obtained based on the pose transformation relationship between the correct matching result and the point cloud landmark. If the current frame point cloud landmark matching fails, the wheel odometry 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 above describes in detail the preferred embodiments of the present invention. It should be understood that those skilled in the art can make numerous modifications and variations based on the concepts of the present invention without inventive effort. Therefore, any technical solutions that can be derived by those skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A high-precision positioning control method for a robot based on point cloud landmarks, characterized in that: The following steps are involved: Obtaining a set of corner points of point cloud landmarks arranged at preset locations as input to a standard point cloud generation function to obtain a standard point cloud set, wherein the point cloud landmarks have a special geometric shape that is significantly different from the general shape of objects in the environment in which the robot is located; 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, and retaining potential candidate point cloud clusters to obtain a candidate point cloud cluster set; Using the ICP algorithm, candidate point cloud clusters in the candidate point cloud cluster set are matched with standard point clouds in the standard point cloud set. 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. The accurate real-time pose of the robot is obtained based on the pose 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 odometry is used to track and update the robot's accurate real-time pose; 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 high-precision positioning control method for a robot based on point cloud landmarks according to claim 1 is characterized in that: The corner point set is: Where, is the set of corner points, is the corner point; The standard point cloud set is: Where, is a standard point cloud set, is the interpolation resolution, is the standard point cloud interpolation step size.

3. The high-precision positioning control method for a robot based on point cloud landmarks according to claim 1, 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 regarded as abnormal points. The abnormal points are filtered out by the mean filtering method, as shown in the following formula: Where, is the laser point of the new point cloud formed after filtering out the abnormal points by the mean filtering method, is a point in the two-dimensional laser point cloud data, 、 、 are the three relative distances, is the outlier threshold; The expressions of the three relative distances are as follows: Where, for The next adjacent point of for The previous adjacent point of for 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 high-precision positioning control method for a robot based on point cloud landmarks according to claim 1, 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 accelerate the calculation. The number of iterations is shown in the following formula: in, is the number of iterations, is the maximum number of iterations, and the candidate point cloud cluster is matched with the standard point cloud by ICP matching. If the iteration converges, the matching is considered successful, and the pose transformation relationship between the point cloud pairs is obtained.

6. The high-precision positioning control method for a robot based on point cloud landmarks according to claim 5, characterized in that: The scoring function is: Where, is the score function, is the nearest neighbor error between the candidate point cloud cluster after pose transformation and the standard point cloud, 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, is the length difference between the point cloud cluster and the standard point cloud.

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

8. The high-precision positioning control method for a robot based on point cloud landmarks according to claim 1, characterized in that: The specific calculation steps of the accurate real-time pose include: If the point cloud landmark in the current frame is successfully matched, 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: Where, is the transformation matrix of the robot coordinate system relative to the world coordinate system, 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 pose is as follows: Where, 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: Where, 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 orientation is used as the controlled variable of the PID controller, the difference between the desired orientation and the actual orientation 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 high-precision positioning control system for a robot based on point cloud landmarks, characterized in that: include: A standard point cloud set acquisition module is used to obtain a set of corner points of point cloud landmarks arranged at preset points as input to a standard point cloud generation function to obtain a standard point cloud set, wherein the point cloud landmarks have special geometric shapes that are significantly different from the general shapes of objects in the robot's environment; An outlier 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 outliers in the two-dimensional laser point cloud data of the current frame; a transition point detection module, configured to detect transition points in the two-dimensional laser point cloud data of the current frame, segment the two-dimensional laser point cloud data using the transition points as endpoints to obtain a plurality of 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 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 clouds in the standard point cloud set. 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. The accurate real-time pose of the robot is obtained based on the pose 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 odometry 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 the target point through a state machine combined with a PID motion control method according to the accurate real-time posture.

Images