A deep point cloud based mobile robot visual servoing control method and system

Abstract

The application discloses a kind of mobile robot vision servo control method and system based on depth point cloud, method includes: obtaining the depth point cloud information of the environment around mobile robot;The obtained depth point cloud information is preprocessed, and the direction and distance information of obstacle are obtained;According to the direction and distance information of obstacle and the current pose of mobile robot, the current relative position of mobile robot and obstacle is calculated;According to the preset desired pose of mobile robot, the current pose of mobile robot and the current relative position of mobile robot and obstacle, the pose error of mobile robot is calculated;According to the pose error of mobile robot, the pose of mobile robot is adjusted.The application solves the problem that traditional vision servo is difficult to extract weak texture feature image feature, and the calculation amount is lower, and robustness is higher, and stability is better.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot technology, specifically relating to a visual servo control method and system for mobile robots based on depth point clouds. Background Technology

[0002] Mobile robots are intelligent systems that integrate environmental perception and behavioral control, and they have broad development prospects in manufacturing, medical rescue, and daily life services. To address the control problems of mobile robots, various sensors are being used extensively, among which visual servo control is one of the most widely used methods today.

[0003] Currently, most visual servoing control methods use image-based approaches, which correlate changes in image feature parameters with changes in robot pose based on image information acquired by sensors, thereby enabling control. However, this method suffers from drawbacks such as difficulty in controlling robot trajectory and high environmental requirements. Another position-based visual servoing control method calculates control variables based on the current pose and the target pose to achieve trajectory control, but it relies too heavily on the accuracy of target pose estimation and has poor robustness to image errors. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a visual servo control method and system for mobile robots based on deep point clouds. This solves the problem that traditional visual servoing is difficult to extract features from images with weak texture features, and has lower computational cost, higher robustness, and better stability.

[0005] This invention provides the following technical solution:

[0006] Firstly, a visual servo control method for mobile robots based on deep point clouds is provided, including:

[0007] Acquire depth point cloud information of the environment surrounding the mobile robot;

[0008] The acquired depth point cloud information is preprocessed to obtain the direction and distance information of the obstacles;

[0009] The current relative position of the mobile robot to the obstacle is calculated based on the direction and distance information of the obstacle and the current pose of the mobile robot.

[0010] The pose error of the mobile robot is calculated based on the preset desired pose of the mobile robot, the current pose of the mobile robot, and the current relative position of the mobile robot and the obstacle.

[0011] Adjust the pose of the mobile robot based on its pose error.

[0012] Furthermore, the depth point cloud information is acquired by a Realsence D435 depth camera, and the preprocessing of the depth point cloud information includes: voxel filtering, coordinate transformation, pass-through filtering, Gaussian filtering, dimensionality reduction, and converting the rectangular coordinates of the point cloud to polar coordinates.

[0013] Furthermore, the current pose of the mobile robot is obtained based on the kinematic model of the mobile robot; the method for establishing the kinematic model of the mobile robot includes:

[0014] Set the starting position of the mobile robot in the XY plane of the world coordinate system. As the mobile robot moves in the world coordinate system, establish its kinematic equations:

[0015]

[0016]

[0017] In the formula, v i and ω i These are the linear velocity and angular velocity of the mobile robot in the robot coordinate system, respectively. The pose (x) of the mobile robot in the world coordinate system i ,y i ,θ i The first derivative with respect to time, assuming the mobile robot's centroid coincides with the center, (x) i ,y i Let θ be the coordinates of the geometric center of the mobile robot. i Let v be the angle between the linear velocity vector of the mobile robot and the X-axis; iL and v iR L1 represents the speed of the left and right tracks of the mobile robot, and L2 represents the distance between the left and right tracks of the mobile robot.

[0018] Furthermore, the method for calculating the current relative position of the mobile robot and the obstacle includes:

[0019]

[0020]

[0021] In the formula, d e Let be the vertical distance between the mobile robot and the obstacle, and d be the straight-line distance between the mobile robot and the obstacle along its x-coordinate X0 direction. and θ represents the angles by which the mobile robot diverges to the left and right with the x-coordinate X0 as the angle bisector. t p1 is the angle between the perpendicular line from the mobile robot to the obstacle and the horizontal coordinate X0, and p1 is the angle along which the mobile robot moves. The straight-line distance between the direction and the obstacle, p2 is the distance the robot travels along. The direction and the straight-line distance from the obstacle.

[0022] Furthermore, the method for calculating the pose error of the mobile robot includes: in the world coordinate system, let:

[0023] Δp=p * -p (5)

[0024]

[0025] in but:

[0026]

