Mobile robot double-point positioning control strategy based on depth camera

Abstract

The invention discloses a mobile robot double-point positioning control strategy based on a depth camera, and belongs to the field of automatic control. The method comprises the following steps: marking two color blocks on a charging pile, calculating coordinates of mass centers of the two color blocks in a camera coordinate system and a vector composed of the two mass centers, calculating an angle between a chassis and the vector by using a vector dot product formula and an arc cosine function, and adjusting the pose of the chassis by taking the angle as a control variable. And then a local coordinate system and a global coordinate system of the vehicle are established according to the adjusted poses, the local pose and the global pose of the chassis are solved in combination with data of a motor encoder, and then the target speed of the chassis is solved. And finally, solving the rotating speed of each wheel according to the dynamic inverse model of the mobile chassis so as to drive the vehicle to move towards the target charging pile. The invention provides a mobile robot double-point positioning control strategy based on a depth camera. The vehicle can be accurately moved to the position of a charging pile, and therefore the automatic charging function of the vehicle is achieved.

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] In the field of smart home and Internet of Things, the automatic charging function is one of the key technologies. The automatic charging function reduces the user's operation steps and provides great convenience. Users do not need to manually plug in and out the charger or find a power socket. They only need to place the device in the charging area and the device will automatically start charging.

[0003] At present, although there are automatic charging solutions based on image recognition, which use depth cameras to recognize 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] In view of the above problems, the present invention proposes a universal camera-based dual-point automatic charging positioning control strategy for a Mecanum wheel mobile chassis. The strategy uses 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 provide a mobile robot dual-point positioning control strategy based on a depth camera to solve the positioning and control problems of a Mecanum wheel mobile chassis during automatic charging.

[0006] The present invention solves the technical problem by adopting the following technical solutions: 1. A two-point positioning control strategy for a mobile robot based on a depth camera, comprising the following steps: (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 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.

[0007] (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, thereby solving the chassis target speed.

[0008] (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.

[0009] Preferably, the following formula is used to calculate the coordinates (xc1, yc1, zc1) and (xc2, yc2, zc2) of the centroids of the two color blocks in the camera coordinate system: Among them, u1, v1 are the pixel coordinates of centroid 1, z1 is the depth value of centroid 1, u2, v2 are the pixel coordinates of centroid 2, z2 is the depth value of centroid 2, fx and fy represent the pixel units of the camera along the x-axis and y-axis, respectively, and cx and cy are the x-coordinate and y-coordinate of the principal point in the image, respectively.

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

[0011] 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: : in =(0, 0, 1).

[0012] Preferably, the chassis posture is adjusted by 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, .

[0013] Preferably, if If the absolute value of is less than the set threshold, then select a centroid and adjust the chassis pose using the following algorithm: in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of the pixel coordinate, the horizontal pixel coordinate value of the centroid .

[0014] like The absolute value of is less than the threshold value. At the current position, the global coordinate system and the local coordinate system that moves with the chassis are established with the chassis center origin, and 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 axis and the Y axis and points outside the paper.

[0015] Preferably, the position and posture of the chassis in the local coordinate system is calculated according to the following formula: Wherein, Dcar_lx and Dcar_ly are the displacements of the chassis in the local coordinate system along the x and y directions respectively; Dwheel1, Dwheel12, Dwheel13, and Dwheel14 are the displacements of wheel 1, wheel 2, wheel 3, and wheel 4 respectively; Acar_l is the rotation angle of the chassis around the Z axis; cx and cy are the calculation correction coefficients of Dcar_lx and Dcar_ly respectively; and cA is the calculation correction coefficient of Acar_l.

[0016] Preferably, the position and posture of the chassis in the global coordinate system is calculated according to the following formula: in, represents the displacement of the chassis along the x direction at time t in the local coordinate system, It represents the displacement of the chassis along the y-axis at time t in the local coordinate system. Dcar_gx[t] and Dcar_gy[t] represent the displacement of the chassis along the X-axis and Y-axis directions of the global coordinate system at time t, respectively. Indicates the angle of the chassis along the Z-axis of the global coordinate system at time t.

[0017] Preferably, the target speed of the chassis is calculated, and 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.

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

[0019] The beneficial effect of the present invention is that 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

[0020] Figure 1 The flow chart for the control strategy is shown; Figure 2 is a schematic diagram of the structure of the Mecanum wheel chassis. DETAILED DESCRIPTION

[0021] The following is combined with Figure 1-2 The present invention will be further described: Combination Figure 1 and Figure 2 , a dual-point positioning control strategy for a mobile robot based on a depth camera, and its specific implementation process is as follows: (1) Mark two color blocks on the charging pile.

[0022] (2) Read the camera data in real time and use the following formula to calculate the coordinates of the centroid of the two color blocks in the camera coordinate system (x c1 , y c1 , z c1 ) and (x c2 , y c2 , z c2 ): MACROBUTTON AMMPlaceRM \* MERGEFORMAT SEQAMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \* MERGEFORMAT 1) Among them, u1, v1 are the pixel coordinates of centroid 1, z1 is the depth value of centroid 1, u2, v2 are the pixel coordinates of centroid 2, z2 is the depth value of centroid 2, and f x and f y Represents the pixel units of the camera along the x-axis and y-axis, c x and c y are the x and y coordinates of the principal point in the image.

[0023] (3) Calculate the vector d between the two centroids MACROBUTTON AMMPlaceRM \*MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 2) (4) Use the vector dot product formula and the arc cosine function to calculate the angle and calculate the Z of the vector and the camera coordinate system c Axis angle : MACROBUTTON AMMPlaceRM \* MERGEFORMAT SEQAMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \* MERGEFORMAT 3) in =(0, 0, 1). (5) Adjust the chassis posture using the following algorithm: MACROBUTTON AMMPlaceRM \*MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 5) 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, .

