Omnidirectional all-drive robot trajectory tracking method based on constraint simplification

Abstract

The invention provides an omni-directional all-drive robot trajectory tracking method based on constraint simplification, and belongs to the technical field of ground wheel type mobile robot trajectory tracking. According to the method, a robot kinematic model is established on the basis of rigid body dynamics, the transformation relation between each set of wheel speed vectors and robot speed information in a world coordinate system is represented, and multiple sets of hard constraint problems caused by motor physical characteristics are converted into soft constraint or unconstrained problems in a target function. And the program running speed and the solving success rate are improved. In addition, tracking errors caused by mechanical friction and noise interference are compensated for on the control outermost ring based on high-frequency speedometer information, and it is ensured that the robot accurately executes according to a control instruction. Through experimental verification, the solving speed of each frame of the method is within 50ms, and the tracking error evaluation of a reference trajectory is controlled within 5cm.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory tracking of ground wheeled mobile robots, and relates to a trajectory tracking method for an omnidirectional, all-wheel-drive robot based on constraint simplification. The robot posture tracking effect is achieved by analyzing the robot kinematic model, establishing the system state equation and objective function, and controlling the outer loop mechanical error compensation. Background Art

[0002] An omnidirectional mobile robot refers to a type of mobile robot that can achieve three-degree-of-freedom movement in a plane without relying on an additional steering mechanism, and can perform independent forward and backward displacement, left and right translation, and rotation in place. Today, the most common omnidirectional mobile robots include four-wheel steering robots (FourWheel Steer, referred to as 4WS), Mecanum robots, and dual / four-wheel differential robots. Among them, the omnidirectional mobile robot based on Mecanum wheels has the advantages of high motion stability and strong flexibility, but its relatively complex wheel structure reduces the energy conversion efficiency, limits the speed of the robot's movement, greatly affects the robot's load capacity and the reliability of movement in unstructured environments, and cannot meet the operational requirements in complex environments.

[0003] The 4WS system abandons the traditional mechanical steering structure and realizes the functions of all-wheel steering and all-wheel drive by using motors to control the direction of the wheels and the drive of the wheels respectively. This method has the ability of omnidirectional and all-wheel drive while maintaining the advantages of traditional mobile robots. It can realize highly flexible movement modes such as turning on the spot and walking diagonally. At the same time, it can also adapt to unstructured harsh environments such as grass and dirt roads, and has strong versatility. The trajectory tracking algorithm based on 4WS is the basis for the entire omnidirectional mobile robot to realize its various functions, and plays a decisive role in whether the unique movement advantages of the omnidirectional mobile robot can be fully utilized. With the development of omnidirectional mobile robots, the performance requirements of trajectory tracking algorithms are getting higher and higher in terms of response speed and execution speed in addition to execution accuracy and smoothness. Research on trajectory tracking algorithms for omnidirectional mobile platforms has been carried out abroad for many years, but the influence of strong coupling, nonlinearity and other factors brought by its independently driven transmission mode has brought great challenges to the robot's motion control. Compared with traditional non-complete constrained mobile robots, the 4WS system has multiple steering and drive motors. During the movement, due to the mechanical structure errors and load conditions of the vehicle body, the contact force between each wheel and the ground is significantly different from the ideal motion model. This requires comprehensive consideration of the robot's movement and close coordination of each drive and steering motor, which places high demands on the robot's trajectory tracking algorithm.

[0004] The literature (Tan, X.; Liu, D.; Xiong, H. Optimal Control Method of Path Tracking for Four-Wheel Steering Vehicles. Actuators 2022, 11, 61.) aims to solve the problem that the commonly used trajectory tracking algorithm of four-wheel steering robots limits the steering angles of the front and rear wheels and cannot fully utilize their additional steering degrees of freedom. Based on the small angle assumption, this paper derives an unrestricted kinematic model. Then, based on the tracking error model, the objective function and constraints for optimizing the system control quantity are designed. After solving the optimization problem in the form of constrained quadratic programming, the control sequence with the minimum performance index is obtained through rolling time domain optimization. The results show that the standard deviations of the lateral error and yaw angle error of the algorithm are less than 0.1m and 3.0°, respectively. Compared with other traditional algorithms, this method has more flexible control over the front and rear wheel angles and higher tracking accuracy. However, this method limits the steering range of the 4WS system. Although it increases the flexibility and accuracy of trajectory tracking compared to the traditional Ackerman structure, the assumption of a small servo angle limits the solution space of the control input and thus fails to fully utilize the omnidirectional motion characteristics of the robot.

