A mobile robot control method and device based on UK theory

Abstract

A mobile robot control method and device based on U-K theory, belonging to the field of computer technology, the method comprises: establishing a dynamic model of the mobile robot so that the controller can provide accurate servo control force; obtaining the servo control force p required by the mobile robot in an ideal state based on the Udwadia-Kalaba equation 1 ; Obtain the servo control force p that the controller needs to provide to compensate for the mobile robot's initial conditions not meeting the constraints 2 When the mobile robot has mass and inertia uncertainty, the servo control force p required to make the mobile robot reach the desired state is obtained by changing the adaptive parameters in the controller. 3 ; The controller of the mobile robot drives the wheel hub motor to generate the corresponding servo control force τ = p according to its state 1 +p 2 +p 3 The controller in the present invention can calculate the precise analytical force required for the mobile robot to achieve the predetermined constraints in real time, and then control the wheel angle to make it reach a safe and stable state.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular to a mobile robot control method and device based on UK theory. Background Art

[0002] With the advent of artificial intelligence and the information age, intelligent, automated, and unmanned modes have become the direction that modern technology is committed to. Mobile robots have become a research hotspot today because they can autonomously track preset trajectories, freeing workers from single and heavy working conditions or harsh working environments such as high temperature and dust. There are many methods to improve the operational stability and safety of mobile robots, such as PID control, fuzzy control, model predictive control, preview control, etc., but these methods do not take into account the uncertainties and constraints that mobile robots may have due to different working conditions. Since the Udwadia-Kalaba (UK) theory can handle various system constraints and uncertainty problems, the present invention uses leakage-type adaptive robust control based on the UK theory to solve the problems of constraints and uncertainties in mobile robot systems.

[0003] The difference between the leakage adaptive robust control based on UK theory and other methods is that: 1. For mechanical systems subject to non-holonomic constraints, UK theory does not need to determine the Lagrange coefficients or other difficult-to-obtain parameters. 2. UK theory does not use linearization or approximation methods for dynamic modeling and controller design. 3. The leakage adaptive leakage controller can adjust the servo control force of the mobile robot by changing its own adaptive parameters. When the preset control effect is achieved, the controller quickly reduces its own adaptive parameters to reduce the servo control force, achieving the preset control effect with minimal energy consumption. Summary of the invention

[0004] In order to solve the above problems, the present invention provides a mobile robot control method and device based on UK theory, which has a reasonable design, solves the shortcomings of the prior art and has good effects.

[0005] In order to achieve the purpose 1 of the invention, the following technical solutions are adopted:

[0006] A mobile robot control method based on UK theory comprises the following steps:

[0007] S1. Establish a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller.

[0008] S2. Based on the Udwadia-Kalaba equation, the servo control force p required by the mobile robot in the ideal state is obtained. 1 ;

[0009] S3. Obtain the servo control force p that the controller needs to provide to compensate for the mobile robot's initial conditions not meeting the constraints. 2 ;

[0010] S4. When the mobile robot has mass and inertia uncertainty, the servo control force p required to make the mobile robot reach the desired state is obtained by changing the adaptive parameters in the controller. 3 ;

[0011] S5. The controller of the mobile robot drives the wheel hub motor to generate the corresponding servo control force τ=p according to its state. 1 +p 2 +p 3 .

[0012] Furthermore, the specific process of S1 is:

[0013] The dynamic equation of the mobile robot in the lateral direction is:

[0014]

[0015] Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Y rl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively.

[0016] The dynamic equation of the mobile robot in the yaw direction is:

[0017]

[0018] Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rr are the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively. i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas:

[0019] Xi =F i x cosδ i -F i y sinδ i ≈F i x -F i y δ i ,i=fl,fr,rl,rr (3)

[0020] Y i =F i x sinδ i +F i y cosδ i ≈F i x δ i -F i y ,i=fl,fr,rl,rr (4)

[0021] where F i x is the tire longitudinal force, F i y is the tire lateral force, δ i is the steering angle, and the tire lateral force F is obtained by the magic formula of the tire i y With slip angle α i Into a linear relationship:

[0022] F i y =-C i α i ,i=fl,fr,rl,rr (5)

[0023] Among them, C i is the turning stiffness of the mobile robot wheels;

[0024] The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas:

[0025]

[0026]

[0027] in, is the lateral velocity of the mobile robot;

[0028] Front wheel slip angle αf and steering angle δ f satisfy:

[0029]

[0030] Rear wheel slip angle α r and steering angle δ r satisfy:

[0031]

[0032] Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for;

[0033]

[0034] Rear wheel slip angle α r for;

[0035]

[0036] The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel satisfy the following formula:

[0037] δ fl =δ fr =δ f δ rl =δ rr =δ r (12)

[0038] Substituting formulas (3) to (12) into formulas (1) and (2), we obtain:

[0039]

[0040]

[0041] The dynamic model of the mobile robot is:

[0042]

[0043] Where M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot.

[0044] In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15):

[0045]

[0046]

[0047]

[0048]

[0049]

[0050]

[0051] G(q,σ,t)=0 (22)

[0052] Where y is the ordinate of the coordinate system of the mobile robot, ψ is the yaw angle of the mobile robot, and C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

[0053] Furthermore, the specific process of S2 is:

[0054] Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as:

[0055] A(q,t)=0 (23)

[0056] A nonholonomic constraint is expressed as:

[0057]

[0058] Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c 1 c 2 … cm' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q;

[0059] By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations:

[0060]

[0061] Where b is the second-order constraint matrix;

[0062] Under ideal conditions, the mobile robot obtains the required control force p according to the UK equation: 1 for:

[0063]

[0064] Furthermore, the specific process of S3 is as follows:

[0065] During the movement of the mobile robot, the mass and moment of inertia change within a certain range. The inertia matrix M(q,σ,t) and the coefficient matrix of the Coriolis force or centrifugal force are It is divided into a nominal part and an uncertain part, which are expressed as follows:

[0066]

[0067] in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively;

[0068] In actual situations, the initial conditions of the mobile robot often cannot meet the required constraints, and an additional control force needs to be provided:

[0069]

[0070] Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0.

[0071] Furthermore, the specific process of S4 is:

[0072] Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p 3 for:

[0073]

[0074] in, They are control forces p3 Control parameters:

[0075]

[0076]

[0077] The following inequality is satisfied:

[0078]

[0079] Adaptive parameter vector Satisfy the following adaptation rate:

[0080]

[0081] Among them, L 1 ,L 2 Adaptive rate The constant control term of , β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters;

[0082] The dynamic model of the mobile robot is expressed as:

[0083]

[0084] Taking into account the uncertainties in mass and inertia:

[0085]

[0086]

[0087]

[0088]

[0089]

[0090] Where Δm is the uncertainty of the mass of the mobile robot, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force acting on the tire, and θ is the steering angle matrix of the front and rear wheels of the mobile robot.

[0091] Furthermore, the specific process of S5 is as follows:

[0092] When the mobile robot is driving on the road, the body slips or yawing, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

[0093] In order to achieve the second object of the invention, the following technical solutions are adopted:

[0094] A mobile robot control device based on UK theory, comprising:

[0095] The first building module is used to build a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller.

[0096] The second building module is used to obtain the servo control force p required by the mobile robot when it is unconstrained based on the Udwadia-Kalaba equation 1 ;

[0097] The third building module is used to obtain the servo control force p that the controller needs to provide to compensate for the mobile robot's initial conditions not meeting the constraints. 2 ;

[0098] The fourth building module is used when the mobile robot has mass and inertia uncertainty. Based on the leakage-type adaptive robustness, the servo control force p required for the mobile robot to reach the desired value is obtained by changing the adaptive parameters in the controller. 3 ;

[0099] The fifth building block: The controller for the mobile robot drives the wheel hub motor to generate the corresponding servo control force τ=p according to its own state 1 +p 2 +p 3 .

[0100] Furthermore, the first establishing module is specifically used for:

[0101] Establish the dynamic equation of the mobile robot in the lateral direction:

[0102]

[0103] Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Yrl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively.

[0104] The dynamic equation of the mobile robot in the yaw direction is:

[0105]

[0106] Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rr are the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively. i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas:

[0107] X i =F i x cosδ i -F i y sinδ i ≈F i x -F i y δ i ,i=fl,fr,rl,rr (3)

[0108] Y i =F i x sinδ i +F i y cosδ i ≈F i x δ i -F i y ,i=fl,fr,rl,rr (4)

[0109] where F i x is the tire longitudinal force, F i y is the tire lateral force, δ i is the steering angle, and the tire lateral force F is obtained by the magic formula of the tirei y With slip angle α i Into a linear relationship:

[0110] F i y =-C i α i ,i=fl,fr,rl,rr (5)

[0111] Among them, C i is the turning stiffness of the mobile robot wheels;

[0112] The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas:

[0113]

[0114]

[0115] in, is the lateral velocity of the mobile robot;

[0116] Front wheel slip angle α f and steering angle δ f satisfy:

[0117]

[0118] Rear wheel slip angle α r and steering angle δ r satisfy:

[0119]

[0120] Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for;

[0121]

[0122] Rear wheel slip angle α r for;

[0123]

[0124] in, is the lateral velocity of the mobile robot;

[0125] The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel satisfy the following formula:

[0126] δ fl =δ fr =δ f δ rl =δ rr =δ r (12)

[0127] Substituting formulas (3) to (12) into formulas (1) and (2) yields:

[0128]

[0129]

[0130] The dynamic model of the mobile robot is:

[0131]

[0132] Where M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot.

[0133] In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15):

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] G(q,σ,t)=0 (22)

[0141] Where y is the ordinate of the coordinate system of the mobile robot, ψ is the yaw angle of the mobile robot, and C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

[0142] Furthermore, the second establishing module is specifically used for:

[0143] Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as:

[0144] A(q,t)=0 (23)

[0145] A nonholonomic constraint is expressed as:

[0146]

[0147] Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c 1 c 2 … c m' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q;

[0148] By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations:

[0149]

[0150] Where b is the second-order constraint matrix;

[0151] Under ideal conditions, the mobile robot obtains the required control force p according to the UK equation: 1 for:

[0152]

[0153] Furthermore, the third establishing module is specifically used for:

[0154] During the movement of the mobile robot, the mass and moment of inertia change within a certain range. The inertia matrix M(q,σ,t) and the coefficient matrix of the Coriolis force or centrifugal force are It is divided into a nominal part and an uncertain part, which are expressed as follows:

[0155]

[0156] in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively;

[0157] In actual situations, the initial conditions of the mobile robot often cannot meet the required constraints, and an additional control force needs to be provided:

[0158]

[0159] Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0.

[0160] Furthermore, the fourth establishing module is specifically used for:

[0161] Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p 3 for:

[0162]

[0163] in, μ, They are control forces p 3 Control parameters:

[0164]

[0165]

[0166] The following inequality is satisfied:

[0167]

[0168] Adaptive parameter vector Satisfy the following adaptation rate:

[0169]

[0170] Among them, L 1 ,L 2 Adaptive rate is the constant control term, β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters.

[0171] The dynamic model of the mobile robot is expressed as:

[0172]

[0173] Taking into account the uncertainties in mass and inertia:

[0174]

[0175]

[0176]

[0177]

[0178]

[0179] Where Δm is the uncertainty of the mass of the mobile robot, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force acting on the tire, and θ is the steering angle matrix of the front and rear wheels of the mobile robot.

[0180] Furthermore, the fifth establishing module is specifically used for:

[0181] When the mobile robot is driving on the road, the body slips or yaws, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

[0182] The present invention has the following beneficial effects:

[0183] 1. Most of the research on analytical mechanics is based on the D'Alembert principle. Although the D'Alembert principle is applicable to most cases, it is difficult to obtain an analytical solution for non-holonomic constraints. The present invention designs a controller based on the Udwadia-Kalaba equation, which can handle the situations where mobile robots have both holonomic and non-holonomic constraints.

[0184] 2. The controller designed in the present invention can calculate in real time the precise analytical force required for the mobile robot to achieve the predetermined constraints, and then control the wheel angle to make it reach a safe and stable state.

[0185] 3. When there is an error between the mobile robot and the predetermined trajectory, the leakage-based adaptive robust controller can quickly change the adaptive parameters and control the wheel motor of the mobile robot to quickly reduce the error to 0. When the predetermined trajectory is reached, the adaptive parameters will decrease rapidly, and then the control force will decrease rapidly, achieving the desired effect with less energy loss. BRIEF DESCRIPTION OF THE DRAWINGS

[0186] Figure 1 Flow chart of a mobile robot control method based on UK theory according to an embodiment of the present invention;

[0187] Figure 2 A schematic structural diagram of a mobile robot according to an embodiment of the present invention;

[0188] Figure 3 A flow chart of a mobile robot design method according to an embodiment of the present invention;

[0189] Figure 4 A working mode diagram of a mobile robot controller according to an embodiment of the present invention;

[0190] Figure 5 is a structural block diagram of a mobile robot control system according to an embodiment of the present invention;

[0191] Figure 6 is the lateral speed of the mobile robot in the embodiment of the present invention Graphs of changes over time;

[0192] Figure 7 is the yaw speed of the mobile robot in the embodiment of the present invention Graphs of changes over time;

[0193] Figure 8 is the front wheel turning angle δ of the mobile robot according to the embodiment of the present invention f Graphs of changes over time;

[0194] Fig. 9 is the rear wheel turning angle δ of the mobile robot according to the embodiment of the present invention r Graphs of changes over time;

[0195] Fig.10 A curve diagram showing the change of the tracking error ||β|| of the mobile robot over time according to an embodiment of the present invention; DETAILED DESCRIPTION

[0196] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and specific embodiments:

[0197] The embodiment of the present invention provides a mobile robot control method and device based on UK theory, which adopts the Udwadia-Kalaba equation to solve the problem that it is difficult to obtain an analytical form of the existing dynamic model. The leakage type adaptive robust controller can effectively solve the uncertainty problems such as mass or inertia in the system.

[0198] In order to make the technical solution, technical route, purpose and advantages of the present invention clearer, the present invention is further described below. It should be understood that the specific implementation methods described herein are only used to explain the present invention and are not used to limit the present invention.

[0199] Example 1

[0200] like Figure 1 As shown, a mobile robot control method based on UK theory specifically includes:

[0201] S1. Establish a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller.

[0202] The specific process of S1 is as follows:

[0203] The dynamic equation of the mobile robot in the lateral direction is:

[0204]

[0205] Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Y rl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively.

[0206] The dynamic equation of the mobile robot in the yaw direction is:

[0207]

[0208] Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rrare the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively. i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas:

[0209] X i =F i x cosδ i -F i y sinδ i ≈F i x -F i y δ i ,i=fl,fr,rl,rr (3)

[0210] Y i =F i x sinδ i +F i y cosδ i ≈F i x δ i -F i y ,i=fl,fr,rl,rr (4)

[0211] where F i x is the tire longitudinal force, F i y is the tire lateral force, δ i is the steering angle, and the tire lateral force F is obtained by the magic formula of the tire i y With slip angle α i Into a linear relationship:

[0212] F i y =-C i α i ,i=fl,fr,rl,rr (5)

[0213] Among them, C i is the turning stiffness of the mobile robot wheels;

[0214] The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas:

[0215]

[0216]

[0217] in, is the lateral velocity of the mobile robot;

[0218] Front wheel slip angle α f and steering angle δ f satisfy