[0027] In the formula, Δp=[x e ,y e ,θ e [P] represents the pose error of the mobile robot. * = [x1,y1,θ1] is the desired pose of the mobile robot, and P = [x0,y0,θ0] is the current pose of the mobile robot.

[0028] Furthermore, if the pose error of the mobile robot is greater than a set threshold, the control rate of the mobile robot is calculated based on the pose error and the Lyapunov function, and a visual servo controller is established to adjust the pose of the mobile robot in real time until the pose error of the mobile robot is less than the set threshold, at which point the process ends and the pose adjustment of the mobile robot is completed.

[0029] Furthermore, the method for calculating the control rate of the mobile robot includes:

[0030] Differentiate the position error Δp ​​of the mobile robot:

[0031]

[0032] Based on the differential of the mobile robot's position error Choose the V function as the Lyapunov function:

[0033]

[0034] Differentiate the V function:

[0035]

[0036] The control rate of the mobile robot is calculated as follows:

[0037]

[0038] Substituting equation (11) into equation (10) to prove the stability of the mobile robot's control law, we get:

[0039]

[0040] In the formula, The differential of the position error of the mobile robot is given by the formula. Let v be the derivative of the V function, v0 and v1 be the velocities of the current and desired poses of the mobile robot, respectively, ω0 and ω1 be the angular velocities of the current and desired poses of the mobile robot, respectively, and k1, k2, k3 > 0, which are all control parameters.

[0041] Secondly, a visual servo control system for a mobile robot based on deep point clouds is provided, including:

[0042] The information acquisition module is used to acquire depth point cloud information of the environment surrounding the mobile robot;

[0043] The preprocessing module is used to preprocess the acquired depth point cloud information to obtain the direction and distance information of obstacles;

[0044] The first data processing module is used to calculate the current relative position of the mobile robot and the obstacle based on the direction and distance information of the obstacle and the current pose of the mobile robot.

[0045] The second data processing module calculates the pose error of the mobile robot based on the preset desired pose of the mobile robot, the current pose of the mobile robot, and the current relative position of the mobile robot and the obstacle.

[0046] The pose adjustment module is used to adjust the pose of the mobile robot based on the pose error of the mobile robot.

[0047] Thirdly, a visual servo control device for a mobile robot based on deep point clouds is provided, including a processor and a storage medium; the storage medium is used to store instructions; the processor is used to operate according to the instructions to execute the steps of the method described in the first aspect.

[0048] Fourthly, a computer-readable storage medium is provided having a computer program stored thereon, characterized in that the program, when executed by a processor, implements the steps of the method described in the first aspect.

[0049] Compared with the prior art, the beneficial effects of the present invention are:

[0050] This invention is based on the depth point cloud information of the environment around the mobile robot. By preprocessing the depth point cloud information, the direction and distance information of obstacles are obtained. Then, the current relative position of the mobile robot and the obstacle and the pose error of the mobile robot are calculated. Finally, the pose of the mobile robot is continuously adjusted according to the pose error of the mobile robot to achieve the desired position. This invention solves the problem of traditional visual servoing which is difficult to extract features from images with weak texture features. It also has lower computational cost, higher robustness and better stability. Attached Figure Description

[0051] Figure 1 This is a flowchart illustrating the visual servo control method for a mobile robot based on deep point clouds in an embodiment of the present invention.

[0052] Figure 2 This is the kinematic model of the mobile robot in this embodiment of the invention;

[0053] Figure 3 This is a schematic diagram of the deep point cloud information preprocessing process in an embodiment of the present invention;

[0054] Figure 4 This is a mathematical model of the relative positional relationship between the mobile robot and the obstacle in this embodiment of the invention. Detailed Implementation

[0055] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0056] Example 1

[0057] like Figure 1 As shown, this embodiment takes a four-tracked, dual-rocker differential mobile robot as an example to provide a visual servo control method for a mobile robot based on depth point clouds, specifically including the following steps:

[0058] Step 1, such as Figure 2 As shown, the starting position of the mobile robot is set in the XY plane of the world coordinate system. When the mobile robot moves in the world coordinate system, its kinematic model equations are established:

[0059]

[0060] in:

[0061]

[0062] In the formula, v i and ω i These are the linear velocity and angular velocity of the mobile robot in the robot coordinate system, respectively. The pose (x) of the mobile robot in the world coordinate systemi ,y i ,θ i The first derivative with respect to time, assuming the mobile robot's centroid coincides with the center, (x) i ,y i Let θ be the coordinates of the geometric center of the mobile robot. i Let v be the angle between the linear velocity vector of the mobile robot and the X-axis; iL and v iR L1 represents the speed of the left and right tracks of the mobile robot, and L2 represents the distance between the left and right tracks of the mobile robot.

[0063] Step 2: Use the Realsence D435 depth camera to collect real-time depth point cloud information of the surrounding environment of the mobile robot.

