A control method and system for a four-drive Mecanum wheel mobile chassis

Abstract

The present invention discloses a control method and system for a four-wheel drive Mecanum wheel mobile chassis, belonging to the field of automatic control. The target driving path of the mobile chassis is obtained, and multiple path points are obtained; the path point closest to the mobile chassis is used as the target point; a local coordinate system and a global coordinate system are established, and the local posture is calculated based on the displacement of each wheel of the mobile chassis at the current moment, and then the global posture is calculated; the linear velocity of each wheel of the four-wheel drive Mecanum wheel mobile chassis is calculated, and the mobile chassis is controlled; it is judged whether the mobile chassis reaches the target point after control, and if not, the control is continued; otherwise, it is judged whether the target point is the end point of the target driving path, and if so, the control is stopped; otherwise, the mobile chassis is controlled to move to the next target point. The present invention can effectively solve the problems of drift and deviation from the target yaw angle during the movement of the mobile chassis, and ensure that the mobile chassis can move according to the specified route.

Description

Technical Field

[0001] The present invention belongs to the field of automatic control, and in particular relates to a control method and system strategy for a four-drive Mecanum wheel mobile chassis. Background Art

[0002] Four-wheel drive Mecanum wheel mobile chassis are compact, highly flexible, and can move in any direction. Therefore, they are often used to carry robotic arms, cameras, radars, IMUs (inertial measurement units), and other equipment for operational purposes. In practical applications, the yaw angle of a mobile chassis is typically obtained using an IMU or odometry. However, IMU measurements have a random walk bias, and the yaw angle calculated by the odometry cannot measure the angular deviation of the chassis when it slips. Furthermore, issues such as chassis manufacturing and assembly errors and motor control accuracy can cause the chassis to drift laterally or deviate from the target yaw angle during movement. Existing path tracking algorithms typically track a fixed sequence of waypoints. If the chassis deviates from the path or the spacing between waypoints is uneven, this can lead to cumulative deviations or inability to accurately regress the path. Traditional path tracking methods require complex index management or foresight point calculation logic. Especially in dynamic environments, the selection of path target points often requires additional control mechanisms. Furthermore, when waypoints are updated or the robot deviates from the global path, traditional methods require replanning the entire path, lacking the ability to dynamically adjust. Therefore, in order to ensure that the mobile chassis can move along the specified route, a new control strategy is needed. Summary of the Invention

[0003] In order to solve the problems in the prior art, the present invention proposes a control method and system for a four-drive Mecanum wheel mobile chassis.

[0004] The technical solution adopted in the present invention is as follows:

[0005] In a first aspect, the present invention discloses a method for controlling a four-drive Mecanum wheel mobile chassis, comprising the following steps:

[0006] 1) Obtain the target driving path of the mobile chassis and discretize the target driving path to form multiple path points;

[0007] 2) Obtain the path point closest to the mobile chassis and use this path point as the target point;

[0008] 3) Establishing a local coordinate system at the center of the mobile chassis that moves with the mobile chassis and a global coordinate system; calculating the local pose of the mobile chassis based on the displacement of each wheel of the mobile chassis at the current moment, and then calculating the global pose of the mobile chassis based on the local pose;

[0009] 4) Calculating the velocity error of the mobile chassis based on the local and global poses of the mobile chassis;

[0010] 5) calculating a target speed of the mobile chassis based on the speed error and a speed setting value of the mobile chassis in a global coordinate system;

[0011] 6) calculating the linear velocity of each wheel of the mobile chassis based on the target velocity, and controlling the mobile chassis according to the linear velocity;

[0012] 7) Determine whether the mobile chassis has reached the target point based on the position of the mobile chassis after control is completed. If it has not reached the target point, control the mobile chassis to move to the target point based on steps 3)-6). If it has reached the target point, further determine whether the target point is the end point of the target driving path. If it is the end point, control of the mobile chassis ends; otherwise, continue to control the mobile chassis to move to the next target point based on steps 2)-7).

[0013] In a second aspect, the present invention discloses a four-drive Mecanum wheel mobile chassis control system for implementing the control method, comprising:

[0014] The target point acquisition module is used to obtain the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; obtain the path point closest to the mobile chassis and use the path point as the target point;

