CN106125728A - A kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method - Google Patents

Abstract

The invention provides a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method, initially set up the kinematics model of system, kinetic model, driving motor model；Then design kinematic controller, on the basis of kinematics model, adjust the speed of system according to the state of given reference locus；Dynamics Controller, on the basis of kinetic model, draws the expectation moment of motor according to the speed of required adjustment system；Drive motor controller, on the basis of driving motor model, for meeting the expectation moment of motor, the suitable driving voltage of system of designing；The robust trajectory tracking control method finally using Backstepping carries out Trajectory Tracking Control to 4 wheel driven wheeled mobile robot.The method that the present invention provides achieves the purpose of the stability improving 4 wheel driven Control of Wheeled Mobile Robots system under complicated uncertain environment, improves the effectiveness controlled containing system under the conditions of uncertain factor.

Description

A kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method

Technical field

The present invention relates to robotic tracking control method, particularly relate to a kind of 4 wheel driven wheeled mobile robot trace Tracking and controlling method.

Background technology

Wheeled mobile robot (Wheeled Mobile Robot, WMR) is as one important point of mobile robotics , compared to traditional industrial robot, have that work efficiency is high, drive and control simple, load-carrying is big, flexible operation is convenient Advantage, therefore has a wide range of applications in each field.Along with wheeled mobile robot service ability, adaptation to varying environment Abilities etc. are greatly improved, and wheeled mobile robot has become as requisite helper in human being's production life.Wherein exist Civil area, wheeled mobile robot can substitute for human work person and is engaged in various heavy task, such as substation equipment Patrol and examine, the tour of market Security Personnel, the occasion such as earthquake relief work.In military field, various unmanned combat aerial vehicles, bomb disposal blasting-proof machine The application of device people etc. is the most increasingly extensive.Additionally, increasingly mature along with intellectualized technology, service robot, all kinds of inspection machine People etc. will have and more be widely applied.

Various sensors are applied on wheel type mobile robot platform, just so that wheeled mobile robot complete right The tour of uncertain complex environment, study and decision-making etc., such wheeled mobile robot just can complete various high-risk Business, such as the patrolling and examining of high voltage substation, the detection etc. of nuclear power station.Additionally, being widely used of wheeled mobile robot also makes the mankind Life more convenient, as played very important at occasion wheeled mobile robots such as the carrying of large cargo, household services Effect.Wheeled mobile robot is the most all operated in the complex environment of the unknown, and this will bring more to robot system Uncertainty and complexity, its kinetic control system to have higher capacity of resisting disturbance, therefore studies under the circumstances not known of complexity The motor control of wheeled mobile robot has higher theory and practice meaning.

At present, the research to wheeled mobile robot motor control is mostly to carry out under conditions of external environment is constant , but when robot runs in various complex environments, the parameter of wheeled mobile robot system will change, This just will affect the stability of kinetic control system.

Summary of the invention

The technical problem to be solved in the present invention is how to improve 4 wheel driven wheeled mobile robot under complicated uncertain environment The stability of control system.

In order to solve above-mentioned technical problem, the technical scheme is that a kind of 4 wheel driven wheeled mobile robot trace of offer Tracking and controlling method, it is characterised in that: the method is made up of following 3 steps:

Step 1: set up the kinematics model of system, kinetic model, driving motor model；

Step 2: design

Kinematic controller, on the basis of kinematics model, adjusts system according to the state of given reference locus The speed of system；

Dynamics Controller, on the basis of kinetic model, draws motor according to the speed of required adjustment system Expectation moment；

Drive motor controller, for, on the basis of driving motor model, for meeting the expectation moment of motor, designing The suitable driving voltage of system；

Step 3: use Backstepping robust trajectory tracking control method 4 wheel driven wheeled mobile robot is carried out track with Track controls, and detailed process is as follows:

Step 3.1: according to given reference locus, by the foundation of system model is obtained machine under corresponding coordinate system People's position and attitude error；

Step 3.2: judge this position and attitude error whether as zero, if zero, then complete corresponding track following；Otherwise, adjust The desired speed of whole kinematics model input；

Step 3.3: under conditions of meeting the desired speed of kinematics model, arranges suitable Torque Control rule；

Step 3.4: be set the voltage driving motor, obtains suitable Control of Voltage rule, makes the phase that system obtains Hope that speed and moment condition meet simultaneously.

Step 3.5: according to the design philosophy of Backstepping, forms a feedback system so that when t → ∞, when t represents Between, position and attitude error is 0, to realize the tracking to reference locus of the actual mobile robot.

Preferably, described feedback system structure is: kinematic controller, Dynamics Controller, drive motor controller, drive Dynamic motor model, kinetic model, kinematics model are sequentially connected with, and drive motor model output result to feed back to driving motor control Device processed, kinetic model output result feeds back to Dynamics Controller and drive motor controller, kinematics model output result Feeding back to Dynamics Controller, kinematics model output result feeds back to kinetics control by forming actual path after integral element The position and attitude error input motion controller of device processed, actual path and reference locus, reference locus is defeated simultaneously as feed-forward signal Enter kinematic controller.

Preferably, the method for building up of described kinematics model is as follows:

Kinematics model is for the relation between descriptive system speed and its pose；

The nonholonomic constraint of Pfaffian form, such as formula (1):

$\left[\begin{array}{ccc}\sin \mathrm{\θ}& -\cos \mathrm{\θ}& 0\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=A\left(q\right)\stackrel{\·}{q}=0---\left(1\right)$

Wherein, θ is the angle between dolly transverse axis and the X-axis of inertial coodinate system, and it can represent the pose angle of robot, A (q)=[sin θ-cos θ 0],It it is the pose of system；

Then the mutually conversion between local coordinate system xoy and inertial coodinate system XoY of the system pose is such as formula (2):

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& cos\mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \mathrm{\ω}\end{array}\right]---\left(2\right)$

