Control method and system of four-drive Mecanum wheel moving chassis

Abstract

The invention discloses a control method and system for a four-drive Mecanum wheel moving chassis, and belongs to the field of automatic control. Obtaining a target driving path of the mobile chassis, and obtaining a plurality of path points; taking a path point closest to the mobile chassis as a target point; establishing a local coordinate system and a global coordinate system, calculating a local pose based on the displacement of each wheel of the mobile chassis at the current moment, and then calculating a global pose; the linear speed of each wheel of the four-drive Mecanum wheel moving chassis is calculated, and the moving chassis is controlled; judging whether the controlled mobile chassis reaches a target point or not, and if not, continuing to control; otherwise, judging whether the target point is the terminal point of the target driving path, and if yes, stopping control; otherwise, controlling the moving chassis to move to the next target point. The problems of drifting, deviation from a target yaw angle and the like of the movable chassis in the moving process can be effectively solved, and it is ensured that the movable chassis can move according to a designated route.

Description

Technical Field

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

[0002] The four-drive Mecanum wheel mobile chassis has a compact structure and high flexibility and can move in any direction. Therefore, it is often used to carry robotic arms, cameras, radars, IMUs (inertial measurement units) and other equipment for operation. In actual applications, the yaw angle of the mobile chassis is usually obtained through an IMU or an odometer. However, the measurement value of the IMU has a zero bias of random walk, and the yaw angle calculated by the odometer cannot measure the angle deviation of the chassis in a slipping state. In addition, due to problems such as chassis processing, assembly errors and motor control accuracy, the chassis may also shift sideways or deviate from the target yaw angle during movement. In addition, existing path tracking algorithms usually track in a fixed order of path points. If the chassis deviates from the path or the spacing between path points is uneven, it will 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. Moreover, when the path points are updated or the robot deviates from the global path, the traditional method needs to replan the entire path and lacks 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 by the present invention is as follows:

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

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

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

[0008] 3) Establishing a local coordinate system that moves with the mobile chassis at the center of the mobile chassis, and establishing 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 speed of each wheel of the mobile chassis based on the target speed, and controlling the mobile chassis according to the linear speed;

[0012] 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, 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, the 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 acquire the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; acquire 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 moving with the mobile chassis at the center of the mobile chassis, and to establish 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 then the global posture of the mobile chassis is calculated based on the local posture; a posture error calculation module is used to calculate the speed error of the mobile chassis based on the local posture and global posture of the mobile chassis;

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

[0017] A wheel speed calculation module, which is used 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 according to 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 ends; otherwise, the control of the mobile chassis continues to move to the next target point.

[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, so that the robot can dynamically adjust the target point, correct the path deviation, avoid the deviation caused by the fixed path point sequence, and improve the path tracking accuracy;

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

[0022] 3) When the 4WD Mecanum wheel mobile chassis deviates from the path or there are dynamic changes in the environment, the 4WD Mecanum wheel mobile chassis can automatically select the new nearest path point as the target without replanning the overall path, thereby significantly improving the robustness of path tracking and improving 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 to achieve smooth tracking, which is particularly suitable for scenarios where path points are sparse or densely distributed, and can adapt to situations where path points are distributed in a variety of ways.