[0015] A posture calculation module is used to establish a local coordinate system that moves with the mobile chassis at the center of the mobile chassis and a global coordinate system; the local posture of the mobile chassis is calculated based on the displacement of each wheel of the mobile chassis at the current moment, and the global posture of the mobile chassis is calculated based on the local posture; the posture error calculation module is used to calculate the velocity error of the mobile chassis based on the local posture and global posture of the mobile chassis;

[0016] a chassis target speed calculation module, configured to calculate a target speed of the mobile chassis based on the speed error and a speed setting value of the mobile chassis in a global coordinate system;

[0017] a wheel speed calculation module, configured to calculate the linear speed of each wheel of the mobile chassis based on the target speed, and control the mobile chassis according to the linear speed;

[0018] The judgment and control module is used to judge whether the mobile chassis has reached the target point based on the posture of the mobile chassis after the control is completed. If it has not reached the target point, the mobile chassis is controlled to move to the target point; if it has reached the target point, it is further judged whether the target point is the end point of the target driving path. If it is the end point, the control of the mobile chassis is terminated; otherwise, the control of the mobile chassis to the next target point is continued.

[0019] Compared with the prior art, the present invention has the following beneficial effects:

[0020] 1) The present invention selects the nearest path point as the tracking target in real time, enabling the robot to dynamically adjust the target point, correct path deviation, avoid deviation caused by a fixed path point sequence, and improve path tracking accuracy;

[0021] 2) The present invention does not rely on pathpoint index management, but instead dynamically updates the tracking target by directly calculating the nearest pathpoint. The algorithm is simple and reliable, with low computational cost, and simplifies the path target selection logic;

[0022] 3) When the 4WD Mecanum wheel mobile chassis deviates from the path or the environment undergoes dynamic changes, it can automatically select the new nearest path point as the target without replanning the entire path, thereby significantly improving the robustness of path tracking and enhancing the flexibility and adaptability of path tracking.

[0023] 4) Regardless of whether the spacing between path points is uniform, the present invention can dynamically select target points and achieve smooth tracking. It is particularly suitable for scenarios where path points are sparse or dense, and can adapt to situations where the distribution of path points is diverse.

[0024] 5) In the case of path updating or obstacle detour, the present invention can combine the obstacle avoidance mechanism to quickly select new path points as targets, and has strong environmental adaptability.

[0025] 6) Since navigation to a new target point is restarted each time the target point is reached, the errors accumulated during the previous stage of navigation can be eliminated in the next target point navigation. Therefore, the present invention can effectively solve the problems of lateral movement or deviation from the target yaw angle of the chassis during movement due to errors such as chassis processing and assembly errors and motor control accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 Block diagram of the control strategy for the four-wheel drive Mecanum wheel mobile chassis;

[0027] Figure 2 This is a structural diagram of a four-wheel drive Mecanum wheel chassis;

[0028] Figure 3 Mobile navigation chart for a four-wheel drive Mecanum wheel mobile chassis;

[0029] Figure 4 The displacement deviation diagram of the chassis moving with four-wheel drive Mecanum wheels;

[0030] Figure 5 Angular deviation diagram for the chassis moving with four-wheel drive Mecanum wheels. DETAILED DESCRIPTION

[0031] The present invention will be further described and illustrated below in conjunction with specific embodiments. The embodiments are merely illustrative of the present disclosure and do not limit its scope. The technical features of the various embodiments of the present invention may be combined accordingly, provided that there is no conflict between them.

[0032] First, the four-wheel drive Mecanum wheel mobile chassis of the present invention includes an IMU, four wheels, and a motor for controlling the rotation of the four wheels, and each wheel is controlled by a separate motor. When controlling the mobile chassis, the present invention controls the mobile chassis in sequence. First, the target driving path of the mobile chassis is obtained, and the target driving path is discretized by a proportional interpolation method to form multiple path points. For the first control, the path point closest to the four-wheel drive Mecanum wheel mobile chassis is first obtained, and the path point is used as the target point; based on the initial position of the mobile chassis (the position includes the rotation angle around the z-axis, the displacement in the x-axis direction, and the displacement in the y-axis direction), a global coordinate system and a local coordinate system that moves with the mobile chassis are established, and the global position and local position are calculated; the speed error of the mobile chassis is collaboratively calculated based on the local position and the global position; then the target speed of the mobile chassis is calculated; finally, the linear velocity of each wheel of the four-wheel drive Mecanum wheel mobile chassis is calculated, and the mobile chassis is controlled according to the linear velocity to drive the mobile chassis toward the current target point. At the same time, the mobile chassis is monitored in real time to see if it has reached the current target point, enabling dynamic adaptation to path deviations or environmental changes. If it has not reached the target point, the mobile chassis continues to be controlled using the current target point as the target point. If it has reached the target point, it is further determined whether it is the final target point (i.e., the end point of the target driving path). If the mobile chassis has reached the final target point of the path, the control process ends; otherwise, the next control loop is entered, and the posture at the end of this control is used as the initial posture for the next control. By continuously repeating the above successive control logic, the mobile chassis eventually completes the path tracking task from the starting point to the end point.

