A dual-point positioning control strategy for mobile robots based on depth cameras

Abstract

The present invention provides a dual-point positioning control strategy for a mobile robot based on a depth camera, which belongs to the field of automatic control. The present invention first marks two color blocks on the charging pile, calculates the coordinates of the center of mass of the two color blocks in the camera coordinate system and the vector composed of the two center of mass, and then uses the vector dot product formula and the inverse cosine function to calculate the angle between the chassis and the vector, and uses the angle as the control variable to adjust the chassis posture. Then, according to the adjusted posture, the local coordinate system and the global coordinate system of the vehicle are established, and the local posture and global posture of the chassis are solved in combination with the motor encoder data, and then the target speed of the chassis is solved. Finally, the rotation speed of each wheel is solved according to the inverse model of the mobile chassis dynamics, so as to drive the vehicle to move to the target charging pile. The present invention provides a dual-point positioning control strategy for a mobile robot based on a depth camera, which can accurately move the vehicle to the charging pile position to realize the automatic charging function of the vehicle.

Description

Technical Field

[0001] The present invention relates to the field of automatic control, and in particular to a dual-point positioning control strategy for a mobile robot based on a depth camera. Background Art

[0002] Automatic charging is a key technology in the smart home and IoT sectors. It reduces the number of user steps and provides significant convenience. Users no longer need to manually plug in or unplug the charger or find an outlet. Simply place the device in a charging area and it will automatically begin charging.

[0003] Currently, although there are automatic charging solutions based on image recognition, which use depth cameras to identify QR codes or other images pasted on charging piles, calculate the relative position of the depth camera and the QR code, and then control the charging device to be charged, the algorithm is complex and the cost of use is high.

[0004] To address the above issues, the present invention proposes a universal camera-based dual-point automatic charging positioning control strategy for a Mecanum wheel mobile chassis. This strategy utilizes the camera on the mobile chassis to provide an efficient and low-cost solution. Summary of the Invention

[0005] The present invention aims to solve the problems existing in the above-mentioned prior art and provides a two-point positioning control strategy for a mobile robot based on a depth camera to solve the positioning and control problems of a Mecanum wheel mobile chassis during the automatic charging process.

[0006] The present invention solves the technical problem by adopting a technical solution: a two-point positioning control strategy for a mobile robot based on a depth camera, comprising the following steps:

[0007] (1) First, mark two color blocks on the charging pile, calculate the coordinates of the center of mass of the two color blocks in the camera coordinate system and the vector composed of the two center of mass, use the vector dot product formula and the inverse cosine function to calculate the angle between the chassis and the vector, and use this angle as the control variable to adjust the chassis posture.

[0008] (2) Then, the local coordinate system and global coordinate system of the vehicle are established based on the adjusted posture, and the local and global postures of the chassis are solved in combination with the motor encoder data, and then the target speed of the chassis is solved.

[0009] (3) Finally, the rotation speed of each wheel is solved according to the inverse model of the mobile chassis dynamics, thereby driving the vehicle to move towards the target charging station.

[0010] Preferably, the following formula is used to calculate the coordinates of the centroids of the two color blocks in the camera coordinate system: and :

[0011] in, , are the pixel coordinates of centroid 1, The depth value of centroid 1, , are the pixel coordinates of centroid 2, is the depth value of centroid 2, and Represents the pixel units of the camera along the x-axis and y-axis respectively, and are the x-coordinate and y-coordinate of the principal point in the image.

[0012] Preferably, the vector d between the two centroids is calculated: .

[0013] Preferably, the angle is calculated using the vector dot product formula and the arc cosine function to calculate the angle between the vector d and the Zc axis of the camera coordinate system. :

[0014] in =(0, 0, 1).

[0015] Preferably, the chassis posture is adjusted by the following algorithm:

[0016]

[0017] in represents the proportionality coefficient, represents the differential coefficient; , , , , respectively, represent the linear speeds of Mecanum wheels 1-4; W represents the left and right installation widths of the Mecanum wheels; L represents the front and rear installation lengths of the Mecanum wheels. .

[0018] Preferably, if If the absolute value of is less than the set threshold, then any center of mass is selected and the chassis pose is adjusted using the following algorithm:

[0019]

[0020] in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of pixel coordinates, the horizontal pixel coordinate value of the centroid ;

[0021] like The absolute value of is less than the threshold. At the current position, a global coordinate system and a local coordinate system that moves with the chassis are established with the chassis center as the origin. The four Mecanum wheel numbers are marked, as shown in Figure 2. In Figure 2, the Z axis of the coordinate system is perpendicular to the X and Y axes and points out of the paper.

