Car-type mobile robot trajectory tracking control method

Abstract

The application relates to a track tracking control method of a wheeled mobile robot. By constructing a kinematic controller of the wheeled mobile robot, a virtual linear velocity and a virtual heading angular velocity are generated according to a pose tracking error of the robot, a lumped disturbance is observed through an extended state observer and is compensated in a designed linear velocity controller and a heading angle controller, so that accurate track tracking control is realized. By considering at least one of the lumped disturbances, including wheel longitudinal slip and / or side slip disturbance, dynamic model uncertainty and unknown input disturbance, the robustness of the track tracking control of the wheeled mobile robot in a complex environment is improved.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a trajectory tracking control method for a vehicle-type mobile robot. Background Technology

[0002] Car-like mobile robots are typical nonholonomic systems, and research on their trajectory tracking control is often based on the assumption of pure rolling without slippage. Due to the complex working environment of car-like mobile robots, their trajectory tracking is inevitably affected by environmental uncertainties. These uncertainties include changes in robot load causing alterations in mass and inertial parameters in the dynamic model, frictional torques on different surfaces, and external disturbances affecting the robot. Among these, longitudinal and lateral slippage due to insufficient tire adhesion is most common in situations such as icy roads in winter, wet and slippery roads in rainy weather, agricultural and forestry roads, or sharp turns. Due to the slippage effect, the conventional assumption of pure rolling without slippage cannot hold, potentially compromising the stability and controllability of the car-like mobile robot and reducing its trajectory tracking control accuracy. Therefore, overcoming the influence of various uncertainties to achieve high-performance trajectory tracking control is a major challenge for car-like mobile robots. Summary of the Invention

[0003] Based on this, to address the problem of reduced trajectory tracking control accuracy of vehicle-type mobile robots under lumped disturbances including at least one of wheel longitudinal and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances, a trajectory tracking control method for vehicle-type mobile robots is provided. This method includes: constructing a kinematic controller for the vehicle-type mobile robot, and generating virtual linear velocity and virtual heading angular velocity based on the pose tracking error of the vehicle-type mobile robot;

[0004] A linear velocity controller is constructed to track the virtual linear velocity, so that the actual forward speed of the vehicle-type mobile robot meets the requirements of the kinematic controller. The lumped disturbance of the linear velocity control system is observed through a first extended state observer.

[0005] A heading angle controller is constructed to track the virtual heading angle, so that the actual attitude angle of the vehicle-type mobile robot meets the requirements of the kinematic controller. The lumped disturbance of the heading angle control system is observed using a second extended state observer.

[0006] The linear velocity and heading angular velocity of the robot are controlled based on the outputs of the linear velocity controller and the heading angle controller to achieve trajectory tracking.

[0007] In one embodiment, the pose error is calculated based on the current pose and desired pose of the vehicle-type mobile robot; the kinematic controller is as follows:

[0008]

[0009] Where, k x k y k θ For positive controller parameters, e x e y e θ These represent the longitudinal position error, lateral position error, and heading angle error of the vehicle-type mobile robot in the robot coordinate system, respectively, compared to the reference pose and the actual pose. r The reference linear velocity for the vehicle-type mobile robot. The reference heading angular velocity for the vehicle-type mobile robot; The virtual velocity command generated for the kinematic controller, v c , These are the virtual linear velocity and virtual heading angular velocity of the vehicle-type mobile robot, respectively.

[0010] In one embodiment, the first extended state observer is an extended state observer for a linear velocity control system, implemented by the following formula:

[0011]

[0012] in, Representing states v and g respectively v The estimated value, v is the linear velocity of the vehicle-type mobile robot, g v The lumped disturbance for the linear velocity control system of a vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance. They are respectively The first derivative, β represents the observation error of the first extended state observer regarding the linear velocity. 11 β 12 For a positive observer gain, τ v Let be the linear motion torque of the rear wheel of the vehicle-type mobile robot, and m be the mass of the vehicle-type mobile robot.

[0013] The linear velocity controller is implemented using the following formula:

[0014]

[0015] Where, τ v Let m be the linear motion torque of the rear wheel of the vehicle-type mobile robot, and k be the mass of the vehicle-type mobile robot. v For system control gain, e v For speed tracking error, v c , These are virtual linear velocity and virtual linear acceleration, respectively. The linear velocity is the observation value of the first extended state observer. For the first extended state observer, the lumped disturbance g of the linear velocity control system of the vehicle-type mobile robot v The estimated value.

[0016] In one embodiment, the heading angle controller includes a virtual control law for the front wheel steering angle, a virtual control law for the front wheel steering angular velocity, and a control law for the front wheel steering torque, used to generate the front wheel steering torque of the vehicle-type mobile robot; the heading angle control system is as follows:

[0017]

[0018] Where θ is the heading angle of the vehicle-type mobile robot, φ is the steering angle of the front wheels of the vehicle-type mobile robot, and ω is the angular velocity of the steering of the front wheels of the vehicle-type mobile robot. Let be the first derivatives of the heading angle, front wheel steering angle, and front wheel steering angular velocity of the vehicle-type mobile robot, respectively; v be the linear velocity of the vehicle-type mobile robot; L be the axle distance between the front and rear wheels of the vehicle-type mobile robot; and J be the first derivatives of the steering angle, front wheel steering angle, and front wheel steering angular velocity of the vehicle-type mobile robot. ω For the moment of inertia of the front wheels of the vehicle-type mobile robot, g ω The lumped disturbance of the heading angle control system of the vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance, τ ω This refers to the steering torque of the front wheels of a vehicle-type mobile robot.

[0019] In one embodiment, the method further includes:

