A tracking control method and system for a non-holonomic four-wheel mobile robot

Abstract

The present invention belongs to the technical field of mobile robot control, and discloses a tracking control method and system for a non-complete four-wheel mobile robot. The method generates a reference trajectory signal by a reference trajectory generator, and inputs it into an outer loop position control module; the desired position control is calculated using a constructed position error dynamics model, and the desired speed control is extracted therefrom, and a continuous control force is obtained by combining dynamics analysis; the desired control is input into an inner loop attitude control module containing an attitude planner constructed based on a function, and the constructed attitude error dynamics model is used to calculate and obtain continuous torque control; a position and attitude control model of a four-wheel mobile robot containing an attitude planner is constructed; and a tracking controller of a non-complete four-wheel mobile robot containing an attitude planner is obtained. The present invention constructs a continuous controller to complete the tracking control of a non-complete four-wheel mobile robot, and solves the zero division problem of the inner loop control caused by state coupling.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot control, and in particular relates to a tracking control method and system for a non-holonomic four-wheel mobile robot. Background Art

[0002] The control of a nonholonomic four-wheel mobile robot is extremely challenging because it belongs to a 2-control-4 nonholonomic system with topological barriers, i.e., continuous feedback cannot globally stabilize the posture of a mechanical system with rotational degrees of freedom.

[0003] The nonholonomic system has non-integrable constraints on the system velocity, which makes the number of independent controls less than the system's configuration degrees of freedom, so it is an underactuated system. The condition indicates that nonholonomic systems cannot be stabilized by continuous time-invariant state feedback, making the control of nonholonomic systems one of the most challenging problems in the field of control today. In order to complete the trajectory tracking control of nonholonomic mobile robots, it is often necessary to introduce hybrid control, which leads to complex step and derivative calculations and a large amount of computation.

[0004] For the tracking problem of a reference robot (specifying the expected time-varying linear velocity / angular velocity), control schemes such as using continuous feedback control laws to perform local asymptotic tracking of linearized motion models have been proposed. The idea of ​​inner and outer loop control has been widely used in the control design of autonomous aircraft and has matured. Since both autonomous aircraft and such robots are underactuated systems, this drives us to apply the idea of ​​inner and outer loop control to four-wheeled mobile robots to solve the underactuated problem. We further considered the problem that directly using the attitude planner to extract the expected yaw angle may result in discontinuous expected signals.

[0005] Through the above analysis, the problems and defects of the prior art are as follows: the prior art has extensively studied the tracking control of wheeled mobile robots, such as discontinuous tracking control of incomplete wheeled robots, tracking control of unicycle robots and tracking control of mobile robots under complex constraints; but the prior control technology has discontinuous control or mixed control, which will cause step or pulse signals to appear in the controller, increasing the burden on the actuator. Summary of the invention

[0006] In order to overcome the problems existing in the related art, the disclosed embodiment of the present invention provides a tracking control method and system for a non-holonomic four-wheel mobile robot. Function can smooth the expected yaw angle, and the control obtained is continuous. At the same time, the state coupling problem of the non-holonomic four-wheel mobile robot system is considered. The dynamic surface technology is improved by introducing speed to avoid the zero division problem of the controller caused by system coupling. A continuous torque controller is designed to avoid the introduction of mixed control. Finally, the local exponential stability similar to the linearized model is obtained by using the continuous linear control law.

[0007] The technical solution is as follows: A tracking control method for a non-holonomic four-wheel mobile robot comprises the following steps:

[0008] S1, the reference trajectory signal is generated by the reference trajectory generator, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer ring position control module;

[0009] S2, the outer loop position control module uses the constructed position error dynamics model to calculate the desired position control and extract the desired speed control from , combined with dynamic analysis to obtain continuous control ;

[0010] S3, the desired position control module outputs the outer loop position control module and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ;

[0011] S4, combined with the outer loop position control module to calculate the continuous control force The continuous torque control is calculated by the inner loop attitude control module As the control input, it acts on the dynamic model of the four-wheel mobile robot with the mobile robot center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity as the states, and performs real-time control of the position and posture of the incomplete four-wheel mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the incomplete four-wheel mobile robot in real time.

[0012] In step S1, the expression of the dynamic model of the non-holonomic four-wheeled mobile robot is:

[0013] ;

[0014] ;

[0015] In the formula, for The first derivative of is the horizontal coordinate of the robot’s geometric center on the plane; for The first derivative of is the ordinate of the robot’s geometric center on the plane; for The first derivative of for The first derivative of for The first derivative of is the distance between the two axles, These are system parameters. is the position of the geometric center of the four-wheel mobile robot, is transposed;

[0016] ;

[0017] ;

[0018] In the formula, is the moment of inertia of the left and right wheels, is the mass of the non-holonomic four-wheel mobile robot, For the diameter of each wheel, is the centroid The distance to the left and right wheel centers, are the drag coefficients for translation and rotation, is the moment of inertia of the robot.