[0005] The literature (Ding, Tao et al. "Trajectory tracking of redundantly actuated mobile robot by MPC velocity control under steering strategy constraint." Mechatronics (2022): n.pag.) proposed that for the 8-drive redundant strong nonlinear system of 4WS, the complexity and time-consuming model solution make it difficult for embedded systems to achieve fast and efficient trajectory tracking. To address this problem, this paper proposes an optimal speed model predictive control under steering strategy constraints. Specifically, the steering modes of 4WS are divided into fixed independent wheel steering, zero-angle pure differential steering and Ackerman steering. Finally, the steering fuzzy selector selects the steering mode and obtains the control quantity of the four steering motors based on this and the reference trajectory. Then, a steering-based model predictive controller is designed to optimize the wheel speed, and finally a satisfactory path tracking control effect can be obtained. This paper fully considers various kinematic models of the 4WS robot, but does not mention the dynamic switching of each mode during the motion process. For some special trajectories, dynamic switching of various modes is required to achieve the optimal tracking effect. Summary of the invention

[0006] In order to overcome the shortcomings of the existing technology in this field, the present invention proposes a trajectory tracking algorithm for an omnidirectional, full-drive robot to achieve high precision and high response speed in complex scenarios. After obtaining a trajectory containing the robot's posture information for a period of time in the future, the method establishes a kinematic model that satisfies the physical constraints of the omnidirectional, full-drive robot; and based on the kinematic model, the system state transfer equation and the optimization objective function are established to obtain a set of optimal control inputs in the next control cycle. The present invention maps the high-dimensional control input (the speed of 4 drive motors and the position of 4 steering motors) to a set of low-dimensional control quantities (linear velocity and angular velocity in the robot's local coordinate system) through a unique mapping relationship, and converts the multi-constraint problem into an unconstrained or constrained simplified problem by reasonably designing the system objective function to simplify the calculation process and improve the feasibility of the target solution. In reality, there are motion errors caused by friction or noise interference in the robot during the movement process, which results in the posture at the next moment in the actual controlled process not being able to accurately match the result predicted by the model. Therefore, IMU pre-integration is used between every two control cycles to obtain a higher-frequency IMU odometer, and the error between the actual control input and the expected control input of the robot between every two control cycles is cumulatively recorded. Based on this, the actual control amount is corrected to achieve online rolling closed-loop optimization and ensure the accuracy and robustness of trajectory tracking.

[0007] The technical solution of the present invention:

[0008] A trajectory tracking method for an omnidirectional and omni-drive robot based on constraint simplification comprises the following steps:

[0009] Step 1, obtaining a set of unique robot speed-wheel speed mapping relationships, and establishing a forward kinematics model of an omnidirectional and all-wheel-drive mobile robot based on the mapping relationships;

[0010] Step 2, based on the forward kinematics model of the omnidirectional and all-wheel-drive mobile robot, define the system state quantities as the robot's trajectory tracking errors on the x and y axes, the orientation angle errors, and the velocity vectors of the four sets of wheels, and define the system control input as the robot's velocity along the x and y axes and the angular velocity around the z axis;

[0011] Step 3, establishing a system state equation based on the system state quantity, and performing Taylor expansion and discretization on the system state equation at the reference state of the trajectory and the reference control input to obtain the system state transfer equation, and then designing an objective function to transform the physical limit of the motor from a multi-hard constraint problem of the control input to an unconstrained problem and solve it, so as to obtain the optimal control input in the next control cycle;

[0012] Step 4: Use IMU pre-integration between every two control cycles to obtain a higher-frequency IMU odometer, accumulate and record the error between the actual control input and the expected control input of the robot between every two control cycles, and based on this, correct the actual control amount to achieve online rolling closed-loop optimization.

[0013] Furthermore, in step 1, the specific method for establishing the kinematic model of the omnidirectional and omni-drive mobile robot is as follows:

[0014] Define the robot's initial position as the world coordinate system origin, and the initial orientation as the positive direction of the x-axis; define the robot's center position as the robot's local coordinate system origin, and the robot's orientation as the x-axis orientation of the robot's local coordinate system; both coordinate systems only consider two-dimensional plane motion, so the z-axis is selected as the positive direction perpendicular to the ground, and both coordinate systems follow the right-handed coordinate system. The odometer information is mapped to the robot's local coordinate system through the transformation matrix φ between the robot's local coordinate system and the world coordinate system. The local-world coordinate system transformation is as follows: Figure 2 shown.