Wherein,For robot pose under local coordinate system, v_xFor robot speed's component in x-axis, v_yFor robot speed's component on the y axis, ω is the angular velocity of robot；

Just the kinesiology of system can be released on the basis of the nonholonomic constraint and system model of Pfaffian form Model, such as formula (3):

$\stackrel{\·}{q}=S\left(q\right)\mathrm{\η}---\left(3\right)$

$\begin{array}{cc}S\left(q\right)=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]& \mathrm{\η}=\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]\end{array}$

Wherein, v is the linear velocity of robot wheel, and ω is the angular velocity of robot.

Preferably, the method for building up of described kinetic model is as follows:

The kinetic model of general wheeled mobile robot, such as formula (4):

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+F\left(\stackrel{\·}{q}\right)+G\left(q\right)+{\mathrm{\τ}}_d=B\left(q\right)\mathrm{\τ}-A^T\left(q\right)\mathrm{\λ}\\ A\left(q\right)\stackrel{\·}{q}=0\end{array}\right.---\left(4\right)$

Wherein, M (q) is the inertial matrix that positive definite is symmetrical；For relevant with speed and position Ge Shi moment battle array with Centripetal force；For frictional force；G (q) is gravity item；τ_dFor including destructuring Unmarried pregnancy bounded unknown disturbance vector；B Q () is Input transformation matrix；τ=[τ_L τ_R]^TFor torque input vector, in some cases, the torque with driving motor is equal； λ is constraint force vector, isSpecial built-in variable；

By to formula (3) derivation, formula (5) can be obtained as follows:

$\stackrel{\·\·}{q}=\stackrel{\·}{S}\left(q\right)\mathrm{\η}+S\left(q\right)\stackrel{\·}{\mathrm{\η}}---\left(5\right)$

The uncertainty assuming system dynamics model is bounded, and meets

|τ_vd|≤ρ_v(τ)

|τ_ωd|≤ρ_ω(τ)

τ_vdFor the uncertain parameter of linear velocity, τ_ωdFor the uncertain parameter of angular velocity, ρ_v(τ)、ρ_ω(τ) bounded it is normal Numerical value；

In the case of ignoring gravity and the interference of other factors, utilize formula (4) and (5) that wheeled mobile robot can be drawn The kinetic model formula (6) of people is as follows:

$\stackrel{\·}{\mathrm{\η}}=\stackrel{-}{M^{-1}}\left(q\right)\[\stackrel{\‾}{B}\left(q\right)\mathrm{\τ}-\stackrel{\‾}{V}(q,\stackrel{\·}{q})\mathrm{\η}-\stackrel{\‾}{F}(q,\stackrel{\·}{q})-{\mathrm{\τ}}_d\]---\left(6\right)$

Wherein,

$\stackrel{\‾}{M}\left(q\right)=S^T\left(q\right)M\left(q\right)S\left(q\right)$

$\stackrel{\‾}{V}(q,\stackrel{\·}{q})=S^T\left(q\right)\[M\left(q\right)\stackrel{\·}{S}\left(q\right)+V(q,\stackrel{\·}{q})S\left(q\right)\]$

$\stackrel{\‾}{F}(q,\stackrel{\·}{q})=S^T\left(q\right)\[F\left(\stackrel{\·}{q}\right)+A^T\left(q\right)\mathrm{\λ}\]$

$\stackrel{\‾}{B}\left(q\right)=S^T\left(q\right)B\left(q\right)$

And then it follows that

$\begin{array}{cccc}\stackrel{\‾}{M}\left(q\right)=\left[\begin{array}{cc}m& 0\\ 0& 1\end{array}\right]& \stackrel{\‾}{V}(q,\stackrel{\·}{q})=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]& \stackrel{\‾}{F}(q,\stackrel{\·}{q})=\left[\begin{array}{c}{F}_{rx}\left(\stackrel{\·}{q}\right)\\ M_r\left(\stackrel{\·}{q}\right)\end{array}\right]& \stackrel{\‾}{B}\left(q\right)=\frac{1}{r}\left[\begin{array}{cc}1& 1\\ -b& b\end{array}\right]\end{array}$

Wherein,

$F_{rx}\left(\stackrel{\·}{q}\right)=f_r\frac{mg}{2}\left(\mathrm{sgn}\right(\stackrel{\·}{x}-b\stackrel{\·}{\mathrm{\θ}})+\mathrm{sgn}(\stackrel{\·}{x}+b\stackrel{\·}{\mathrm{\θ}}\left)\right)$

$\begin{array}{c}{M}_r\left(\stackrel{\·}{q}\right)=\mathrm{\μ}\frac{bmg}{2}\left(\mathrm{sgn}\right(\stackrel{\·}{y}-b\stackrel{\·}{\mathrm{\θ}})-\mathrm{sgn}(\stackrel{\·}{y}+b\stackrel{\·}{\mathrm{\θ}}\left)\right)\\ +f_r\frac{bmg}{2}\left(\mathrm{sgn}\right(\stackrel{\·}{x}-b\stackrel{\·}{\mathrm{\θ}})-\mathrm{sgn}(\stackrel{\·}{x}+b\stackrel{\·}{\mathrm{\θ}}\left)\right)\end{array}$

B is the half of left and right wheels spacing, and r is radius of wheel, and m is the quality of car body, and g is gravity constant.

Preferably, the method for building up of described driving motor model is as follows:

The output torque tau of driving motor and the relation such as formula (7) of electric current:

τ=[τ_L τ_R]^T=[2kni_L 2kni_R]^T (7)

Wherein, k is constant coefficient, and n is the speed reducing ratio of motor, i_L, i_RIt is respectively the electric current of left and right sides motor；