[0019] In step S2, the position error dynamics model includes:

[0020] Define position error and speed error, the expression is:

[0021] ;

[0022] In the formula, is the position error of the four-wheel mobile robot, is the speed error of the four-wheel mobile robot, is a reference signal generated by a reference trajectory generator;

[0023] According to the position dynamics model, the outer loop position error system equation is:

[0024] ;

[0025] In the formula, is the position error of the four-wheel mobile robot The first derivative of is the speed error of the four-wheel mobile robot The first derivative of They are coupling terms generated by the inner and outer loop control design. The reference signal The first derivative of For desired speed control The first derivative of All are system parameters;

[0026] ;

[0027] In the formula, , , is the yaw angle to be designed, is transposed.

[0028] In step S2, a continuous control force is obtained , including the following steps:

[0029] The first step is to define the first Function, the expression is:

[0030] ;

[0031] In the formula, For the first function, for The transpose of

[0032] The coupling terms are separated and the expression is:

[0033] ;

[0034] In the formula, is the yaw angle error;

[0035] use The inequality will Separated into:

[0036] ;

[0037] In the formula, is the normal number to be designed;

[0038] Desired position control for:

[0039] ;

[0040] In the formula, is the normal number to be designed, Substitution In, we have:

[0041] ;

[0042] In the formula, for function The first derivative of ;

[0043] The second step is to control the desired position Extraction of desired speed control , of the following form:

[0044] ;

[0045] In the formula, The expected position The weight of

[0046] Define the second function:

[0047] ;

[0048] In the formula, For the second function;

[0049] The derivative is expressed as:

[0050] ;

[0051] In the formula, for function The first derivative of ;

[0052] Control for:

[0053] ;

[0054] In the formula, is the normal number to be designed;

[0055] Control Substitution get:

[0056] ;

[0057] In step S3, the attitude error dynamics model includes:

[0058] Through the attitude planner and velocity dynamic surface technology, the expected yaw angle of the attitude is obtained , the expression is:

[0059] ;

[0060] In the formula, Indicates the expected yaw angle of the attitude The first derivative of represents the yaw angle extracted by the outer loop control, represents the inverse tangent 3 function, The expected position The weight is about Function of

[0061] The expression for defining the inner loop attitude error is:

[0062] ;

[0063] In the formula, is the steering wheel angle The tangent function of ; is the steering wheel angle The error of the tangent function, for Virtual control, is the angular velocity error of the steering wheel steering angle, for Virtual control of

[0064] The kinematic model of the inner loop attitude error is obtained as:

[0065] ;

[0066] ;

[0067] In the formula, for The first derivative of for The first derivative of for The first derivative of for The first derivative of for The first derivative of .

[0068] Further, continuous torque control is obtained In, set, , the attitude dynamics equation is transformed into:

[0069] ;

[0070] Further, continuous torque control is obtained The specific steps include:

[0071] Step 1, define the third function :

[0072] ;

[0073] Deriving it:

[0074] ;

[0075] In the formula, for function The first derivative of ;

[0076] Take the stabilization function:

[0077] ;

[0078] In the formula, is the normal number to be designed;

[0079] Substitution get:

[0080] ;

[0081] Step 2, define the fourth function :

[0082] ;

[0083] Taking its derivative we get:

[0084] ;

[0085] In the formula, express function The first derivative of ;

[0086] Take the angular velocity stabilization function:

[0087] ;

[0088] In the formula, is the normal number to be designed;

[0089] Substitution get:

[0090] ;

[0091] Define the fifth function :

[0092] ;

[0093] The derivative of it is:

[0094] ;

[0095] In the formula, express function The first derivative of ;

[0096] Torque control :

[0097] ;

[0098] In the formula, is the normal number to be designed;

[0099] Substitution becomes:

[0100] ;

[0101] In step S4, the position and posture of the non-holonomic four-wheeled mobile robot are controlled in real time to complete asymptotic tracking of the reference trajectory, and the position and posture of the non-holonomic four-wheeled mobile robot are displayed in real time, including:

[0102] ;

[0103] ;

[0104] ;

[0105] ;

[0106] ;

[0107] ;

[0108] In the formula, are all positive design parameters.

[0109] Another object of the present invention is to provide a nonholonomic four-wheeled mobile robot tracking control system, the system implements the nonholonomic four-wheeled mobile robot tracking control method, the system comprising:

[0110] The reference trajectory generator is used to generate a reference trajectory signal, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer loop position control module.

[0111] The outer loop position control module is used to calculate the desired position control using the constructed position error dynamic model and extract the desired speed control from , combined with dynamic analysis to obtain continuous control ;

[0112] based on The attitude planner constructed by the function is included in the inner loop attitude control module to control the desired position output by the outer loop position control module. and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ;

