A patrol robot positioning method based on map radar point cloud

Abstract

The present invention relates to a patrol robot positioning method based on a map radar point cloud, comprising the following steps: S1: calculating the coordinate system T_wbi of the wheel contact point with the ground based on the real-time estimated wheel radius r_i and the coordinate system T_wi of the wheel center point; S2: performing prediction plane fitting to obtain a prediction value: specifically, performing plane fitting on the contact points of the four wheels with the ground to obtain an estimated contact surface N of the wheel with the ground; S3: performing observation plane fitting to obtain an observation value: specifically, using the four wheel contact points with the ground as indexes, finding the nearest A points in the scene point cloud map P respectively, and then performing plane fitting respectively to obtain four planes M1, M2, M3, and M4 belonging to the map point cloud; S4: constructing the residual between the predicted value and the observed value: f_k=a_i=[a_1, a_2, a_3, a_4], where a_i represents the plane angle between N and Mi; S6: using a nonlinear least squares method to obtain the current position of the patrol robot;

Description

Technical Field

[0001] The invention relates to a positioning method for an inspection robot based on map radar point cloud. Background Art

[0002] Inspection-type wheeled mobile robots are four-wheeled robots that move on the ground. They generally require autonomous navigation capabilities, and a key technology for this is real-time positioning of the robot. A common approach in the market is sensor fusion (specifically, integrating radar, IMU, wheel speedometer, camera, and other data to achieve positioning). This sensor fusion approach essentially constructs a nonlinear least-squares problem and uses an iterative method to reduce the residual errors generated by each component's observations. This invention primarily aims to further reduce these residual errors based on existing multi-sensor fusion positioning technology. Summary of the Invention

[0003] To achieve this, the present invention provides a patrol robot positioning method based on map radar point cloud, comprising the following steps:

[0004] S1: Based on the real-time estimation of the wheel radius and the wheel center point posture, the posture of the wheel contact point with the ground is T_Wbi_toM=T_Wi_toM*[0,0,-r_i,1];

[0005] S2: Performing plane fitting on the contact points between the four wheels and the ground to obtain an estimated contact surface N between the wheels and the ground;

[0006] S3: Using the four wheel-ground contact points as indexes, find the nearest A points in the scene point cloud map P, and then perform plane fitting to obtain the four planes M1, M2, M3, and M4 belonging to the map point cloud.

[0007] S4: Construct the plane angle a_i between N and Mi

[0008] S5: Construct a new residual f_k=[a_1,a_2,a_3,a_4] based on the plane angle

[0009] S6: Use nonlinear least squares method to get the current vehicle body posture

[0010]

[0011] Furthermore, the A points are 6 points.

[0012] The beneficial effect of the present invention is that: the method of the present invention is based on the original multi-sensor fusion positioning technology, and adds a constraint that the robot is located on the ground to help improve the positioning stability and accuracy of the robot. DETAILED DESCRIPTION

[0013] The present invention will be further described below with reference to the following examples. The examples are only part of the present invention and are only used to explain the present invention, and do not constitute any limitation to the scope of the present invention.

[0014] The processes shown in the embodiments of the present application are merely illustrative and do not necessarily include all content and operations / steps, nor do they necessarily have to be performed in the order described. For example, some operations / steps may be decomposed, combined, or partially merged, so the actual execution order may vary depending on the actual situation.

[0015] To understand the solution of the present invention, we first set the following parameters: the point cloud map of the scene: P, the center point pose of the four wheels: T_Wi_toM (i=1, 2, 3, 4), the real-time estimated radius of the four wheels: r_i (i=1, 2, 3, 4), the residual r of the current pose estimation in the sensor fusion specifically includes the radar matching residual: fi, and the visual residual: fj;

[0016]

[0017] The present invention provides a positioning method for an inspection robot based on a map radar point cloud, comprising the following steps:

[0018] S1: Based on the real-time estimated wheel radius r_i and the coordinate system T_wi of the wheel center point, calculate the coordinate system T_wbi of the wheel contact point with the ground

[0019]

[0020] S2: Performing a prediction plane fitting to obtain a predicted value: Specifically, performing a plane fitting on the contact points of the four wheels with the ground to obtain an estimated contact surface N between the wheels and the ground; wherein,

[0021] S3: Perform observation plane fitting to obtain observation values: Specifically, using the four wheel-ground contact points as indexes, find the nearest A points (preferably 6 points) in the scene point cloud map P, and then perform plane fitting on each of them to obtain four planes M1, M2, M3, and M4 belonging to the map point cloud;

[0022] S4: Construct the residual between the predicted value and the observed value: f_k = a_i = [a_1, a_2, a_3, a_4], where a_i represents the angle between the planes N and Mi;

[0023] S6: Using nonlinear least square method to obtain the current position of the inspection robot;

[0024]

[0025] The method of the present invention is based on the original multi-sensor fusion positioning technology, and adds a constraint that the robot is located on the ground to help improve the positioning stability and accuracy of the robot.

[0026] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A patrol robot positioning method based on map radar point cloud, characterized by: The following steps are included: S1: Based on real-time estimation of wheel radius r i , and the coordinate system of the wheel center point Calculate the coordinate system of the wheel contact point with the ground S2: performing a prediction plane fitting to obtain a predicted value: Specifically, performing a plane fitting on the contact points between the four wheels and the ground to obtain an estimated contact surface N between the wheels and the ground; in, S3: Perform observation plane fitting to obtain observation values: Specifically, using the four wheel-ground contact points as indexes, find the nearest A points in the scene point cloud map P, and then perform plane fitting on each of them to obtain four planes M1, M2, M3, and M4 belonging to the map point cloud; S4: Construct the residual between the predicted value and the observed value: f k =a i =[a1, a2, a3, a4], where a i represents the angle between the planes N and Mi; S6: Use the nonlinear least square method to obtain the current position of the inspection robot; Among them, r represents the residual, f i represents radar matching residual, f j Represents the visual residual.

2. The inspection robot positioning method based on map radar point cloud according to claim 1, characterized in that: in, The A points are 6 points.

Images