[0020] Based on the heading angular velocity error of the vehicle-type mobile robot, a heading angular error sliding surface and a virtual control law for the front wheel rotation angle based on the heading angular error sliding surface are designed.

[0021] The sliding surface s of the vehicle-type mobile robot for heading angle error θc This can be achieved using the following formula:

[0022]

[0023] Where, k θc >0 represents the control gain, e θc Let θ be the heading angle error, and θ be the heading angle of the vehicle-type mobile robot. c The virtual heading angle for the vehicle-type mobile robot. This represents the heading angular velocity error.

[0024] The virtual control law for the front wheel steering angle is implemented using the following formula:

[0025]

[0026] Where, φ cThe virtual front wheel steering angle serves as the tracking target. When the linear velocity of the vehicle-type mobile robot is greater than or equal to a preset value, the virtual front wheel steering angle control law is activated. For virtual front wheel steering angular velocity; v, Here, L represents the linear velocity and linear acceleration of the vehicle-type mobile robot, respectively; L is the axle distance between the front and rear wheels of the vehicle-type mobile robot; and k is the linear velocity and linear acceleration of the vehicle-type mobile robot. θc k s A positive control gain For the heading angular velocity error, s θc For the designed sliding surface, It is the first derivative of the virtual heading angular velocity.

[0027] In one embodiment, the method includes:

[0028] Based on the tracking error of the front wheel angle of the vehicle-type mobile robot, a virtual control law for calculating the front wheel steering angular velocity in the heading angle controller is designed.

[0029] The virtual control law for front wheel steering angular velocity ω c This can be achieved using the following formula:

[0030]

[0031] Where, k φ >0 represents the control gain, φ c , These are the virtual front wheel steering angle and the virtual front wheel steering angular velocity, respectively. e is an estimated value of the front wheel steering angle φ. φ This represents the front wheel steering angle tracking error.

[0032] In one embodiment, the method includes:

[0033] Based on the front wheel angle tracking error of the vehicle-type mobile robot, design the front wheel steering torque controller in the heading angle controller to calculate the front wheel steering torque;

[0034] The front wheel steering torque controller is implemented using the following formula:

[0035]

[0036] Where, τ ω J is the steering torque of the front wheel of the vehicle-type mobile robot. ω For the moment of inertia of the front wheels of the vehicle-type mobile robot, Let k be the virtual front wheel steering angle acceleration. ω >0 represents the control gain, e ω For front wheel steering angle tracking error, For the second extended state observer of the vehicle-type mobile robot, the lumped disturbance g of the heading angle control system ω The estimated value.

[0037] In one embodiment, the second extended state observer is a front wheel steering angle system extended state observer, implemented by the following formula:

[0038]

[0039] in, These represent the front wheel steering angle φ, the front wheel steering angular velocity ω, and the lumped disturbance g of the heading angle control system, respectively. ω The estimated value, They are respectively The first derivative, β represents the observation error of the second extended state observer regarding the front wheel steering angle. 21 β 22 β 23 A positive observer gain; J ω Let τ be the moment of inertia of the front wheels of the vehicle-type mobile robot. ω This refers to the steering torque of the front wheels of a vehicle-type mobile robot.

[0040] In one embodiment, the method further includes:

[0041] Construct the kinematic and dynamic models of the vehicle-type mobile robot; wherein,

[0042] The kinematic model characterizes the position state change equation of the vehicle-type mobile robot under the control inputs of linear velocity and front wheel steering angular velocity;

[0043] The dynamic model characterizes the relationship between the running speed and the input torque of the vehicle-type mobile robot under the presence of lumped disturbances.

[0044] In one embodiment, virtual linear velocity and virtual heading angular velocity are generated based on the pose tracking error of the vehicle-type mobile robot, and used as inputs to the linear velocity controller and the heading angular velocity controller.

[0045] In one embodiment, controlling the linear velocity and angular velocity of the vehicle-type mobile robot based on the outputs of the linear velocity controller and the heading angle controller to achieve trajectory tracking includes:

[0046] The virtual linear velocity and virtual heading angular velocity output by the kinematics controller are used as the tracking targets of the linear velocity controller and the heading angular velocity controller, respectively. The linear motion torque of the rear wheel of the vehicle-type mobile robot output by the linear velocity controller and the steering torque of the front wheel output by the heading angular velocity controller are used as inputs to the dynamics model to obtain the linear velocity and heading angular velocity of the vehicle-type mobile robot. These are then used as inputs to the kinematics model to obtain the pose of the vehicle-type mobile robot. Combined with the reference pose, the pose tracking error is obtained. The pose tracking error can be used as inputs to the kinematics controller to obtain new virtual linear velocity and virtual heading angular velocity for continuous trajectory tracking.

[0047] The aforementioned trajectory tracking control method for a vehicle-type mobile robot constructs a kinematic controller for the robot. Based on the robot's pose tracking error, it generates virtual linear velocity and virtual heading angular velocity. These virtual linear velocity and heading angular velocity are tracked by a linear velocity controller and a heading angular velocity controller, respectively. An extended state observer observes lumped disturbances, which are then compensated for within the controller to achieve trajectory tracking control of the vehicle-type mobile robot. By controlling the robot's heading angle and considering lumped disturbances, trajectory tracking control is achieved even in situations involving wheel slippage and / or sideslip, improving its robustness in complex environments. Attached Figure Description

[0048] Figure 1 This is a block diagram of a vehicle-type mobile robot trajectory control system in one embodiment;

[0049] Figure 2 Here is a flowchart of a trajectory tracking control method for a vehicle-type mobile robot in one embodiment;

[0050] Figure 3 This is a schematic diagram of the trajectory tracking of a lawnmower robot on a circular trajectory in one embodiment;