[0113] A continuous tracking controller for a nonholonomic four-wheeled mobile robot with a posture planner is used to calculate continuous control forces in conjunction with the outer loop position control module. The continuous torque control is calculated by the inner loop attitude control module As the control input, it acts on the dynamic model of the four-wheel mobile robot with the mobile robot center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity as the states, and performs real-time control of the position and posture of the incomplete four-wheel mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the incomplete four-wheel mobile robot in real time.

[0114] Furthermore, the incomplete four-wheel mobile robot tracking and control system is mounted on a computer device, which includes: at least one processor, a memory, and a computer program stored in the memory and executable on the at least one processor, and the processor implements the functions of the incomplete four-wheel mobile robot tracking and control system when executing the computer program.

[0115] Combining all the above technical solutions, the beneficial effects of the present invention are as follows: the present invention provides a continuous time-invariant controller to realize the trajectory tracking control method and system of a non-holonomic four-wheel mobile robot, which relaxes the The main use of the inner and outer loop control ideas combined Function, converting the underactuated system into a fully-actuated system for controller design. The introduction of velocity dynamic surface and appropriate coupling term allocation method solves the inner loop control zero division problem caused by state coupling. A continuous controller is designed to improve the system's ability to resist external interference and the control accuracy of the mobile robot's trajectory tracking.

[0116] The present invention uses the inner and outer loop control concept to transform the under-actuated position control into a full-drive form, designs the desired position control of the full-drive, and properly distributes the coupling terms using inequalities, thereby extracting a continuous force controller. The function is combined with the attitude planner to extract a smooth desired yaw angle for attitude control design. Taking into account the zero division problem of the controller caused by state coupling, the desired yaw angle is filtered using the velocity dynamic surface technology to solve it, and then a continuous torque controller is designed to avoid the introduction of mixed control. In addition, both inner and outer loop controls use the backstepping design method, which is easy to combine with adaptive control to improve control accuracy and enhance the robustness of the system. It avoids the zero division phenomenon in the controller and causes the introduction of mixed or discontinuous control. The control design of the present invention is non-time-varying and smooth in the state space except for the translational velocity origin, which relaxes the condition.

[0117] The continuous tracker design method for the non-holonomic four-wheel mobile robot proposed in the present invention helps to improve the accuracy of robot control and also helps to improve the quality of products and services. The designed controller will not have signals such as step or pulse, which greatly reduces the execution burden of the controller and enhances the safety of the system.

[0118] The commercial value is reflected in the fact that the transformation of incomplete excitation robot technology may give rise to new business models, such as service-based subscription models, data-driven value-added services, etc. Continuous control technology generally optimizes the operation mode of the robot, reduces unnecessary wear and failure, extends the service life of the equipment, and reduces the cost of equipment maintenance and replacement.

[0119] This invention combines the inner and outer loop control ideas and The function is introduced into the non-holonomic four-wheel mobile robot system, and a continuous control force is designed; combined with the speed dynamic surface design and the appropriate coupling term processing method, the torque control caused by state coupling is avoided to appear zero division or pulse signal, and a continuous torque controller is designed. In summary, the control design of the present invention is time-invariant and is smooth in the state space except for the translation velocity origin, which relaxes the Conditions, a continuous tracking controller is designed to enable the non-holonomic four-wheeled mobile robot to track a given trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0120] The accompanying drawings herein are incorporated in and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the description, serve to explain the principles of the present disclosure;

[0121] Figure 1 1 is a schematic diagram of a coordinate system and structure of a nonholonomic four-wheel mobile robot provided in an embodiment of the present invention;

[0122] Figure 2 is a structural diagram of a position and posture dynamics model calculation module provided by an embodiment of the present invention;

[0123] Figure 3 is a schematic diagram of the structure of an outer loop control module provided by an embodiment of the present invention;

[0124] Figure 4 is a schematic diagram of the structure of an inner loop control module provided by an embodiment of the present invention;

[0125] Figure 5 It is a schematic diagram of the trajectory tracking control structure of a non-holonomic four-wheel mobile robot under the inner and outer loop control framework provided by an embodiment of the present invention;

[0126] Figure 6 is a schematic diagram of a tracking control method for a non-holonomic four-wheel mobile robot provided by an embodiment of the present invention;

[0127] Figure 7 is a schematic diagram of a non-holonomic four-wheel mobile robot tracking control system provided by an embodiment of the present invention;

[0128] Figure 8 This is a control effect display module diagram of a non-holonomic four-wheel mobile robot provided by an embodiment of the present invention;

[0129] Fig. 9 is a simulation diagram of a virtual ring road coordinate system provided by an embodiment of the present invention;

[0130] Fig.10 is the simulation coordinate of the non-holonomic four-wheel mobile robot provided in the embodiment of the present invention Quickly converge to the expected path result graph;

[0131] Fig.11 It is a result diagram of the coordinate y quickly converging to the expected path in the simulation of the non-holonomic four-wheel mobile robot provided by an embodiment of the present invention;

[0132] Fig.12 is a two-dimensional graph of a tracking route in a simulation of a non-holonomic four-wheeled mobile robot provided by an embodiment of the present invention;