[0219]

[0220] Rear wheel slip angle α r and steering angle δ r satisfy

[0221]

[0222] Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for;

[0223]

[0224] Rear wheel slip angle α r for;

[0225]

[0226] The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel satisfy the following formula:

[0227] δ fl =δ fr =δ f δ rl =δ rr =δ r (12)

[0228] Substituting formulas (3) to (12) into formulas (1) and (2), we obtain:

[0229]

[0230]

[0231] The dynamic model of the mobile robot is:

[0232]

[0233] Where M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot.

[0234] In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15):

[0235]

[0236]

[0237]

[0238]

[0239]

[0240]

[0241] G(q,σ,t)=0 (22)

[0242] Where y is the ordinate of the coordinate system of the mobile robot, ψ is the yaw angle of the mobile robot, and C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

[0243] S2. Based on the Udwadia-Kalaba equation, the servo control force p required by the mobile robot in the ideal state is obtained. 1 ;

[0244] The specific process of S2 is:

[0245] Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as:

[0246] A(q,t)=0 (23)

[0247] A nonholonomic constraint is expressed as:

[0248]

[0249] Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c 1 c 2 … c m' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q;

[0250] Constrain Taking the derivative with respect to time t, we obtain the constraint equation in the second-order form:

[0251] By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations:

[0252]

[0253] Where b is the second-order constraint matrix;

[0254] Under ideal conditions, the mobile robot obtains the required control force p according to the UK equation: 1 for:

[0255]

[0256] S3. Obtain the servo control force p that the controller needs to provide to compensate for the mobile robot's initial conditions not meeting the constraints. 2 ;

[0257] The specific process of S3 is as follows:

[0258] During the movement of the mobile robot, the mass and moment of inertia change within a certain range. It is divided into a nominal part and an uncertain part, which are expressed as follows:

[0259]

[0260] in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively;

[0261] In actual situations, the initial conditions of the mobile robot often cannot meet the constraints required by the UK equation, and an additional control force needs to be provided:

[0262]

[0263] Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0.

[0264] S4. When the mobile robot has mass and inertia uncertainty, the servo control force p required for the mobile robot to reach the desired state is adjusted by changing the adaptive parameters in the controller. 3 ;

[0265] The specific process of S4 is as follows:

[0266] Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p 3 for:

[0267]

[0268] in, μ, They are control forces p 3 Control parameters:

[0269]

[0270]

[0271] The following inequality is satisfied:

[0272]

[0273] Adaptive parameter vector Satisfy the following adaptation rate:

[0274]

[0275] Among them, L 1 ,L 2 Adaptive rate The constant control term of , β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters;

[0276] The dynamic model of the mobile robot is expressed as:

[0277]

[0278] Taking into account the uncertainties in mass and inertia:

[0279]

[0280]

[0281]

[0282]

[0283]

[0284] Where Δm is the uncertainty of the mass of the mobile robot, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force acting on the tire, and θ is the steering angle matrix of the front and rear wheels of the mobile robot.

[0285] S5. The controller of the mobile robot drives the wheel hub motor to generate the corresponding servo control force τ=p according to its state. 1 +p 2 +p 3 .

[0286] The specific process of S5 is as follows:

[0287] When the mobile robot is driving on the road, the body slips or yawing, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

[0288] Compare the running trajectory of the mobile robot with the expected trajectory, and judge and adjust the controller parameters of the mobile robot based on the comparison results. When the expected trajectory state is set for the mobile robot, the actual position of the mobile robot is measured and recorded with a position sensor. Due to the existence of machining assembly errors, there will be deviations in the rotational inertia I of the mobile robot on the steering shaft, and the running trajectory may be deviated. Therefore, after comparing the expected trajectory with the actual running trajectory, adjust the rotational inertia I of the wheel about the steering shaft so that the expected trajectory can be close to the actual running trajectory. Find the optimal value through multiple tests to better verify the performance of the controller.

[0289] like Figure 2The figure shows a simplified structural diagram of the mobile robot of the present invention. The XOY coordinate system is fixed on a horizontal road surface, and the four wheels of the mobile robot can be independently controlled and powered. is the lateral velocity of the mobile robot, is the yaw velocity of the mobile robot, δ fl ,δ fr ,δ rl ,δ rr are the turning angles of the left front wheel, right front wheel, left rear wheel and right rear wheel, I is the moment of inertia of the intelligent mobile robot, and the trajectory of the mobile robot we expect should coincide with the center line of the road.

[0290] like Figure 3 The design flow chart of the mobile robot controller of the present invention is shown as follows. First, the mobile robot is regarded as an unconstrained mechanical system, and the unconstrained servo control force P is calculated according to the UK equation. 1 Next, consider that the initial conditions of the mobile robot do not satisfy the servo constraint, and the controller needs to provide a servo control force p 2 Next, considering that the system may have uncertainties such as mass or inertia when the mobile robot is in operation, the controller needs to provide a servo control force p 3 The controller provides control force τ = p 1 +p 2 +p 3 The mobile robot obtains its working state through the gyroscope, accelerometer sensor, and photoelectric encoder, and determines whether the controller needs to be adjusted until the working state of the mobile robot is the expected working state.