[0033] like Figure 1 Figure 1 shows a block diagram of a four-wheel drive Mecanum wheel mobile chassis control method. The motion of the Mecanum wheel mobile chassis is controlled based on the yaw angle of the IMU and the displacement of each wheel. The specific implementation process is as follows:

[0034] (1) Obtaining the target driving path of the mobile chassis and discretizing the target driving path to form multiple path points;

[0035] (2) Obtain the path point closest to the mobile chassis and use this path point as the target point;

[0036] (3) Establish a local coordinate system at the center of the mobile chassis that moves with the mobile chassis, and establish a global coordinate system; calculate the local pose of the mobile chassis based on the displacement of each wheel of the mobile chassis at the current moment, and then calculate the global pose of the mobile chassis based on the local pose; and mark the four Mecanum wheel numbers, such as Figure 2 As shown. Figure 2 The Z axis of the local coordinate system is perpendicular to the X axis and the Y axis and points out of the paper. That is, when establishing the local coordinate system, the direction of the mobile chassis is used as the direction of the x axis of the local coordinate system, and the opposite direction of gravity is used as the direction of the z axis of the local coordinate system. The direction of the y axis of the local coordinate system is determined according to the right-hand rule.

[0037] When establishing a global coordinate system, the rotation angle of the mobile chassis is determined based on the IMU on the mobile chassis. The direction of the mobile chassis is used as the direction of the x-axis of the global coordinate system, the direction of the z-axis is used as the opposite direction of gravity, and the direction of the y-axis of the global coordinate system is determined according to the right-hand rule.

[0038] Specifically, the local pose can be calculated by the following formula:

[0039]

[0040] Among them, D car_lx [t] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t; D car_ly [t] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t; D whee1 [t]、D wheel2 [t]、D wheel3 [t]、D wheel4 [t] are the displacements of the four wheels of the four-wheel drive Mecanum wheel mobile chassis at time t; A car_l [t] is the rotation angle of the mobile chassis around the Z axis in the local coordinate system at time t; c x D car_lx Calculation correction coefficient of [t]; c y D car_ly Calculation correction coefficient of [t]; c A A car_l [t] is the calculation correction coefficient; W is the width of the mobile chassis; L is the length of the mobile chassis.

[0041] The calculation process of the global pose is as follows:

[0042] 1) Based on the rotation angle A car_l [t], convert the rotation angle value into a value between 0 and 2π, and obtain the yaw angle A of the mobile chassis in the local coordinate system car_ll [t],

[0043]

[0044] Among them, A car_ll [t] is the yaw angle of the mobile chassis in the local coordinate system at time t; c n The number of revolutions of the mobile chassis.

[0045] 2) Based on the yaw angle A obtained in step 1) car_ll [t] Calculate the yaw angle A of the mobile chassis around the Z axis in the global coordinate system car_g [t];

[0046]

[0047] Among them, A car_g [t] is the yaw angle of the mobile chassis around the Z axis at time t in the global coordinate system; V rx V is the speed setting value of the mobile chassis along the x-axis in the local coordinate system; ry V is the speed setting value of the mobile chassis along the y-axis in the local coordinate system; rA is the speed setting value of the mobile chassis around the z-axis in the local coordinate system, ΔA car_ll [t]=A car_ll [t]-A car_ll [t-1],A car_ll [t] is the yaw angle at time t, A car_ll [t-1] is the yaw angle at time t-1; ΔA c =A car_ll [t]-A car_ll [0], A car_ll [0] is the yaw angle at time 0; ΔA I =(A IMU [t]-A IMU [0]), A IMU [t] represents the rotation angle of the mobile chassis output by the IMU at time t, A IMU [0] represents the rotation angle of the mobile chassis output by the IMU at time 0, ΔA I It represents an intermediate variable and has no practical meaning; η is a preset value; ΔA Ic Indicates an intermediate variable and has no practical significance; ΔA Ic =(A IMU [t]-A IMU [0])-(A car_ll [t]-A car_ll [0]).