[0015]

[0016] Wherein, vector V is the linear velocity along the x and y axes and the angular velocity around the z axis in the robot's world coordinate system, and x, y, θ are the robot's position information in the world coordinate system; v x , v y , ω is the linear velocity and angular velocity of the robot in the local coordinate system.

[0017] Considering a fixed-shape omnidirectional omni-drive mobile robot as a rigid body, the position of any point on the rigid body in the world coordinate system is:

[0018] X=X O +φ·ρ

[0019] Among them, X O is the position of the robot center in the world coordinate system, is the position of any wheel in the robot's local coordinate system. For the robot, ρ does not change with time, so we can get:

[0020]

[0021] Assuming that the robot's local coordinate system and the world coordinate system are both unit orthogonal coordinate systems, we can get:

[0022]

[0023] Substituting the two-dimensional coordinates of each set of wheels in the robot's local coordinate system into the above formula, we can get:

[0024]

[0025] Where i represents four sets of wheels, i = 1 to 4; v xi and v yi Respectively represent the speed of one set of wheels along the x-axis and y-axis in the robot's local coordinate system; is the two-dimensional coordinate of each wheel in the robot's local coordinate system. Based on this, a unique robot speed-wheel speed mapping relationship can be found, expressed as a matrix H, and the forward kinematic model of the omnidirectional and all-wheel drive robot can be established:

[0026] H.[v x v y ω] T =[v x1 v x2 v x3 v x4 v y1 v y2 v y3 v y4 ] T =ξ

[0027]

[0028] Where L is the wheelbase of the robot, W is the wheelbase of the robot, and ξ is the velocity vector of each wheel in the local coordinate system of the robot.

[0029] Furthermore, in step 2, the system state quantity χ is defined as [e x e y e θ ξ] T , the system control input is u=[v x v y ω] T , where e x , e y They represent the error between the actual position of the robot and the reference trajectory on the x-axis and y-axis, respectively, and e θis the angular error between the actual orientation of the robot and the reference trajectory, and ξ is the velocity vector of each wheel in the local coordinate system of the robot. Existing research results generally use the drive motor speed and the steering motor position as the control input of the robot system equation, and set multiple sets of constraints based on the physical limits of the motor. In the subsequent iterative optimization process, such multi-variable and multi-constrained problems are more likely to lead to too slow a solution speed or even no solution. Therefore, the present invention converts multiple sets of motor states into system state quantities, and uses the macroscopic linear velocity and angular velocity of the robot as control inputs. In this way, although the system state quantity has more variables, ξ does not set fixed constraints during iteration, and does not participate in the operation of updating the next system state. The purpose of setting the system state quantity in this way is to convert the physical limits of the drive motor and the steering motor from the multi-hard constraint problem of the control input into an unconstrained problem in the objective function, and reduce the calculation time of the solver when solving.

[0030] Furthermore, in step 3, the construction process of the system state transfer equation is as follows:

[0031] Assume the system state equation is: In an ideal state, the robot moves along the reference trajectory, so in (χ exp ,u exp ) Performing a first-order Taylor expansion on the system state equation can yield the system state transfer equation:

[0032]

[0033] Among them, u exp is the reference control input in the reference trajectory at the current moment, χ exp =[0 0 0 ξ exp ],ξ exp for u exp The reference wheel velocity vector, R, is calculated by the forward kinematics model of the omnidirectional and omni-wheel drive robot in step 1. n (χ,u) is the remainder of the Nopea, which will be ignored in the subsequent steps.

[0034] The Taylor expansion expression of the system state equation is expressed in matrix form, and its discrete expression is obtained according to the forward Euler method:

[0035]

[0036] Among them, A is the system state matrix, B is the system input matrix, C is the initial state matrix, T is the control period, and χ(k) is the value of the system state quantity at time k.

[0037] Furthermore, in step 3, the objective function is established, and the specific process is as follows:

[0038] The state transfer equation can obtain the state information of the robot at the future moment through the current state quantity and control input. The ultimate goal of the present invention is to enable the 4WS robot to move smoothly and quickly along the reference trajectory, so it is necessary to ensure that the state of the robot at the future moment coincides with the reference trajectory. Therefore, it is necessary to design an objective function to constrain the state quantity, control quantity and high-order smoothness of the system. For the control quantity, its increment is defined as Δu(k)=u(k)-u(k-1). In order to ensure the smoothness of the coordination of each motor during the movement of the robot, it is defined as:

[0039]

[0040] Here, τ is the scaling factor to avoid the vector being perpendicular.

[0041] In the actual motion process, the position of the steering motor and the smoothness of each increment play a decisive role in the smoothness of the robot motion. Since the four groups of drive motors and the steering motor are strongly coupled, the change amount of the drive motor is not constrained in the objective function. The present invention represents the increment of each steering motor as the wheel speed vector (v xi (k),v yi (k)) and the wheel velocity vector at the next moment (v xi (k+1),v yi (k+1)) is in the form of the inverse of the dot product. The result obtained in this way is positively correlated with the increment of the steering motor in the interval (0, π), which can avoid complex inverse trigonometric function operations. In addition, when the optimization result exceeds the physical limit of the steering motor, the steering motor can reversely rotate a small angle to cooperate with the drive motor to reverse to achieve the same effect. At the same time, the calculation of its incremental cost is not affected. Therefore, the final objective function format is:

[0042]

[0043] in, is the current state error, that is, the position error and attitude error with the reference trajectory, χ 1:3 It means taking the first three elements of χ, which determines the accuracy of trajectory tracking, by adjusting the weight matrix Q 1 The weight parameter of each factor is used to adjust the importance of each state quantity; is the increment of the state quantity, which determines the smoothness of the robot's motion and can be adjusted by adjusting the matrix Q 2 The weight value in is used to set the weight of each steering motor. This part needs to be fine-tuned according to the actual motor situation. The control amount increment should be as close to 0 as possible without affecting the tracking effect. The weight of each control amount increment can be set by adjusting the weight value in the matrix R. The final e2 represents the relaxation factor to ensure that there is a solution in the end.

[0044] Furthermore, in step 3, after the state transfer equation and objective function are established, the system will start with the current state quantity and use it as the initial condition for iterative optimization; then, the optimization problem in the finite prediction time domain is constructed through the system state transfer equation and objective function; wherein, the system state transfer equation is used to obtain the state quantity predicted by the system at the next moment according to the system state quantity and control quantity at the current moment, the objective function is used to quantify the control target, and minimizing the objective function ensures the minimization of the trajectory tracking error and the continuity during the motion process, while the constraint conditions ensure the feasibility of the calculation results. After solving the optimization problem, a series of optimal control inputs of the system are obtained, but only the first control input is applied to the actual system. Then, in the next control cycle, re-optimization is performed according to the updated state, so that real-time adjustment and dynamic optimization can be achieved through the rolling window, and finally high-precision and high-robustness trajectory tracking is achieved.

[0045] Furthermore, in step 4, after calculating the ideal control command, the robot will generate execution errors during execution due to mechanical friction, noise interference, driver program, etc. Such errors are unpredictable in the real environment, so such errors need to be corrected online. The robot's control frequency is consistent with the laser odometer frequency at 10Hz. In order to compensate for the above errors in real time, it is necessary to calculate the actual execution value ξ of the steering motor between every two frames of control commands. real The reference value ξ sent by the host computer ref Therefore, the 100Hz IMU odometer is obtained through IMU pre-integration, and the prediction period between k and k+1 is subdivided into k+Δμ l (l=1,...,10) 10 moments, in each subdivided moment, according to the posture information obtained by the IMU odometer, the forward kinematics model of the omnidirectional all-wheel drive robot is used to obtain the position of each motor at k+Δμ l The real-time status at each moment and the current error e(μ) is accumulated and recorded. By accumulating errors and using correction formulas, each group of motors can be kept in the best state in each control cycle to achieve faster convergence speed; the correction formula can be expressed as:

[0046]

[0047] Before each final generator command is issued, a compensation state quantity is applied to each steering motor, K p ,K i ,K d is the compensation parameter.

[0048] Beneficial effects of the present invention: The present invention converts the multi-hard constraint problem of the omnidirectional and full-drive robot when establishing the kinematic model into an unconstrained problem by a constraint simplification method, and reduces the calculation amount of each group of costs by designing an objective function. This method can improve the tracking accuracy, solution speed and success rate of the trajectory tracking problem of the omnidirectional and full-drive robot, and realize the lightweight of the program. In addition, the present invention takes into account the motor execution error caused by friction, noise and other factors during the actual movement of the robot, and designs an online correction method for the motor control quantity, and realizes a faster convergence speed of trajectory tracking by reasonably designing the correction function. It has been experimentally verified that the solution speed of each frame of this method is within 50ms, and the tracking error evaluation of the reference trajectory is controlled within 5cm. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Flow chart of the method of the present invention.

