Omni-directional and full-drive robot trajectory tracking method based on constraint simplification

Abstract

The application provides an omnidirectional full-drive robot trajectory tracking method based on constraint simplification, and belongs to the technical field of ground wheeled mobile robot trajectory tracking. The method establishes a robot kinematics model on the basis of rigid body dynamics, represents the transformation relationship between each group of wheel speed vectors and robot speed information in a world coordinate system, and converts a plurality of hard constraint problems caused by motor physical characteristics into soft constraint or unconstrained problems in a target function, so that the program running speed and the solution success rate are improved. In addition, the tracking error caused by mechanical friction and noise interference is compensated based on high-frequency odometer information in the outermost control loop, so that the robot accurately executes according to the control instruction. Through experiment verification, the solution speed of each frame of the method is less than 50 ms, and the tracking error evaluation of the reference trajectory is controlled to be less than 5 cm.

Description

Technical Field

[0001] This invention belongs to the field of trajectory tracking technology for ground wheeled mobile robots, and relates to a trajectory tracking method for omnidirectional and all-drive robots based on constraint simplification. The method achieves robot pose tracking by analyzing robot kinematics models, establishing system state equations and objective functions, and compensating for mechanical errors in the outer control loop. Background Technology

[0002] Omnidirectional mobile robots are a type of mobile robot that can achieve three degrees of freedom of motion in a plane without relying on an additional steering mechanism, meaning they can independently perform forward and backward displacement, left and right translation, and rotation in place. Currently, the most common omnidirectional mobile robots include four-wheel steering robots (4WS), Mecanum robots, and dual / four-wheel differential robots. Among these, omnidirectional mobile robots based on Mecanum wheels have advantages such as high motion stability and strong flexibility; however, their relatively complex wheel structure reduces energy conversion efficiency, limits the robot's speed, and significantly affects the robot's load capacity and reliability in unstructured environments, making them unsuitable for operations in complex environments.

[0003] The 4WS system abandons the traditional mechanical steering structure, achieving all-wheel steering and all-wheel drive by using motors to control the direction and drive of each wheel separately. This approach, while maintaining the advantages of traditional mobile robots, provides omnidirectional and all-wheel drive capabilities, enabling highly flexible movements such as turning in place and moving diagonally. It can also adapt to unstructured and harsh environments such as grass and dirt roads, demonstrating strong versatility. The trajectory tracking algorithm based on 4WS is the foundation for the various functions of the omnidirectional mobile robot and plays a decisive role in fully leveraging its unique motion advantages. With the development of omnidirectional mobile robots, the performance requirements for trajectory tracking algorithms are increasingly demanding, in addition to accuracy and smoothness, as well as response and execution speed. Research on trajectory tracking algorithms for omnidirectional mobile platforms has been conducted abroad for many years, but the strong coupling and nonlinearity caused by its independent drive transmission method pose significant challenges to the robot's motion control. Compared to traditional nonholonomic constraint mobile robots, the 4WS system has multiple steering and drive motors. During movement, due to errors in the mechanical structure of the robot body and the load conditions, the contact force between each wheel and the ground differs significantly from the ideal motion model. This requires comprehensive consideration of the robot's motion and close coordination among the various drive and steering motors, which places higher demands on the robot 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.) addresses the problem that commonly used trajectory tracking algorithms for four-wheel steering robots restrict the steering angles of the front and rear wheels, failing to fully utilize their additional steering degrees of freedom. This paper derives an unrestricted kinematic model based on the small-angle assumption. Then, based on the tracking error model, an objective function and constraints for optimizing the system control input 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 this algorithm are less than 0.1 m and 3.0°, respectively. Compared with other traditional algorithms, this method offers 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 small servo angles restricts the solution space of the control input, thus failing 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 MPCvelocity control under steering strategy constraint." Mechatronics (2022): n.pag.) points out that for the 8-drive redundant strongly nonlinear system of 4WS, the complexity and time consumption of model solving make it difficult for embedded systems to achieve fast and efficient trajectory tracking. To address this problem, this paper proposes an optimal velocity 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, a steering fuzzy selector selects the steering mode and obtains the control quantities 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 satisfactory path tracking control results 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 optimal tracking results. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies in this field, this invention proposes a trajectory tracking algorithm for omnidirectional, all-wheel-drive robots in complex scenarios, achieving high precision and high response speed. After obtaining a trajectory containing the robot's pose information for a future period, this method establishes a kinematic model that satisfies the physical constraints of the omnidirectional, all-wheel-drive robot. Based on this kinematic model, it establishes system state transition equations and an optimization objective function, obtaining a set of optimal control inputs for the next control cycle. This invention maps high-dimensional control inputs (the speeds of four drive motors and the positions of four 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. By rationally designing the system objective function, the multi-constraint problem is transformed into an unconstrained or simplified constraint problem, simplifying the calculation process and improving the feasibility of the objective solution. In reality, robots experience motion errors due to friction or noise interference during movement, causing the actual pose at the next moment in the controlled process to not accurately match the model's prediction. Therefore, IMU pre-integration is used between every two control cycles to obtain higher frequency IMU odometry, and the error between the robot's actual control input and the expected control input is accumulated and recorded between every two control cycles. Based on this, the actual control quantity is corrected to achieve closed-loop optimization of online rolling, ensuring the accuracy and robustness of trajectory tracking.