[0048] 3) Calculate D car_lx and D car_lyThe difference between two adjacent sampling times (i.e., the displacement difference of the mobile chassis along the x-axis and y-axis in the local coordinate system is calculated based on the local posture); the displacement difference of the mobile chassis along the x-axis and y-axis in the local coordinate system is:

[0049]

[0050] Among them, diff_D car_lx [t] is D car_lx The difference between time t and time t-1, D car_lx is the displacement of the mobile chassis along the x-axis in the local coordinate system (i.e. diff_D car_lx [t] is D car_lx [t] and D car_lx [t-1] difference); D car_lx [t] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t; D car_lx [t-1] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t-1; diff_D car_ly [t] is D car_ly The difference between time t and time t-1, D car_ly is the displacement of the mobile chassis along the y-axis in the local coordinate system (i.e. diff_D car_ly [t] is D car_ly [t] and D car_ly [t-1] difference); D car_ly [t] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t; D car_ly [t-1] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t-1.

[0051] 4) Based on the displacement difference diff_D in the X and Y axis directions in the local coordinate system obtained in step 3) car_lx [t] and diff_D car_ly [t] Calculate the displacement difference of the mobile chassis along the x-axis of the global coordinate system between two consecutive samplings:

[0052] diff_D car_gx [t] = cos(A car_g [t])*diff_D car_lx [t]+sin(A car_g [t])*diff_D car_ly [t]

[0053] Among them, diff_D car_gx [t] is the displacement difference of the mobile chassis along the x-axis of the global coordinate system at time t.

[0054] 5) Based on the displacement difference diff_D in the X and Y axis directions in the local coordinate system obtained in step 3) car_lx [t] and diff_D car_ly [t] Calculate the displacement difference of the mobile chassis along the y-axis of the global coordinate system between two consecutive samplings:

[0055] diff_D car_gy [t]=sin(A car_g [t])*diff_D car_lx [t]+cos(A car_g [t])*diff_D carl_y [t]

[0056] Among them, diff_D car_gy [t] is the displacement difference of the mobile chassis along the Y-axis of the global coordinate system at time t.

[0057] 6) Based on the displacement difference diff_D in the x- and y-axis directions in the global coordinate system obtained in steps 4) and 5) car_gx [t] and diff_D car_gy [t] Calculate the displacement of the mobile chassis along the x and y axes in the global coordinate system:

[0058]

[0059] Among them, D car_gx [t] and D car_gy [t] represents the displacement of the mobile chassis along the x and y axes of the global coordinate system at time t, respectively; n is the total time the mobile chassis was last controlled. The yaw angle of the mobile chassis about the Z axis and the displacement of the mobile chassis along the x and y axes in the global coordinate system together constitute the global pose of the mobile chassis in the global coordinate system.

[0060] (4) Calculate the velocity error of the mobile chassis based on the local and global poses of the mobile chassis. Specifically, the velocity error V of the mobile chassis rotating around the z-axis of the local coordinate system is err_A for:

[0061] V rA =0

[0062] Among them, V err_A [t] represents the speed error of the mobile chassis rotating around the Z axis of the local coordinate system at time t; k p1 、k i1 、k d1 Calculate V err_A [t] proportional term coefficient, integral term coefficient and differential term coefficient; e A [t]=A car_r [t]-Acar_g [t]; e A [t] is an intermediate variable and has no actual meaning; A car_r [t] is the yaw angle setting value of the mobile chassis around the Z axis in the global coordinate system at time t.

[0063] In the local coordinate system, the velocity error V of the mobile chassis in the y-axis direction is err_y for:

[0064]