[0133] Fig.13 is a diagram of the starting stage of a non-holonomic four-wheel mobile robot driving on a virtual road provided by an embodiment of the present invention;

[0134] Fig.14 is a phase diagram of a non-holonomic four-wheel mobile robot entering a virtual circular road provided by an embodiment of the present invention;

[0135] Fig.15 is a diagram of a stage in which a non-holonomic four-wheel mobile robot provided by an embodiment of the present invention travels along a virtual circular road;

[0136] Fig.16 This is a diagram of the stages of a non-holonomic four-wheel mobile robot driving out of a virtual circular road provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0137] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific implementation disclosed below.

[0138] The innovation of the present invention lies in that: the present invention designs a continuous time-invariant controller with a switching algorithm to realize the position tracking of a given trajectory by a nonholonomic four-wheeled mobile robot, introduces the inner and outer loop control ideas into the control design of a nonholonomic mobile robot, and The function is introduced into the attitude planner design to avoid the calculation disaster of step and pulse derivatives. At the same time, the zero division problem caused by state coupling is solved by designing speed dynamic surface technology and appropriate coupling term processing method to avoid the introduction of discontinuous and mixed control problems. The controller design method is easy to combine with adaptive control and has strong robustness and anti-interference ability.

[0139] Embodiment 1: The tracking control method of a non-holonomic four-wheel mobile robot provided by the embodiment of the present invention comprises:

[0140] S1, the reference trajectory signal is generated by the reference trajectory generator, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer ring position control module;

[0141] S2, the outer loop position control module uses the constructed position error dynamics model to calculate the desired position control and extract the desired speed control from , combined with dynamic analysis to obtain continuous control ;

[0142] S3, the desired position control module outputs the outer loop position control module and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ;

[0143] S4, combined with the outer loop position control module to calculate the continuous control force The continuous torque control is calculated by the inner loop attitude control module As the control input, it acts on the dynamic model of the four-wheel mobile robot with the mobile robot center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity as the states, and performs real-time control of the position and posture of the incomplete four-wheel mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the incomplete four-wheel mobile robot in real time.

[0144] like Figure 5 As shown, the force and torque obtained by state feedback are used as input to enter the robot dynamic system for tracking control. Acting on the dynamic model of the four-wheel mobile robot with the center coordinates, yaw angle, forward speed, steering wheel steering angle and angular velocity of the mobile robot as the state, the position and posture of the four-wheel under-actuated mobile robot are controlled in real time, the asymptotic tracking of the reference trajectory is completed, and the position and posture of the four-wheel under-actuated mobile robot are displayed in real time. Exemplarily, in step S1, the dynamic model of the non-holonomic four-wheel mobile robot is as follows:

[0145] ;

[0146] ;

[0147] In the formula, for The first derivative of is the horizontal coordinate of the robot’s geometric center on the plane; for The first derivative of is the ordinate of the robot’s geometric center on the plane; for The first derivative of for The first derivative of for The first derivative of is the distance between the two axles, These are system parameters. is the position of the geometric center of the four-wheel mobile robot, is transposed;

[0148] ;

[0149] ;

[0150] In the formula, is the moment of inertia of the left and right wheels, is the mass of the four-wheel mobile robot, For the diameter of each wheel, is the centroid The distance to the left and right wheel centers, are the drag coefficients for translation and rotation, is the moment of inertia of the robot.

[0151] like Figure 2 As shown, in step S2, the position and posture dynamics model calculation module is synthesized;

[0152] Among them, control It is calculated by the subsequent outer loop control module. is the yaw angle of the four-wheel mobile robot, represents the torque control calculated by the subsequent inner loop control module, is the steering wheel angle The tangent function of .

[0153] The calculation process of the position and attitude dynamics model calculation module: the control force obtained by the outer loop control module The yaw angle of the four-wheeled mobile robot output by the attitude dynamics model calculation module (inner loop attitude control module) The position of the geometric center of the four-wheel mobile robot is obtained through the position dynamics model calculation module (outer loop position control module, i.e., outer loop position controller). and its forward speed ; The torque control signal output by the inner loop control module (inner loop attitude controller) and the forward speed of the four-wheeled mobile robot As the input signal, it enters the attitude dynamics model calculation module (inner loop attitude control module) to obtain the yaw angle of the four-wheel mobile robot. and the angular velocity of the steering wheel Signal.

[0154] Among them, the calculation formula of the outer loop position control module includes:

[0155] ;

[0156] The calculation formula of the inner loop attitude control module includes:

[0157] ;

[0158] In the formula, , for The first derivative of for The square of .

[0159] Exemplarily, for a desired position and attitude, a position error dynamics model and an attitude error dynamics model are constructed, and the tracking control problem is transformed into a stabilization problem of the error dynamics model.

[0160] Wherein, in step S2, the position error dynamics model includes:

[0161] Define position error and speed error, the expression is:

[0162] ;