[0291] Figure 4 FIG. 1 is a schematic diagram of the working mode of the mobile robot controller of the present invention. Figure 4 As shown, the input of the controller is the preset trajectory of the mobile robot. The control force required by the mobile robot's drive motor and steering motor is calculated by the mobile robot tracking controller. The control force is adjusted through closed-loop control of the four wheels of the mobile robot through the torque controller, so that the mobile robot moves according to the expected trajectory.

[0292] Figure 5This is a block diagram of the structure of the mobile robot control system of an embodiment of the present invention. The mobile robot system is equipped with a gyroscope, an accelerometer sensor, a current sensor, a motor driver, a photoelectric encoder and other devices. The system is controlled by a main controller to control the four wheel hub motors of the mobile robot to drive the four wheels to move forward or rotate. The main controller sends instructions to the four motor drivers to drive the motors to work. The position information of the mobile robot is obtained by the photoelectric encoder and fed back to the motor driver. The mobile robot records the current information through the current sensor and calculates the actual torque of the mobile robot. The motor driver compares the actual torque with the given torque to adjust the motor of the mobile robot, so that the mobile robot moves according to the preset trajectory.

[0293] Figure 6 is the lateral speed of the mobile robot in the embodiment of the present invention The ideal speed of the mobile machine is 0. From the image, it can be seen that the lateral speed of the mobile robot can be quickly reduced to 0 under the controller designed by the present invention.

[0294] Figure 7 is the yaw speed of the mobile robot in the embodiment of the present invention The ideal speed of the mobile robot is 0. From the image, it can be seen that under the controller of the present invention, the yaw speed can be quickly reduced to zero.

[0295] Figure 8 is the front wheel turning angle (δ f (δ fl =δ fr )) is a graph showing changes over time. The present invention believes that As a constant, take According to τ=Jθ, it can be known that the driving force can control the wheel angle, and the ideal front wheel angle of the mobile robot is 0. It can be seen from the image that under the controller involved in the present invention, the front wheel angle can be quickly reduced to 0.

[0296] Fig. 9 is the front wheel turning angle (δ r (δ rl =δ rr )) is a graph showing changes over time. The present invention believes that As a constant, take According to τ=Jθ, it can be known that the driving force can control the wheel angle, and the ideal rear wheel angle of the mobile robot is 0. From the image, it can be seen that under the controller involved in the present invention, the front wheel angle can be quickly reduced to 0.

[0297] Fig.10It is a curve diagram of the change of the tracking error (||β||) of the mobile robot of the embodiment of the present invention over time. The ideal tracking error of the mobile robot is 0. It can be seen from the image that under the controller of the present invention, the tracking error can be quickly reduced to 0.

[0298] according to Figure 3 and Figure 4 The model and control system structure block diagram of the mobile robot in the paper are used to illustrate the parameters mentioned in the article, as shown in Table 1. The steering angle of each wheel of the mobile robot is calculated by the motor angle information obtained by the position sensor of the photoelectric encoder of each wheel rotating motor, and the yaw angle of the mobile robot is It can be calculated based on the gyroscope and accelerometer sensors. The servo control force τ required by the mobile robot and the δ required to be controlled by the controller f ,δ r It can be obtained according to formula (30). Some parameter definitions and units are shown in Table 2.

[0299]

[0300]

[0301] Table 1

[0302]

[0303] Table 2

[0304] Example 2

[0305] A mobile robot control device based on UK theory, comprising:

[0306] The first building module is used to build a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller. The first building module is specifically used for:

[0307] Establish the dynamic equation of the mobile robot in the lateral direction:

[0308]

[0309] Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Y rl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively.

[0310] The dynamic equation of the mobile robot in the yaw direction is:

[0311]

[0312] Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rr are the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively. i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas:

[0313] X i =F i x cosδ i -F i y sinδ i ≈F i x -F i y δ i ,i=fl,fr,rl,rr (3)

[0314] Y i =F i x sinδ i +F i y cosδ i ≈F i x δ i -F i y ,i=fl,fr,rl,rr (4)

[0315] where F i x is the tire longitudinal force, F i y is the tire lateral force, δ i is the steering angle, and the tire lateral force F is obtained by the magic formula of the tire i y With slip angle α i Into a linear relationship:

[0316] The tire lateral force F is obtained by the magic formula of the tire i y Slip angle α i Into a linear relationship:

[0317] F i y =-C i α i ,i=fl,fr,rl,rr (5)

[0318] Among them, C i is the turning stiffness of the mobile robot wheels;

[0319] The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas:

[0320]

[0321]

[0322] in, is the lateral velocity of the mobile robot;

[0323] Front wheel slip angle α f and steering angle δ f satisfy

[0324]

[0325] Rear wheel slip angle α r and steering angle δ r satisfy

[0326]

[0327] Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for;

[0328]

[0329] Rear wheel slip angle α r for;

[0330]

[0331] in, is the lateral velocity of the mobile robot;

[0332] The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel satisfy the following formula:

[0333] δ fl =δ fr =δ f δ rl =δ rr =δ r (12)

[0334] Substituting formulas (3) to (12) into formulas (1) and (2) yields:

[0335]

[0336]

[0337] The dynamic model of the mobile robot is:

[0338]

[0339] Where M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot.

[0340] In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15):

[0341]

[0342]

[0343]