[0065] Among them, k p2 、k i2 、k d2 Respectively represent the calculation of V err_y 1 The proportional term coefficient, integral term coefficient and differential term coefficient of [t]; V err_y 1 [t] is time t and V rx ≠0,V ry =0,V rA =0,e A When ε<ε, the speed error of the moving chassis in the y-axis direction in the local coordinate system; k p3 、k i3 、k d3 Respectively represent the calculation of V err_y 2 [t] proportional term coefficient, integral term coefficient and differential term coefficient; V err_y 2 [t] is time t and V rx =0,V ry =0,V rA When ≠0, the speed error of the moving chassis in the y-axis direction in the local coordinate system; ε is the preset value. e 1y [t]、e 2y [t] is an intermediate variable and has no actual meaning; D car_ly [0] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time 0.

[0066] In the local coordinate system, the velocity error V of the mobile chassis in the x-axis direction is err_x for:

[0067]

[0068] Among them, k p4 、k i4 、k d4 Respectively represent the calculation of V err_x 1[t] proportional term coefficient, integral term coefficient and differential term coefficient; V err_x 1 [t] is time t and V rx =0,V ry ≠0,V rA =0,e A When ε<ε, the speed error of the mobile chassis in the x-axis direction in the local coordinate system; k p5 、k i5 、k d5 Respectively represent the calculation of V err_x 2 [t] proportional term coefficient, integral term coefficient and differential term coefficient; V err_x 2 [t] is time t and V rx =0,V ry =0,V rA When ≠0, the speed error of the moving chassis in the x-axis direction in the local coordinate system;

[0069]

[0070] e 1x [t]、e 2x [t] is an intermediate variable and has no actual meaning; D car_lx [0] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time 0.

[0071] (4) Calculate the target speed of the mobile chassis based on the speed error of the mobile chassis rotating around the z-axis of the local coordinate system, the speed error in the Y-axis direction, and the speed error in the X-axis direction obtained in step (3). The target speed of the mobile chassis is:

[0072]

[0073] Among them, V tx [t] represents the target speed of the mobile chassis in the x-axis direction in the local coordinate system at time t; V ty [t] represents the target speed of the mobile chassis in the y-axis direction in the local coordinate system at time t; V tA [t] represents the target speed of the mobile chassis rotating around the Z axis in the local coordinate system at time t; V err_x [t] represents the velocity error of the mobile chassis in the x-axis direction in the local coordinate system at time t; V err_y [t] represents the velocity error of the mobile chassis in the y-axis direction in the local coordinate system at time t; V err_A [t] represents the velocity error of the mobile chassis rotating around the Z axis in the local coordinate system at time t.

[0074] (6) Based on the target speed V obtained in step (5) tx [t]、V ty [t] and V tA [t] Calculate the linear velocity between each wheel, the linear velocity of each wheel is:

[0075]

[0076] Among them, V wheel1 [t]、V wheel2 [t]、V wheel3 [t]、V wheel4 [t] are the linear velocities of the four wheels of the four-wheel drive Mecanum wheel mobile chassis.

[0077] (7) Determine whether the mobile chassis has reached the target point based on the position of the mobile chassis after the control is completed. If it has not reached the target point, then control the mobile chassis to move to the target point based on steps (3)-(6); if it has reached the target point, then further determine whether the target point is the end point of the target driving path. If it is the end point, then stop the control; otherwise, continue to control the mobile chassis to move to the next target point based on steps (2)-(7).

[0078] Specifically, in a specific embodiment of the present invention, the coordinates (x 底盘 ,y 底盘 ), yaw angle θ 底盘 and the coordinates of the target point (x 点 ,y 点 ), target point angle θ 点 , and then by the formula Calculate the distance r between the coordinates of the mobile chassis and the coordinates of the target point, using the formula Δθ=|θ 底盘 -θ 点 Calculate the angle difference Δθ between the coordinates of the mobile chassis and the coordinates of the target point. If both the distance r and the angle difference Δθ are less than the set threshold, the target point is considered reached. If the target point is the last path point, the destination is considered reached.

[0079] The rotation speed of each wheel can be collected by relevant sensors such as encoders, Hall sensors, etc. It should be noted that the sensors here do not only refer to encoders and Hall sensors, but are a general term for sensors that can collect motor position.

[0080] This embodiment also provides a four-drive Mecanum wheel mobile chassis control system for implementing the control method, including:

[0081] The target point acquisition module is used to obtain the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; obtain the path point closest to the mobile chassis and use the path point as the target point;