[0064] Step 3, as follows Figure 3 As shown, the depth point cloud information obtained in step two is preprocessed, including: voxel filtering, coordinate transformation, pass-through filtering, Gaussian filtering, dimensionality reduction, and converting the rectangular coordinates of the point cloud to polar coordinates to obtain the direction and distance information of the obstacle.

[0065] Step 4, as follows Figure 4 As shown, based on the direction and distance information of the obstacle obtained in step three and the current pose of the mobile robot, a mathematical relationship is established to calculate the current relative position of the mobile robot and the obstacle, specifically:

[0066]

[0067] Calculation yielded:

[0068]

[0069] In the formula, d e Let be the vertical distance between the mobile robot and the obstacle, and d be the straight-line distance between the mobile robot and the obstacle along its x-coordinate X0 direction. and θ represents the angles by which the mobile robot diverges to the left and right with the x-coordinate X0 as the angle bisector. t p1 is the angle between the perpendicular line from the mobile robot to the obstacle and the horizontal coordinate X0, and p1 is the angle along which the mobile robot moves. The straight-line distance between the direction and the obstacle, p2 is the distance the robot travels along. The direction and the straight-line distance from the obstacle.

[0070] Step 5: Calculate the pose error of the mobile robot based on the preset desired pose, the current pose of the mobile robot, and the current relative position of the mobile robot to the obstacle. Specifically, in the world coordinate system, set the desired pose P of the mobile robot.* = [x1, y1, θ1], obtain the current pose P = [x0, y0, θ0] of the mobile robot based on the kinematic model of the mobile robot, and construct the pose error model of the mobile robot:

[0071] Δp=p * -p (5)

[0072]

[0073] Where, Δp=[x e ,y e ,θ e [This represents the pose error of the mobile robot.] but:

[0074]

[0075] Step Six: If the pose error of the mobile robot is greater than the set threshold ε, then calculate the control law of the mobile robot based on the pose error and the Lyapunov function, and establish a visual servo controller to adjust the pose of the mobile robot in real time, specifically as follows:

[0076] Differentiate the position error Δp ​​of the mobile robot:

[0077]

[0078] Based on the differential of the mobile robot's position error Choose the V function as the Lyapunov function:

[0079]

[0080] Differentiating the V function (9):

[0081]

[0082] From formula (10), the control rate of the mobile robot can be obtained as follows:

[0083]

[0084] In the formula, The differential of the position error of the mobile robot is given by the formula. Let v be the derivative of the V function, v0 and v1 be the velocities of the current and desired poses of the mobile robot, respectively, ω0 and ω1 be the angular velocities of the current and desired poses of the mobile robot, respectively, and k1, k2, k3 > 0, which are all control parameters.

[0085] Substituting equation (11) into equation (10) to prove the stability of the mobile robot's control law, we get:

[0086]

[0087] As can be seen from formulas (9) and (12), the selected V function satisfies the Lyapunov stability condition, and the control law of the proposed mobile robot is stable.

[0088] Step 7: Repeat steps 1 to 6 until the pose error of the mobile robot is less than the set threshold ε. End the process and the mobile robot will reach the desired position with an attitude perpendicular to the obstacle direction, thus completing the pose adjustment of the mobile robot.

[0089] Example 2

[0090] This embodiment provides a visual servo control system for a mobile robot based on depth point clouds, including:

[0091] The information acquisition module is used to acquire depth point cloud information of the environment surrounding the mobile robot;

[0092] The preprocessing module is used to preprocess the acquired depth point cloud information to obtain the direction and distance information of obstacles;

[0093] The first data processing module is used to calculate the current relative position of the mobile robot and the obstacle based on the direction and distance information of the obstacle and the current pose of the mobile robot.

[0094] The second data processing module calculates the pose error of the mobile robot based on the preset desired pose of the mobile robot, the current pose of the mobile robot, and the current relative position of the mobile robot and the obstacle.

[0095] The pose adjustment module is used to adjust the pose of the mobile robot based on the pose error of the mobile robot.

[0096] Example 3

[0097] This embodiment provides a visual servo control device for a mobile robot based on deep point clouds, including a processor and a storage medium; the storage medium is used to store instructions; the processor is used to perform operations according to the instructions to execute the steps of the method described in Embodiment 1.

[0098] Example 4

[0099] This embodiment provides a computer-readable storage medium storing a computer program thereon, characterized in that the program, when executed by a processor, implements the steps of the method described in Embodiment 1.

[0100] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0101] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0102] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0103] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0104] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A visual servo control method for a mobile robot based on deep point clouds, characterized in that, include: Acquire depth point cloud information of the environment surrounding the mobile robot; The acquired depth point cloud information is preprocessed to obtain the direction and distance information of the obstacles; The current relative position of the mobile robot to the obstacle is calculated based on the direction and distance information of the obstacle and the current pose of the mobile robot. The pose error of the mobile robot is calculated based on the preset desired pose of the mobile robot, the current pose of the mobile robot, and the current relative position of the mobile robot and the obstacle. Adjust the pose of the mobile robot based on its pose error; If the pose error of the mobile robot is greater than the set threshold, the control rate of the mobile robot is calculated based on the pose error of the mobile robot and the Lyapunov function, and a visual servo controller is established to adjust the pose of the mobile robot in real time until the pose error of the mobile robot is less than the set threshold, then the process ends and the pose adjustment of the mobile robot is completed. The method for calculating the control rate of the mobile robot includes: pose error of mobile robot Differentiate: （8）； Based on the differential of the mobile robot's position error We choose the V function as the Lyapunov function: （9）； Differentiate the V function: （10）； The control rate of the mobile robot is calculated as follows: （11）； Substituting equation (11) into equation (10) to prove the stability of the mobile robot's control law, we get: （12）； In the formula, For the differential of the position error of the mobile robot, For the pose error of the mobile robot, Let v0 and v1 be the differential of the V function, and v0 and v1 be the velocities of the mobile robot in its current and desired poses, respectively. and These are the angular velocities of the mobile robot's current pose and desired pose, respectively. All of these are control parameters; Let p1 be the straight-line distance between the mobile robot and the obstacle along its x-coordinate X0, and p1 be the distance between the mobile robot and the obstacle along its x-coordinate X0. The straight-line distance between the direction and the obstacle, p2 is the distance the robot travels along. The direction and the straight-line distance from the obstacle and These are the angles from which the mobile robot diverges to the left and right with the x-axis X0 as the angle bisector.