[0344]

[0345]

[0346]

[0347] G(q,σ,t)=0 (22)

[0348] Where m is the mass of the mobile robot, I is the moment of inertia of the mobile robot, and y is the ordinate of the coordinate system where the mobile robot is located. are the lateral velocity and lateral acceleration of the mobile robot, ψ is the yaw angle of the mobile robot, are the yaw angular velocity and yaw angular acceleration of the mobile robot, respectively, l f is the distance from the center of mass of the mobile robot to the center of mass of the front wheel, l r is the distance from the center of mass of the mobile robot to the center of mass of the rear wheel, u is the longitudinal speed of the mobile robot, C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

[0349] The second building module is used to obtain the servo control force p required by the mobile robot in the ideal state based on the Udwadia-Kalaba equation 1 ; The second establishment module is specifically used for:

[0350] Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as:

[0351] A(q,t)=0 (23)

[0352] A nonholonomic constraint is expressed as:

[0353]

[0354] Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c 1 c 2 … c m' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q;

[0355] By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations:

[0356]

[0357] Where b is the second-order constraint matrix;

[0358] Under ideal conditions, the mobile robot obtains the required control force p according to the UK equation:1 for:

[0359]

[0360] The third building module is used to obtain the servo control force p that the controller needs to provide to compensate for the mobile robot's initial conditions not meeting the constraints. 2 ; The third establishment module is specifically used for:

[0361] During the movement of the mobile robot, the mass and moment of inertia change within a certain range. It is divided into a nominal part and an uncertain part, which are expressed as follows:

[0362]

[0363] in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively;

[0364] In actual situations, the initial conditions of the mobile robot often cannot meet the required constraints, and an additional control force needs to be provided:

[0365]

[0366] Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0.

[0367] The fourth building module is used to obtain the servo control force p required for the mobile robot to achieve the desired result by changing the adaptive parameters in the controller when the mobile robot has mass and inertia uncertainty. 3 ; The fourth establishment module is used to:

[0368] Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p 3 for:

[0369]

[0370] in, μ, They are control forces p 3 Control parameters:

[0371]

[0372]

[0373] The following inequality is satisfied:

[0374]

[0375] Adaptive parameter vector Satisfy the following adaptation rate:

[0376]

[0377] Among them, L 1 ,L 2 Adaptive rate The constant control term of , β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters;

[0378] The dynamic model of the mobile robot is expressed as:

[0379]

[0380] Taking into account the uncertainties in mass and inertia:

[0381]

[0382]

[0383]

[0384]

[0385]

[0386] Where Δm is the uncertainty of the mass of the mobile robot, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force acting on the tire, and θ is the steering angle matrix of the front and rear wheels of the mobile robot.

[0387] The fifth building block: The controller for the mobile robot drives the wheel hub motor to generate the corresponding servo control force τ=p according to its own state 1 +p 2 +p 3 .

[0388] When the mobile robot is driving on the road, the body slips or yaws, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

[0389] Of course, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by technicians in this technical field within the essential scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A mobile robot control method based on UK theory, characterized in that: The following steps are involved: S1. Establish a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller. S2, based on the Udwadia-Kalaba equation, obtain the servo control force p1 required by the mobile robot in the ideal state; S3, obtaining the servo control force p2 that the controller needs to provide to compensate for the mobile robot's initial conditions not satisfying the constraints; S4, when the mobile robot has mass and inertia uncertainty, the servo control force p3 required for the mobile robot to reach the desired state is obtained by changing the adaptive parameters in the controller; S5, the controller of the mobile robot drives the wheel hub motor to generate a corresponding servo control force τ=p1+p2+p3 according to its state; The specific process of S4 is as follows: Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p3 is: in, is the adaptive parameter vector, q is the generalized coordinate of the mobile robot, t is the time, is the mobile robot speed, is the nominal part of the inertia matrix, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, P is the m'×m' dimensional constant coefficient matrix, They are the control parameters of the control force p3: The following inequality is satisfied: Where c is the first-order constraint matrix, σ is the uncertain parameter, C is the coefficient matrix of the Coriolis force or centrifugal force, ΔC is the uncertain part of the coefficient matrix of the Coriolis force or centrifugal force, and G is the gravity of the mobile robot; Adaptive parameter vector Satisfy the following adaptation rate: Among them, L1, L2 are adaptive rates The constant control term of , β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters; The dynamic model of the mobile robot is expressed as: Where M is the inertia matrix of the mobile robot, is the acceleration of the mobile robot, τ(t) is the control input of the mobile robot; Taking into account the uncertainties in mass and inertia: Among them, ΔM is the uncertain part of the inertia matrix, is the nominal part of the coefficient matrix of the Coriolis or centrifugal force, C fl ,C fr ,C rl ,C rr are the lateral stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot, respectively, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the center of mass of the mobile robot to the rear wheel axle, u is the longitudinal speed of the mobile robot, F i x is the tire longitudinal force, i = fl, fr, rl, rr, Δm is the uncertainty of the mobile robot mass, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force on the tire, θ is the steering angle matrix of the front and rear wheels of the mobile robot, and δ f is the front wheel steering angle, δ r is the rear wheel steering angle.