Consider that under systematic parameter uncertainty and disturbed condition, the balance of voltage equation such as formula (8) of direct current generator is shown:

$U=(L+\mathrm{\Δ}L)\frac{di}{dt}+(R+\mathrm{\Δ}R)i+(k_e+{\mathrm{\Δk}}_e)\mathrm{\ω}+d\left(t\right)---\left(8\right)$

Wherein, U is armature voltage, and L is armature inductance, and R is armature resistance, k_eFor motor torque constant, Δ represents parameter Uncertainty, d (t) represents uncertain interference；

Utilize formula (7) and (8), just can show that the mathematical model formula (9) driving motor is as follows:

$\stackrel{\·}{\mathrm{\τ}}=\stackrel{\‾}{L}U-\stackrel{\‾}{R}\mathrm{\τ}-\stackrel{\‾}{K}\mathrm{\η}+D---\left(9\right)$

Wherein,U=[U_L U_R]^T

D=[D_L D_R]^T=Δ LU-Δ R τ-Δ K η+d (t) is the uncertainty that system is total.

Preferably, the specifically comprising the following steps that of Trajectory Tracking Control

The reference locus of step A, first initialization system

Wherein, q_r=[x_r y_r θ_r]^TFor reference locus, η_r=[v_r ω_r]^T, v_rFor reference line speed, ω_rFor reference angle speed Degree；

Formula (2) is utilized to draw the error e of pose under local coordinate_q, such as formula (10):

$e_q=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\mathrm{\θ}}\end{array}\right]=T_e(q_r-q)=\left[\begin{array}{ccc}cos\mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& cos\mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]---\left(10\right)$

Consider the systematic parameter of this 4 wheel driven wheeled mobile robot, position and attitude error is carried out data process；By instead The design philosophy of footwork, designs Lyapunov function for this kinematics model subsystem, it is assumed that the Li Yapu of kinesiology control law Promise husband's function is

$V_1=\frac{1}{2}{e}_x^2+\frac{1}{2}{e}_y^2+1-{\mathrm{cose}}_{\mathrm{\θ}}---\left(11\right)$

By the data of formula (11) are processed, it can be deduced that make in the case of t → ∞ (t is the time), e_qWhen=0 The velocity control law of design is

$\begin{array}{c}v=V_r{\mathrm{cose}}_{\mathrm{\θ}}+k_1{e}_x\\ \mathrm{\ω}={\stackrel{\·}{\mathrm{\θ}}}_r+e_y{V}_r+k_2{\mathrm{sine}}_{\mathrm{\θ}}\end{array}---\left(12\right)$

Wherein, k₁, k₂For constant coefficient, k₁＞ 0, k₂＞ 0；

Utilize the determination of stability condition of Lyqpunov function, have

Step B, exports the formula (12) in Motion Controlling Model as desired speed, is denoted as [v_r ω_r]^T, then move Mechanics tracking error is e_n=[e_v e_w]^T=[v_r-v w_r-w]^T, it is assumed that the liapunov function of definition Dynamics Controller is

$V_2=V_1+\frac{1}{2}{e}_v^2+\frac{1}{2}{e}_w^2---\left(13\right)$

By processing the data of formula (12), setting Torque Control rule is

$\left\{\begin{array}{c}{\mathrm{\τ}}_L=\frac{mr}{2}(\stackrel{\·}{v_r}+k_3{e}_v-\frac{1}{m}{\mathrm{\ρ}}_v(\mathrm{\τ}\left){\mathrm{sgne}}_v\right)-\frac{Ir}{2b}(\stackrel{\·}{{\mathrm{\ω}}_r}+k_4{e}_{\mathrm{\ω}}-\frac{1}{I}{\mathrm{\ρ}}_{\mathrm{\ω}}(\mathrm{\τ}\left){\mathrm{sgne}}_{\mathrm{\ω}}\right)\\ {\mathrm{\τ}}_R=\frac{mr}{2}(\stackrel{\·}{v_r}+k_3{e}_v-\frac{1}{m}{\mathrm{\ρ}}_v(\mathrm{\τ}\left){\mathrm{sgne}}_v\right)+\frac{Ir}{2b}(\stackrel{\·}{{\mathrm{\ω}}_r}+k_4{e}_{\mathrm{\ω}}-\frac{1}{I}{\mathrm{\ρ}}_{\mathrm{\ω}}(\mathrm{\τ}\left){\mathrm{sgne}}_{\mathrm{\ω}}\right)\end{array}\right.---\left(14\right)$

Wherein, k₃, k₄For constant coefficient, k₃＞ 0, k₄＞ 0；

Utilize the determination of stability condition of Lyqpunov function, have

Step C, assumes initially that driving the uncertain of motor is bounded, and does slowly varying, i.e.

$\begin{array}{cc}\left|D\right|<{\mathrm{\&rho;}}_D\left(t\right)& \stackrel{\&CenterDot;}{D}=0\end{array}---\left(15\right)$

p_DT () is bounded constant value；

Using the output formula (14) of Dynamics Controller as the expectation moment of system, it is denoted as [τ_Lr τ_Rr]^T, driving torque Error is

e_τ=[e_τL e_τR]^T

=[τ_Lr-τ_L τ_Rr-τ_R]^T (16)

The liapunov function of definition electric machine controller is

$V_3=V_2+\frac{1}{2}{e}_{\mathrm{\&tau;}L}^2+\frac{1}{2}{e}_{\mathrm{\&tau;}R}^2---\left(17\right)$

By processing the data of formula (17), setup control rule is

$\left\{\begin{array}{c}{U}_L=\frac{{\mathrm{R\&tau;}}_L}{2kn}+k_{e}v-k_{e}b\mathrm{\&omega;}+\stackrel{\&CenterDot;}{{\mathrm{\&tau;}}_{Lr}}+k_5{e}_{\mathrm{\&tau;}L}-\frac{{\mathrm{L\&rho;}}_D\left(t\right)}{2kn}{\mathrm{sgne}}_{\mathrm{\&tau;}L}\\ U_R=\frac{{\mathrm{R\&tau;}}_R}{2kn}+k_{e}v-k_{e}b\mathrm{\&omega;}+\stackrel{\&CenterDot;}{{\mathrm{\&tau;}}_{Rr}}+k_5{e}_{\mathrm{\&tau;}R}-\frac{{\mathrm{L\&rho;}}_D\left(t\right)}{2kn}{\mathrm{sgne}}_{\mathrm{\&tau;}R}\end{array}\right.---\left(18\right)$

Wherein, k₅, k₆For constant coefficient, k₅＞ 0, k₆＞ 0；

Utilize the determination of stability condition of Lyapunov function, have

Further, and if only if e_p=e_η=e_τWhen=0,Formula (18) is understood permissible by Lyqpunov theorem Make system progressively stable.

Preferably, described kinematics model needs to make the following assumptions when setting up:

1) what four driving wheels were symmetrical is distributed in same plane；

2) it is point cantact between driving wheel and ground, ignores thickness；