[0082] A posture calculation module is used to establish a local coordinate system that moves with the mobile chassis at the center of the mobile chassis and a global coordinate system; the local posture of the mobile chassis is calculated based on the displacement of each wheel of the mobile chassis at the current moment, and the global posture of the mobile chassis is calculated based on the local posture; the posture error calculation module is used to calculate the velocity error of the mobile chassis based on the local posture and global posture of the mobile chassis;

[0083] a chassis target speed calculation module, configured to calculate a target speed of the mobile chassis based on the speed error and a speed setting value of the mobile chassis in a global coordinate system;

[0084] a wheel speed calculation module, configured to calculate the linear speed of each wheel of the mobile chassis based on the target speed, and control the mobile chassis according to the linear speed;

[0085] The judgment and control module is used to judge whether the mobile chassis has reached the target point based on the posture of the mobile chassis after the control is completed. If it has not reached the target point, the mobile chassis is controlled to move to the target point; if it has reached the target point, it is further judged whether the target point is the end point of the target driving path. If it is the end point, the control of the mobile chassis is terminated; otherwise, the control of the mobile chassis to the next target point is continued.

[0086] As for the system embodiment, since it basically corresponds to the method embodiment, the relevant parts can be referred to the partial description of the method embodiment, and the implementation methods of the remaining modules will not be repeated here. The system embodiment described above is only illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of the present invention. Ordinary technicians in this field can understand and implement it without paying any creative work.

[0087] Embodiments of the system of the present invention can be applied to any device with data processing capabilities, such as a computer or other device. System embodiments can be implemented through software, hardware, or a combination of software and hardware. For example, a software implementation, as a logical device, is implemented by a processor of any device with data processing capabilities, reading corresponding computer program instructions from non-volatile memory into internal memory and executing them.

[0088] Figure 3 This is the navigation effect of the mobile chassis in the actual map. The red dotted line in the figure represents the path that the mobile chassis is to follow, and the blue solid line is the actual route that the mobile chassis has taken. In order to verify the effectiveness of the algorithm, the path to be followed is discretized into 9 path points, which are marked as P i , i=0,…,8. The mobile chassis moves from P0 to P8 in sequence. At the initial moment of the algorithm, P0 coincides with the center of the chassis. A global coordinate system is established with P0 as the center of the circle, and a local coordinate system that moves with the vehicle is established with the center of the chassis as the center of the circle, with P1 as the target point. When the mobile chassis moves to P1, all calculated values ​​in the local coordinate system are cleared, and a global coordinate system and a local coordinate system are established with P1 as the center of the circle, with P2 as the target point, and so on. Figure 3 It can be seen from the figure that the actual path of the mobile chassis basically coincides with the path to be followed.

[0089] For the convenience of measurement, the path point P i , i=0,…,8 as measurement points, Figure 4 It is the displacement deviation between the moving chassis and the path point when the moving chassis reaches each path point, and the maximum value is only 4cm. Figure 5 The angle deviation between the mobile chassis and the path point is set when the mobile chassis reaches each path point, with a maximum value of only 0.4°. Experimental results show that the control accuracy of this control method can meet most positioning requirements.

[0090] The above-described embodiments merely illustrate several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. Persons skilled in the art will readily appreciate that variations and modifications may be made without departing from the scope of the present invention, all of which fall within the scope of protection of the present invention.

Claims