[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 started each time the target point is reached, the errors accumulated in 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, assembly errors and motor control accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

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

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

[0031] The present invention is further described and illustrated below in conjunction with specific embodiments. The embodiments are merely exemplary of the present disclosure and do not define the scope of limitation. The technical features of each embodiment of the present invention may be combined accordingly without conflicting with each other.

[0032] First, the four-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 motor alone. When the present invention controls the mobile chassis, the control of the mobile chassis is performed successively. 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 a plurality of path points. For the first control, the path point closest to the four-drive Mecanum wheel mobile chassis is first obtained, and the path point is used as the target point; based on the posture of the mobile chassis at the initial moment (the posture 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 moving with the mobile chassis are established, and the global posture and local posture are calculated; the speed error of the mobile chassis is calculated based on the local posture and the global posture; the target speed of the mobile chassis is calculated again; finally, the linear velocity of each wheel of the four-drive Mecanum wheel mobile chassis is calculated, and the mobile chassis is controlled this time according to the linear velocity, and the mobile chassis is driven to move toward the current target point. At the same time, real-time monitoring is performed to determine whether the mobile chassis has reached the current target point, so as to achieve dynamic control that adapts to path deviations or environmental changes. If the target point has not been reached, the mobile chassis is controlled with the current target point as the target point. If the target point has been reached, 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 cycle is entered, and the next control is performed with the posture at the end of this control as the initial posture. By continuously repeating the above-mentioned successive control logic, the mobile chassis finally completes the path tracking task from the starting point to the end point.

[0033] like Figure 1 As shown in FIG. 1 , it is a block diagram of a control method for a four-drive Mecanum wheel mobile chassis, which controls the motion of the Mecanum wheel mobile chassis according to the yaw angle of the IMU and the displacement of each wheel. The specific implementation process is as follows:

[0034] (1) Obtaining a 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 the 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 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;

[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 [t] calculation correction coefficient; c y D car_ly [t] calculation correction coefficient; 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 0~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 value of 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 significance; η 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 samplings (i.e., the displacement difference of the mobile chassis along the x-axis and y-axis directions 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 directions 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 direction of the global coordinate system during 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 direction of the global coordinate system during 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-axis and y-axis directions in the global coordinate system obtained in step 4) and step 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; n is the total time the mobile chassis was last controlled. The yaw angle of the mobile chassis around the Z axis in the global coordinate system and the displacement of the mobile chassis along the x and y axes of the global coordinate system together constitute the global position 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 [t] proportional term coefficient, integral term coefficient and differential term coefficient; 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 ≠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 mobile 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 speed 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 the wheels, 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) judging whether the mobile chassis has reached the target point according to the position of the mobile chassis after the control is completed; if it has not reached the target point, completing the control of the mobile chassis to move to the target point based on steps (3)-(6); if it has reached the target point, further judging whether the target point is the end point of the target driving path; if it is the end point, stopping the control; otherwise, continuing to complete the control of 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 the distance r and the angle difference Δθ are both less than the set threshold, it is considered that the target point has been reached. If the target point is the last path point, it is considered that the end point has been 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 sensor here does not only refer to encoders and Hall sensors, but refers to the general term for sensors that can collect the motor position.

[0080] In this embodiment, a four-drive Mecanum wheel mobile chassis control system for implementing the control method is also provided, including:

[0081] The target point acquisition module is used to acquire the target driving path of the mobile chassis and discretize the target driving path to form multiple path points; acquire 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 moving with the mobile chassis at the center of the mobile chassis, and to establish 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 then the global posture of the mobile chassis is calculated based on the local posture; a posture error calculation module is used to calculate the speed error of the mobile chassis based on the local posture and global posture of the mobile chassis;

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

[0084] A wheel speed calculation module, which is used 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 according to 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 ends; otherwise, the control of the mobile chassis continues to move to the next target point.

[0086] For the system embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer 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 schematic, 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 may be selected according to actual needs to achieve the purpose of the scheme of the present invention. Ordinary technicians in this field can understand and implement it without paying creative work.

[0087] The embodiments of the system of the present invention can be applied to any device with data processing capabilities, and the device with data processing capabilities can be a device or apparatus such as a computer. The system embodiments can be implemented by software, or by hardware or a combination of software and hardware. Taking software implementation as an example, as a device in a logical sense, the corresponding computer program instructions in the non-volatile memory are read into the memory by the processor of any device with data processing capabilities and run.

[0088] Figure 3 This is the navigation effect diagram 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 traveled. In order to verify the effectiveness of the algorithm, the path to be followed is discretized into 9 path points, 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 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 moving chassis and the path point is set when the moving chassis reaches each path point, and the maximum value is only 0.4°. The experimental results show that the control accuracy of this control method can meet most positioning requirements.

[0090] The above-mentioned embodiments only express several implementation methods of the present invention, and the description is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. For ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope 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) Obtaining the target driving path of the mobile chassis and discretizing the target driving path to form multiple path points; 2) Obtain the path point closest to the mobile chassis and use the path point as the target point; 3) Establishing a local coordinate system that moves with the mobile chassis at the center of the mobile chassis, and establishing 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; 4) Calculating 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 the 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, the 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).

2. The control method of the four-drive Mecanum wheel mobile chassis according to claim 1, characterized in that: In the 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 2, characterized in that: 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 [t] calculation correction coefficient; c y D car_ly [t] calculation correction coefficient; 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.