3) radius when car body turns to is more than wheel radius；

4) four driving wheels will not produce longitudinal slip with ground when relative motion；

5) robot body regards the rigid body of motion on wheel as, and moves the most in the plane.

Preferably, during described kinematics model, need to provide system model between local coordinate system and global coordinate system Mutual transforming relationship.

Preferably, during described kinematics model, owing to the lateral velocity of this wheeled mobile robot four wheels is typically Zero, when robot turns to, its frame for movement determines that its outside sliding is necessary.Therefore to complete the motion of this design Learn model, introduce the nonholonomic constraint of Pfaffian form.

Preferably, when described kinetic model is set up, have ignored the interference of gravity and extraneous factor.

Preferably, when described driving motor model is set up, in order to simplify theory analysis, it is assumed that the driving motor of four-wheel is all adopted With direct current generator and the motor driver of identical parameters, and there is identical speed reducing ratio.

Preferably, use Backstepping robust trajectory tracking control method 4 wheel driven wheeled mobile robot is carried out track with Track controls.

Preferably, before utilizing the robust trajectory tracking control strategy design control law of Backstepping, supposition system is needed The gain of uncertain disturbance the unknown dynamic process be bounded, then it will be assumed that the mathematical model of bound function and controlled device Combine one liapunov function of structure, and this function can make system for the either element in uncertain disturbance set all There is robustness.

The present invention is by the design of the robust trajectory tracking control method of Backstepping in master controller, it is achieved that in complexity Improve the purpose of stability of 4 wheel driven Control of Wheeled Mobile Robots system under uncertain environment, improve containing uncertainty because of The effectiveness that under the conditions of element, system controls.

Accompanying drawing explanation

The 4 wheel driven wheeled mobile robot trace tracking control system structured flowchart that Fig. 1 provides for the present embodiment；

The 4 wheel driven wheeled mobile robot trace tracking and controlling method flow chart that Fig. 2 provides for the present embodiment；

Fig. 3 is that PID controls track following result figure；

Fig. 4 is the track following result figure of Trajectory Tracking Control method of the present invention；

Fig. 5 is that PID controls pose angle tracking error figure；

Fig. 6 is the pose angle tracking error figure of Trajectory Tracking Control method of the present invention；

Fig. 7 is PID control line speed Tracking Error Graph；

Fig. 8 is the linear velocity tracking error figure of Trajectory Tracking Control method of the present invention.

Detailed description of the invention

Below in conjunction with specific embodiment, the present invention is expanded on further.Should be understood that these embodiments are merely to illustrate the present invention Rather than restriction the scope of the present invention.In addition, it is to be understood that after having read the content that the present invention lectures, people in the art The present invention can be made various changes or modifications by member, and these equivalent form of values fall within the application appended claims equally and limited Scope.

The 4 wheel driven wheeled mobile robot trace tracking control system structured flowchart that Fig. 1 provides for the present embodiment, described 4 wheel driven wheeled mobile robot trace tracking control system includes:

Kinematic controller 103, on the basis of system kinematics model 108, according to the shape of given reference locus 101 State adjusts the speed of system；

Dynamics Controller 104, on the basis of system dynamics model 107, obtains according to the speed of required adjustment system Go out the expectation moment of motor；

Drive motor controller 105, on the basis of system drive motor model 106, for meeting the expectation moment of motor, The suitable driving voltage of system of designing.

According to assumed condition, the longitudinal sliding motion between car body and ground is negligible, and has v_ix=r ω_i, wherein, v_ix For i-th wheel general speed vector v_iLongitudinal component.Consider the running status of four wheels, mutual between each wheel Relation is