1. A control method for a four-drive Mecanum wheel mobile chassis, characterized in that: The following steps are involved: 1) Obtain the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; 2) Obtain the path point closest to the mobile chassis and use it as the target point; 3) Establish a local coordinate system at the center of the mobile chassis that moves with the mobile chassis, and establish a global coordinate system; calculate the local pose of the mobile chassis based on the displacement of each wheel of the mobile chassis at the current moment, and then calculate the global pose of the mobile chassis based on the local pose; 4) Calculate the velocity error of the mobile chassis based on the local and global poses of the mobile chassis; 5) calculating a target speed of the mobile chassis based on the speed error and a speed setting value of the mobile chassis in a global coordinate system; 6) calculating the linear speed of each wheel of the mobile chassis based on the target speed, and controlling the mobile chassis according to the linear speed; 7) Determine whether the mobile chassis has reached the target point based on the position of the mobile chassis after control is completed. If it has not reached the target point, control the mobile chassis to move to the target point based on steps 3)-6). If it has reached the target point, further determine whether the target point is the end point of the target driving path. If it is the end point, control of the mobile chassis ends. Otherwise, control the mobile chassis to the next target point continues based on steps 2)-7). In step 3), the local pose is: ； Among them, D car_lx [t] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t; D car_ly [t] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t; D whee1 [t]、D wheel2 [t]、D wheel3 [t]、D wheel4 [t] are the displacements of the four wheels of the four-wheel drive Mecanum wheel mobile chassis at time t; A car_l [t] is the rotation angle of the mobile chassis around the Z axis in the local coordinate system at time t; c x D car_lx Calculation correction coefficient of [t]; c y D car_ly Calculation correction coefficient of [t]; c A A car_l [t] is the calculation correction coefficient; W is the width of the mobile chassis; L is the length of the mobile chassis; In step 3), the calculation of the global position of the mobile chassis in the global coordinate system includes: Performing numerical conversion on the rotation angle to obtain the yaw angle of the mobile chassis in the local coordinate system; Calculating the yaw angle of the mobile chassis around the Z axis in the global coordinate system based on the yaw angle; Calculating the displacement difference of the mobile chassis along the x-axis and y-axis directions in the local coordinate system based on the local posture; Calculating the displacement difference of the mobile chassis along the x-axis and y-axis directions of the global coordinate system during two consecutive samplings based on the displacement difference of the mobile chassis along the x-axis and y-axis directions in the local coordinate system and the yaw angle; Then, based on the displacement difference of the mobile chassis along the x-axis and y-axis of the global coordinate system during two consecutive samplings, the displacement of the mobile chassis along the x-axis and y-axis in the global coordinate system is calculated to obtain the global position of the mobile chassis in the global coordinate system; The yaw angle of the mobile chassis in the local coordinate system is: ； Among them, A car_ll [t] is the yaw angle of the mobile chassis in the local coordinate system at time t; c n The number of revolutions of the mobile chassis; The yaw angle of the mobile chassis around the Z axis in the global coordinate system is: ； in, is the yaw angle of the mobile chassis around the Z axis in the global coordinate system at time t; is the speed setting value of the mobile chassis along the x-axis in the local coordinate system; The speed setting value of the mobile chassis along the y-axis in the local coordinate system; The speed setting value of the moving chassis around the z-axis in the local coordinate system; ; , is the yaw angle at time 0; , It represents the rotation angle of the mobile chassis output by the IMU at time t, Indicates the rotation angle of the mobile chassis output by the IMU at time 0; is the preset value; .

2. The control method of the four-drive Mecanum wheel mobile chassis according to claim 1, characterized in that: In step 3), when establishing the global coordinate system, the rotation angle of the mobile chassis is determined based on the IMU on the mobile chassis, the orientation direction of the mobile chassis is used as the direction of the x-axis of the global coordinate system, the opposite direction of gravity is used as the direction of the z-axis of the global coordinate system, and the direction of the y-axis of the global coordinate system is determined according to the right-hand rule; When establishing a local coordinate system, the direction of the mobile chassis is used as the direction of the x-axis of the local coordinate system, the opposite direction of gravity is used as the direction of the z-axis of the local coordinate system, and the direction of the y-axis of the local coordinate system is determined according to the right-hand rule.

3. The control method of the four-drive Mecanum wheel mobile chassis according to claim 1, characterized in that: The displacement difference of the mobile chassis along the x-axis and y-axis in the local coordinate system is: ； in, for and The difference between is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t; is the displacement of the mobile chassis along the x-axis in the local coordinate system at time t-1; for and The difference between is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t; is the displacement of the mobile chassis along the y-axis in the local coordinate system at time t-1; The displacement difference of the mobile chassis along the x-axis direction of the global coordinate system during two consecutive samplings is: ； in, is the displacement difference of the mobile chassis along the x-axis of the global coordinate system at time t; The displacement difference of the mobile chassis along the y-axis direction of the global coordinate system during two consecutive samplings is: ； in, is the displacement difference of the mobile chassis along the y-axis of the global coordinate system at time t; The displacements of the mobile chassis along the x-axis and y-axis in the global coordinate system are: ； in, is the displacement of the chassis along the x-axis in the global coordinate system at time t; is the displacement of the mobile chassis along the y-axis in the global coordinate system at time t; n is the total time the mobile chassis was last controlled.

