A trajectory tracking control method for tire slip estimation in orchard environment
By using a dynamic model and a neural network adaptive sliding mode control method, the problem of the trajectory tracking controller sliding in soft soil in an orchard environment was solved, achieving efficient and accurate trajectory tracking.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NANJING UNIV OF SCI & TECH
- Filing Date
- 2022-12-22
- Publication Date
- 2026-05-12
AI Technical Summary
Existing trajectory tracking algorithms exhibit poor performance in orchard environments, especially prone to tracking failures under severe constraints and complex conditions. In particular, kinematic model-based controllers are ineffective when slippage occurs in soft soil in orchards.
An adaptive sliding mode control method based on a dynamic model is adopted. By estimating the longitudinal sideslip parameters online and combining kinematic and dynamic models, a dual closed-loop control system is designed. The neural network is used to estimate the unknowns online and accurately control the torque of the left and right wheels to achieve trajectory tracking.
This technology enables efficient and accurate trajectory tracking in orchard environments, improving response speed and precision, and allowing for stable tracking of the desired trajectory even in the presence of longitudinal and lateral slippage.
Smart Images
Figure CN115840368B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of tracking control, and more specifically to a trajectory tracking control method for estimating tire slippage in an orchard environment. Background Technology
[0002] Trajectory tracking is a fundamental requirement for the normal operation of mobile robots, and its continuous development plays an indispensable role in robotics research. In recent years, numerous scholars both domestically and internationally have studied the trajectory tracking problem of various mobile robots, leading to the continuous development and maturation of trajectory tracking technology. Existing trajectory tracking algorithms are mainly divided into pure trajectory tracking and Stanley's algorithm based on kinematic models, as well as algorithms such as LQR and MPC based on dynamic models, each with its own advantages. Pure trajectory tracking can achieve trajectory tracking on roads with varying curvatures, but it may fail to track at high vehicle speeds. The LQR algorithm is a control method based on state feedback, which can linearize nonlinear models and solve for the optimal solution by designing an optimal quadratic function. However, this algorithm ignores many nonlinear components in practical application scenarios, and the solution process is costly.
[0003] Numerous studies have been conducted on trajectory tracking in orchard environments, but most existing trajectory tracking algorithms are based on kinematic models to design trajectory tracking controllers, such as Pure-Pursuit (PP) and Stanley's algorithm. While these algorithms are simple in concept and can achieve good performance for most trajectories, their performance deteriorates when faced with demanding desired trajectories or complex real-world environments, sometimes even resulting in tracking failure. This paper addresses the issue of designing controllers based on kinematic models. Summary of the Invention
[0004] The purpose of this invention is to provide a trajectory tracking control method for tire slip estimation in an orchard environment, which can achieve efficient and accurate trajectory tracking in an orchard environment.
[0005] The technical solution to achieve the purpose of this invention is as follows:
[0006] This invention designs a novel control method for online estimation of longitudinal sideslip parameters and observation of sideslip velocity in sliding mode. It is a neural network adaptive sliding mode control method based on a dynamic model. By using the error between the given desired trajectory and the actual output pose, the kinematic controller can provide the desired linear velocity and angular velocity. The dynamic controller can then achieve torque output, accurately controlling the torque output of the left and right wheels, thereby controlling the actual linear velocity of the left and right wheels. This invention proposes a trajectory tracking control method for tire slip estimation in an orchard environment, specifically including the following steps:
[0007] Step 1: Based on the existing kinematic model of the differential mobile robot under ideal conditions, establish a kinematic model of the differential mobile robot where the drive axis does not coincide with the geometric center;
[0008] Step 2: Based on the differential mobile robot kinematics model, and combined with the desired trajectory model, establish the trajectory tracking error equation;
[0009] Step 3: Based on the trajectory tracking error equation, and using the Lyapunov direct method, design the outer loop kinematic controller, and obtain the control velocity and angular velocity of the system under ideal conditions through the error.
[0010] Step 4: Establish the existing differential mobile robot dynamics model based on the Lagrange method, and design an inner loop dynamics controller based on the dynamics model to convert the linear velocity and angular velocity under ideal conditions into torque control on the left and right wheels;
[0011] Step 5: Use a neural network model to estimate the unknowns and uncertain parameters in the inner loop dynamics model online.
[0012] Compared with the prior art, the advantages of the present invention are as follows:
[0013] 1. Taking wheeled mobile robots whose drive axis center does not coincide with their geometric center as the research object, this approach is universal and can optimize kinematic models and controller design.
[0014] 2. When designing the controller, the slippage caused by the loose soil in the orchard environment was taken into account. By compensating for the degree of slippage, the response speed and accuracy of the actual trajectory tracking task were improved. Attached Figure Description
[0015] Figure 1 This is a simplified structural diagram of the mobile robot studied in this invention.
[0016] Figure 2 This is a block diagram of the dual closed-loop control system of the present invention.
[0017] Figure 3 This is a flowchart of kinematic modeling and error equation derivation.
[0018] Figure 4 This is a flowchart of a controller design based on tracking error.
[0019] Figure 5 This is a flowchart of neural network adaptive control. Detailed Implementation
[0020] To better understand the steps, advantages, and implementation process of this invention, the invention will be further described below with reference to the accompanying drawings.
[0021] This invention utilizes a dual-closed-loop control system based on existing kinematic and dynamic models to provide precise control laws for the output torque on the left and right wheels of a mobile robot. This achieves a trajectory tracking control method for estimating tire slippage in a field orchard environment. (See attached diagram.) Figures 1-5 . Figure 1 This is a simplified diagram of the differential mobile robot structure when the center of mass and the drive axis do not coincide, which simplifies the subsequent derivation. Figure 2 The block diagram shows a dual closed-loop control system for trajectory tracking under conditions of simultaneous longitudinal and lateral slippage in an orchard environment. Figure 3 The distance between the center of mass and the center of the drive shaft will produce a certain radius of rotation. The state equation can be derived by using the forward or inverse kinematic equations. The difference between the state equation and the state equation for generating the desired trajectory can be calculated, and the first derivative of the state equation can be obtained to obtain the trajectory tracking error equation. Figure 4 The kinematic controller is derived through error equations, and a dynamic model is established to design the controller. This is mainly achieved by establishing the Lagrange equations, taking the second derivative of the state equations and substituting it into the equations. The actual parameters of the mobile robot are then substituted into the equations to obtain a dynamic model for the robot. Based on this model, the sliding surface and kinematic controller are designed. Figure 5 This is a flowchart for online estimation of unknown disturbances in the dynamic model. It mainly involves adjusting the network weights using the gradient descent method based on the actual output of the system and the control output of the kinematic controller.
[0022] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.
[0023] The method of this invention is based on the existing dual closed-loop control system, and designs the inner loop controller and the outer loop controller. The inner loop employs an RBF neural network, such as... Figure 2 As shown, taking an apple-picking robot as an example, this robot is equipped with four independent motors, each consisting of one linear motor and one rotary motor. It also has lidar and GNSS sensors to provide the desired route. A trajectory tracking control method based on tire slippage estimation in an orchard environment is proposed. This involves building a kinematic model of the mobile robot where the center of mass and the drive axis center do not coincide; designing a kinematic controller based on the Lyapunov direct method; establishing a robot dynamics model based on the Lagrange method; designing and building a neural network model to estimate unknown disturbances in the dynamics model online. The specific steps are as follows:
[0024] Step 1: Based on the existing kinematic model of the differential mobile robot under ideal conditions, establish a kinematic model of the differential mobile robot where the drive axis does not coincide with the geometric center, and combine it with... Figure 3 Specifically, it includes:
[0025] Step 1.1: Simplify the mobile robot model as follows Figure 1 As shown, the system state variables are determined using the characteristics of the differential mobile robot;
[0026] Step 1.2: Using physical knowledge and combining existing kinematic models, derive the kinematic model of the differential mobile robot when the drive axis and the geometric center do not coincide in the global coordinate system;
[0027] Step 2: Based on the differential mobile robot kinematics model, and combined with the desired trajectory model, establish the trajectory tracking error equation, specifically including:
[0028] Step 2.1: Establish the state equation of the desired trajectory based on the desired trajectory. The equation form must have the same input and output form as the kinematic state equation model.
[0029] Step 2.2: Use the coordinate transformation matrix to transform the kinematic state equation in the local coordinate system into the equation in the global coordinate system as shown in equation (1), and combine it with the desired trajectory equation in the global coordinate system to derive the trajectory tracking error equation as shown in equation (2):
[0030]
[0031]
[0032] Where (x,y,θ) represents the pose, (v,ω) represents the outer loop controller output, and d represents the distance between the drive shaft center and the geometric center. e ,y e ,θ e ) represents the pose error, (v d ,ω d ) represents the desired linear velocity and angular velocity.
[0033] Step 3: Based on the trajectory tracking error equation, design the outer-loop kinematic controller using the Lyapunov direct method, specifically including:
[0034] Step 3.1: Take the first derivative of the trajectory tracking error equation to obtain the trajectory tracking error model;
[0035] Step 3.2: Construct the Lyapunov function as shown in equation (3), which needs to include all state error quantities existing in the system and appear in the form of square terms to ensure that the constructed Lyapunov function is positive definite;
[0036]
[0037] in, The horizontal axis error function is (y e ,θ e ) represent the longitudinal error and the angular error, respectively, and k2 is the parameter of the Lyapunov function.
[0038] Step 3.3: Calculate the first derivative of the constructed Lyapunov function and substitute it into the system trajectory tracking error model to obtain the system stability judgment condition;
[0039] Step 3.4: Design the system control input according to the Lyapunov stability criteria, so that its first derivative is negative definite.
[0040] Step 4: Establish a differential mobile robot dynamic model in an orchard environment based on the existing Lagrange method, and design an inner-loop dynamic controller based on the dynamic model, combined with... Figure 4 Specifically, it includes:
[0041] Step 4.1: The root motion Lagrange method is used to establish the dynamic equations for the differential mobile robot under ideal conditions. The inputs are the linear velocity and angular velocity output by the outer loop motion controller.
[0042] Step 4.2: Calculate the second derivative of the kinematic state equation, set parameters such as robot mass, moment of inertia, and tire radius, and decompose the total disturbance of the system into the left and right tires;
[0043] Step 4.3: Substitute the second derivative of the state and the parameters of the mobile robot into the dynamic equations to establish a dynamic model;
[0044]
[0045] in, Represents the system's inertia matrix. Let η be the system state estimation error. d =[η R η L ] T η is the total disturbance experienced by the system. L η R These are the components of the total disturbance on the left and right wheels, respectively. Represented as the system's input transformation matrix, This represents the output torque vector of the left and right drive wheels of the mobile robot.
[0046] Step 5: Through the neural network model, the unknowns and uncertain parameters in the dynamic model are estimated online, and sliding mode control is used to reduce system oscillations. The network weights are continuously adjusted by the difference between the actual output and the expected value.
[0047] Step 5.1: Combining Figure 5 The neural network structure is defined as follows: 2 input neurons, 5 hidden neurons, and the transformation function is shown in Equation (5). The input neurons represent the linear velocity and angular velocity output by the outer loop motion controller, respectively, and the output is the estimated value of the unknown parameter terms in the inner loop dynamic model, as shown in Equation (6).
[0048]
[0049] y = W T h(x) (6)
[0050] Among them, c j (j=1,2,…,5) is the center of symmetry of the transformation function, b j (j = 1, 2, ..., 5) represents the width of the transformation function, W is the weight vector, and h is the transformation function. This is the estimated value output by the network model.
[0051] Step 5.2: Design the sliding surface using the linear and angular velocities output by the kinematic controller to force the system to reach the sliding surface and move along it, thereby reducing the system's oscillation amplitude and frequency;
[0052] Step 5.3: Compare the actual output value of the system with the expected value, and adjust the weights of the system using the gradient descent method.
[0053] This invention describes an apple-picking robot equipped with four linear motors and four rotary motors that moves along a desired trajectory defined by a path plan. It provides efficient and robust control against longitudinal and lateral slippage present in orchard environments. The invention introduces slippage parameters to describe the degree of longitudinal slippage and uses a sliding mode observer to estimate the lateral slippage, obtaining the required control input. This control input is then distributed to the eight motors through a task allocation principle, enabling efficient trajectory tracking of the picking robot even when longitudinal and lateral slippage coexist in an orchard environment. This significantly improves the stability and accuracy of the system's trajectory tracking. When the device is operating normally, this method achieves 100% tracking of the desired trajectory.
[0054] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A trajectory tracking control method for tire slip estimation in an orchard environment, used in a dual closed-loop control system, characterized in that, Including the following steps: Step 1: Establish a differential kinematic model of the mobile robot whose drive axis does not coincide with the geometric center; Step 2: Based on the differential mobile robot kinematics model, and combined with the desired trajectory model, establish the trajectory tracking error equation; Step 3: Based on the trajectory tracking error equation, and using the Lyapunov direct method, design the outer loop kinematic controller, and obtain the control velocity and angular velocity of the system under ideal conditions through the error. Step 4: Establish a differential mobile robot dynamic model based on the Lagrange method, and design an inner loop dynamic controller based on the dynamic model to convert the linear velocity and angular velocity under ideal conditions into torque control on the left and right wheels; Step 5: Through the neural network model, the unknowns and uncertain parameters in the inner loop dynamics model are estimated online, and sliding mode control is used to reduce system oscillations. The network weights are continuously adjusted by the difference between the actual output and the expected value. Step 5 specifically includes: Step 5.1: Specify the neural network input and output, number of neurons, number of intermediate hidden neurons, transformation function, and the center, width, and initial weight parameters of the transformation function; Step 5.2: Design the sliding surface using the linear and angular velocities output by the kinematic controller, forcing the system to reach the sliding surface and move along it; Step 5.3: Compare the actual output value of the system with the expected value, and adjust the weights of the neural network using the gradient descent method.
2. The trajectory tracking control method according to claim 1, characterized in that, Step 1 specifically includes: Step 1.1: Determine the system state variables using the characteristics of the differential mobile robot; Step 1.2: Based on the existing kinematic model, derive the kinematic model of the differential mobile robot when the driving axis and the geometric center do not coincide in the global coordinate system.
3. The trajectory tracking control method according to claim 2, characterized in that, Step 2 specifically includes: Step 2.1: Establish the state equation of the desired trajectory based on the desired trajectory. The equation form must have the same input and output form as the kinematic model equation. Step 2.2: Use the coordinate transformation matrix to transform the kinematic model equations in the local coordinate system into the equations in the global coordinate system as shown in equation (1), and combine them with the desired trajectory equation in the global coordinate system to derive the trajectory tracking error equation as shown in equation (2): (1) (2) in, For position, For the outer loop controller output, The distance between the center of the drive shaft and the geometric center. For pose error, Let be the desired linear velocity and angular velocity.
4. The trajectory tracking control method according to claim 3, characterized in that, Step 3 specifically includes: Step 3.1: Take the first derivative of the trajectory tracking error equation to obtain the trajectory tracking error model; Step 3.2: Construct a Lyapunov function that includes all state error quantities present in the system and appears in the form of squared terms, making the constructed Lyapunov function positive definite; Step 3.3: Calculate the first derivative of the constructed Lyapunov function and substitute it into the system trajectory tracking error model to obtain the system stability judgment condition; Step 3.4: Design the system control input according to the Lyapunov stability criteria, so that its first derivative is negative definite.
5. The trajectory tracking control method according to claim 4, characterized in that, The Lyapunov function is: in, The horizontal axis error function, These are longitudinal error and angular error, respectively. For Lyapunov function parameters.
6. The trajectory tracking control method according to claim 1, characterized in that, Step 4, which establishes the differential mobile robot dynamics model based on the Lagrange method, specifically includes: Step 4.1: The root motion Lagrangian method is used to establish the dynamic equations for the differential mobile robot under ideal conditions, with the output of the motion controller as the input; Step 4.2: Calculate the second derivative of the kinematic model, set the robot parameters, and decompose the total disturbance of the system into the left and right tires; Step 4.3: Substitute the second derivative and the mobile robot parameters into the dynamic equations to establish a dynamic model.
7. The trajectory tracking control method according to claim 6, characterized in that, The robot parameters include mass, moment of inertia, and tire radius.
8. The trajectory tracking control method according to claim 6, characterized in that, The dynamic model is as follows: in, Represents the system's inertia matrix. For system state estimation error, The total disturbance experienced by the system. , These are the components of the total disturbance on the left and right wheels, respectively. Represented as the system's input transformation matrix, This represents the output torque vector of the left and right drive wheels of the mobile robot.
9. The trajectory tracking control method according to claim 1, characterized in that, There are 2 neurons and 5 hidden neurons in the middle. The transformation function is: in, The center of symmetry of the transformation function, To transform the function width, For the weight vector, For transformation function, The estimated value output by the network model. .