2. A mobile robot control method based on UK theory according to claim 1, characterized in that: The specific process of S1 is as follows: The dynamic equation of the mobile robot in the lateral direction is: Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Y rl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground; The dynamic equation of the mobile robot in the yaw direction is: Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rr are the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively; i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas: X i =F i x cosδ i -F i y sind i ≈F i x -F i y d i (3) Y i =F i x sind i +F i y cosδ i ≈F i x d i -F i y (4) where F i x is the tire longitudinal force, F i y is the tire lateral force, δ i is the steering angle, and the tire lateral force F is obtained by the magic formula of the tire i y With slip angle α i Into a linear relationship: F i y =-C i α i ,i=fl, fr, rl, rr (5) Among them, C i is the turning stiffness of the mobile robot wheels; The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas: in, is the lateral velocity of the mobile robot; Front wheel slip angle α f and the front wheel steering angle δ f satisfy: Rear wheel slip angle α r and rear wheel steering angle δ r satisfy: Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for; Rear wheel slip angle α r for; The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel meet the following requirements: d fl =d fr =d f d rl =d rr =d r (12) Substituting formulas (3) to (12) into formulas (1) and (2), we obtain: The dynamic model of the mobile robot is: Where, M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot; In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15): G(q,σ,t)=0 (22) Where y is the ordinate of the coordinate system of the mobile robot, ψ is the yaw angle of the mobile robot, and C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

3. A mobile robot control method based on UK theory according to claim 2, characterized in that: The specific process of S2 is: Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as: A(q,t)=0 (23) A nonholonomic constraint is expressed as: Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c1 c2…c m' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q; By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations: Where b is the second-order constraint matrix; Under ideal conditions, the required control force p1 of the mobile robot is obtained according to the UK equation:

4. A mobile robot control method based on UK theory according to claim 3, characterized in that: The specific process of S3 is as follows: During the movement of the mobile robot, the mass and moment of inertia change within a certain range. The inertia matrix M(q,σ,t) and the coefficient matrix of the Coriolis force or centrifugal force are It is divided into a nominal part and an uncertain part, which are expressed as follows: in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis or centrifugal force, respectively, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively; In actual situations, the initial conditions of the mobile robot often cannot meet the required constraints, and an additional control force needs to be provided: Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0.

5. A mobile robot control method based on UK theory according to claim 4, characterized in that: The specific process of S5 is as follows: When the mobile robot is driving on the road, the body slips or yawing, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

6. A device for implementing a mobile robot control method based on UK theory, characterized in that: include: The first building module is used to build a dynamic model of the mobile robot so that the controller can provide accurate servo control force. The controller adopts a leakage-based adaptive robust controller. The second building module is used to obtain the servo control force p1 required for the mobile robot in an ideal state based on the Udwadia-Kalaba equation; The third building module is used to obtain the servo control force p2 that the controller needs to provide to compensate for the mobile robot's initial conditions not satisfying the constraints; The fourth building module is used for when the mobile robot has mass and inertia uncertainty, the controller changes the adaptive parameters to make the mobile robot reach the desired servo control force p3; The fifth building module: the controller for the mobile robot drives the wheel hub motor to generate a corresponding servo control force τ=p1+p2+p3 according to its state; The fourth establishment module is specifically used for: Due to the uncertainty of the robot's working state, the mobile robot needs to provide additional servo control force. The servo control force p3 is: in, is the adaptive parameter vector, q is the generalized coordinate of the mobile robot, t is the time, is the mobile robot speed, is the nominal part of the inertia matrix, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, P is the m'×m' dimensional constant coefficient matrix, They are the control parameters of the control force p3: The following inequality is satisfied: Where c is the first-order constraint matrix, σ is the uncertain parameter, C is the coefficient matrix of the Coriolis force or centrifugal force, ΔC is the uncertain part of the coefficient matrix of the Coriolis force or centrifugal force, and G is the gravity of the mobile robot; Adaptive parameter vector Satisfy the following adaptation rate: Among them, L1, L2 are adaptive rates The constant control term of , β is the tracking error of the mobile robot, δ' is the leakage term parameter 1, λ is the leakage term parameter 2, and l is the number of leakage term parameters; The dynamic model of the mobile robot is expressed as: Where M is the inertia matrix of the mobile robot, is the acceleration of the mobile robot, τ(t) is the control input of the mobile robot; Taking into account the uncertainties in mass and inertia: Among them, ΔM is the uncertain part of the inertia matrix, is the nominal part of the coefficient matrix of the Coriolis or centrifugal force, C fl ,C fr ,C rl ,C rr are the lateral stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot, respectively, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the center of mass of the mobile robot to the rear wheel axle, u is the longitudinal speed of the mobile robot, F i x is the tire longitudinal force, i = fl, fr, rl, rr, Δm is the uncertainty of the mobile robot mass, is the nominality of the mobile robot's mass, ΔI is the uncertainty of the mobile robot's moment of inertia, is the nominality of the mobile robot’s moment of inertia, J is the constant matrix determined by the tire stiffness and the lateral force on the tire, θ is the steering angle matrix of the front and rear wheels of the mobile robot, and δ f is the front wheel steering angle, δ r is the rear wheel steering angle.