[0050] Figure 2 Schematic diagram of the robot's local coordinate system and world coordinate system.

[0051] Figure 3 Comparison diagram of the reference trajectory and the actual tracking trajectory.

[0052] Figure 4 It is the robot reference speed, command speed and actual execution speed curve during trajectory tracking. DETAILED DESCRIPTION

[0053] The specific implementation of the present invention is described in detail below in conjunction with the technical scheme and the accompanying drawings.

[0054] like Figure 1 As shown, an embodiment of the present invention provides an omnidirectional omni-drive robot trajectory tracking method based on constraint simplification, comprising the following steps:

[0055] Step 1: Plan a reference trajectory containing pose information

[0056] This example uses an omnidirectional, all-wheel-drive mobile robot independently developed by the laboratory, and uses a ouster OS-0 model 3D laser radar for perception and positioning, and the computing unit uses the 11th generation i7 NUC. First, the robot combines its own odometer information and obstacle map information to plan a collision-free trajectory that meets the robot's kinematic constraints. The trajectory contains the robot's position, speed (including angular velocity and linear velocity), posture and other tracking information in the future.

[0057] Step 2: Establish the kinematic model of the omnidirectional and omni-drive robot

[0058] Assume that the robot's wheelbase is L = 0.52 and the wheelbase is W = 0.286. When establishing the kinematic model, in order to simplify the velocity vectors of each wheel, the robot's velocity information needs to be rotated to the robot's local coordinate system. The schematic diagram is as follows: Figure 2 shown.

[0059]

[0060] During the actual movement, the final control input of the robot needs to fall on the four sets of drive motors and steering motors, so it is necessary to establish a state mapping equation group:

[0061] [v x1 v x2 v x3 v x4 v y1 v y2 v y3 v y4 ] T =h(v x ,v y ,ω)

[0062] Through this set of equations, a unique high-dimensional-low-dimensional control quantity mapping relationship can be obtained. This embodiment adopts a kinematic decomposition method based on rigid body dynamics. The positions of the four motors of the robot in the local coordinate system of the robot are known: {(0.143, 0.26)(-0.143, 0.26)(-0.143, -0.26)(0.143, -0.26)}. This embodiment only considers the movement of the robot in two-dimensional space. According to rigid body dynamics, it can be obtained:

[0063]

[0064] Known is the translational velocity in the local system, φρ is the two-dimensional coordinate of the motor mentioned above, and according to Chalet's theorem, there is an angular velocity vector Make

[0065]

[0066] Considering the two-dimensional motion, ω is the angular velocity of the robot around the z-axis, and the calculated result is:

[0067]

[0068] Step 3: Construct state transfer equation and objective function

[0069] Define the state variables of the system. Since the final task to be completed is trajectory tracking, the robot posture error e x e ye θ As part of the state quantity, and [v x1 v x2 v x3 v x4 v y1 v y2 v y3 v y4 ] T As the remaining part of the state quantity, the speed of each set of wheels is expressed in the form of a vector to facilitate subsequent calculations. The system state equation can be expressed as:

[0070]

[0071] Since all the desired poses are on the planned reference trajectory, the above equation can be transformed into the desired state χ exp =[0 0 0 ...] T The first-order Taylor expansion is performed at the position to obtain the system state transfer equation. The motor state quantity is transferred to the intermediate state quantity in the system equation. Because the actual performance of the motor must be considered during movement, the physical limit of the steering motor is about 45° / s angular velocity, and the physical limit of the drive motor is about 1.5m / s, 0.5m / s 2 , so the objective function can be expressed as:

[0072]

[0073] Where Q 1 and Q 2 They control the accuracy and smoothness of trajectory tracking respectively, so when designing Q 2 The matrix needs to take into account the piecewise function control smoothness and physical limits, where g(Δξ) is the smoothing cost function, and by adjusting Q 1 and Q 2 The various parameters in the matrix are used to adjust the actual trajectory tracking effect and the smoothness of the motor state.

[0074]

[0075] Step 4: Outer ring mechanical error compensation