[0051] Figure 4 This is a schematic diagram of the pose tracking of a lawnmower robot along a circular trajectory in one embodiment;

[0052] Figure 5 This is a schematic diagram illustrating the front wheel angle tracking of a lawnmower robot along a circular trajectory in one embodiment.

[0053] Figure 6 This is a schematic diagram illustrating the pose tracking error of a lawnmower robot on a circular trajectory in one embodiment.

[0054] Figure 7 This is a schematic diagram of lumped disturbance estimation for a lawnmower robot on a circular trajectory in one embodiment.

[0055] Figure 8This is a schematic diagram illustrating the input torque of a lawnmower robot following a circular trajectory in one embodiment.

[0056] Figure 9 This is a schematic diagram of the trajectory tracking of a lawnmower robot under a composite trajectory in one embodiment;

[0057] Figure 10 This is a schematic diagram of pose tracking of a lawnmower robot under a composite trajectory in one embodiment;

[0058] Figure 11 This is a schematic diagram of the front wheel angle tracking of a lawnmower robot under a composite trajectory in one embodiment;

[0059] Figure 12 This is a schematic diagram illustrating the pose tracking error of a lawnmower robot under a composite trajectory in one embodiment.

[0060] Figure 13 This is a schematic diagram of lumped disturbance estimation for a lawnmower robot under a composite trajectory in one embodiment.

[0061] Figure 14 This is a schematic diagram of the input torque of a lawnmower robot under a composite trajectory in one embodiment. Detailed Implementation

[0062] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0063] Numerous 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 than those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.

[0064] The following describes some embodiments of the trajectory tracking control method for vehicle-type mobile robots according to the present invention, with reference to the accompanying drawings. The method of the present invention can be applied to the cloud or a server, or to a device terminal capable of trajectory tracking control, such as vehicle-type lawnmower robots, vehicle-type agricultural robots, vehicle-type pool cleaning robots, vehicle-type snow sweeping / blowing robots, vehicle-type leaf sweeping / blowing robots, vehicle-type inspection robots, and vehicle-type automated guided vehicles (AGVs). The following description uses the application of this method to a vehicle-type mobile robot as an example.

[0065] Vehicle-type mobile robots are typically four-wheeled mobile robots with front and rear wheels. The rear wheels move in the same direction as the robot, while the front wheels are allowed to rotate around a vertical axis. As a type of wheeled mobile robot, vehicle-type mobile robots are typical nonholonomic systems. In related technologies, motion control schemes for vehicle-type mobile robots are based on ideal constraints of pure rolling without slippage. However, when vehicle-type mobile robots operate in complex environments, such as icy roads, wet roads, or forestry roads, the ideal constraint assumptions cannot be satisfied, and longitudinal slippage (longitudinal slip) and / or lateral slippage (side slip) of the wheels easily occur. This violates the nonholonomic constraints of the system, affecting the reliability of the robot's motion control.

[0066] like Figure 1 As shown, the scheme adopted by the trajectory tracking control method for a vehicle-type mobile robot of the present invention is provided. This control scheme consists of a kinematic controller, a linear velocity controller, and a heading angle controller. The kinematic controller is designed for the nonholonomic motion constraints of the vehicle-type mobile robot to generate virtual linear velocity and virtual heading angle. Based on the dynamic model of the vehicle-type mobile robot, an extended state observer is used to estimate the lumped disturbance and design the motion and steering control laws of the vehicle-type mobile robot to achieve its trajectory tracking control.

[0067] In one embodiment, the vehicle-type mobile robot is subject to kinematic disturbances caused by wheel longitudinal slip and sideslip during operation, and its kinematic model can be described as follows:

[0068]

[0069] Where x and y represent the Cartesian coordinates of the rear axle center of the vehicle-type mobile robot, θ is the heading angle of the vehicle-type mobile robot, and φ is the front wheel steering angle of the vehicle-type mobile robot. Let x, y, θ, and φ be the first derivatives, respectively; v and ω be the control inputs of the vehicle-type mobile robot, where v represents the linear velocity of the vehicle-type mobile robot and ω represents the steering angular velocity of the front wheels of the vehicle-type mobile robot; L is the axle distance between the front and rear wheels of the vehicle-type mobile robot; δ1 and v y δ represents the front wheel slip angle and lateral velocity caused by wheel sideslip. v The longitudinal slippage speed caused by the wheel's longitudinal slippage.

[0070] Under the zero potential energy assumption of the vehicle-type mobile robot, based on the Euler-Lagrange equations, the dynamic model of the vehicle-type mobile robot can be described as follows:

[0071]

[0072]

[0073] in, For the linear acceleration of the vehicle-type mobile robot, The g value represents the angular acceleration of the front wheels of the vehicle-type mobile robot, where m is the mass of the vehicle-type mobile robot; v The lumped disturbance of the linear velocity control system of the vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance; g ω The lumped disturbance for the heading angle control system of a vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance; J ω τ represents the moment of inertia of the front wheels. v τ is the linear motion torque of the rear wheels of the vehicle-type mobile robot. ω This refers to the steering torque of the front wheels of a vehicle-type mobile robot.

[0074] In one embodiment, the current pose of the vehicle-type mobile robot tending towards the reference pose Based on the relationship between the current pose and the reference pose, the robot pose tracking error e can be obtained. q As shown in equation (3):

[0075]

[0076] Among them, e x e y e θ These represent the longitudinal position error, lateral position error, and heading angle error of the vehicle-type mobile robot in the robot coordinate system, respectively, compared to the reference pose. Differentiating equation (3), we obtain the following differential equation for the pose tracking error:

[0077]