[0007] The technical solution of this invention:

[0008] A trajectory tracking method for an omnidirectional, all-drive robot based on constraint simplification includes the following steps:

[0009] Step 1: Obtain a unique set of robot speed-wheel speed mapping relationships, and establish a forward kinematics model of the 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-drive mobile robot, define the system state variables as the robot trajectory tracking error on the x and y axes, the orientation angle error, and the velocity vectors of the four sets of wheels. Define the system control input as the robot's velocity along the x and y axes and its angular velocity around the z axis.

[0011] Step 3: Establish the system state equation based on the system state variables, and perform Taylor expansion and discretization on the system state equation at the reference state and reference control input of the trajectory to obtain the system state transition equation. Then, design the objective function to transform the physical limit of the motor from a problem with multiple hard constraints of the control input into an unconstrained problem and solve it to obtain the optimal control input in the next control cycle.

[0012] Step 4: IMU pre-integration is used between every two control cycles to obtain higher frequency IMU odometry. The error between the robot's actual control input and the desired control input is accumulated and recorded between every two control cycles. Based on this, the actual control quantity is corrected to achieve online rolling closed-loop optimization.

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

[0014] The robot's initial position is defined as the origin of the world coordinate system, and its initial orientation is the positive x-axis. The robot's center position is defined as the origin of the robot's local coordinate system, and its orientation is the x-axis orientation of the local coordinate system. Both coordinate systems only consider two-dimensional planar motion; therefore, the positive z-axis is chosen to be perpendicular to the ground and pointing upwards. Both coordinate systems follow a right-handed coordinate system. The odometry information is mapped to the robot's local coordinate system using a transformation matrix φ between the local and world coordinate systems. The local-world coordinate system transformation is as follows: Figure 2 As shown.

[0015]

[0016] Where vector V represents 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, and θ represent the robot's pose information in the world coordinate system; v x v y ω represents the linear velocity and angular velocity of the robot in the local coordinate system.

[0017] Consider a fixed-shape omnidirectional, all-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 It is the position of the robot's center within the world coordinate system. Let ρ be the position of any wheel in the robot's local coordinate system. For the robot, ρ does not change with time, so we can obtain:

[0020]

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

[0022]

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

[0024]

[0025] Where i represents four sets of wheels, i = 1 to 4; v xi and v yi These represent the velocities of one set of wheels along the x-axis and y-axis in the robot's local coordinate system, respectively. Let each wheel have two-dimensional coordinates in the robot's local coordinate system. Based on this, a unique robot velocity-wheel velocity mapping relationship can be found, represented by matrix H, and a forward kinematics model of the omnidirectional, all-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 robot's wheelbase, W is the robot's wheel track, and ξ is the velocity vector of each wheel in the robot's local coordinate system.

[0029] Furthermore, in step 2, the system state variable χ = [e x e y e θ ξ] T The system control input is u = [v x v y ω] T , where e x e y e represents the error between the robot's actual position and the reference trajectory along the x-axis and y-axis, respectively. θLet ξ be the orientation angle error between the robot's actual orientation and the reference trajectory, and let ξ be the velocity vector of each wheel in the robot's local coordinate system. Existing research generally uses the drive motor speed and steering motor position as the control inputs to the robot system equations and sets multiple sets of constraints based on the physical limits of the motors. In subsequent iterative optimization, such multivariable and multi-constraint problems can easily lead to slow solution speeds or even no solution. Therefore, this invention transforms multiple sets of motor states into system state variables, using the robot's macroscopic linear velocity and angular velocity as control inputs. Although this system state variable has many variables, ξ is not subject to fixed constraints during iteration and does not participate in updating the system state in the next step. The purpose of setting the system state variable in this way is to transform the physical limits of the drive motor and steering motor from a problem with multiple hard constraints in the control inputs into an unconstrained problem in the objective function, and to reduce the computation time of the solver during the solution process.

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