4. The control method of the four-drive Mecanum wheel mobile chassis according to claim 3 is characterized in that: In step 3), the calculation of the global position and posture of the mobile chassis in the global coordinate system includes: Numerical conversion is performed 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 directions of the global coordinate system during two consecutive samplings, the displacement of the mobile chassis along the x-axis and y-axis directions in the global coordinate system is calculated to obtain the global position of the mobile chassis in the global coordinate system.

5. The control method of the four-drive Mecanum wheel mobile chassis according to claim 4, characterized in that: 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: Among them, A car_g [t] is the yaw angle of the mobile chassis around the Z axis in the global coordinate system at time t; 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 ΔA is the speed setting value of the mobile chassis around the z-axis in the local coordinate system; car_ll [t] = A car_ll [t]-A car_ll [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; η is a preset value; ΔA Ic =(A IMU [t]-A IMU [0])-(A car_ll [t]-A car_ll [0]).

6. The control method of the four-drive Mecanum wheel mobile chassis according to claim 5, characterized in that: The displacement difference of the mobile chassis along the x-axis and y-axis directions in the local coordinate system is: Among them, diff_D car_lx [t] is D car_lx [t] and D car_lx The difference between [t-1]; 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 [t] and D car_ly The difference between [t-1]; 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; The displacement difference of the mobile chassis along the x-axis direction of the global coordinate system during two consecutive samplings is: 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] 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; The displacement difference of the mobile chassis along the y-axis direction of the global coordinate system during two consecutive samplings is: diff_D car_gy [t]=sin(A car_g )*diff_D car_lx [t]+cos(A car_g )*diff_D carl_y [t] 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; The displacements of the mobile chassis along the x-axis and y-axis directions in the global coordinate system are respectively: Among them, D car_gx [t] is the displacement of the chassis along the x-axis in the global coordinate system at time t; D car_gy [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.

7. The control method of the four-drive Mecanum wheel mobile chassis according to claim 6, 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: Among them, V err_A [t] 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 V err_A [t] proportional term coefficient, integral term coefficient and differential term coefficient; e A [t] = A car_r -A car_g [t]; e A [t] is the intermediate variable; A car_r It is the setting value of the yaw angle of the mobile chassis around the Z axis in the global coordinate system; The speed error of the mobile chassis in the y-axis direction in the local coordinate system is: Among them, 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 p2 , k i2 , k d2 Respectively represent the calculation of V err_y 1 [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 k ≠0, the speed error of the moving chassis in the y-axis direction in the local coordinate system; p3 , k i3 , k d3 Respectively represent the calculation of V err_y 2 [t] is the proportional term coefficient, integral term coefficient and differential term coefficient; ε is the preset value; e 1y [t]、e 2y [t] is the intermediate variable; D car_ly [0] is the displacement of the mobile chassis along the y-axis in the local coordinate system at time 0; The speed error of the mobile chassis in the x-axis direction in the local coordinate system is: Among them, 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 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 2 [t] is time t and V rx =0,V ry =0,V rA When k ≠0, the speed error of the moving chassis in the x-axis direction in the local coordinate system; p5 , k i5 , k d5 Respectively represent the calculation of V err_x 2 The proportional, integral and differential term coefficients of [t]; is the intermediate variable; D car_lx [0] is the displacement of the mobile chassis along the x-axis in the local coordinate system at time 0.

8. The control method of the four-drive Mecanum wheel mobile chassis according to claim 7, characterized in that: In step 5), the target speed of the mobile chassis is: 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 speed error of the mobile chassis rotating around the Z axis in the local coordinate system at time t; V rx Indicates the reference speed of the mobile chassis in the x-axis direction; V ry Indicates the reference speed of the mobile chassis in the y-axis direction; V rA Indicates the reference rotation speed of the mobile chassis around the z-axis.

9. The control method of the four-drive Mecanum wheel mobile chassis according to claim 8, characterized in that: In step 6), the linear speed of each wheel is: 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 at time t.

10. A four-drive Mecanum wheel mobile chassis control system for implementing the control method of claim 1, characterized in that: include: A target point acquisition module is used to acquire a 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 that moves with the mobile chassis at the center of the mobile chassis, and to establish 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, which is used to calculate the speed error of the mobile chassis based on the local posture and global posture of the mobile chassis; A chassis target speed calculation module, which is used to calculate the target speed of the mobile chassis based on the speed error and the speed setting value of the mobile chassis in the global coordinate system; A wheel speed calculation module, which is used 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 according to 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 ends; otherwise, the control of the mobile chassis continues to move to the next target point.

Images