4. The control method of the four-drive Mecanum wheel mobile chassis according to claim 3, characterized in that: In step 4), the speed error of the mobile chassis includes the speed error of the mobile chassis rotating around the z-axis of the local coordinate system, the speed error of the mobile chassis in the y-axis direction in the local coordinate system, and the speed error of the mobile chassis in the x-axis direction in the local coordinate system; the speed error of the mobile chassis rotating around the z-axis of the local coordinate system is: ； in, is the speed error k of the mobile chassis rotating around the z-axis of the local coordinate system at time t p1 、k i1 、k d1 Calculate The proportional term coefficient, integral term coefficient and differential term coefficient; ; is an intermediate variable; The yaw angle of the mobile chassis around the Z axis in the global coordinate system is set to the value; The speed error of the mobile chassis in the y-axis direction in the local coordinate system is: ； in, is time t and When k p2 、k i2 、k d2 Respectively represent calculation The proportional term coefficient, integral term coefficient and differential term coefficient; is time t and When k p3 、k i3 、k d3 Respectively represent calculation The proportional term coefficient, integral term coefficient and differential term coefficient; is the preset value; ; 、 is an intermediate variable; At time 0, the displacement of the mobile chassis along the y-axis in the local coordinate system; The speed error of the mobile chassis in the x-axis direction in the local coordinate system is: ； in, is time t and When , the speed error of the mobile chassis in the x-axis direction in the local coordinate system; k p4 、k i4 、k d4 Respectively represent calculation The proportional term coefficient, integral term coefficient and differential term coefficient; is time t and When , the speed error of the mobile chassis in the x-axis direction in the local coordinate system; k p5 、k i5 、k d5 Respectively represent calculation The proportional term coefficient, integral term coefficient and differential term coefficient; , 、 is an intermediate variable; The displacement of the mobile chassis along the x-axis in the local coordinate system at time 0.

5. The control method of the four-drive Mecanum wheel mobile chassis according to claim 4, characterized in that: In step 5), the target speed of the mobile chassis is: ； in, represents the target speed of the mobile chassis in the x-axis direction in the local coordinate system at time t; represents the target speed of the mobile chassis in the y-axis direction in the local coordinate system at time t; represents the target speed of the mobile chassis rotating around the Z axis in the local coordinate system at time t; represents the velocity error of the mobile chassis in the x-axis direction in the local coordinate system at time t; represents the velocity error of the mobile chassis in the y-axis direction in the local coordinate system at time t; It represents the speed error of the mobile chassis rotating around the Z axis in the local coordinate system at time t; Indicates the reference speed of the mobile chassis in the x-axis direction; Indicates the reference speed of the mobile chassis in the y-axis direction; Indicates the reference rotation speed of the mobile chassis around the z-axis.

6. The control method of the four-drive Mecanum wheel mobile chassis according to claim 5, characterized in that: In step 6), the linear speed of each wheel is: ； in, 、 、 、 are the linear velocities of the four wheels of the four-wheel drive Mecanum wheel moving chassis at time t.

7. A four-drive Mecanum wheel mobile chassis control system that implements the control method of claim 1, characterized in that: include: A target point acquisition module is used to obtain the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; Get the path point closest to the mobile chassis and use it as the target point; A posture calculation module is used to establish a local coordinate system at the center of the mobile chassis that moves with the mobile chassis and a global coordinate system; calculate the local posture of the mobile chassis based on the displacement of each wheel of the mobile chassis at the current moment, and then calculate the global posture of the mobile chassis based on the local posture; A posture error calculation module is used to calculate the velocity error of the mobile chassis based on the local posture and global posture of the mobile chassis; a chassis target speed calculation module, configured to calculate a target speed of the mobile chassis based on the speed error and a speed setting value of the mobile chassis in a global coordinate system; a wheel speed calculation module, configured to calculate the linear speed of each wheel of the mobile chassis based on the target speed, and control the mobile chassis according to the linear speed; The judgment and control module is used to judge whether the mobile chassis has reached the target point based on the posture of the mobile chassis after the control is completed. If it has not reached the target point, the mobile chassis is controlled to move to the target point; if it has reached the target point, it is further judged whether the target point is the end point of the target driving path. If it is the end point, the control of the mobile chassis is terminated; otherwise, the control of the mobile chassis to the next target point is continued.

Images