[0022] Preferably, the position and orientation of the chassis in the local coordinate system is calculated according to the following formula:

[0023]

[0024] in, and are the displacements of the chassis along the x and y directions in the local coordinate system respectively; 、 、 、 are the displacements of wheel No. 1, wheel No. 2, wheel No. 3, and wheel No. 4 respectively; is the rotation angle of the chassis around the Z axis; and They are and Calculation correction coefficient of ; for The calculation correction coefficient of .

[0025] Preferably, the position and orientation of the chassis in the global coordinate system is calculated according to the following formula:

[0026]

[0027] in, represents the displacement of the chassis along the x-direction at time t in the local coordinate system, represents the displacement of the chassis along the y direction at time t in the local coordinate system; and They represent the displacement of the chassis along the X and Y axes of the global coordinate system at time t, Indicates the angle of the chassis along the Z-axis of the global coordinate system at time t.

[0028] Preferably, the target speed of the chassis is calculated as follows:

[0029]

[0030] in, Indicates the speed setting value along the X-axis direction of the global coordinate system. , , , represents the proportionality coefficient, , represents the differential coefficient.

[0031] Preferably, it is determined whether the charging mark is received. If so, the entire control process is terminated. Otherwise, the position and target speed of the chassis in the global coordinate system are repeatedly calculated to control the chassis to move toward the charging pile.

[0032] The beneficial effects of the present invention are as follows: the present invention provides a dual-point positioning control strategy for a mobile robot based on a depth camera, which solves the positioning and control problems of a Mecanum wheel mobile chassis during automatic charging. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 The flow chart of the control strategy is shown below;

[0034] Figure 2 is a schematic diagram of the structure of the Mecanum wheel chassis. DETAILED DESCRIPTION

[0035] The following is combined with Figure 1-2 The present invention will be further described:

[0036] Combine Figure 1 and Figure 2 , a dual-point positioning control strategy for a mobile robot based on a depth camera, its specific implementation process is as follows:

[0037] (1) Mark two color blocks on the charging pile.

[0038] (2) Read the camera data in real time and use the following formula to calculate the coordinates of the center of mass of the two color blocks in the camera coordinate system and :

[0039]

[0040] in, , are the pixel coordinates of centroid 1, The depth value of centroid 1, , are the pixel coordinates of centroid 2, is the depth value of centroid 2, and Represents the pixel units of the camera along the x-axis and y-axis respectively, and are the x-coordinate and y-coordinate of the principal point in the image.

[0041] (3) Calculate the vector d between the two centroids

[0042]

[0043] (4) Use the vector dot product formula and the arc cosine function to calculate the angle and calculate the difference between the vector and the camera coordinate system. Axis angle :

[0044]

[0045] in =(0, 0, 1).

[0046] (5) Adjust the chassis posture using the following algorithm:

[0047]

[0048] in represents the proportionality coefficient, represents the differential coefficient; , , , , respectively, represent the linear speeds of Mecanum wheels 1-4; W represents the left and right installation widths of the Mecanum wheels; L represents the front and rear installation lengths of the Mecanum wheels. .

[0049] (6) If If the absolute value of is less than the set threshold, then any center of mass is selected and the chassis pose is adjusted using the following algorithm:

[0050]

[0051] in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of pixel coordinates, the horizontal pixel coordinate value of the centroid ;

[0052] like The absolute value of is less than the threshold. At the current position, a global coordinate system and a local coordinate system that moves with the chassis are established with the chassis center as the origin. The four Mecanum wheel numbers are marked, as shown in Figure 2. In Figure 2, the Z axis of the coordinate system is perpendicular to the X and Y axes and points out of the paper.

[0053] (8) Calculate the position of the chassis in the local coordinate system according to the following formula:

[0054]

[0055] in, and are the displacements of the chassis along the x and y directions in the local coordinate system respectively; 、 、 、 are the displacements of wheel No. 1, wheel No. 2, wheel No. 3, and wheel No. 4 respectively; is the rotation angle of the chassis around the Z axis; and They are and Calculation correction coefficient of ; for The calculation correction coefficient of .

[0056] (9) Calculate the chassis position in the global coordinate system according to the following formula:

[0057]