[0163] In the formula, is the position error of the four-wheel mobile robot, is the speed error of the four-wheel mobile robot, is a reference signal generated by a reference trajectory generator;

[0164] The present invention innovatively proposes that the outer loop position error system equation is obtained based on the position dynamics model:

[0165] ;

[0166] In the formula, is the position error of the four-wheel mobile robot The first derivative of is the speed error of the four-wheel mobile robot The first derivative of They are coupling terms generated by the inner and outer loop control design. The reference signal The first derivative of For desired speed control The first derivative of All are system parameters;

[0167] ;

[0168] In the formula, , , is the yaw angle to be designed, is transposed.

[0169] Exemplarily, the outer loop position control module calculates the control force include:

[0170] Step 1: Definition Function, the expression is:

[0171] ;

[0172] In the formula, For the first function, for The transpose of

[0173] Separate the coupled terms:

[0174] ;

[0175] In the formula, is the yaw angle error;

[0176] The invention innovatively proposes to utilize The inequality will Separated into:

[0177] ;

[0178] In the formula, is the normal number to be designed;

[0179] The invention proposes innovatively that the desired position control for:

[0180] ;

[0181] In the formula, is the normal number to be designed, Substitution In, we have:

[0182] ;

[0183] In the formula, for function The first derivative of ;

[0184] The second step is to control the desired position Extraction of desired speed control , of the following form:

[0185] ;

[0186] In the formula, The expected position The weight of

[0187] Define the second function:

[0188] ;

[0189] In the formula, For the second function;

[0190] The derivative is expressed as:

[0191] ;

[0192] In the formula, for function The first derivative of ;

[0193] Control for:

[0194] ;

[0195] In the formula, is the normal number to be designed;

[0196] Control Substitution get:

[0197] ;

[0198] For example, the structure of the outer loop control module is as follows: Figure 3 (Expected position control calculation module) as shown.

[0199] The input signals of the outer loop control module include: the real-time position of the geometric center of the four-wheel mobile robot provided by the position dynamics model calculation module and real-time forward speed And the reference trajectory The output information of the outer loop control module includes continuous force control and desired position control .

[0200] The operation process of the outer loop control module: the real-time position of the geometric center of the four-wheel mobile robot and reference trajectory The trajectory tracking error is obtained by the tracking error calculation module ; Combined with reference trajectory The first derivative of The desired position control can be obtained ; The desired speed control is obtained by extracting the size numerical calculation module and the desired yaw angle , further obtaining continuous force control .

[0201] Exemplarily, in step S3, for the attitude error dynamics model.

[0202] The invention innovatively proposes that the expected yaw angle of the attitude can be obtained through the attitude planner and the velocity dynamic surface technology. And it avoids the zero division problem that occurs in subsequent designs:

[0203] ;

[0204] In the formula, Indicates the expected yaw angle of the attitude The first derivative of represents the yaw angle extracted by the outer loop control, represents the inverse tangent 3 function, The expected position The weight is about Function of

[0205] By introducing the speed dynamic surface technology, the zero division problem of the control design is avoided, which will be demonstrated in the subsequent controller design.

[0206] Define the inner loop attitude error:

[0207] ;

[0208] In the formula, is the tangent function of the steering wheel angle, ; is the steering wheel angle The error of the tangent function, for Virtual control, is the angular velocity error of the steering wheel steering angle, for Virtual control of

[0209] The kinematic model of the inner loop attitude error is obtained:

[0210] ;

[0211] ;

[0212] ;

[0213] In the formula, for The first derivative of for The first derivative of for The first derivative of for The first derivative of for The first derivative of .

[0214] Exemplarily, in step S3, the torque control module calculates include:

[0215] The attitude dynamics equation is:

[0216] ;

[0217] Get continuous torque control The specific steps include:

[0218] Step 1, define the third function :

[0219] ;

[0220] Deriving it:

[0221] ;

[0222] In the formula, for function The first derivative of ;

[0223] Take the stabilization function:

[0224] ;

[0225] In the formula, is the normal number to be designed;

[0226] Substitution get: ;

[0227] here It is to compensate for the disturbance stabilization term in the outer loop position control. It can be found that by introducing the speed into the dynamic surface technology, the design of virtual control is avoided. The zero division phenomenon occurs when .

[0228] Step 2, define the fourth function :

[0229] ;

[0230] Taking its derivative we get:

[0231] ;

[0232] In the formula, express function The first derivative of ;

[0233] Take the angular velocity stabilization function:

[0234] ;

[0235] In the formula, is the normal number to be designed;

[0236] Substitution get: ;

[0237] Define the fifth function :

[0238] ;

[0239] The derivative of it is:

[0240] ;

[0241] In the formula, express function The first derivative of ;

[0242] Torque control :

[0243] ;

[0244] In the formula, is the normal number to be designed;

[0245] Substitution becomes:

[0246] ;

[0247] For example, the structure of the inner loop control module is as follows: Figure 4 shown.