[0076] The control frequency of the robot during operation is 10Hz. Higher-frequency odometer information is obtained through IMU pre-integration, and ten frames of error correction cycles are inserted between every two control cycles. Between two control cycles of the robot, the odometer information is used for kinematic inverse decomposition to obtain the state information of the four groups of motors, and the error e(μ) between the state of this group of motors and the instructions issued in the previous control cycle is calculated. This is the error caused by environmental noise and mechanical error interference. The error value generated every 0.01s is recorded, and the compensation value is calculated according to the correction formula when the next control cycle arrives, so that it can be corrected when the instructions are issued in the current control cycle, so that the robot can track at the desired speed. The final trajectory tracking effect is as follows: Figure 3 As shown, Figure 3 The green curve represents the reference trajectory, and the red curve represents the actual execution trajectory. It can be seen from the figure that the maximum tracking error occurs at the peak of the sine curve, and the maximum error is less than 0.1m. The reference trajectory and the actual trajectory basically coincide with each other at trajectories with smaller curvatures. The comparison effect of macro speed information and expected speed information during robot movement is shown in the figure. Figure 4 As shown, it can be seen that there is a certain difference between the control instructions finally issued and the reference instructions. This is the interference caused to the actuator by factors such as friction and noise. Under the action of the correction formula, it can be seen that the final execution result of the robot is basically consistent with the expected execution result.

[0077] The above is a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. An omnidirectional and omni-drive robot trajectory tracking method, characterized in that: The following steps are involved: Step 1, obtaining a set of unique robot speed-wheel speed mapping relationships, and establishing a forward kinematics model of an omnidirectional and all-wheel-drive mobile robot based on the mapping relationships; Step 2, based on the forward kinematics model of the omnidirectional and all-wheel-drive mobile robot, define the system state quantities as the robot's trajectory tracking errors on the x and y axes, the orientation angle errors, and the velocity vectors of the four sets of wheels, and define the system control input as the robot's velocity along the x and y axes and the angular velocity around the z axis; Step 3, establishing a system state equation based on the system state quantity, and performing Taylor expansion and discretization on the system state equation at the reference state of the trajectory and the reference control input to obtain a system state transfer equation, and then designing an objective function to transform the physical limit of the motor from a multi-hard constraint problem of the control input to an unconstrained problem and solve it, to obtain the optimal control input in the next control cycle; Step 4: Use IMU pre-integration between every two control cycles to obtain a higher-frequency IMU odometer, accumulate and record the error between the actual control input and the expected control input of the robot between every two control cycles, and based on this, correct the actual control amount to achieve online rolling closed-loop optimization.

2. The omnidirectional omni-drive robot trajectory tracking method according to claim 1, characterized in that: In step 1, the specific method for establishing the kinematic model of the omnidirectional and omni-drive mobile robot is as follows: Define the robot's initial position as the world coordinate system origin, and the initial orientation as the positive direction of the x-axis; define the robot's center position as the robot's local coordinate system origin, and the robot's orientation as the x-axis orientation of the robot's local coordinate system; both coordinate systems only consider two-dimensional plane motion, so the z-axis is selected as the positive direction perpendicular to the ground, and both coordinate systems follow the right-handed coordinate system; map the odometer information to the robot's local coordinate system through the transformation matrix φ between the robot's local coordinate system and the world coordinate system: Wherein, vector V is the linear velocity along the x and y axes and the angular velocity around the z axis in the robot's world coordinate system, and x, y, θ are the robot's position information in the world coordinate system; v x , v y , ω is the linear velocity and angular velocity of the robot in the local coordinate system; Consider the fixed-shape omnidirectional omni-drive mobile robot as a rigid body. The position of any point on the rigid body in the world coordinate system is: X=X O +f·r Among them, X O is the position of the robot center in the world coordinate system, is the position of any wheel in the robot's local coordinate system; for the robot, ρ does not change with time, so we get: Assuming that the robot's local coordinate system and the world coordinate system are both unit orthogonal coordinate systems, we get: Substituting the two-dimensional coordinates of each set of wheels in the robot's local coordinate system into the above formula, we get: Where i represents four sets of wheels, i = 1 to 4; v xi and v yi Respectively represent the speed of one set of wheels along the x-axis and y-axis in the robot's local coordinate system; The two-dimensional coordinates of each wheel in the robot's local coordinate system are obtained; based on this, a unique robot speed-wheel speed mapping relationship is found, expressed as a matrix H, and the forward kinematics model of the omnidirectional and all-wheel drive robot is established: H·[v x v y [ω] T =[v x1 v x2 v x3 v x4 v y1 v y2 v y3 v y4 ] T =ξ Where L is the wheelbase of the robot, W is the wheelbase of the robot, and ξ is the velocity vector of each wheel in the local coordinate system of the robot.

3. The omnidirectional omni-drive robot trajectory tracking method according to claim 2, characterized in that: In step 2, the system state quantity χ is defined as x e y e θ ξ] T , the system control input is u=[v x v y ω] T , where e x , e y They represent the error between the actual position of the robot and the reference trajectory on the x-axis and y-axis, respectively, and e θ is the orientation angle error between the actual orientation of the robot and the reference trajectory, and ξ is the velocity vector of each wheel in the local coordinate system of the robot.