[0058] in, represents the displacement of the chassis along the x-direction at time t in the local coordinate system, represents the displacement of the chassis along the y direction at time t in the local coordinate system; and They represent the displacement of the chassis along the X and Y axes of the global coordinate system at time t, Indicates the angle of the chassis along the Z-axis of the global coordinate system at time t.

[0059] (10) Calculate the target speed of the chassis. The calculation process is as follows:

[0060]

[0061] in, Indicates the speed setting value along the X-axis direction of the global coordinate system. , , , represents the proportionality coefficient, , represents the differential coefficient.

[0062] (11) Determine whether the charging mark is received. If it is received, the entire control process ends. Otherwise, repeat steps 9 and 10 to calculate the position and target speed of the chassis in the global coordinate system and control the chassis to move toward the charging pile.

[0063] Although the invention has been shown and described with reference to preferred embodiments, it will be understood by those skilled in the art that various changes in form and details may be made therein within the scope of the claims.

Claims

1. A dual-point positioning control strategy for a mobile robot based on a depth camera, characterized by: The following steps are involved: (1) First, mark two color blocks on the charging pile, calculate the coordinates of the center of mass of the two color blocks in the camera coordinate system and the vector formed by the two center of mass, use the vector dot product formula and the arc cosine function to calculate the angle between the chassis and the vector, and use this angle as the control variable to adjust the chassis posture; (2) Then, the local coordinate system and global coordinate system of the vehicle are established based on the adjusted posture, and the local and global postures of the chassis are solved by combining the motor encoder data, and then the target speed of the chassis is solved; (3) Finally, the rotation speed of each wheel is solved according to the inverse model of the mobile chassis dynamics, thereby driving the vehicle to move towards the target charging pile; Use the following formula to calculate the coordinates of the center of mass of the two color blocks in the camera coordinate system: and : in, , are the pixel coordinates of centroid 1, The depth value of centroid 1, , are the pixel coordinates of centroid 2, is the depth value of centroid 2, and Represents the pixel units of the camera along the x-axis and y-axis respectively, and are the x- and y-coordinates of the principal point in the image; Compute the vector d between the two centroids: ; Use the vector dot product formula and the arc cosine function to calculate the angle and calculate the angle between the vector d and the camera coordinate system. Axis angle : in =(0, 0, 1); The chassis pose is adjusted using the following algorithm: in, represents the proportionality coefficient, represents the differential coefficient; , , , , respectively, represent the linear speed of Mecanum wheels 1-4; W represents the installation width of the Mecanum wheels on the left and right; L represents the installation length of the Mecanum wheels before and after. ; Calculate the chassis pose in the global coordinate system according to the following formula: in, represents the displacement of the chassis along the x-direction at time t in the local coordinate system, represents the displacement of the chassis along the y direction at time t in the local coordinate system; and They represent the displacement of the chassis along the X and Y axes of the global coordinate system at time t, Indicates the angle of the chassis along the Z-axis of the global coordinate system at time t.

2. The dual-point positioning control strategy for a mobile robot based on a depth camera according to claim 1, characterized in that: like If the absolute value of is less than the set threshold, then any center of mass is selected and the chassis pose is adjusted using the following algorithm: in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of pixel coordinates, the horizontal pixel coordinate value of the centroid ; like The absolute value of is less than the threshold. At the current position, a global coordinate system and a local coordinate system that moves with the chassis are established with the chassis center as the origin. The four Mecanum wheel numbers are marked. The Z axis of the coordinate system is perpendicular to the X and Y axes and points out of the paper.

3. The dual-point positioning control strategy for a mobile robot based on a depth camera according to claim 2, characterized in that: Calculate the chassis pose in the local coordinate system according to the following formula: in, and are the displacements of the chassis along the x and y directions in the local coordinate system respectively; 、 、 、 are the displacements of wheel No. 1, wheel No. 2, wheel No. 3, and wheel No. 4 respectively; is the rotation angle of the chassis around the Z axis; and They are and Calculation correction coefficient of ; for Calculate the correction coefficient.

4. The dual-point positioning control strategy for a mobile robot based on a depth camera according to claim 3, characterized in that: Calculate the target speed of the chassis. The calculation process is as follows: in, Indicates the speed setting value along the X-axis direction of the global coordinate system. , , , represents the proportionality coefficient, , represents the differential coefficient.

5. The dual-point positioning control strategy for a mobile robot based on a depth camera according to claim 4, characterized in that: Determine whether the charging mark is received. If it is received, the entire control process ends. Otherwise, the chassis posture and target speed in the global coordinate system are repeatedly calculated to control the chassis to move towards the charging pile.

Images