$\left\{\begin{array}{c}{v}_L=v_{1x}=v_{2x}\\ v_R=v_{3x}=v_{4x}\\ v_F=v_{1y}=v_{4y}\\ v_B=v_{2y}=v_{3y}\end{array}\right.---\left(19\right)$

Wherein, v_LFor the longitudinal velocity of left side wheels, v_RFor the longitudinal velocity of right-hand wheel, v_FFor the lateral velocity of front side wheel, v_B Lateral velocity for rear side wheel.v_1xIt it is the general speed vector v of first wheel_xLongitudinal component, v_2xIt is the total of second wheel Velocity vector v_xLongitudinal component, v_3xIt it is the general speed vector v of the 3rd wheel_xLongitudinal component, v_4xIt it is four wheels General speed vector v_xLongitudinal component, v_1yIt it is the general speed vector v of first wheel_yCross stream component, v_2yIt is second wheel General speed vector v_yCross stream component, v_3yIt it is the general speed vector v of the 3rd wheel_yCross stream component, v_4yIt is the 4th wheel The general speed vector v of son_yCross stream component.

So two wheels, two, the right side wheels component in x-axis phase respectively on the left of this 4 wheel driven wheeled mobile robot With, two, front side wheel, two wheels of rear side component on the y axis are the most identical.This wheeled mobile robot four wheels Lateral velocity be typically zero, when robot turns to, its frame for movement determines that its outside sliding is necessary.Introduce The nonholonomic constraint of Pfaffian form, the most permissible kinematics model going out systemSo by changing the big of η Urine can realize 4 wheel driven wheeled mobile robot poseControl.

What kinetic model 107 described is the relation between its speed with corresponding motor output driving moment, by one As wheeled mobile robot kinetic model

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+V(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+F\left(\stackrel{\&CenterDot;}{q}\right)+G\left(q\right)+{\mathrm{\&tau;}}_d=B\left(q\right)\mathrm{\&tau;}-A^T\left(q\right)\mathrm{\&lambda;}\\ A\left(q\right)\stackrel{\&CenterDot;}{q}=0\end{array}\right.$

Coupling system kinematics modelAnd to its derivation, in the situation ignoring gravity and the interference of other factors Under just can draw the kinetic model of 4 wheel driven wheeled mobile robot

$\stackrel{\&CenterDot;}{\mathrm{\&eta;}}=\stackrel{-}{M^{-1}}\left(q\right)\&lsqb;\stackrel{\&OverBar;}{B}\left(q\right)\mathrm{\&tau;}-\stackrel{\&OverBar;}{V}(q,\stackrel{\&CenterDot;}{q})\mathrm{\&eta;}-\stackrel{\&OverBar;}{F}(q,\stackrel{\&CenterDot;}{q})-{\mathrm{\&tau;}}_d\&rsqb;$

Motor model 106 is driven to describe the relation between moment and the motor signal of telecommunication exported on driving wheel.System control Device processed is built upon comprising on the mathematical model basis driving motor, in order to simplify theory analysis, it is assumed that the driving electricity of four-wheel Machine all uses direct current generator and the motor driver of identical parameters, and has identical speed reducing ratio n.In order to make the car body left and right sides Driving wheel there is identical velocity of rotation, it is necessary to make the moment being input on two driving wheels of left and right sides identical, therefore may be used To draw the relation τ=[τ of output torque and the electric current driving motor_L τ_R]^T=[2kni_L 2kni_R]^T, in conjunction with the electricity of direct current generator Pressure balanced equation, just can draw the driving motor model of robot

Fig. 2 is the flow chart of Trajectory Tracking Control strategy embodiment of the present invention, according to the robust track following control of Backstepping System strategy, comprises the following steps:

Step one: 201 obtain given reference locusBy the foundation of system model is obtained accordingly Robot pose error e under coordinate system_q202；

Step 2: judge this position and attitude error whether as 0 203, if zero, then complete corresponding track following 207；Instead It, then need under the control of this law of progression, adjusts the input desired speed 204 of kinematics model；

Step 3: desired velocity conditions in meeting kinematics model, arranges suitable Torque Control rule [τ_Lr τ_Rr]^T205；

Step 4: for enabling a system to the stable realization tracking to setting track, need to meet system simultaneously and obtain the phase The speed hoped and moment.Therefore to meet the two condition simultaneously, need the voltage to driving motor to be set, obtain suitable When Control of Voltage rule.

Step 5: according to the design philosophy of Backstepping, forms a feedback system and makes when t → ∞, e_q=0, with reality The tracking to reference locus of the mobile robot on reality border.

According to the parameter designing of controller noted above, from original state (0,0), following the tracks of one section of track is Y=sin (0.2 π X) The emulation ginseng of sine wave, pose angle θ (0)=Orad, reference value 1m/s of linear velocity, 4 wheel driven wheeled mobile robot and controller Number is arranged as shown in table 1.

In order to demonstrate the effectiveness of this control strategy, by robust trajectory tracking control strategy based on Backstepping and PID Control compares, PID controller parameter k_p=3.2, k_i=0.6, k_d=0.36, wherein k_pFor proportionality coefficient, k_iFor integration Coefficient, k_iFor differential coefficient, comparative result such as Fig. 3 to Fig. 8.

Table 14 wheel driven wheeled mobile robot is arranged with the simulation parameter of controller

By regulatory PID control and robust control simulation comparison based on Backstepping, from Fig. 3 Yu Fig. 4 it can be seen that to together One track is tracked, and the maximum error that robust control based on Backstepping is followed the tracks of is much smaller compared to regulatory PID control；Contrast Fig. 5 Yu Fig. 6, Fig. 7 Yu Fig. 8 understand, and robust control based on Backstepping controls pose angle tracking maximum error than PID and reduces 0.18rad, linear velocity is followed the tracks of maximum error and is reduced 0.2m/s, and convergence time reduces 60%.From analysis above, right In same pursuit path, robust control based on Backstepping is more preferable than traditional PID control effect.

Claims (10)

1. a 4 wheel driven wheeled mobile robot trace tracking and controlling method, it is characterised in that: the method is by following 3 step groups Become:

Step 1: set up the kinematics model of system, kinetic model, driving motor model；

Step 2: design

Kinematic controller, on the basis of kinematics model, adjusts system according to the state of given reference locus Speed；

Dynamics Controller, on the basis of kinetic model, draws the phase of motor according to the speed of required adjustment system Hope moment；

Drive motor controller, for, on the basis of driving motor model, for meeting the expectation moment of motor, designing system Suitably driving voltage；

Step 3: use the robust trajectory tracking control method of Backstepping that 4 wheel driven wheeled mobile robot is carried out track following control System, detailed process is as follows:

Step 3.1: according to given reference locus, by the foundation of system model is obtained in corresponding coordinate Xi Xia robot position Appearance error；

Step 3.2: judge this position and attitude error whether as zero, if zero, then complete corresponding track following；Otherwise, adjust fortune The dynamic desired speed learning mode input；

Step 3.3: under conditions of meeting the desired speed of kinematics model, arranges suitable Torque Control rule；

Step 3.4: be set the voltage driving motor, obtains suitable Control of Voltage rule, makes the expectation speed that system obtains Degree and moment condition meet simultaneously.

Step 3.5: according to the design philosophy of Backstepping, forms a feedback system so that when t → ∞, t express time, position Appearance error is 0, to realize the tracking to reference locus of the actual mobile robot.

2. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 1, it is characterised in that: described Feedback system structure is: kinematic controller, Dynamics Controller, drive motor controller, driving motor model, kinetic simulation Type, kinematics model are sequentially connected with, and drive motor model output result to feed back to drive motor controller, and kinetic model exports Result feeds back to Dynamics Controller and drive motor controller, and kinematics model output result feeds back to Dynamics Controller, Kinematics model output result feeds back to Dynamics Controller, actual path and reference by forming actual path after integral element The position and attitude error input motion controller of track, reference locus is simultaneously as feed-forward signal input motion controller.

3. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 1, it is characterised in that: described The method for building up of kinematics model is as follows:

Kinematics model is for the relation between descriptive system speed and its pose:

The nonholonomic constraint of Pfaffian form, such as formula (1):

$\&lsqb;\begin{array}{ccc}\sin \mathrm{\&theta;}& -cos\mathrm{\&theta;}& 0\end{array}\&rsqb;\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ \stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]=A\left(q\right)\stackrel{\&CenterDot;}{q}=0---\left(1\right)$

Wherein, θ is the angle between dolly transverse axis and the X-axis of inertial coodinate system, and it can represent the pose angle of robot, A (q) =[sin θ-cos θ 0],It it is the pose of system；

Then the mutually conversion between local coordinate system xoy and inertial coodinate system XoY of the system pose is such as formula (2):

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ \stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\&theta;}& \sin \mathrm{\&theta;}& 0\\ -\sin \mathrm{\&theta;}& cos\mathrm{\&theta;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \mathrm{\&omega;}\end{array}\right]---\left(2\right)$