[0031] Let the system state equations be: In an ideal scenario, the robot moves along a reference trajectory, so in (χ) exp u exp Performing a first-order Taylor expansion of the system state equations at position ) yields the system state transition equations:

[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 obtained by calculating the forward kinematics model of the omnidirectional all-drive robot in step 1. n (χ, u) is the Nopea remainder, which will be ignored in the following discussion.

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

[0035]

[0036] Where 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 variable at time k.

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

[0038] The state transition equation can obtain the robot's state information at future moments through the current state variables and control inputs. The ultimate goal of this invention is to enable the 4WS robot to move smoothly and quickly along a reference trajectory, so it is necessary to ensure that the robot's future state coincides with the reference trajectory. Therefore, it is necessary to design an objective function to constrain the system's state variables, control variables, and their higher-order smoothness. For the control variables, its increment is defined as Δu(k) = u(k) - u(k-1). To ensure the smoothness of the coordination between motors during robot movement, the following is defined:

[0039]

[0040] Where τ is the scaling factor to avoid vectors being perpendicular.

[0041] In actual motion, the position of the steering motor and the smoothness of its increments play a decisive role in the smoothness of the robot's motion. Furthermore, since the four sets of drive motors are strongly coupled with the steering motor, no constraints are placed on the changes in the drive motors in the objective function. This invention represents each increment of the steering motor as the wheel velocity vector (v) at the current moment. xi (k),v yi (k) and the wheel velocity vector (v) at the next moment xi (k+1),v yi The result is obtained by taking the reciprocal of the (k+1) dot product, which is positively correlated with the increment of the steering motor in the interval (0, π). This avoids complex inverse trigonometric function calculations. Furthermore, when an instruction occurs that the optimization result exceeds the physical limit of the steering motor, the steering motor can reverse by a small angle to achieve the same effect in conjunction with the reverse rotation of the drive motor, without affecting the calculation of its incremental cost. Therefore, the final objective function format is:

[0042]

[0043] in, It is to make the current state error, i.e., the position error and attitude error relative to the reference trajectory, χ 1:3 This indicates that the first three elements of χ are taken. This part determines the accuracy of trajectory tracking. The importance of each state variable is adjusted by adjusting the weight parameters of each factor in the weight matrix Q1. It is the increment of the state variable, which determines the smoothness of the robot's movement. The weight of each steering motor can be set by adjusting the weight value in matrix Q2. This part needs to be finely tuned according to the actual motor conditions. The goal is to make the control increment as close to zero as possible without affecting the tracking performance. This can be achieved by adjusting the weight values ​​in matrix R to set the weight of each control increment; the final e 2 This represents the relaxation factor, used to ensure that a solution eventually exists.

[0044] Furthermore, in step 3, after establishing the state transition equation and objective function, the system starts with the current state quantity as the initial condition for iterative optimization. Subsequently, an optimization problem is constructed within the finite prediction time domain using the system state transition equation and objective function. The system state transition equation is used to obtain the predicted state quantity of the system at the next moment based on the current system state quantity and control quantity. The objective function quantifies the control objective, minimizing the objective function ensures the minimization of trajectory tracking error and the continuity of motion, while constraints ensure the feasibility of the calculation results. After solving this optimization problem, a series of optimal control inputs for the system are obtained, but only the first control input is applied to the actual system. Then, in the next control cycle, the system is re-optimized based on the updated state, thereby achieving real-time adjustment and dynamic optimization through a rolling window, ultimately achieving high-precision and highly robust trajectory tracking.

[0045] Furthermore, in step 4, after calculating the ideal control command, the robot may experience execution errors due to mechanical friction, noise interference, and driver issues during execution. These errors are unpredictable in real-world environments, therefore online correction is necessary. The robot's control frequency is kept consistent with the laser odometry frequency at 10Hz. To compensate for these errors in real time, the actual value ξ executed by the steering motor needs to be calculated between every two frames of control commands. real Reference value ξ issued by the host computer ref The error e(μ) between them. Therefore, an IMU odometry of 100Hz is obtained through IMU pre-integration, with the prediction period subdivided into k+Δμ between k and k+1. l (l=1,...,10) Ten time points are defined. Within each subdivided time point, the attitude information obtained from the IMU odometry is used to obtain the k+Δμ of each motor in the omnidirectional all-drive robot forward kinematics model. l The real-time status at each moment is recorded, and the current error e(μ) is accumulated and recorded. By accumulating the error and using the correction formula, each group of motors can be kept in optimal condition in each control cycle, thereby achieving faster convergence speed; the correction formula can be expressed as:

[0046]

[0047] Before each final generator command is issued, a compensation state variable, K, is applied to each steering motor. p ,K i ,K d For compensation parameters.

[0048] The beneficial effects of this invention are as follows: This invention transforms the multi-hard-constraint problem of establishing a kinematic model for an omnidirectional, all-drive robot into an unconstrained problem through a constraint simplification method, and reduces the computational cost of each set of costs by designing an objective function. This method improves the tracking accuracy, solution speed, and success rate of the trajectory tracking problem for omnidirectional, all-drive robots, achieving program lightweighting. Furthermore, considering the motor execution errors caused by friction, noise, and other factors during actual robot movement, this invention designs an online correction method for motor control quantities, achieving faster convergence speed for trajectory tracking through a reasonably designed correction function. Experimental verification shows that the solution speed per frame is within 50ms, and the tracking error evaluation for the reference trajectory is controlled within 5cm. Attached Figure Description

[0049] Figure 1 This is a flowchart of the method of the present invention.

[0050] Figure 2 This is a schematic diagram of the robot's local coordinate system and world coordinate system.

[0051] Figure 3 This is a comparison chart of the reference trajectory and the actual tracking trajectory.

[0052] Figure 4 The curves represent the robot's reference speed, command issuance speed, and actual execution speed during trajectory tracking. Detailed Implementation

[0053] The specific embodiments of the present invention are described in detail below with reference to the technical solutions and accompanying drawings.

[0054] like Figure 1 As shown, this embodiment of the invention provides a constraint-simplified trajectory tracking method for an omnidirectional, all-drive robot, comprising the following steps:

[0055] Step 1: Plan a reference trajectory that includes pose information.

[0056] This embodiment uses an omnidirectional, all-drive mobile robot independently developed in the laboratory, and employs an ouster OS-0 3D LiDAR for perception and localization, with an 11th generation i7 NUC computing unit. First, the robot combines its own odometry information and obstacle map information to plan a collision-free trajectory that satisfies the robot's kinematic constraints. This trajectory includes the robot's position, velocity (including angular velocity and linear velocity), attitude, and other tracking information for a future period.

[0057] Step 2: Establish the kinematic model of the omnidirectional omnidirectional robot.

[0058] Assume the robot's wheelbase is L = 0.52 and its track width is W = 0.286. To simplify the velocity vectors of each wheel when building the kinematic model, the robot's velocity information needs to be rotated to the robot's local coordinate system. A schematic diagram is shown below. Figure 2 As shown.

[0059]

[0060] In actual motion, the robot's final control input needs to be applied to four sets of drive motors and steering motors, therefore a set of state mapping equations needs to be established:

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

[0062] This set of equations yields a unique mapping relationship between high-dimensional and low-dimensional control variables. This embodiment employs a kinematic decomposition method based on rigid body dynamics. The positions of the robot's four motors in the robot's local coordinate system are known: {(0.143,0.26)(-0.143,0.26)(-0.143,-0.26)(0.143,-0.26)}. This embodiment only considers the robot's motion in two-dimensional space. According to rigid body dynamics, the following can be obtained:

[0063]

[0064] Known The angular velocity is the translational velocity in the local frame of reference, and φρ is the two-dimensional coordinate of the motor mentioned above. According to Charlene's theorem, there exists an angular velocity vector. Make

[0065]

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

[0067]

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

[0069] Define the system's state variables. Since the ultimate task is trajectory tracking, the robot pose error e is defined as... x e ye θ As part of the state variable, 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 variables, the velocity of each set of wheels is expressed as a vector, facilitating subsequent calculations. The system state equation can be represented as:

[0070]

[0071] Since all desired poses lie on the planned reference trajectory, the above equations are applied to the desired state χ. exp =[0 0 0 ...] T A first-order Taylor expansion is performed to obtain the system state transition equation. The motor state variables are transferred to intermediate state variables in the system equation. Because the actual performance of the motors must be considered during motion, the physical limit of the steering motor is approximately 45° / s², and the physical limit of the drive motor is approximately 1.5 m / s². 2 Therefore, the objective function can be expressed as:

[0072]

[0073] Q1 and Q2 control the accuracy and smoothness of trajectory tracking, respectively. Therefore, when designing the Q2 matrix, it is necessary to consider the piecewise function control of smoothness and physical limits. Here, g(Δξ) is the smoothing cost function. The actual trajectory tracking effect and motor state smoothness are adjusted by adjusting the parameters in the Q1 and Q2 matrices.

[0074]

[0075] Step 4, Outer Ring Mechanical Error Compensation

[0076] The robot operates at a control frequency of 10Hz. Higher frequency odometry information is obtained through IMU pre-integration, and a ten-frame error correction cycle is inserted between every two control cycles. Between two control cycles, inverse kinematic decomposition is performed using the odometry information to obtain the state information of four sets of motors. The error e(μ) between the state of each motor set and the command issued in the previous control cycle is calculated; this represents the error caused by environmental noise and mechanical interference. The error value generated every 0.01s is recorded. At the arrival of the next control cycle, a compensation value is calculated according to the correction formula, allowing for correction when the command is issued in the current control cycle. This enables the robot to track at the desired speed, and the final trajectory tracking effect is as follows: Figure 3As shown, Figure 3 The green curve represents the reference trajectory, and the red curve represents the actual execution trajectory. As shown in the figure, the maximum tracking error occurs at the peak of the sine curve, and the maximum error is less than 0.1m. In areas with less curvature, the reference trajectory and the actual trajectory largely coincide. The comparison between macroscopic velocity information and expected velocity information during robot movement is illustrated in the diagram. Figure 4 As shown, there is a certain difference between the final control command and the reference command. This is due to the interference caused by factors such as friction and noise on the actuator. Under the effect 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 description represents a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A trajectory tracking method for an omnidirectional, all-drive robot, characterized in that, Includes the following steps: Step 1: Obtain a unique set of robot speed-wheel speed mapping relationships, and establish a forward kinematics model of the omnidirectional and all-wheel drive mobile robot based on the mapping relationships; Step 2, based on the forward kinematics model of the omnidirectional, all-drive mobile robot, define the system state variables as the robot trajectory tracking... x , y The system control input is defined by the robot's axis error, orientation angle error, and the velocity vectors of the four wheels. x , y Axial velocity and rotation z Axial angular velocity; Step 3: Based on the system state variables, establish the system state equations, and perform Taylor expansion and discretization of the system state equations at the reference state and reference control input of the trajectory to obtain the system state transition equations. Then, design an objective function to transform the physical limit of the motor from a problem with multiple hard constraints on the control input into an unconstrained problem and solve it to obtain the optimal control input in the next control cycle. The specific process of establishing the objective function is as follows: Define the control increment as To ensure the smooth coordination of the motors during robot movement, the following definition is made: in, To avoid scaling factors when vectors are perpendicular; and Each of these represents a set of wheels along the lower edge of the robot's local coordinate system. x shaft and y The speed of the shaft, i Representing four sets of wheels, i =1~4; The final objective function format is: in, It is the current state error, that is, the position error and attitude error relative to the reference trajectory. Indicates taking system state variables The first three elements determine the accuracy of trajectory tracking, which is achieved by adjusting the weight matrix. The weight parameters of each factor are used to adjust the importance of each state variable; This is the increment of the state variables, which determines the smoothness of the robot's motion. This is achieved by adjusting the matrix. The weight of each steering motor is set using the weight values ​​in the parameters. This part requires fine-tuning based on the actual motor conditions. The goal is to make the control increment as close to zero as possible without affecting the tracking performance, by adjusting the matrix. The weight values ​​in the code are used to set the weight of each control increment; finally... This represents the relaxation factor, used to ensure that a solution eventually exists. The system state transition equation is used to obtain the predicted system state at the next moment based on the current system state and control quantity. The objective function is used to quantify the control objective. Minimizing the objective function ensures the minimization of trajectory tracking error and the continuity of motion, resulting in a series of optimal control inputs for the system. Only the first control input is applied to the actual system. Then, in the next control cycle, the system is re-optimized based on the updated state, thereby achieving real-time adjustment and dynamic optimization through a rolling window, ultimately realizing trajectory tracking. Step 4: IMU pre-integration is used between every two control cycles to obtain higher frequency IMU odometry. The error between the robot's actual control input and the desired control input is accumulated and recorded between every two control cycles. Based on this, the actual control quantity is corrected to achieve online rolling closed-loop optimization.

2. The omnidirectional omnidirectional 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, all-drive mobile robot is as follows: Define the robot's initial position as the origin of the world coordinate system, and its initial orientation as... x The positive axis; define the robot's center position as the origin of the robot's local coordinate system, and the robot's orientation as the origin of the robot's local coordinate system. x Axis orientation; both coordinate systems only consider two-dimensional planar motion, therefore the positive z-axis is chosen to be perpendicular to the ground and pointing upwards. Both coordinate systems follow a right-handed coordinate system; the transformation matrix between the robot's local coordinate system and the world coordinate system is used. Mapping odometry information to the robot's local coordinate system: Where, vector The lower edge of the robot world coordinate system x , y The linear velocity of the shaft and its surroundings z angular velocity of the axis, x , y, θ This refers to the robot's pose information in the world coordinate system; Let be the linear velocity and angular velocity of the robot in the local coordinate system; Considering a fixed-shape omnidirectional, all-drive mobile robot as a rigid body, the position of any point on the rigid body in the world coordinate system is: in, It is the position of the robot's center within the world coordinate system. It represents the position of any wheel in the robot's local coordinate system; for the robot... It does not change with time, therefore we get: Assuming that both the robot's local coordinate system and world coordinate system are 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 equation, we obtain: in, i Representing four sets of wheels, i =1~4; and Each of these represents a set of wheels along the lower edge of the robot's local coordinate system. x shaft and y The speed of the shaft; Given the two-dimensional coordinates of each wheel in the robot's local coordinate system, a unique robot velocity-wheel velocity mapping relationship is found, represented as a matrix. Furthermore, a forward kinematics model for an omnidirectional, all-drive robot was established: in, For the robot's wheelbase, For the robot's wheelbase, Let be the velocity vector of each wheel in the robot's local coordinate system.

3. The omnidirectional, all-drive robot trajectory tracking method according to claim 2, characterized in that, In step 2, the system state variables are defined. System control input is ,in These represent the robot's actual position and the reference trajectory, respectively. x axis, y Inter-axis error This represents the orientation angle error between the robot's actual orientation and the reference trajectory. Let be the velocity vector of each wheel in the robot's local coordinate system.

4. The omnidirectional, all-drive robot trajectory tracking method according to claim 3, characterized in that, The process of constructing the system state transition equations is as follows: Let the system state equations be: In an ideal scenario, the robot moves along a reference trajectory, so in Performing a first-order Taylor expansion on the system state equations yields the system state transition equations: in, This is the reference control input in the reference trajectory at the current moment. , for The reference wheel velocity vector is calculated using the forward kinematics model of the omnidirectional all-drive robot. For Nopeah's remainder; The Taylor expansion of the system state equations is expressed in matrix form, and its discrete expression is obtained using the forward Euler method: in, The system state matrix, Input matrix to the system, The initial state matrix, To control the cycle, for k The value of the system state variables at any given time.

5. The omnidirectional, all-drive robot trajectory tracking method according to claim 4, characterized in that, In step 4, the robot's control frequency is kept consistent with the laser odometer frequency at 10Hz. To compensate for external mechanical errors in real time, the actual value executed by the steering motor needs to be calculated between every two frames of control commands. Reference values ​​issued by the host computer Error between Therefore, a 100Hz IMU odometry was obtained through IMU pre-integration, which is beneficial for the prediction period. and Subdivided into There are 10 moments in total. l =1,2,...,10, at each subdivision time step, the attitude information obtained from the IMU odometry is used to obtain the forward kinematics model of the omnidirectional all-drive robot, and the position of each motor is calculated. The system monitors the real-time status of each moment and records the current error cumulatively. By accumulating errors and using correction formulas, each group of motors can be kept in optimal condition during each control cycle, thus achieving faster convergence speed; the correction formula is expressed as: Before each final generator command is issued, a compensation state variable is applied to each steering motor. For compensation parameters.

Images