[0248] The input signals of the inner loop control module include: the desired position control signal output by the outer loop control module and the forward speed of the four-wheeled mobile robot , and the real-time yaw angle obtained by feedback from the dynamics model of the non-holonomic four-wheeled mobile robot , real-time steering wheel angle and real-time angular velocity .

[0249] The output signals of the inner loop control module include: the torque control signal calculated by the inner loop control module .

[0250] Exemplarily, in step S4, the constructed outer loop control module (outer loop position controller), inner loop control module (inner loop posture controller), non-holonomic four-wheel mobile robot dynamics model calculation module (robot dynamic system), and reference trajectory generator module are combined. The reference trajectory generator module mainly uses the existing path planning method to generate a reference trajectory (by selecting waypoints and using path planning methods such as minimum acceleration, random spanning tree or particle swarm algorithm to generate a reference trajectory) to obtain the following: Figure 5 The trajectory tracking control structure diagram of the non-holonomic four-wheel mobile robot is shown.

[0251] The reference trajectory signal is generated by the reference trajectory generator, combined with the position of the vehicle body geometric center fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module. and its forward speed The real-time data enters the outer loop position control module, and the outer loop position control module calculates the desired position control , and extract the desired speed control from it , combined with dynamic analysis to design continuous control force ; Combined with the yaw angle feedback from the non-holonomic four-wheel mobile robot dynamics model calculation module , steering wheel angle and the angular velocity Real-time data and the desired position control output by the outer loop position control module and the forward speed of the four-wheeled mobile robot , enter the inner loop attitude control module containing the attitude planner, and calculate the continuous torque control ;

[0252] Force control calculated by the joint outer loop position control module and the torque control calculated by the inner loop attitude control module The position and posture of the non-complete four-wheeled mobile robot are controlled in real time through the dynamic model calculation module, so that it can complete the asymptotic tracking of the reference trajectory; the control result display module can display the position and posture effects of the non-complete four-wheeled mobile robot in real time.

[0253] The force control calculated by the outer loop position control module The results and the torque control calculated by the inner loop attitude control module As a result, a position and attitude control model of a nonholonomic four-wheeled mobile robot with an attitude planner is constructed; the obtained mobile robot center coordinates, yaw angle, forward speed, steering wheel steering angle and angular velocity are input into the position and attitude control model of the four-wheeled mobile robot with an attitude planner, and a continuous tracking controller of a nonholonomic four-wheeled mobile robot with an attitude planner is obtained, which specifically includes:

[0254] ;

[0255] ;

[0256] ;

[0257] ;

[0258] ;

[0259] ;

[0260] ;

[0261] ;

[0262] In the formula, are all positive design parameters.

[0263] From the above embodiments, it can be seen that the present invention uses inner and outer loop control to transform the under-actuated position control into a full-drive form, designs the desired position control of the full-drive, and properly distributes the coupling terms using inequalities, thereby extracting a continuous force controller. The function is combined with the attitude planner to extract a smooth desired yaw angle for attitude control design. Considering the zero division problem of the controller caused by state coupling, the desired yaw angle is filtered using the velocity dynamic surface technology to solve it, and then a continuous torque controller is designed to avoid the introduction of mixed control. In addition, both the inner and outer loop controls use the backstepping design method, which is easy to combine with adaptive control to improve control accuracy and enhance the robustness of the system. Using a continuous force and torque controller for trajectory tracking control, the maximum relaxation Conditional constraints.

[0264] Example 2, as another embodiment of the present invention, Figure 6 The tracking control method of a non-holonomic four-wheeled mobile robot provided by an embodiment of the present invention comprises: obtaining historical data sets such as the coordinates of the center of mass, yaw angle, forward speed, steering wheel steering angle and angular velocity of the non-holonomic four-wheeled mobile robot; a dynamic model of the non-holonomic four-wheeled mobile robot based on a posture planner; constructing an outer loop trajectory tracking force controller using a position dynamics model; and Function constructs a smooth attitude planner; uses the expected yaw angle to construct the attitude error dynamics model; constructs the inner loop attitude torque controller; and obtains the trajectory tracking control result of the four-wheel mobile robot with attitude planner.

[0265] The present invention provides a non-holonomic four-wheel mobile robot tracking control system, comprising:

[0266] The reference trajectory generator is used to generate a reference trajectory signal, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer ring position control module;

[0267] The outer loop position control module is used to calculate the desired position control using the constructed position error dynamic model and extract the desired speed control from , combined with dynamic analysis to obtain continuous control ;

[0268] based on The attitude planner constructed by the function is included in the inner loop attitude control module to control the desired position output by the outer loop position control module. and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ;

[0269] A continuous tracking controller for a nonholonomic four-wheeled mobile robot with a posture planner is used to calculate continuous control forces in conjunction with the outer loop position control module. The continuous torque control is calculated by the inner loop attitude control module As the control input, it acts on the dynamic model of the four-wheel mobile robot with the mobile robot center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity as the states, and performs real-time control of the position and posture of the incomplete four-wheel mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the incomplete four-wheel mobile robot in real time.

