Positioning control strategy for automatic charging process of Mecanum wheel mobile chassis based on IMU and camera

Abstract

The invention discloses a positioning control strategy for an automatic charging process of a Mecanum wheel mobile chassis based on an IMU and a camera, and belongs to the field of automatic control. The method comprises the following steps: firstly, marking a visual mark on a charging pile, adjusting a chassis to a specified angle by reading angle information of an inertial measurement unit (IMU), and adjusting the transverse displacement of the chassis to a specified position by reading a pixel coordinate of a central point of the visual mark (such as a color block) marked in a camera; and establishing a local coordinate system and a global coordinate system of the chassis according to the adjusted pose, further solving a local pose and a global pose by combining an IMU (Inertial Measurement Unit), then solving a chassis target speed according to the local pose and the global pose of the chassis and a chassis speed set value, and finally solving the rotating speed of each wheel according to a mobile chassis inverse kinematics model. Therefore, the vehicle is driven to move towards the target charging pile. The invention provides an automatic charging positioning control strategy based on the IMU and the camera, and the vehicle can be accurately moved to the position of the charging pile, so that the automatic charging function of the vehicle is realized.

Description

Technical Field

[0001] The present invention relates to the field of automatic control, and in particular to a positioning control strategy for an automatic charging process of a Mecanum wheel mobile chassis based on an IMU and a camera. Background Art

[0002] In the field of smart home and Internet of Things, automatic charging function is one of the key technologies. For example, automatic sweeping robots, electric vehicles, mobile chassis and other devices all need automatic charging function to ensure that they can continue to work and enhance user experience.

[0003] At present, common automatic charging solutions include infrared solutions, laser radar solutions, image recognition solutions, etc. The infrared solution requires the installation of two infrared transmitting tubes on the charging pile. The device to be charged determines which infrared light signal area it is located in and its relative posture with the charging pile based on the data from the infrared receiving tube, thereby controlling the device to be charged to move to the charging pile for charging. The laser radar solution distinguishes different concave and convex areas on the surface of the charging pile, and uses the characteristics of high reflectivity in the concave area and high absorption rate in the convex area to identify these specific areas for recharging. However, both solutions require changes to the structure of the charging pile, which is costly and has deviations in positioning accuracy.

[0004] The image recognition solution uses a depth camera to identify the QR code or other images attached to the charging pile, calculates the relative position of the depth camera and the QR code, and then controls the charging device to be charged. Although the image recognition solution does not require changes to the structure of the charging pile, it relies on the depth camera and the algorithm is complex.

[0005] In order to reduce the changes to the charging pile structure or the dependence on specific equipment during the automatic recharging process, the present invention proposes a universal automatic charging positioning control strategy for a Mecanum wheel mobile chassis based on IMU and camera. This strategy uses the IMU and camera on the mobile chassis to provide an efficient and low-cost solution. Summary of the invention

[0006] The present invention aims to solve the problems existing in the above-mentioned prior art, and provides a positioning control strategy for the automatic charging process of a Mecanum wheel mobile chassis based on IMU and camera, so as to solve the positioning and control problems of the Mecanum wheel mobile chassis during the charging process.

[0007] The present invention solves the technical problem by adopting a technical solution: an automatic charging positioning control strategy for a Mecanum wheel mobile chassis based on an IMU and a camera, comprising the following steps: (1) Adjust the chassis to the specified angle by reading the IMU angle information, adjust the chassis lateral displacement to the specified position by reading the centroid pixel coordinates of the marked color block in the camera, and establish the chassis global coordinate system with the adjusted posture as the starting point, and then solve the local posture and global posture of the chassis in combination with the IMU; (2) The chassis target speed is then calculated based on the chassis posture, and finally the wheel speeds are calculated based on the inverse model of the mobile chassis dynamics, thereby driving the vehicle to move toward the target charging station.

[0008] Preferably, read the yaw angle value of the IMU and calculate the angle error : (1) in, is the angle during charging; The yaw angle measured by the IMU.

[0009] Preferably, if If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: (2) in represents the proportionality coefficient, represents the differential coefficient; , , , , respectively represent the linear speeds of Mecanum wheels 1-4; W represents the installation width of the Mecanum wheels on the left and right sides; L represents the installation length of the Mecanum wheels on the front and back sides.

[0010] like If the absolute value of is less than the threshold, the camera data is read and the horizontal pixel coordinate value of the color block centroid is calculated through the OpenCV library. .

[0011] Preferably, the pixel coordinate difference of the centroid is calculated : (3) in, The reference value for pixel coordinates.

[0012] Preferably, if If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: (4) in represents the proportionality coefficient, represents the differential coefficient.

[0013] like The absolute value of is less than the threshold value. 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.

[0014] Preferably, the position and posture of the chassis in the local coordinate system is calculated according to the following formula: (5) 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 Calculate the correction coefficient of .

[0015] Preferably, the position and posture of the chassis in the global coordinate system is calculated according to the following formula: (6) 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.