Wherein,For robot pose under local coordinate system, v_xFor robot speed's component in x-axis, v_yFor machine Device people's speed component on the y axis, ω is the angular velocity of robot；

Just the kinematics model of system can be released on the basis of the nonholonomic constraint and system model of Pfaffian form, Such as formula (3):

$\stackrel{\&CenterDot;}{q}=S\left(q\right)\mathrm{\&eta;}---\left(3\right)$

$\begin{array}{cc}S\left(q\right)=\left[\begin{array}{cc}cos\mathrm{\&theta;}& 0\\ \sin \mathrm{\&theta;}& 0\\ 0& 1\end{array}\right]& \mathrm{\&eta;}=\left[\begin{array}{c}v\\ \mathrm{\&omega;}\end{array}\right]\end{array}$

Wherein, v is the linear velocity of robot wheel, and ω is the angular velocity of robot.

4. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 2, it is characterised in that: described The method for building up of kinetic model is as follows:

The kinetic model of general wheeled mobile robot, such as formula (4):

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+V(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+F\left(\stackrel{\&CenterDot;}{q}\right)+G\left(q\right)+{\mathrm{\&tau;}}_d=B\left(q\right)\mathrm{\&tau;}-A^T\left(q\right)\mathrm{\&lambda;}\\ A\left(q\right)\stackrel{\&CenterDot;}{q}=0\end{array}\right.---\left(4\right)$

Wherein, M (q) is the inertial matrix that positive definite is symmetrical；For relevant with speed with position Ge Shi moment battle array is with centripetal Power；For frictional force；G (q) is gravity item；τ_dFor including destructuring Unmarried pregnancy bounded unknown disturbance vector；B(q) For Input transformation matrix；τ=[τ_L τ_R]^TFor torque input vector, in some cases, the torque with driving motor is equal；λ is Restraining forces vector, beSpecial built-in variable；

By to formula (3) derivation, formula (5) can be obtained as follows:

$\stackrel{\&CenterDot;\&CenterDot;}{q}=\stackrel{\&CenterDot;}{S}\left(q\right)\mathrm{\&eta;}+S\left(q\right)\stackrel{\&CenterDot;}{\mathrm{\&eta;}}---\left(5\right)$

The uncertainty assuming system dynamics model is bounded, and meets

|τ_vd|≤ρ_v(τ)

|τ_ωd|≤ρ_ω(τ)

τ_vdFor the uncertain parameter of linear velocity, τ_ωdFor the uncertain parameter of angular velocity, ρ_v(τ)、ρ_ω(τ) bounded constant value it is；

In the case of ignoring gravity and the interference of other factors, utilize formula (4) and (5) that wheeled mobile robot can be drawn Kinetic model formula (6) is as follows:

$\stackrel{\&CenterDot;}{\mathrm{\&eta;}}=\stackrel{-}{M^{-1}}\left(q\right)\&lsqb;\stackrel{\&OverBar;}{B}\left(q\right)\mathrm{\&tau;}-\stackrel{\&OverBar;}{V}(q,\stackrel{\&CenterDot;}{q})\mathrm{\&eta;}-\stackrel{\&OverBar;}{F}(q,\stackrel{\&CenterDot;}{q})-{\mathrm{\&tau;}}_d\&rsqb;---\left(6\right)$

Wherein,

$\stackrel{\&OverBar;}{M}\left(q\right)=S^T\left(q\right)M\left(q\right)S\left(q\right)$

$\stackrel{\&OverBar;}{V}(q,\stackrel{\&CenterDot;}{q})=S^T\left(q\right)\&lsqb;M\left(q\right)\stackrel{\&CenterDot;}{S}\left(q\right)+V(q,\stackrel{\&CenterDot;}{q})S\left(q\right)\&rsqb;$

$\stackrel{\&OverBar;}{F}(q,\stackrel{\&CenterDot;}{q})=S^T\left(q\right)\&lsqb;F\left(\stackrel{\&CenterDot;}{q}\right)+A^T\left(q\right)\mathrm{\&lambda;}\&rsqb;$

$\stackrel{\&OverBar;}{B}\left(q\right)=S^T\left(q\right)B\left(q\right)$

And then it follows that

$\begin{array}{cccc}\stackrel{\&OverBar;}{M}\left(q\right)=\left[\begin{array}{cc}m& 0\\ 0& 1\end{array}\right]& \stackrel{\&OverBar;}{V}(q,\stackrel{\&CenterDot;}{q})=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]& \stackrel{\&OverBar;}{F}(q,\stackrel{\&CenterDot;}{q})=\left[\begin{array}{c}{F}_{rx}\left(\stackrel{\&CenterDot;}{q}\right)\\ M_r\left(\stackrel{\&CenterDot;}{q}\right)\end{array}\right]& \stackrel{\&OverBar;}{B}\left(q\right)=\frac{1}{r}\left[\begin{array}{cc}1& 1\\ -b& b\end{array}\right]\end{array}$