[0024] (6) If If the absolute value of is less than the set threshold, then select a centroid and adjust the chassis pose using the following algorithm: MACROBUTTON AMMPlaceRM \*MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 7) in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of the pixel coordinate, the horizontal pixel coordinate value of the centroid .

[0025] (7) If The absolute value of is less than the threshold value. At the current position, the global coordinate system and the local coordinate system that moves with the chassis are established with the chassis center origin, and the four Mecanum wheel numbers are marked, as shown in REF _Ref157792281 \h Figure 2. In REF _Ref157792281 \h Figure 2, the Z axis of the coordinate system is perpendicular to the X axis and the Y axis and points outside the paper.

[0026] (8) Calculate the position and posture of the chassis in the local coordinate system according to the following formula: MACROBUTTON AMMPlaceRM \*MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 8) Among them, D car_lx and D car_ly are the displacements of the chassis along the x and y directions in the local coordinate system; D wheel1 , D wheel12 , D wheel13 , D wheel14 are the displacements of wheel 1, wheel 2, wheel 3, and wheel 4 respectively; A car_l is the rotation angle of the chassis around the Z axis; c x and c y D car_lx and D car_ly The calculation correction coefficient of A A car_l The calculation correction coefficient of .

[0027] (9) Calculate the position and posture of the chassis in the global coordinate system according to the following formula: MACROBUTTON AMMPlaceRM\* MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 9) in, represents the displacement of the chassis along the x direction at time t in the local coordinate system, It represents the displacement of the chassis along the y direction at time t in the local coordinate system. car_gx [t] and D car_gy [t] represents 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] (10) Calculate the target speed of the chassis. The calculation process is as follows: MACROBUTTON AMMPlaceRM \*MERGEFORMAT SEQ AMEqn \h \* MERGEFORMAT ( SEQ AMEqn \c \* Arabic \*MERGEFORMAT 11) in, Indicates the speed setting value along the X-axis direction of the global coordinate system. , , represents the proportionality coefficient, , represents the differential coefficient.

[0029] (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.

[0030] Although the present 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 in that: 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 composed of 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 the 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 in combination with the motor encoder data, thereby solving the chassis target speed; (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.

2. According to claim 1, a mobile robot dual-point positioning control strategy based on a depth camera is characterized in that: The following formula is used to calculate the coordinates (xc1, yc1, zc1) and (xc2, yc2, zc2) of the centroid of the two color blocks in the camera coordinate system: Among them, u1, v1 are the pixel coordinates of centroid 1, z1 is the depth value of centroid 1, u2, v2 are the pixel coordinates of centroid 2, z2 is the depth value of centroid 2, fx and fy represent the pixel units of the camera along the x-axis and y-axis, respectively, and cx and cy are the x-coordinate and y-coordinate of the principal point in the image, respectively.

3. According to claim 2, a mobile robot dual-point positioning control strategy based on a depth camera is characterized in that: Compute the vector d between the two centroids: .

4. According to claim 3, a mobile robot dual-point positioning control strategy based on a depth camera is characterized in that: Use 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. : in =(0, 0, 1).

5. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 4 is characterized in that: The chassis posture 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, .

6. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 5, characterized in that: like If the absolute value of is less than the set threshold, then select a centroid and adjust the chassis pose using the following algorithm: in represents the proportionality coefficient, represents the differential coefficient, , is the reference value of the pixel coordinate, the horizontal pixel coordinate value of the centroid ; like The absolute value of is less than the threshold value. At the current position, the global coordinate system and the local coordinate system that moves with the chassis are established with the chassis center origin, and 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 axis and the Y axis and points outside the paper.

7. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 6, characterized in that: According to the following formula, the position and posture of the chassis in the local coordinate system are calculated: Wherein, Dcar_lx and Dcar_ly are the displacements of the chassis in the local coordinate system along the x and y directions respectively; Dwheel1, Dwheel12, Dwheel13, and Dwheel14 are the displacements of wheel 1, wheel 2, wheel 3, and wheel 4 respectively; Acar_l is the rotation angle of the chassis around the Z axis; cx and cy are the calculation correction coefficients of Dcar_lx and Dcar_ly respectively; and cA is the calculation correction coefficient of Acar_l.

8. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 7, characterized in that: According to the following formula, calculate the position of the chassis in the global coordinate system: in, represents the displacement of the chassis along the x direction at time t in the local coordinate system, It represents the displacement of the chassis along the y-axis at time t in the local coordinate system. Dcar_gx[t] and Dcar_gy[t] represent the displacement of the chassis along the X-axis and Y-axis directions of the global coordinate system at time t, respectively. Indicates the angle of the chassis along the Z-axis of the global coordinate system at time t.

9. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 8, 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.

10. The dual-point positioning control strategy of a mobile robot based on a depth camera according to claim 9, characterized in that: Determine whether the charging mark is received. If it is received, the entire control process ends. 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.

Images