[0270] Figure 7 Schematic diagram of the tracking control system of a non-holonomic four-wheeled mobile robot provided by an embodiment of the present invention. The trajectory tracking control of the non-holonomic four-wheeled mobile robot was tested in the following situations: the process of an unmanned vehicle entering and exiting a roundabout and driving around a roundabout. Fig. 9 Coordinate system simulation diagram of the ring road. The coordinate system is established. The control tasks are as follows: the car starts from A on the straight road. First calm down to B (press the speed Time is estimated point), and then the linear velocity By Radius Right turn angle to C, where the angle between CO and OX is , cut into the roundabout at the same time, and then go around the radius of The roundabout rotates close to Then to point D, with a radius Turn right to E, then enter the straight road along the tangent, and finally stabilize to F. The length of EF is , the calming time is equal to .

[0271] After designing the controller, simulate and take

[0272] ;

[0273] Pick: ;

[0274] Therefore .

[0275] System parameters: ;

[0276] Controller parameters: ;

[0277] The initial state of the mobile robot: ;

[0278] The simulation results are as follows Fig.10 Simulation coordinates of four-wheel mobile robot Quickly converge to the expected path result graph, Fig.11 Simulation coordinates of four-wheel mobile robot Quickly converge to the expected path result graph, Fig.12 A 2D plot of the path followed by a four-wheeled mobile robot in simulation.

[0279] By observation Fig.10 , Fig.11 The simulation results show that the two coordinates of the four-wheel mobile robot and It will quickly converge to the desired path and track along the path. The two-dimensional graph of the tracking route is as follows Fig.12 As shown, after rapid preliminary adjustments, the four-wheeled mobile robot drives along the ring road and then drives out of the exit. Figure 13-Figure 16 They are the complete processes of the incomplete four-wheel mobile robot's starting phase simulation diagram, the phase simulation diagram of entering the ring road, the phase simulation diagram of driving along the ring road, and the phase simulation diagram of exiting the ring road.

[0280] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered by the protection scope of the present invention.

Claims

1. A tracking control method for a non-holonomic four-wheel mobile robot, characterized in that: The method comprises the following steps: S1, the reference trajectory signal is generated by the reference trajectory generator, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer ring position control module; S2, the outer loop position control module uses the constructed position error dynamics model to calculate the desired position control and extract the desired speed control from , combined with dynamic analysis to obtain continuous control force ; S3, the desired position control module outputs the outer loop position control module and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ; S4, combined with the outer loop position control module to calculate the continuous control force The continuous torque control is calculated by the inner loop attitude control module As control input, it acts on the dynamic model of the four-wheeled mobile robot with the center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity of the mobile robot as the state, and performs real-time control on the position and posture of the non-holonomic four-wheeled mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the non-holonomic four-wheeled mobile robot in real time; In step S4, the position and posture of the non-holonomic four-wheeled mobile robot are controlled in real time to complete asymptotic tracking of the reference trajectory, and the position and posture of the non-holonomic four-wheeled mobile robot are displayed in real time, including: ； In the formula, is the normal number to be designed, is the normal number to be designed, is the position error of the four-wheel mobile robot, The reference signal The first derivative of ; ； In the formula, is the desired speed control given later, The expected position The amount of ； In the formula, is the expected yaw angle of attitude The first derivative of is the forward speed of the four-wheel mobile robot, is the expected yaw angle of attitude, The yaw angle extracted for the outer loop control; ； In the formula, For control, is the speed error of the four-wheel mobile robot, for The transpose of is the yaw angle to be designed, for The first derivative of is the inverse tangent 3 function; ； In the formula, for Virtual control, is the distance between the two axles, is the yaw angle error; ； In the formula, for Virtual control, is the tangent function of the steering wheel angle, ; is the error of the steering wheel angle tangent function, for The first derivative of ; ； In the formula, For torque control, is the angular velocity error of the steering wheel steering angle, is the steering wheel rotation angular velocity, for The first derivative of are all positive design parameters, These are system parameters.

2. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 1, characterized in that: In step S1, the expression of the dynamic model of the non-holonomic four-wheeled mobile robot is: ； In the formula, is the horizontal coordinate of the robot’s geometric center on the plane, for The first derivative of ; is the ordinate of the robot’s geometric center on the plane, for The first derivative of ; for The first derivative of for The first derivative of for The first derivative of is the distance between the two axles, These are system parameters. is the position of the geometric center of the four-wheel mobile robot, is transposed; ； In the formula, is the moment of inertia of the left and right wheels, is the mass of the non-holonomic four-wheel mobile robot, For each wheel diameter, is the centroid The distance to the left and right wheel centers, are the drag coefficients for translation and rotation, is the moment of inertia of the robot.

3. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 2, characterized in that: In step S2, the position error dynamics model includes: Define position error and speed error, the expression is: ； In the formula, is the position error of the four-wheel mobile robot, is the speed error of the four-wheel mobile robot, is the reference signal generated by the reference trajectory generator, is the desired speed control given later; According to the position dynamics model, the outer loop position error system equation is: ； In the formula, is the position error of the four-wheel mobile robot The first derivative of is the speed error of the four-wheel mobile robot The first derivative of They are coupling terms generated by the inner and outer loop control design. The reference signal The first derivative of For desired speed control The first derivative of All are system parameters; ； In the formula, , , is the yaw angle to be designed.

4. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 3, characterized in that: In step S2, a continuous control force is obtained , including the following steps: The first step is to define the first Function, the expression is: ； In the formula, For the first function, for The transpose of The coupling terms are separated and the expression is: ； In the formula, is the yaw angle error; use The inequality will Separated into: ； In the formula, is the normal number to be designed; Desired position control for: ； In the formula, is the normal number to be designed, Substitution In, we have: ； In the formula, for function The first derivative of ; The second step is to control the desired position Extraction of desired speed control , of the following form: ； In the formula, The expected position The amount of Define the second function: ； In the formula, For the second function; The derivative is expressed as: ； In the formula, for function The first derivative of ; Control for: ； In the formula, is the normal number to be designed; Control Substitution get: 。 5. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 4, characterized in that: In step S3, the attitude error dynamics model includes: Through the attitude planner and velocity dynamic surface technology, the expected yaw angle of the attitude is obtained , the expression is: ； In the formula, Indicates the expected yaw angle of the attitude The first derivative of represents the yaw angle extracted by the outer loop control, represents the inverse tangent 3 function, The expected position The amount of The expression for defining the inner loop attitude error is: ； In the formula, is the tangent function of the steering wheel angle, ; is the error of the steering wheel angle tangent function, for Virtual control, is the angular velocity error of the steering wheel steering angle, for Virtual control of The kinematic model of the inner loop attitude error is obtained as: ； In the formula, for The first derivative of for The first derivative of for The first derivative of for The first derivative of for The first derivative of .

6. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 5, characterized in that: Get continuous torque control In, set, , the attitude dynamics equation is transformed into: 。 7. The tracking control method of a non-holonomic four-wheel mobile robot according to claim 6, characterized in that: Get continuous torque control The specific steps include: Step 1, define the third function : ； The derivative is: ； In the formula, for function The first derivative of ; Take the stabilization function: ； In the formula, is the normal number to be designed; Substitution get: ； Step 2, define the fourth function : ； Taking its derivative we get: ； In the formula, express function The first derivative of ; Take the angular velocity stabilization function: ； In the formula, is the normal number to be designed; Substitution get: ； Define the fifth function : ； The derivatives are: ； In the formula, express function The first derivative of ; Torque control : ； In the formula, is the normal number to be designed; Substitution becomes: 。 8. A non-holonomic four-wheel mobile robot tracking control system, characterized in that: The system implements the tracking control method of the non-holonomic four-wheel mobile robot as claimed in any one of claims 1 to 7, and the system comprises: The reference trajectory generator is used to generate a reference trajectory signal, combined with the position of the geometric center of the mobile robot fed back by the non-holonomic four-wheel mobile robot dynamics model calculation module and the forward speed of the four-wheeled mobile robot The real-time data is input into the outer ring position control module; The outer loop position control module is used to calculate the desired position control using the constructed position error dynamic model , and extract the desired speed control , combined with dynamic analysis to obtain continuous control force ; based on The attitude planner constructed by the function is included in the inner loop attitude control module to control the desired position output by the outer loop position control module. and the forward speed of the four-wheeled mobile robot , the input contains In the attitude planner constructed by the function, the expected yaw angle of the attitude is obtained The yaw angle of the vehicle body is fed back by the calculation module of the non-holonomic four-wheel mobile robot dynamics model , Steering wheel angle , steering wheel rotation angular velocity and the forward speed of the four-wheeled mobile robot The real-time data is combined with the attitude error dynamics model constructed by the attitude expected yaw angle to calculate the continuous torque control ; A continuous tracking controller for a nonholonomic four-wheeled mobile robot with a posture planner is used to calculate continuous control forces in conjunction with the outer loop position control module. The continuous torque control is calculated by the inner loop attitude control module As the control input, it acts on the dynamic model of the four-wheel mobile robot with the mobile robot center coordinates, yaw angle, forward speed, steering wheel angle and angular velocity as the states, and performs real-time control of the position and posture of the incomplete four-wheel mobile robot, completes the asymptotic tracking of the reference trajectory, and displays the position and posture of the incomplete four-wheel mobile robot in real time.

9. The non-holonomic four-wheel mobile robot tracking control system according to claim 8, characterized in that: The system is mounted on a computer device, which includes: at least one processor, a memory, and a computer program stored in the memory and executable on the at least one processor. When the processor executes the computer program, the functions of the above-mentioned non-complete four-wheel mobile robot tracking control system are implemented.

Images