[0016] Preferably, the formula for calculating the speed error of the chassis is as follows: (7) in .

[0017] Preferably, the formula for calculating the target speed of the chassis is as follows (8) in, Indicates the speed setting value along the X-axis direction of the global coordinate system.

[0018] Preferably, it is determined whether a charging mark is received, and if so, the entire control process is terminated, otherwise, the chassis continues to be controlled to move toward the charging pile.

[0019] The beneficial effect of the present invention is that the present invention provides a positioning control strategy for the automatic charging process of a Mecanum wheel mobile chassis based on an IMU and a camera, which solves the positioning and control problems of the Mecanum wheel mobile chassis during the charging process. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 The flow chart for the control strategy is shown; Figure 2 This 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 is further described: Combination Figure 1 and Figure 2 This embodiment discloses a positioning control strategy for the automatic charging process of a Mecanum wheel mobile chassis based on an IMU and a camera. The specific implementation process is as follows: (1) Mark a color block on the charging station.

[0022] (2) Read the yaw angle value of the IMU and calculate the angle error : (1) in, is the angle during charging; The yaw angle measured by the IMU.

[0023] (3) If If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: (2) in represents the proportionality coefficient, represents the differential coefficient; , , , , respectively represent the linear speeds of Mecanum wheels 1-4; W represents the installation width of the Mecanum wheels on the left and right sides; L represents the installation length of the Mecanum wheels on the front and back sides.

[0024] (4) If If the absolute value of is less than the threshold, the camera data is read and the horizontal pixel coordinate value of the color block centroid is calculated through the OpenCV library. .

[0025] (5) Calculate the pixel coordinate difference of the centroid : (3) in, The reference value for pixel coordinates.

[0026] (6) If If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: (4) in represents the proportionality coefficient, represents the differential coefficient.

[0027] (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, such as REF _Ref157792281 \h Figure 2 As shown. In REF _Ref157792281 \h Figure 2 The Z axis of the median coordinate system is perpendicular to the X axis and the Y axis and points out of the paper.

[0028] (8) Calculate the position and posture of the chassis in the local coordinate system according to the following formula: (5) 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 Calculate the correction coefficient of .

[0029] (9) Calculate the position and posture of the chassis in the global coordinate system according to the following formula: (6) 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.

[0030] (10) Calculate the chassis speed error according to the following formula: (7) in , .

[0031] (11) Calculation of chassis target speed The calculation module is used to calculate the target speed of the chassis. The calculation process is as follows: (8) in, Indicates the speed setting value along the X-axis direction of the global coordinate system.

[0032] (12) Determine whether the charging mark is received. If it is received, the entire control process ends. Otherwise, repeat steps 8 to 11 to control the chassis to move toward the charging pile.

[0033] 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. An automatic charging positioning control strategy for a Mecanum wheel mobile chassis based on IMU and camera, characterized in that: The following steps are involved: (1) Adjust the chassis to the specified angle by reading the IMU angle information, adjust the chassis lateral displacement to the specified position by reading the centroid pixel coordinates of the marked color block in the camera, and establish the chassis global coordinate system with the adjusted posture as the starting point, and then solve the local posture and global posture of the chassis in combination with the IMU; (2) The chassis target speed is then calculated based on the chassis posture, and finally the wheel speeds are calculated based on the inverse model of the mobile chassis dynamics, thereby driving the vehicle to move toward the target charging station.

2. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 1 is characterized by: Read the yaw angle value of the IMU and calculate the angle error : （1） in, is the angle during charging; The yaw angle measured by the IMU.

3. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 2 is characterized by: like If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: （2） in represents the proportionality coefficient, represents the differential coefficient; , , , , respectively represent the linear speeds 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 on the front and back; like If the absolute value of is less than the threshold, the camera data is read and the horizontal pixel coordinate value of the color block centroid is calculated through the OpenCV library. .

4. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 1 is characterized by: Calculate the pixel coordinate difference of the centroid : （3） in, The reference value for pixel coordinates.

5. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 4 is characterized by: like If the absolute value of is greater than the threshold, the chassis posture is adjusted by the following algorithm: （4） in represents the proportionality coefficient, represents the differential coefficient; like The absolute value of is less than the threshold value. 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.

6. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 5 is characterized by: According to the following formula, the position and posture of the chassis in the local coordinate system are calculated: （5） 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 .

7. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 6 is characterized by: According to the following formula, calculate the position of the chassis in the global coordinate system: （6） 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.

8. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 7 is characterized by: The formula for calculating the chassis speed error is as follows: （7） in .

9. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 8 is characterized by: The formula for calculating the target speed of the chassis is as follows （8） in, Indicates the speed setting value along the X-axis direction of the global coordinate system.

10. The automatic charging positioning control strategy of the Mecanum wheel mobile chassis based on IMU and camera according to claim 9 is characterized by: Determine whether the charging mark is received. If it is received, end the entire control process. Otherwise, continue to control the chassis to move toward the charging pile.

Images