[0078]

[0079]

[0080] Among them, v r φ r These are the reference linear velocity and reference front wheel rotation angle for the vehicle-type mobile robot. This represents the heading angular velocity of the vehicle-type mobile robot.

[0081] It should be noted that the actual control inputs of the vehicle-type mobile robot trajectory tracking control system are the linear velocity v and the front wheel rotation angle φ, and the trajectory tracking output is the current pose q. This invention considers forward motion (v > 0) and front wheel rotation angle φ ∈ [-0.5, 0.5], and assumes |e θ |<π。If the control trajectory of the vehicle-type mobile robot is a smooth trajectory and the reference pose q r and reference front wheel steering angle φ rGiven this information, the trajectory tracking problem of a vehicle-type mobile robot can be transformed into finding a suitable input vector z = [vφ]. T This causes the pose tracking error e to be... q It converges to zero.

[0082] Based on the kinematic model of the vehicle-type mobile robot shown in equation (1), the pose tracking error of the vehicle-type mobile robot can be written as:

[0083]

[0084]

[0085]

[0086] The following control input is designed using the backstepping control method:

[0087]

[0088] Where, k x k y k θ For positive controller parameters, e x e y e θ These represent the longitudinal position error, lateral position error, and heading angle error of the vehicle-type mobile robot in the robot coordinate system, respectively, compared to the reference pose and the actual pose. r The reference linear velocity for the vehicle-type mobile robot. The reference heading angular velocity for the vehicle-type mobile robot; The virtual velocity command generated for the kinematic controller, v c , These are the virtual linear velocity and virtual heading angular velocity of the vehicle-type mobile robot, respectively.

[0089] Consider the following candidate Lyapunov functions:

[0090]

[0091] Differentiating V and substituting equation (5) into it, we can obtain:

[0092]

[0093] Where, ω r The reference front wheel steering angular velocity is used for vehicle-type mobile robots.

[0094] According to equation (8), when t→∞, we have For any initial input conditions, the pose tracking error of the vehicle-type mobile robot will asymptotically converge to zero.

[0095] The aforementioned kinematic controller can obtain the pose tracking error of the vehicle-type mobile robot based on its current pose and reference pose, and determine the virtual linear velocity and virtual heading angular velocity that can converge the pose tracking error to zero. By configuring a suitable dynamic controller, the actual forward speed and heading angle can be adjusted through the input torque of the vehicle-type mobile robot to meet the requirements of the kinematic controller.

[0096] In one embodiment, a dynamic controller for a vehicle-type mobile robot can be designed, including a linear velocity controller, a heading angle controller, and first and second extended state observers. These controllers are used to adjust the input torque of the vehicle-type mobile robot and control its actual forward speed and heading angle based on the robot's virtual linear velocity and virtual heading angle, taking into account lumped disturbances including at least one of wheel longitudinal and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances. Depending on the application scenario of the vehicle-type mobile robot, the naming conventions and types of interference or disturbances may differ. It should be understood that any interference or disturbance that affects the robot system should be included in the lumped disturbance.

[0097] In some embodiments, to observe the lumped disturbance of the linear velocity control system using the linear velocity information of a vehicle-type mobile robot, the following first extended state observer can be designed:

[0098]

[0099] in, Representing states v and g respectively v The estimated value, v is the linear velocity of the vehicle-type mobile robot, g v The lumped disturbance for the linear velocity control system of a vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance. They are respectively The first derivative, β represents the observation error of the first extended state observer regarding the linear velocity. 11 β 12 For a positive observer gain, τ v Let be the linear motion torque of the rear wheel of the vehicle-type mobile robot, and m be the mass of the vehicle-type mobile robot.

[0100] Define linear velocity tracking error Substituting the first extended state observer, we can obtain the derivative of the linear velocity tracking error of the vehicle-type mobile robot. As shown in equation (10):

[0101]

[0102] To stabilize the linear velocity tracking error, the following linear velocity controller can be designed:

[0103]

[0104] Where, k v This is the system control gain.

[0105] The above embodiment obtains the linear velocity tracking error of the vehicle-type mobile robot, tracks the virtual linear velocity of the vehicle-type mobile robot based on the linear velocity controller, and outputs the linear motion torque of the rear wheels of the vehicle-type mobile robot.

[0106] In some embodiments, by combining equations (1) and (2), the heading angle control system of the vehicle-type mobile robot can be obtained as follows:

[0107]

[0108] As shown in the above formula, the heading angle controller of a vehicle-type mobile robot is related to many control factors, and the controller design can be completed by combining sliding mode control. The heading angle controller includes the virtual control law for the front wheel steering angle, the virtual control law for the front wheel steering angular velocity, and the control law for the front wheel steering torque.

[0109] Based on the virtual heading angular velocity of the vehicle-type mobile robot With heading angular velocity The difference is used to determine the heading angular velocity error of the vehicle-type mobile robot. as follows:

[0110]

[0111] Design heading angle error sliding surface s θc :

[0112]

[0113] Where, k θc >0 controls the gain.

[0114] Therefore, the virtual control law for the front wheel steering angle can be designed as follows:

[0115]

[0116] Where, k s For a positive control gain, φ c The virtual front wheel steering angle serves as the tracking target for the front wheel steering angle. When the linear velocity of the vehicle-type mobile robot is greater than or equal to a preset value, the virtual front wheel steering angle control law is activated. For virtual front wheel steering angular velocity, It is the first derivative of the virtual heading angular velocity.