Wherein,

$F_{rx}\left(\stackrel{\&CenterDot;}{q}\right)=f_r\frac{mg}{2}\left(\mathrm{sgn}\right(\stackrel{\&CenterDot;}{x}-b\stackrel{\&CenterDot;}{\mathrm{\&theta;}})+\mathrm{sgn}(\stackrel{\&CenterDot;}{x}+b\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left)\right)$

$\begin{array}{c}{M}_r\left(\stackrel{\&CenterDot;}{q}\right)=\mathrm{\&mu;}\frac{bmg}{2}\left(\mathrm{sgn}\right(\stackrel{\&CenterDot;}{y}-b\stackrel{\&CenterDot;}{\mathrm{\&theta;}})-\mathrm{sgn}(\stackrel{\&CenterDot;}{y}+b\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left)\right)\\ +f_r\frac{bmg}{2}\left(\mathrm{sgn}\right(\stackrel{\&CenterDot;}{x}-b\stackrel{\&CenterDot;}{\mathrm{\&theta;}})-\mathrm{sgn}(\stackrel{\&CenterDot;}{x}+b\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left)\right)\end{array}$

B is the half of left and right wheels spacing, and r is radius of wheel, and m is the quality of car body, and g is gravity constant.

5. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 3, it is characterised in that: described The method for building up driving motor model is as follows:

The output torque tau of driving motor and the relation such as formula (7) of electric current:

τ=[τ_L τ_R]^T=[2kni_L 2kni_R]^T (7)

Wherein, k is constant coefficient, and n is the speed reducing ratio of motor, i_L, i_RIt is respectively the electric current of left and right sides motor；

Consider that under systematic parameter uncertainty and disturbed condition, the balance of voltage equation such as formula (8) of direct current generator is shown:

$U=(L+\mathrm{\&Delta;}L)\frac{di}{dt}+(R+\mathrm{\&Delta;}R)i+(k_e+{\mathrm{\&Delta;k}}_e)\mathrm{\&omega;}+d\left(t\right)---\left(8\right)$

Wherein, U is armature voltage, and L is armature inductance, and R is armature resistance, k_eFor motor torque constant, Δ represents the most true of parameter Qualitative, d (t) represents uncertain interference；

Utilize formula (7) and (8), just can show that the mathematical model formula (9) driving motor is as follows:

$\stackrel{\&CenterDot;}{\mathrm{\&tau;}}=\stackrel{\&OverBar;}{L}U-\stackrel{\&OverBar;}{R}\mathrm{\&tau;}-\stackrel{\&OverBar;}{K}\mathrm{\&eta;}+D---\left(9\right)$

Wherein,U=[U_L U_R]^T

D=[D_L D_R]^T=Δ LU-Δ R τ-Δ K η+d (t) is the uncertainty that system is total.

6. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 4, it is characterised in that: track Specifically comprising the following steps that of tracing control

The reference locus of step A, first initialization system

Wherein, q_r=[x_r y_r θ_r]^TFor reference locus, η_r=[v_r ω_r]^T, v_rFor reference line speed, ω_rFor reference angular velocities；

Formula (2) is utilized to draw the error e of pose under local coordinate_q, such as formula (10):

$e_q=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\mathrm{\&theta;}}\end{array}\right]=T_e(q_r-q)=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& 0\\ -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\&theta;}}_r-\mathrm{\&theta;}\end{array}\right]---\left(10\right)$

Consider the systematic parameter of this 4 wheel driven wheeled mobile robot, position and attitude error is carried out data process；By Backstepping Design philosophy, for this kinematics model subsystem design Lyapunov function, it is assumed that the Liapunov of kinesiology control law Function is

$V_1=\frac{1}{2}{e}_x^2+\frac{1}{2}{e}_y^2+1-\cos e_{\mathrm{\&theta;}}---\left(11\right)$

By the data of formula (11) are processed, it can be deduced that make in the case of t → ∞ (t is the time), e_qDesign when=0 Velocity control law be