7. The device for implementing the mobile robot control method based on UK theory according to claim 6, characterized in that: The first establishing module is specifically used for: Establish the dynamic equation of the mobile robot in the lateral direction: Where m is the mass of the mobile robot, is the lateral acceleration of the mobile robot, u is the longitudinal velocity of the mobile robot, is the yaw angular velocity of the mobile robot, Y fl , Y fr , Y rl , Y rr They are the lateral forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground; The dynamic equation of the mobile robot in the yaw direction is: Where, I is the moment of inertia of the mobile robot, is the yaw angular acceleration of the mobile robot, l f is the distance from the center of mass of the mobile robot to the front wheel axis, l r is the distance from the mass center of the mobile robot to the rear wheel axle, d is the lateral distance from the mass center of the mobile robot to the mass center of the wheel, X fl , X fr , X rl , X rr are the longitudinal forces between the left front wheel, right front wheel, left rear wheel, right rear wheel and the ground respectively; i and Y i (i=fl,fr,rl,rr) are obtained by the following formulas: X i =F i x cosδ i -F i y sind i ≈F i x -F i y d i (3) Y i =F i x sind i +F i y cosδ i ≈F i x d i -F i y (4) where F i x is the tire longitudinal force, F i y is the tire lateral force, and the tire lateral force F is obtained by the magic formula of the tire i y With slip angle α i Into a linear relationship: F i y =-C i α i ,i=fl, fr, rl, rr (5) Among them, C i is the turning stiffness of the mobile robot wheels; The lateral velocity v of the front wheel of the mobile robot f and rear wheel lateral velocity v r They are obtained by the following formulas: in, is the lateral velocity of the mobile robot; Front wheel slip angle α f and steering angle δ f satisfy: Rear wheel slip angle α r and steering angle δ r satisfy: Due to the slip angle α f , α r and steering angle δ f , δ r are very small, and equations (8) and (9) are simplified to: tan(α f +δ f )=α f +δ f , tan(α r +δ r )=α r +δ r , so the front wheel slip angle α f for; Rear wheel slip angle α r for; The steering angles of the left front wheel, right front wheel, left rear wheel, and right rear wheel satisfy the following formula: d fl =d fr =d f d rl =d rr =d r (12) Substituting formulas (3) to (12) into formulas (1) and (2) yields: The dynamic model of the mobile robot is: Where, M(·) is the inertia matrix of the mobile robot, is the mobile robot acceleration, is the external force on the mobile robot under unconstrained conditions, C(·) is the coefficient matrix of the Coriolis force or centrifugal force, is the mobile robot speed, is the Coriolis force or centrifugal force on the mobile robot, G(·) is the gravity of the mobile robot, q is the generalized coordinate of the mobile robot, σ is an uncertain parameter, t is time, and τ(t) is the control input of the mobile robot; In order to control the stability of the vehicle by controlling the front and rear wheel angles, the lateral force and longitudinal force of the left front wheel and right front wheel, left rear wheel and right rear wheel of the vehicle are equal. Considering the zero treatment, (13) and (14) are substituted into (15): G(q,σ,t)=0 (22) Where y is the ordinate of the coordinate system of the mobile robot, ψ is the yaw angle of the mobile robot, and C fl ,C fr ,C rl ,C rr They are the cornering stiffness of the left front wheel, right front wheel, left rear wheel and right rear wheel tires of the mobile robot respectively.

8. The device for implementing the mobile robot control method based on UK theory according to claim 7, characterized in that: The second establishing module is specifically used for: Mobile robots are often subject to complete constraints or non-complete constraints. The complete constraints are expressed as: A(q,t)=0 (23) A nonholonomic constraint is expressed as: Among them, A is the m'×n' dimensional first-order constraint matrix coefficient matrix, c is the first-order constraint matrix, c=[c1 c2…c m' ] T ; m' is the number of constraints on the mobile robot, n' is the dimension of the generalized coordinate q; By differentiating constraints (23) and (24) with respect to time t, we obtain the second-order constraint equations: Where b is the second-order constraint matrix; Under ideal conditions, the required control force p1 of the mobile robot is obtained according to the UK equation:

9. The device for implementing the mobile robot control method based on UK theory according to claim 8, characterized in that: The third establishing module is specifically used for: During the movement of the mobile robot, the mass and moment of inertia change within a certain range. The inertia matrix M(q,σ,t) and the coefficient matrix of the Coriolis force or centrifugal force are It is divided into a nominal part and an uncertain part, which are expressed as follows: in, and are the nominal parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force, ΔM(q,σ,t) and are the uncertain parts of the inertia matrix and the coefficient matrix of the Coriolis force or centrifugal force respectively; In actual situations, the initial conditions of the mobile robot often cannot meet the required constraints, and an additional control force needs to be provided: Where P is a constant coefficient matrix of m'×m' dimensions, and κ is an arbitrary constant greater than 0; The fifth establishment module is specifically used for: When the mobile robot is driving on the road, the body of the robot slips or yawing, and the information is transmitted to the controller. The controller changes the side slip angle of the wheel by controlling the wheel hub motor, so that the mobile robot quickly reaches the desired state. When the mobile robot reaches the desired state, the information is fed back to the controller, and the controller quickly reduces the adaptive parameters, so that the mobile robot can meet the desired working state at a lower energy cost, forming a closed-loop feedback working mode.

Images