[0117] From the kinematic model of the vehicle-type mobile robot, it can be seen that when the input linear velocity v is close to zero, the vehicle-type mobile robot will be unable to control the heading angle θ. Therefore, when v is greater than or equal to the preset value, the aforementioned virtual control law for the front wheel steering angle can be used. At the start of driving, v is less than the preset value, and the front wheel steering angle φ can be kept constant (i.e., This preset value can be set to v≥0.1m / s.

[0118] Furthermore, a second extended state observer, as shown in equation (16), can be designed based on the heading angle control system:

[0119]

[0120] in, These represent the front wheel steering angle φ, the front wheel steering angular velocity ω, and the lumped disturbance g of the heading angle control system, respectively. ω The estimated value, They are respectively The first derivative, β represents the front wheel steering angle observation error of the second extended state observer. 21 β 22 β 23 The observer gain is positive.

[0121] The aforementioned steering angle controller can obtain the tracking target of the vehicle-type mobile robot's steering angle controller based on the steering angular velocity error of the vehicle-type mobile robot, thereby realizing trajectory tracking of the vehicle-type mobile robot.

[0122] In some embodiments, a virtual control law for the front wheel steering angular velocity of a vehicle-type mobile robot can be configured. First, the front wheel steering angle tracking error can be obtained:

[0123]

[0124] Among them, e φ For the front wheel steering angle tracking error, φ c For the virtual front wheel turning angle of the vehicle-type mobile robot, This is the estimated value of the front wheel rotation angle for a vehicle-type mobile robot.

[0125] Differentiating equation (17), we get:

[0126]

[0127] Where, β 21 To control the gain, This is the derivative of the virtual front wheel rotation angle of the vehicle-type mobile robot.

[0128] Therefore, the virtual control law for the steering angular velocity of the front wheel of the vehicle-type mobile robot is shown in equation (19):

[0129]

[0130] Where, k φ >0 controls the gain.

[0131] The above embodiments obtain the front wheel angle tracking error of the vehicle-type mobile robot, and can track the front wheel angle change and output the front wheel steering angle velocity through the virtual control law of front wheel steering angular velocity.

[0132] In some embodiments, the front wheel steering angular velocity tracking error of a vehicle-type mobile robot can be defined:

[0133]

[0134] Where, ω c For the virtual front wheel steering angular velocity of the vehicle-type mobile robot, This is an estimated value for the steering angular velocity of the front wheels of a vehicle-type mobile robot.

[0135] Differentiating the above formula, we get:

[0136]

[0137] Furthermore, the front wheel steering torque controller for the vehicle-type mobile robot is designed as follows:

[0138]

[0139] Where, k ω >0 controls the gain.

[0140] The front wheel steering torque controller in the above embodiment can obtain the front wheel steering torque of the vehicle mobile robot based on the tracking error of the front wheel steering angular velocity. The front wheel steering torque and the rear wheel linear motion torque obtained by the linear velocity controller can be used as inputs to the vehicle mobile robot dynamic model (2) to control the movement of the vehicle mobile robot. The linear velocity v and the front wheel steering angular velocity ω of the vehicle mobile robot are obtained based on the output of the dynamic model, so as to realize the tracking of the virtual linear velocity and virtual heading angular velocity output by the kinematic controller of the vehicle mobile robot.

[0141] In one embodiment, such as Figure 2 As shown, a trajectory tracking control method for a vehicle-type mobile robot is provided, the method comprising:

[0142] Step S210: Construct a kinematic controller for the vehicle-type mobile robot and generate virtual linear velocity and virtual heading angular velocity based on the pose tracking error of the vehicle-type mobile robot.

[0143] The process of constructing the kinematic controller for the vehicle-type mobile robot is shown in equations (1) to (7). The pose tracking error can be realized through equation (3). The kinematic controller can generate the virtual linear velocity and virtual heading angular velocity of the vehicle-type mobile robot based on the input pose tracking error. During the trajectory tracking control process, the pose tracking error gradually converges to zero, so that the pose of the vehicle-type mobile robot gradually converges to the reference pose.

[0144] Step S220: Construct a linear velocity controller, track the virtual linear velocity, and observe the lumped disturbance of the linear velocity control system through a first extended state observer.

[0145] Among them, the linear velocity controller (11) can be used to track the virtual linear velocity and output the linear motion torque of the rear wheel of the vehicle-type mobile robot.

[0146] The first extended state observer is an extended state observer for the linear velocity control system. This observer is used to observe the lumped disturbances in the linear velocity control system, including at least one of wheel longitudinal slip and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances. As input to the linear velocity controller, this allows the vehicle-type mobile robot to fully consider the interference caused by the complex environment when performing virtual linear velocity tracking, thereby improving the accuracy of trajectory tracking.

[0147] Step S230: Construct a heading angle controller, track the virtual heading angle, and ensure that the actual heading angle of the vehicle-type mobile robot meets the requirements of the kinematic controller. Use the second extended state observer to observe the lumped disturbance of the heading angle control system.

[0148] Among them, the heading angle controller can be used to track the virtual heading angular velocity of the vehicle-type mobile robot and output the virtual front wheel turning angle, virtual front wheel steering angular velocity, and front wheel steering torque of the vehicle-type mobile robot.

[0149] The second extended state observer is the front wheel steering angle system extended state observer. This observer is used to observe the lumped disturbances in the heading angle control system of the vehicle-type mobile robot, including at least one of wheel longitudinal slip and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances. As input to the heading angle controller, the vehicle-type mobile robot can fully consider the interference caused by the complex environment when performing virtual heading angular velocity tracking, thereby improving the accuracy of trajectory tracking.

[0150] Step S240: Based on the output of the linear velocity controller and the output of the heading angle controller, control the linear velocity and heading angle of the robot to achieve trajectory tracking.

[0151] The linear velocity controller outputs the linear motion torque of the rear wheels of the vehicle-type mobile robot, and the heading angle controller outputs the steering torque of the front wheels of the vehicle-type mobile robot. The vehicle-type mobile robot can control its movement based on the linear motion torque of the rear wheels and the steering torque of the front wheels to achieve trajectory tracking.

[0152] The method described in the above embodiments constructs a kinematic controller for a vehicle-type mobile robot, generates virtual linear velocity and virtual heading angular velocity based on the robot's pose tracking error, tracks these virtual linear velocity and virtual heading angular velocity using a linear velocity controller and a heading angular velocity controller, and observes lumped disturbances using an extended state observer to achieve trajectory tracking control of the vehicle-type mobile robot. By controlling the heading angle of the vehicle-type mobile robot and considering lumped disturbances, trajectory tracking control of the robot is achieved under at least one of the following disturbances: possible wheel longitudinal slip and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances, thereby improving the robustness of trajectory tracking control of the vehicle-type mobile robot in complex environments.

[0153] In one embodiment, the heading angle controller of the vehicle-type mobile robot is used to track the virtual heading angular velocity of the vehicle-type mobile robot. The heading angle controller includes a virtual control law for the front wheel steering angle, a virtual control law for the front wheel steering angular velocity, and a virtual control law for the front wheel steering torque.

[0154] Among them, the virtual control law for the front wheel angle is used to track the virtual front wheel angle of the vehicle-type mobile robot, and the start time of the virtual control law for the front wheel angle is determined according to the linear velocity of the vehicle-type mobile robot. In some cases, the virtual control law for the front wheel angle is implemented by equation (15).

[0155] The virtual control law for the front wheel steering angular velocity is used to generate the virtual front wheel steering angular velocity of the vehicle-type mobile robot based on the front wheel steering angle tracking error. In some cases, the virtual control law for the front wheel steering angular velocity is implemented by equation (19).

[0156] Among them, the front wheel steering torque control law is used to output the front wheel steering torque of the vehicle-type mobile robot based on the tracking error of the front wheel steering angular velocity.

[0157] In some cases, the virtual control law for the front wheel steering angle, the virtual control law for the front wheel steering angular velocity, and the control law for the front wheel steering torque are configured in sequence, and the output of each controller can be used as the input of the next controller to achieve tracking of the heading angle of the vehicle-type mobile robot.

[0158] In some cases, the heading angle controller is equipped with a second extended state observer to obtain an estimate of the lumped disturbance in the heading angle control system, which includes at least one of wheel longitudinal and / or sideslip disturbances, dynamic model uncertainty, and unknown input disturbances, thereby improving the accuracy of trajectory tracking.

[0159] In one embodiment, the above method further includes: constructing a kinematic model and a dynamic model of the vehicle-type mobile robot; wherein the kinematic model characterizes the position state change equation of the vehicle-type mobile robot under the control inputs of linear velocity and front wheel steering angular velocity; and the dynamic model characterizes the correspondence between the running speed and the input torque of the vehicle-type mobile robot in the presence of lumped disturbances.

[0160] The kinematic model of the vehicle-type mobile robot is shown in Equation (1), and the dynamic model is shown in Equation (2).

[0161] In one embodiment, step S240, which controls the linear velocity and angular velocity of the robot based on the output of the linear velocity controller and the output of the heading angle controller to achieve trajectory tracking, further includes:

[0162] The virtual linear velocity and virtual heading angular velocity output by the kinematics controller are used as the tracking targets of the linear velocity controller and the heading angular velocity controller, respectively. The linear motion torque of the rear wheel output by the linear velocity controller and the steering torque of the front wheel output by the heading angular velocity controller are used as inputs to the dynamics model to obtain the linear velocity and heading angular velocity of the vehicle-type mobile robot. These are then used as inputs to the kinematics model to obtain the pose of the vehicle-type mobile robot. Combined with the reference pose, the pose tracking error is obtained. This pose tracking error can continue to be used as inputs to the kinematics controller to obtain new virtual linear velocity and virtual heading angular velocity for continuous trajectory tracking, thereby improving the reliability of trajectory tracking and enabling the vehicle-type mobile robot to run along a preset trajectory.

[0163] To illustrate the robustness of the above trajectory tracking control method, simulation experiments on circular and composite trajectories are conducted using a vehicle-type lawnmower robot as an example.

[0164] Simulation Experiment - Circular Trajectory:

[0165] The kinematic and dynamic parameters of the lawnmower robot are as follows: m = 30 kg, L = 1.2 m, J ω =5kg·m 2 .

[0166] The simulation parameters are set as follows: sampling time 0.01s, simulation time 30s, and the initial reference pose of the lawnmower robot is x. r (0)=0, y r (0) = 0, and the parameters for the circular reference trajectory are selected as follows:

[0167]

[0168] θ r =0.2t rad ,t≥0

[0169] The actual initial pose of the lawnmower robot is set to q(0) = [0 0 0 0]. T The lumped disturbance of the linear velocity control system and the lumped disturbance of the heading angle control system, which include at least one of wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance, are set as follows:

[0170] g v =0.5+cos(t)sin(t)m / s 2

[0171] g ω = -0.3 + 2cos(0.4t)sin(t) rad / s 2

[0172] The aforementioned lumped disturbance is introduced within 15–20 seconds. The kinematic controller parameter is set to k. x =2.5, k y =1.5, k θ =2, the parameters of the first extended state observer are set to ω. o =40, β 11 =2ω o ,β 12 =ω o 2 The linear velocity controller parameter is set to k. v =3.5, the second extended state observer parameter is set to β. 21 =3ω o ,β 22 =3ω o 2 ,β 23 =ω o 3 The heading angle controller parameters are set to k. θr =3,k s =1,k φ =2.5, k ω =3.25.

[0173] Simulation results are as follows Figures 3-8 As shown. Figure 3 The image shows the trajectory tracking performance of the lawnmower robot, with the dashed boxes indicating the locations where lumped disturbances are introduced. It can be observed that regardless of whether or not the lumped disturbances are present, the designed controller achieves accurate trajectory tracking control and exhibits good tracking performance. Figure 4 The tracking results of the lawnmower robot in various directions are described. Figure 5 To improve the front wheel cornering tracking effect of the lawnmower robot, Figure 6The pose tracking error of the lawnmower robot is shown. It can be seen that the lawnmower robot can track the reference trajectory within 4 seconds. When a disturbance occurs, the controller can effectively suppress the disturbance and only cause a small tracking error. Figure 7 The extended state observer designed in China can accurately estimate lumped disturbances in real time, but there will be some estimation error at the beginning. Figure 8 The input torque is the torque for the front and rear wheels of the lawnmower robot. When a disturbance occurs, the input torque changes to suppress the impact of the disturbance on the trajectory tracking effect.

[0174] Simulation Experiment - Composite Trajectory:

[0175] The kinematic and dynamic parameters of the vehicle-mounted lawnmower robot are as follows: m = 30 kg, L = 1.2 m, J ω =5kg·m 2 .

[0176] The parametric equations for the composite reference trajectory are:

[0177]

[0178] Where G(t) = U(t-20) - U(t-26.28) + U(t-36.28) - U(t-42.56) + U(t-62.36) - U(t-78.84) + U(t-83.12), U(t) is the unit step function, and v is the forward linear velocity of the lawnmower robot. r = 1 m / s.

[0179] The simulation parameters are as follows: the simulation time is set to 90s, and the actual initial pose of the lawnmower robot is set to q(0) = [5 0 00]. T The lumped disturbance of the linear velocity control system and the lumped disturbance of the heading angle control system, which include at least one of wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance, are set as follows:

[0180]

[0181] The two types of lumped disturbances mentioned above were added at t=21–25s, t=36–45s, and t=60–68s, respectively. The remaining parameter settings were the same as in the circular trajectory simulation experiment. The simulation results are as follows: Figures 9-14 As shown.

[0182] The trajectory tracking control results of the lawnmower robot are as follows: Figure 9 As shown, the reference trajectory is rectangular with rounded corners. It can be observed that regardless of whether it is affected by lumped disturbances, the lawnmower robot can accurately track the reference trajectory, demonstrating good tracking performance and anti-interference characteristics. Figure 10 The pose tracking effect of the lawnmower robot is described. Figure 11To improve the front wheel cornering tracking effect of the lawnmower robot, Figure 12 The pose tracking error of the lawnmower robot is shown. It can be seen that the lawnmower robot can accurately track the reference trajectory. When a lumped disturbance is introduced, the controller can suppress the disturbance and control the lawnmower robot to quickly return to the reference trajectory. Figure 11 The change in front wheel angle starting from the 20th second is caused by the lawnmower robot starting to turn, while the fluctuation in front wheel angle starting from the 21st second is caused by the addition of lumped disturbance, and the changes in other time periods are similar. Figure 13 The extended state observer is used to estimate the lumped disturbance. It can be seen that it achieves real-time and accurate estimation of the lumped disturbance. However, there will be a certain estimation error at the beginning. At the same time, the estimated value of the disturbance by the extended state observer will also change at the corner, that is, the corner is also considered as a disturbance. Figure 14 The input torque is the torque of the lawnmower robot. When a disturbance occurs, the input torque changes to suppress the impact of the disturbance on the trajectory tracking control.

[0183] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to fall within the scope of this specification.

[0184] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this patent should be determined by the appended claims.

Claims

1. A trajectory tracking control method for a vehicle-type mobile robot, characterized in that, The method includes: Construct a kinematic controller for a vehicle-type mobile robot, and generate virtual linear velocity and virtual heading angular velocity based on the pose tracking error of the vehicle-type mobile robot; A linear velocity controller is constructed to track the virtual linear velocity, so that the actual forward speed of the vehicle-type mobile robot meets the requirements of the kinematic controller. The lumped disturbance of the linear velocity control system is observed through a first extended state observer. A heading angle controller is constructed to track the virtual heading angle, so that the actual attitude angle of the vehicle-type mobile robot meets the requirements of the kinematic controller. The lumped disturbance of the heading angle control system is observed using a second extended state observer. The linear velocity and angular velocity of the vehicle-type mobile robot are controlled according to the outputs of the linear velocity controller and the heading angle controller to achieve trajectory tracking; The heading angle controller includes a virtual control law for the front wheel steering angle, a virtual control law for the front wheel steering angular velocity, and a virtual control law for the front wheel steering torque, used to generate the front wheel steering torque of the vehicle-type mobile robot; the heading angle control system is as follows: in, For the heading angle of the vehicle-type mobile robot, For the turning angle of the front wheels of the vehicle-type mobile robot, The steering angular velocity of the front wheels of the vehicle-type mobile robot. , , These are the first derivatives of the heading angle, front wheel steering angle, and front wheel steering angular velocity of the vehicle-type mobile robot, respectively. For the linear velocity of the vehicle-type mobile robot, The distance between the front and rear wheels of a vehicle-type mobile robot. Let the moment of inertia be the front wheel of the vehicle-type mobile robot. The lumped disturbance in the heading angle control system of a vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance. This refers to the steering torque of the front wheels of a vehicle-type mobile robot.

2. The method according to claim 1, characterized in that, The pose tracking error is calculated based on the current pose and reference pose of the vehicle-type mobile robot. The kinematic controller is as follows: in, , , For positive controller parameters, , , These are the longitudinal position error, lateral position error, and heading angle error of the reference pose and actual pose of the vehicle-type mobile robot in the robot coordinate system. The reference linear velocity for the vehicle-type mobile robot. The reference heading angular velocity for the vehicle-type mobile robot; Virtual velocity commands generated for the kinematic controller. , These are the virtual linear velocity and virtual heading angular velocity of the vehicle-type mobile robot, respectively.

3. The method according to claim 1, characterized in that, The first extended state observer is an extended state observer for a linear velocity control system, implemented using the following formula: in, , Representing states respectively , The estimated value, For the linear velocity of the vehicle-type mobile robot, The lumped disturbance for the linear velocity control system of a vehicle-type mobile robot includes at least one of the following: wheel longitudinal slip and / or sideslip disturbance, dynamic model uncertainty, and unknown input disturbance. , They are respectively , The first derivative, The observation error of the first extended state observer regarding the linear velocity. , For a positive observer gain, For the linear motion torque of the rear wheels of the vehicle-type mobile robot, For the mass of the vehicle-type mobile robot; The linear velocity controller is implemented using the following formula: in, For the linear motion torque of the rear wheels of the vehicle-type mobile robot, For the quality of vehicle-type mobile robots, For system control gain, For linear velocity tracking error, , These are virtual linear velocity and virtual linear acceleration, respectively. The linear velocity is the observation value of the first extended state observer. Lumped disturbance of the linear velocity control system of the vehicle-type mobile robot by the first extended state observer The estimated value.

4. The method according to claim 1, characterized in that, The method further includes: Based on the heading angular velocity error of the vehicle-type mobile robot, a heading angular error sliding surface and a virtual control law for the front wheel rotation angle based on the heading angular error sliding surface are designed; The sliding surface of the vehicle-type mobile robot's heading angle error This can be achieved using the following formula: in, To control the gain, For heading angle error, The heading angle of the vehicle-type mobile robot. The virtual heading angle for the vehicle-type mobile robot. This refers to the error in heading angular velocity. The virtual control law for the front wheel steering angle is implemented using the following formula: in, The virtual front wheel steering angle serves as the tracking target. When the linear velocity of the vehicle-type mobile robot is greater than or equal to a preset value, the virtual front wheel steering angle control law is activated. This represents the virtual front wheel steering angular velocity. , These are the linear velocity and linear acceleration of the vehicle-type mobile robot. The distance between the front and rear wheels of a vehicle-type mobile robot. , A positive control gain For the designed sliding surface, It is the first derivative of the virtual heading angular velocity.

5. The method according to claim 1, characterized in that, The method includes: Based on the tracking error of the front wheel angle of the vehicle-type mobile robot, a virtual control law for calculating the front wheel steering angular velocity in the heading angle controller is designed. The virtual control law for front wheel steering angular velocity This can be achieved using the following formula: in, To control the gain, , These are the virtual front wheel steering angle and the virtual front wheel steering angular velocity, respectively. Front wheel steering angle The estimated value, This represents the front wheel steering angle tracking error.

6. The method according to claim 1, characterized in that, The method includes: Based on the tracking error of the front wheel angle of the vehicle-type mobile robot, design the front wheel steering torque control law in the heading angle controller for calculating the front wheel steering torque; The front wheel steering torque control law is implemented using the following formula: in, For the steering torque of the front wheels of the vehicle-type mobile robot, For the moment of inertia of the front wheels of the vehicle-type mobile robot, For virtual front wheel steering angle acceleration, To control the gain, For front wheel steering angle tracking error, Lumped disturbance of the heading angle control system by the second extended state observer of the vehicle-type mobile robot The estimated value.

7. The method according to claim 1, characterized in that, The second extended state observer is the front wheel steering angle system extended state observer, which is implemented by the following formula: in, , , These represent the front wheel steering angles. Front wheel steering angular velocity Lumped disturbances of heading angle control system The estimated value, , , They are respectively , , The first derivative, This represents the observation error of the second extended state observer regarding the front wheel steering angle. , , Positive observer gain; For the moment of inertia of the front wheels of the vehicle-type mobile robot, This refers to the steering torque of the front wheels of a vehicle-type mobile robot.

8. The method according to any one of claims 1 to 7, characterized in that, The method further includes: Construct the kinematic and dynamic models of the vehicle-type mobile robot; wherein, The kinematic model characterizes the position state change equation of the vehicle-type mobile robot under the control inputs of linear velocity and front wheel steering angular velocity; The dynamic model characterizes the relationship between the running speed and the input torque of the vehicle-type mobile robot under the presence of lumped disturbances.

9. The method according to claim 1, characterized in that, The step of controlling the linear velocity and heading angular velocity of the vehicle-type mobile robot based on the outputs of the linear velocity controller and the heading angle controller to achieve trajectory tracking includes: The virtual linear velocity and virtual heading angular velocity output by the kinematics controller are used as the tracking targets of the linear velocity controller and the heading angular velocity controller, respectively. The linear motion torque of the rear wheel of the vehicle-type mobile robot output by the linear velocity controller and the steering torque of the front wheel output by the heading angular velocity controller are used as inputs to the dynamics model to obtain the linear velocity and heading angular velocity of the vehicle-type mobile robot. These are then used as inputs to the kinematics model to obtain the pose of the vehicle-type mobile robot. Combined with the reference pose, the pose tracking error is obtained. The pose tracking error can be used as inputs to the kinematics controller to obtain new virtual linear velocity and virtual heading angular velocity for continuous trajectory tracking.

Images