4. The omnidirectional omni-drive robot trajectory tracking method according to claim 3, characterized in that: The construction process of the system state transfer equation is as follows: Assume the system state equation is: In an ideal state, the robot moves along the reference trajectory, so in (χ exp ,u exp ) Performing a first-order Taylor expansion on the system state equation can yield the system state transfer equation: Among them, u exp is the reference control input in the reference trajectory at the current moment, χ exp =[000ξ exp ],ξ exp for u exp The reference wheel velocity vector, R, is calculated by the forward kinematics model of the omnidirectional and all-wheel drive robot. n (χ,u) is the remainder of the Nopah; The Taylor expansion expression of the system state equation is expressed in matrix form, and its discrete expression is obtained according to the forward Euler method: Among them, A is the system state matrix, B is the system input matrix, C is the initial state matrix, T is the control period, and χ(k) is the value of the system state quantity at time k.

5. The omnidirectional omni-drive robot trajectory tracking method according to claim 4, characterized in that: In step 3, the objective function is established, and the specific process is as follows: The control increment is defined as Δu(k) = u(k) - u(k-1); to ensure the smoothness of the coordination of each motor during the movement of the robot, define: Where τ is the scaling factor to avoid the vector being perpendicular; The final objective function format is: in, is the current state error, that is, the position error and attitude error with the reference trajectory, χ 1:3 It means taking the first three elements of χ, which determines the accuracy of trajectory tracking. The importance of each state quantity is adjusted by adjusting the weight parameter of each factor in the weight matrix Q1; It is the increment of the state quantity. This part determines the smoothness of the robot's motion. The weight of each steering motor is set by adjusting the weight value in the matrix Q2. This part needs to be fine-tuned according to the actual motor situation. The control increment is made as close to 0 as possible without affecting the tracking effect. The weight of each control increment is set by adjusting the weight value in the matrix R. The final e 2 represents the relaxation factor to ensure that there is a solution in the end.

6. The omnidirectional omni-drive robot trajectory tracking method according to claim 5, characterized in that: In step 3, the system state transfer equation is used to obtain the predicted state quantity of the system at the next moment based on the system state quantity and control quantity at the current moment, the objective function is used to quantify the control target, and minimizing the objective function ensures the minimization of the trajectory tracking error and the continuity during the motion process, and obtains a series of optimal control inputs for the system, and only applies the first control input to the actual system; then, in the next control cycle, re-optimize according to the updated state, so as to achieve real-time adjustment and dynamic optimization through a rolling window, and finally realize trajectory tracking.

7. The omnidirectional omni-drive robot trajectory tracking method according to claim 6, characterized in that: In step 4, the control frequency of the robot is consistent with the laser odometer frequency of 10 Hz. In order to compensate the outer ring mechanical error in real time, it is necessary to calculate the actual value of the steering motor between every two frames of control instructions. real The reference value ξ sent by the host computer ref The error e(μ) between them; therefore, the 100Hz IMU odometer is obtained through IMU pre-integration, and the prediction period between k and k+1 is subdivided into k+Δμ l There are 10 moments in total, l = 1, 2, ..., 10. In each subdivided moment, the posture information obtained by the IMU odometer is used through the forward kinematics model of the omnidirectional all-wheel drive robot to obtain the position of each motor at k + Δμ l The real-time status at the moment, and the current error e(μ) is accumulated and recorded; by accumulating the error and the correction formula, each group of motors can be kept in the best state in each control cycle to achieve a faster convergence speed; the correction formula is expressed as: Before each final generator command is issued, a compensation state quantity is applied to each steering motor, K p ,K i ,K d is the compensation parameter.

Images