2. The mobile robot visual servo control method based on depth point cloud according to claim 1, characterized in that, The depth point cloud information was acquired using a Realsence D435 depth camera. The preprocessing of the depth point cloud information included: voxel filtering, coordinate transformation, pass-through filtering, Gaussian filtering, dimensionality reduction, and converting the Cartesian coordinates of the point cloud to polar coordinates.

3. The mobile robot visual servo control method based on depth point cloud according to claim 1, characterized in that, The current pose of the mobile robot is obtained based on the kinematic model of the mobile robot; the method for establishing the kinematic model of the mobile robot includes: Set the starting position of the mobile robot in the XY plane of the world coordinate system. As the mobile robot moves in the world coordinate system, establish its kinematic equations: （1）； （2）； In the formula, and These are the linear velocity and angular velocity of the mobile robot in the robot coordinate system, respectively. pose of the mobile robot in the world coordinate system The first derivative with respect to time, assuming the mobile robot's center of mass coincides with the center, The coordinates of the geometric center of the mobile robot are: Let v be the angle between the linear velocity vector of the mobile robot and the X-axis; iL and v iR L1 represents the speed of the left and right tracks of the mobile robot, and L2 represents the distance between the left and right tracks of the mobile robot.

4. The mobile robot visual servo control method based on depth point cloud according to claim 1, characterized in that, The method for calculating the current relative position of the mobile robot and the obstacle includes: （3）； （4）； In the formula, The vertical distance between the mobile robot and the obstacle. The angle between the perpendicular line from the mobile robot to the obstacle and the horizontal coordinate X0.

5. The mobile robot visual servo control method based on depth point cloud according to claim 4, characterized in that, The method for calculating the pose error of the mobile robot includes: in the world coordinate system, let: （5）； （6）； in , ,but: （7）； In the formula, For the desired pose of the mobile robot, This represents the current pose of the mobile robot.

6. A visual servo control system for a mobile robot based on deep point clouds, characterized in that, For performing the method according to any one of claims 1 to 5, comprising: The information acquisition module is used to acquire depth point cloud information of the environment surrounding the mobile robot; The preprocessing module is used to preprocess the acquired depth point cloud information to obtain the direction and distance information of obstacles; The first data processing module is used to calculate the current relative position of the mobile robot and the obstacle based on the direction and distance information of the obstacle and the current pose of the mobile robot. The second data processing module calculates the pose error of the mobile robot based on the preset desired pose of the mobile robot, the current pose of the mobile robot, and the current relative position of the mobile robot and the obstacle. The pose adjustment module is used to adjust the pose of the mobile robot based on the pose error of the mobile robot.

7. A visual servo control device for a mobile robot based on depth point clouds, characterized in that, It includes a processor and a storage medium; the storage medium is used to store instructions; the processor is used to operate according to the instructions to perform the steps of the method according to any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the program implements the steps of the method according to any one of claims 1 to 5.

Images