$\begin{array}{c}v=V_r{\mathrm{cose}}_{\mathrm{\&theta;}}+k_1{e}_x\\ \mathrm{\&omega;}=\stackrel{\&CenterDot;}{{\mathrm{\&theta;}}_r}+e_y{V}_r+k_2\sin e_{\mathrm{\&theta;}}\end{array}---\left(12\right)$

Wherein, k₁, k₂For constant coefficient, k₁＞ 0, k₂＞ 0；

Utilize the determination of stability condition of Lyapunov function, have

Step B, exports the formula (12) in Motion Controlling Model as desired speed, is denoted as [v_r ω_r]^T, then kinetics Tracking error is e_η=[e_v e_w]^T=[v_r-v w_r-w]^T, it is assumed that the liapunov function of definition Dynamics Controller is

$V_2=V_1+\frac{1}{2}{e}_v^2+\frac{1}{2}{e}_w^2---\left(13\right)$

By processing the data of formula (12), setting Torque Control rule is

$\left\{\begin{array}{c}{\mathrm{\&tau;}}_L=\frac{mr}{2}(\stackrel{\&CenterDot;}{v_r}+k_3{e}_v-\frac{1}{m}{\mathrm{\&rho;}}_v(\mathrm{\&tau;}\left)\mathrm{sgn}{e}_v\right)-\frac{Ir}{2b}(\stackrel{\&CenterDot;}{{\mathrm{\&omega;}}_r}+k_4{e}_{\mathrm{\&omega;}}-\frac{1}{I}{\mathrm{\&rho;}}_{\mathrm{\&omega;}}(\mathrm{\&tau;}\left)\mathrm{sgn}{e}_{\mathrm{\&omega;}}\right)\\ {\mathrm{\&tau;}}_R=\frac{mr}{2}(\stackrel{\&CenterDot;}{v_r}+k_3{e}_v-\frac{1}{m}{\mathrm{\&rho;}}_v(\mathrm{\&tau;}\left)\mathrm{sgn}{e}_v\right)+\frac{Ir}{2b}(\stackrel{\&CenterDot;}{{\mathrm{\&omega;}}_r}+k_4{e}_{\mathrm{\&omega;}}-\frac{1}{I}{\mathrm{\&rho;}}_{\mathrm{\&omega;}}(\mathrm{\&tau;}\left)\mathrm{sgn}{e}_{\mathrm{\&omega;}}\right)\end{array}\right.---\left(14\right)$

Wherein, k₃, k₄For constant coefficient, k₃＞ 0, k₄＞ 0；

Utilize the determination of stability condition of Lyapunov function, have

Step C, assumes initially that driving the uncertain of motor is bounded, and does slowly varying, i.e.

$\begin{array}{cc}\left|D\right|<{\mathrm{\&rho;}}_D\left(t\right)& \stackrel{\&CenterDot;}{D}=0\end{array}---\left(15\right)$

ρ_DT () is bounded constant value；

Using the output formula (14) of Dynamics Controller as the expectation moment of system, it is denoted as [π_Lr τ_Rr]^T, driving torque error For

e_τ=[e_τL e_τR]^T

=[τ_Lr-τ_L τ_Rr-τ_R]^T (16)

The liapunov function of definition electric machine controller is

$V_3=V_2+\frac{1}{2}{e}_{\mathrm{\&tau;}L}^2+\frac{1}{2}{e}_{\mathrm{\&tau;}R}^2---\left(17\right)$

By processing the data of formula (17), setup control rule is

$\left\{\begin{array}{c}{U}_L=\frac{{\mathrm{R\&tau;}}_L}{2kn}+k_{e}v-k_{e}b\mathrm{\&omega;}+\stackrel{\&CenterDot;}{{\mathrm{\&tau;}}_{Lr}}+k_5{e}_{\mathrm{\&tau;}L}-\frac{{\mathrm{L\&rho;}}_D\left(t\right)}{2kn}\mathrm{sgn}{e}_{\mathrm{\&tau;}L}\\ U_R=\frac{{\mathrm{R\&tau;}}_R}{2kn}+k_{e}v-k_{e}b\mathrm{\&omega;}+\stackrel{\&CenterDot;}{{\mathrm{\&tau;}}_{Rr}}+k_5{e}_{\mathrm{\&tau;}R}-\frac{{\mathrm{L\&rho;}}_D\left(t\right)}{2kn}\mathrm{sgn}{e}_{\mathrm{\&tau;}R}\end{array}\right.---\left(18\right)$

Wherein, k₅, k₆For constant coefficient, k₅＞ 0, k₆＞ 0；

Utilize the determination of stability condition of Lyapunov function, have

Further, and if only if e_p=e_η=e_τWhen=0,Understood formula (18) by Lyapunov theorem and can make system Progressively stable.

7. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 2, it is characterised in that: described When kinematics model is set up, need to make the following assumptions:

1) what four driving wheels were symmetrical is distributed in same plane；

2) it is point cantact between driving wheel and ground, ignores thickness；

3) radius when car body turns to is more than wheel radius；

4) four driving wheels will not produce longitudinal slip with ground when relative motion；

5) robot body regards the rigid body of motion on wheel as, and moves the most in the plane.

8. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 3, it is characterised in that: described When kinetic model is set up, have ignored the interference of gravity and extraneous factor.

9. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 4, it is characterised in that: described When driving motor model to set up, in order to simplify theory analysis, it is assumed that the motor that drives of four-wheel all uses the unidirectional current of identical parameters Machine and motor driver, and there is identical speed reducing ratio.

10. a kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method as claimed in claim 9, it is characterised in that: Before design control law, the gain needing uncertain disturbance the unknown dynamic process of supposition system is bounded, then it will be assumed that Bound function combine with the mathematical model of controlled device structure one liapunov function, this function can make system for Either element in uncertain disturbance set all has robustness.