CN105867130A - Trail tracking error constraint safety control method for rehabilitation walk training robot - Google Patents

Abstract

The invention belongs to the field of control over wheel-type rehabilitation robots, and particularly relates to a trail tracking error constraint safety control method for a rehabilitation walk training robot. According to the trail tracking error constraint safety control method for the rehabilitation walk training robot, in the moving process of the rehabilitation walk training robot, trail tracking errors at a transient state phase and a steady state phase are constrained simultaneously. Based on a dynamic model of the rehabilitation walk training robot, a nonlinear controller is designed, a trail tracking error state equation in three motion directions, namely the x-axis direction, the y-axis direction and the rotary angle direction, is established; based on the Lyapunov stability theory, constraint conditions for asymptotic stability of all trail tracking error systems are constructed, a solving method for a nonlinear controller parameter matrix is obtained, the actual motion trail of the rehabilitation walk training robot at the transient state phase and the steady state phase is controlled within a specified range, and the safety of a rehabilitee is guaranteed.

Description

The track following error constraints method of controlling security of rehabilitation ambulation training robot

Technical field

The invention belongs to the control field of wheeled healing robot, particularly relate to a kind of rehabilitation ambulation training machine The track following error constraints method of controlling security of people.

Background technology

Walking is one of the mankind's daily self-support important basic activity of life, along with population at advanced age increases, old People's leg muscle strength gradually weakens, if strengthening old people's ambulation training not in time, can cause walking-function Gradually lose, it is impossible to realize daily living on one's own life.Owing to China lacks care-giving professional, family is blue or green simultaneously How year population decline, help old people to recover the social problem that walking movement function becomes serious.Therefore, Development rehabilitation ambulation training robot, helps old people to carry out ambulation training safely significant.

Rehabilitation ambulation training robot needs to follow the tracks of the track that doctor specifies and is trained patient, relevant rehabilitation The existing many achievements in research of robotic tracking control method, but these achievements all have ignored transient period Tracking performance.Robot operates in indoor circumstances not known, if transient period track following error is excessive, Robot can collide people around or object, threatens the safety of trainer.Existing result of study only only accounts for machine The tracking performance of device people's steady-state process, causes these control methods to be respectively provided with certain limitation in actual applications Property.Up to the present, retrain transient period and the controlling party of steady-state process track following error the most simultaneously Method.How the method for controlling security of research track tracking error of the present invention constraint, retrain rehabilitation ambulation training machine Device people track following error in whole motor process, ensures that the safety of user is significant.

Summary of the invention

The present invention is aiming at the problems referred to above, it is provided that one make rehabilitation ambulation training robot in motor process, The track of transient period and steady-state process track following error the most affined rehabilitation ambulation training robot with Track error constraints method of controlling security.

For achieving the above object, the present invention adopts the following technical scheme that, the present invention comprises the following steps:

Step 1) set up x, y-axis and the track following error state equation of three directions of motion of the anglec of rotation, system Kinematics model as follows:

$M_{0}K\left(\mathrm{\θ}\right)\stackrel{\·\·}{X}\left(t\right)+M_0\stackrel{\·}{K}(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{X}\left(t\right)=B\left(\mathrm{\θ}\right)u\left(t\right)---\left(1\right)$

Wherein

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+{{\mathrm{mr}}_0}^2\end{array}\right],K\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}1& 0& p\\ 0& 1& q\\ 0& 0& 1\end{array}\right],X\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],$

$B\left(\mathrm{\θ}\right)=\left[\begin{array}{cccc}-{\mathrm{sin\θ}}_1& {\mathrm{sin\θ}}_2& {\mathrm{sin\θ}}_3& -{\mathrm{sin\θ}}_4\\ {\mathrm{cos\θ}}_1& -{\mathrm{cos\θ}}_2& {\mathrm{cos\θ}}_3& {\mathrm{cos\θ}}_4\\ {\mathrm{\λ}}_1& -{\mathrm{\λ}}_2& -{\mathrm{\λ}}_3& {\mathrm{\λ}}_4\end{array}\right],u\left(t\right)=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right],$

$\begin{array}{c}{\mathrm{\λ}}_1=l_1\cos ({\mathrm{\θ}}_1-{\mathrm{\φ}}_1)\\ {\mathrm{\λ}}_2=l_2\cos ({\mathrm{\θ}}_2-{\mathrm{\φ}}_2)\\ {\mathrm{\λ}}_3=l_3\cos ({\mathrm{\θ}}_3-{\mathrm{\φ}}_3)\\ {\mathrm{\λ}}_4=l_4\cos ({\mathrm{\θ}}_4-{\mathrm{\φ}}_4)\end{array},\begin{array}{c}p=\frac{1}{2}\[({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\sin \mathrm{\θ}+({\mathrm{\λ}}_2-{\mathrm{\λ}}_4)\cos \mathrm{\θ}\]\\ q=\frac{1}{2}\[({\mathrm{\λ}}_2-{\mathrm{\λ}}_4)\sin \mathrm{\θ}-({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\cos \mathrm{\θ}\]\end{array}$

X (t) is the actual run trace of rehabilitation ambulation training robot, and u (t) represents and controls input power, and M represents health The quality of multiple ambulation training robot, m represents the quality of rehabilitation clients, I₀Represent rotary inertia,For coefficient matrix；θ represents trunnion axis and robot center and first wheel line of centres Between angle, θ=θ₁, according to rehabilitation walking robot structure,θ₃=θ+π, l_iRepresent that system gravity is to each distance taking turns subcenter, r₀Expression center is to the distance of center of gravity, φ_iRepresent x ' axle The l corresponding with each wheel_iBetween angle, i=1_,2_,3_,4；f₁,f₂,f₃,f₄Represent the control of four driving wheels respectively Input power processed, λ_1,λ_2,λ_3,λ₄Represent the center of gravity distance to each wheel, φ respectively_1,φ_2,φ_3,φ₄Represent that level is sat respectively Parameter and center of gravity are to the angle of each wheel line of centres；

Step 2) actual run trace X (t) of rehabilitation ambulation training robot, doctor specifies training track X_d(t), Movement locus and movement velocity tracking error e₁(t) and e₂T () is respectively

e₁(t)=X (t)-X_d(t) (2)

$e_2\left(t\right)=\stackrel{\·}{X}\left(t\right)-{\stackrel{\·}{X}}_d\left(t\right)---\left(3\right)$

Wherein X (t)=[x (t) y (t) θ (t)]^TExpression x, the actual motion track in y-axis and anglec of rotation direction, X_d(t)=[x_d(t) y_d(t) θ_d(t)]^TRepresent x, the expectation pursuit movement track in y-axis and anglec of rotation direction； e₁(t)=[e₁₁(t) e₁₂(t) e₁₃(t)]^TExpression x, the track following error in y-axis and anglec of rotation direction, e₂(t)=[e₂₁(t) e₂₂(t) e₂₃(t)]^TRepresent x, y-axis direction speed and the tracking error of angular velocity of rotation；

Step 3) gamma controller is set:

Wherein K_d=diag{K_d1,K_d2,K_d3}’K_p=diag{K_p1,K_p2,K_p3}’Represent the puppet of B (θ) Inverse matrix, K_d,K_pRepresent controller parameter matrix；

Controller formula (4) is substituted into system model formula (1),

${\stackrel{\·\·}{e}}_1\left(t\right)=K_d{e}_2\left(t\right)+K_p{e}_1\left(t\right)---\left(5\right)$

X, y-axis and anglec of rotation direction track following error system state equation can be obtained by formula (2), (3), (5) For

$\begin{array}{c}{\stackrel{\·}{e}}_{1i}\left(t\right)=e_{2i}\left(t\right)\\ {\stackrel{\·}{e}}_{2i}\left(t\right)=K_{di}{e}_{2i}\left(t\right)+K_{pi}{e}_{1i}\left(t\right)\end{array},i=1,2,3---\left(6\right)$

Step 4) Lyapunov function is set:

$\begin{array}{c}V\left(t\right)=V_1\left(t\right)+V_2\left(t\right)+V_3\left(t\right)\\ =\frac{1}{2}{e}_1^T\left(t\right){\mathrm{Pe}}_1\left(t\right)+\frac{1}{2}{e}_2^T\left(t\right){\mathrm{Qe}}_2\left(t\right)\end{array}---\left(7\right)$

Wherein

$V_i\left(t\right)=\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)---\left(8\right)$

Along track following error system (6) to formula (8) derivation, when following constraints is set up,

$\left[\begin{array}{cccc}{Q}_{ii}{K}_{di}& Q_{ii}{K}_{pi}& 0& 0\\ P_{ii}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& {\mathrm{\ϵ}}_i& 0\end{array}\right]\≤0---\left(9\right)$

Have

${\stackrel{\·}{V}}_i\≤-{\mathrm{\ϵ}}_i\left|e_{2i}\left(t\right)\right|---\left(10\right)$

Set up, i.e. x, the track following error system Asymptotic Stability of y-axis and three directions of motion of the anglec of rotation, wherein ε_i Represent the little positive number being arbitrarily designated；

Step 5) to formula (10) two ends from 0 to t integration,

${\∫}_0^t{\stackrel{\·}{V}}_i\left(t\right)dt\≤{\∫}_0^t(-{\mathrm{\ϵ}}_i|e_{2i}\left(t\right)\left|\right)dt---\left(11\right)$

Arrange further

V_i(t)-V_i(0)≤-ε_i(|e_1i(t)|-|e_1i(0)|) (12)

When following condition is set up

$\left[\begin{array}{ccc}-\frac{1}{2}{Q}_{ii}& 0& 0\\ 0& -\frac{1}{2}{P}_{ii}& 0\\ 0& 0& V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\end{array}\right]\≤0---\left(13\right)$

Following formula is had to set up

$-\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)-\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\≤0---\left(14\right)$

Obtained by formula (8), (14), (12)

${\mathrm{\ϵ}}_i\left|e_{1i}\left(t\right)\right|\≤{\mathrm{\ϵ}}_i^2+{\mathrm{\ϵ}}_i\left|e_{1i}\left(0\right)\right|---\left(15\right)$

Then have

|e_1i(t)|≤ε_i+|e_1i(0)| (16)

The safety movement scope of actual path X (t) of rehabilitation ambulation training robot is

|x(t)-x_d(t)|≤ε₁+|x(0)-x_d(0)| (17)

|y(t)-y_d(t)|≤ε₂+|y(0)-y_d(0)| (18)

|θ(t)-θ_d(t)|≤ε₂+|θ(0)-θ_d(0)| (19)。

As a kind of preferred version, output pwm signal is supplied to by the present invention based on MSP430 series monolithic Electric-motor drive unit, makes the track following error of rehabilitation ambulation training robot limit within the specified range；With MSP430 series monolithic is master controller, and the input of master controller connects motor speed measuring module, output connects motor Drive module；Motor-drive circuit is connected with direct current generator；Power-supply system gives each power electrical apparatus.

As another kind of preferred version, main controller controls method of the present invention is: read motor encoder The given control command signal X of feedback signal and master controller_d(t) andCalculate error signal；Root According to error signal, master controller calculates the controlled quentity controlled variable of motor according to predetermined control algolithm, gives motor and drives Moving cell, electric machine rotation drives wheel maintain Equilibrium and press specific mode motion.

Beneficial effect of the present invention.

Present invention kinetic model based on rehabilitation ambulation training robot, according to movement velocity and movement position Tracking error, designs gamma controller, sets up x, and the track following of y-axis and three directions of motion of the anglec of rotation is by mistake Difference state equation；Based on Lyapunov stable theory, build each track following error system asymptotically stable about Bundle condition, it is thus achieved that the method for solving of gamma controller parameter matrix, by rehabilitation ambulation training robot transient state The actual motion TRAJECTORY CONTROL of stage and steady-state process within the specified range, ensures rehabilitation clients's safety.

Binding kinetics model of the present invention, the gamma controller of design, x, y-axis and the anglec of rotation three can be set up The track following error state equation of the direction of motion；Respectively build three track following error systems asymptotic surely Fixed condition, on the basis of constrained trajectory tracking error, asks for the safety movement of rehabilitation ambulation training robot Position range.Controller of the present invention design is simple, it is easy to accomplish, controller not only makes rehabilitation ambulation training machine Device people realizes track following, and can active constraint actual motion track, realize temporarily in safety movement region State stage and the tracking performance of steady-state process, this control method can improve the safety of trainer.

Accompanying drawing explanation

The present invention will be further described with detailed description of the invention below in conjunction with the accompanying drawings.Scope is not It is limited only to the statement of herein below.

Fig. 1 is controller of the present invention work block diagram；

Fig. 2 is present system coordinate diagram；

Fig. 3 is MSP430 single-chip minimum system of the present invention；

Fig. 4 is master controller peripheral expansion circuit of the present invention；

Fig. 5 is hardware general principles circuit of the present invention.

In Fig. 2, xOy is fixed coordinate system, and x ' Cy ' is robot mechanism coordinate system, and G is robot and health Multiple person constitutes the center of gravity of man-machine system.

Detailed description of the invention

As it can be seen, general steps of the present invention is as follows:

1) according to rehabilitation ambulation training robot doctor specified the movement velocity of training track and movement position with Track error, designs gamma controller, binding kinetics model, sets up x, y-axis and three motion sides of the anglec of rotation To track following error state equation；

2) design Lyapunov function, builds each asymptotically stable constraints of track following error system, Obtain the method for solving of gamma controller parameter matrix, make rehabilitation ambulation training robot transient period and steady The actual motion profile constraints in state stage is within the specified range；

3) based on MSP430 series monolithic, output pwm signal is supplied to electric-motor drive unit, makes rehabilitation Ambulation training robot follows the tracks of the training track that doctor specifies in the range of home.

The present invention specifically comprises the following steps that

Step 1) kinetic model based on rehabilitation ambulation training robot, according to movement velocity and movement position Tracking error, sets up x, y-axis and the track following error state equation of three directions of motion of the anglec of rotation, system Kinematics model is as follows

$M_{0}K\left(\mathrm{\θ}\right)\stackrel{\·\·}{X}\left(t\right)+M_0\stackrel{\·}{K}(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{X}\left(t\right)=B\left(\mathrm{\θ}\right)u\left(t\right)---\left(1\right)$

Wherein

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+{{\mathrm{mr}}_0}^2\end{array}\right],K\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}1& 0& p\\ 0& 1& q\\ 0& 0& 1\end{array}\right],X\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],$

$B\left(\mathrm{\θ}\right)=\left[\begin{array}{cccc}-\sin {\mathrm{\θ}}_1& \sin {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_3& -\sin {\mathrm{\θ}}_4\\ \cos {\mathrm{\θ}}_1& -\cos {\mathrm{\θ}}_2& \cos {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_4\\ {\mathrm{\λ}}_1& {-\mathrm{\λ}}_2& {-\mathrm{\λ}}_3& {\mathrm{\λ}}_4\end{array}\right],u\left(t\right)=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right],$

$\begin{array}{c}{\mathrm{\λ}}_1=l_1\cos ({\mathrm{\θ}}_1-{\mathrm{\φ}}_1)\\ {\mathrm{\λ}}_2=l_2\cos ({\mathrm{\θ}}_2-{\mathrm{\φ}}_2)\\ {\mathrm{\λ}}_3=l_3\cos ({\mathrm{\θ}}_3-{\mathrm{\φ}}_3)\\ {\mathrm{\λ}}_4=l_4\cos ({\mathrm{\θ}}_4-{\mathrm{\φ}}_4)\end{array},\begin{array}{c}p=\frac{1}{2}\[({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\sin \mathrm{\θ}+({\mathrm{\λ}}_2-{\mathrm{\λ}}_4)\cos \mathrm{\θ}\]\\ q=\frac{1}{2}\[({\mathrm{\λ}}_2-{\mathrm{\λ}}_4)\sin \mathrm{\θ}-({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\cos \mathrm{\θ}\]\end{array}$

X (t) is the actual run trace of rehabilitation ambulation training robot, and u (t) represents and controls input power, and M represents health The quality of multiple ambulation training robot, m represents the quality of rehabilitation clients, I₀Represent rotary inertia,For coefficient matrix.θ represents trunnion axis and robot center and first wheel line of centres Between angle, i.e. θ=θ₁, from rehabilitation walking robot structure,θ₃=θ+π,l_iRepresent that system gravity is to each distance taking turns subcenter, r₀Expression center to the distance of center of gravity, φ_iRepresent the l that x ' axle is corresponding with each wheel_iBetween angle, i=1,2,3,4.

Step 2) kinetic model based on rehabilitation ambulation training robot, according to movement velocity and movement position Tracking error, sets up x, y-axis and the track following error state equation of three directions of motion of the anglec of rotation, and rehabilitation walks Actual run trace X (t) of row image training robot, doctor specifies training track X_d(t), if movement locus and motion speed Degree tracking error e₁(t) and e₂T () is respectively

e₁(t)=X (t)-X_d(t) (2)

$e_2\left(t\right)=\stackrel{\·}{X}\left(t\right)-{\stackrel{\·}{X}}_d\left(t\right)---\left(3\right)$

Wherein X (t)=[x (t) y (t) θ (t)]^TExpression x, the actual motion track in y-axis and anglec of rotation direction, X_d(t)=[x_d(t) y_d(t) θ_d(t)]^TRepresent x, the expectation pursuit movement track in y-axis and anglec of rotation direction. e₁(t)=[e₁₁(t) e₁₂(t) e₁₃(t)]^TExpression x, the track following error in y-axis and anglec of rotation direction, e₂(t)=[e₂₁(t) e₂₂(t) e₂₃(t)]^TRepresent x, y-axis direction speed and the tracking error of angular velocity of rotation.

Design gamma controller is as follows

Wherein K_d=diag{K_d1,K_d2,K_d3, K_p=diag{K_p1,K_p2,K_p3,Represent the puppet of B (θ) Inverse matrix.

Controller (4) is substituted into system model (1),

${\stackrel{\·\·}{e}}_1\left(t\right)=K_d{e}_2\left(t\right)+K_p{e}_1\left(t\right)---\left(5\right)$

X, y-axis and anglec of rotation direction track following error system state equation can be obtained by formula (2), (3), (5) For

$\begin{array}{c}{\stackrel{\·}{e}}_{1i}\left(t\right)=e_{2i}\left(t\right)\\ {\stackrel{\·}{e}}_{2i}\left(t\right)=K_{di}{e}_{2i}\left(t\right)+K_{pi}{e}_{1i}\left(t\right)\end{array},i=1,2,3---\left(6\right)$

Step 3) design Lyapunov function, build each track following error system asymptotically stable constraint bar Part, it is thus achieved that the method for solving of gamma controller parameter matrix, makes rehabilitation ambulation training robot transient period With the actual motion TRAJECTORY CONTROL of steady-state process within the specified range；Design Lyapunov function is as follows

$\begin{array}{c}V\left(t\right)=V_1\left(t\right)+V_2\left(t\right)+V_3\left(t\right)\\ =\frac{1}{2}{e}_1^T\left(t\right){\mathrm{Pe}}_1\left(t\right)+\frac{1}{2}{e}_2^T\left(t\right){\mathrm{Qe}}_2\left(t\right)\end{array}---\left(7\right)$

Wherein

$V_i\left(t\right)=\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)---\left(8\right)$

Along track following error system (6) to formula (8) derivation, when following constraints is set up,

$\left[\begin{array}{cccc}{Q}_{ii}{K}_{di}& Q_{ii}{K}_{pi}& 0& 0\\ P_{ii}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& {\mathrm{\ϵ}}_i& 0\end{array}\right]\≤0---\left(9\right)$

Have

${\stackrel{\·}{V}}_i\≤-{\mathrm{\ϵ}}_i\left|e_{2i}\left(t\right)\right|---\left(10\right)$

Set up, i.e. x, the track following error system Asymptotic Stability of y-axis and three directions of motion of the anglec of rotation, wherein ε_iRepresent the little positive number being arbitrarily designated.

Step 4) design Lyapunov function, build each track following error system asymptotically stable constraint bar Part, it is thus achieved that the method for solving of gamma controller parameter matrix, makes rehabilitation ambulation training robot transient period With the actual motion profile constraints of steady-state process within the specified range；It is characterized in that: to formula (10) two ends from 0 To t integration,

${\∫}_0^t{\stackrel{\·}{V}}_i\left(t\right)dt\≤{\∫}_0^t(-{\mathrm{\ϵ}}_i|e_{2i}\left(t\right)\left|\right)dt---\left(11\right)$

Arrange further

V_i(t)-V_i(0)≤-ε_i(|e_1i(t)|-|e_1i(0)|) (12)

When following condition is set up

$\left[\begin{array}{ccc}-\frac{1}{2}{Q}_{ii}& 0& 0\\ 0& -\frac{1}{2}{P}_{ii}& 0\\ 0& 0& V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\end{array}\right]\≤0---\left(13\right)$

Following formula is had to set up

$-\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)-\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\≤0---\left(14\right)$

Obtained by formula (8), (14), (12)

${\mathrm{\ϵ}}_i\left|e_{1i}\left(t\right)\right|\≤{\mathrm{\ϵ}}_i^2+{\mathrm{\ϵ}}_i\left|e_{1i}\left(0\right)\right|---\left(15\right)$

Then have

|e_1i(t)|≤ε_i+|e_1i(0)| (16)

Therefore, the safety movement scope of actual path X (t) of rehabilitation ambulation training robot is

|x(t)-x_d(t)|≤ε₁+|x(0)-x_d(0)| (17)

|y(t)-y_d(t)|≤ε₂+|y(0)-y_d(0)| (18)

|θ(t)-θ_d(t)|≤ε₂+|θ(0)-θ_d(0)| (19)

Step 5) based on MSP430 series monolithic, output pwm signal is supplied to electric-motor drive unit, make The track following error of rehabilitation ambulation training robot limits within the specified range, with MSP430 series monolithic For master controller, the input of master controller connects motor speed measuring module, output connects motor drive module；Motor drives Galvanic electricity road is connected with direct current generator；Power-supply system gives each power electrical apparatus.

Main controller controls method is the control life that the feedback signal reading motor encoder is given with master controller Make signal X_d(t) andCalculate error signal.According to error signal, master controller is according to predetermined control Algorithm processed calculates the controlled quentity controlled variable of motor, gives electric-motor drive unit, and electric machine rotation drives wheel to maintain self Balance and press specific mode motion.

The present invention solves the Security Control Problem of rehabilitation ambulation training manipulator trajectory tracking error constraints, base In kinetic model and gamma controller, setting up x, the track following of y-axis and three directions of motion of the anglec of rotation is by mistake Difference state equation.Each asymptotically stable constraints of track following error system is built by Lyapunov function, Thus solve controller parameter matrix, by rehabilitation ambulation training robot transient period and the reality of steady-state process Movement locus controls within the specified range, is effectively increased the safety of trainer.

It is understood that above with respect to the specific descriptions of the present invention, be merely to illustrate the present invention and be not subject to It is limited to the technical scheme described by the embodiment of the present invention, it will be understood by those within the art that, still The present invention can be modified or equivalent, to reach identical technique effect；Need are used as long as meeting Want, all within protection scope of the present invention.

Claims (3)

1. the track following error constraints method of controlling security of rehabilitation ambulation training robot, it is characterised in that bag Include following steps:

Step 1) set up x, y-axis and the track following error state equation of three directions of motion of the anglec of rotation, system Kinematics model as follows:

$M_{0}K\left(\mathrm{\θ}\right)\stackrel{\·\·}{X}\left(t\right)+M_0\stackrel{\·}{K}(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{X}\left(t\right)=B\left(\mathrm{\θ}\right)u\left(t\right)---\left(1\right)$

Wherein

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+{{\mathrm{mr}}_0}^2\end{array}\right],K\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}1& 0& p\\ 0& 1& q\\ 0& 0& 1\end{array}\right],X\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],$

$B\left(\mathrm{\θ}\right)=\left[\begin{array}{cccc}-{\mathrm{sin\θ}}_1& {\mathrm{sin\θ}}_2& {\mathrm{sin\θ}}_3& -{\mathrm{sin\θ}}_4\\ {\mathrm{cos\θ}}_1& -{\mathrm{cos\θ}}_2& {\mathrm{cos\θ}}_3& {\mathrm{cos\θ}}_4\\ {\mathrm{\λ}}_1& -{\mathrm{\λ}}_2& -{\mathrm{\λ}}_3& {\mathrm{\λ}}_4\end{array}\right],u\left(t\right)=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right],$

$\begin{array}{c}{\mathrm{\λ}}_1=l_1\cos ({\mathrm{\θ}}_1-{\mathrm{\φ}}_1)\\ {\mathrm{\λ}}_2=l_2\cos ({\mathrm{\θ}}_2-{\mathrm{\φ}}_2)\\ {\mathrm{\λ}}_3=l_3\cos ({\mathrm{\θ}}_3-{\mathrm{\φ}}_3)\\ {\mathrm{\λ}}_4=l_4\cos ({\mathrm{\θ}}_4-{\mathrm{\φ}}_4)\end{array},\begin{array}{c}p=\frac{1}{2}\[({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\sin \mathrm{\θ}+({\mathrm{\λ}}_2-{\mathrm{\λ}}_4)\cos \mathrm{\θ}\]\\ q=\frac{1}{2}\[({\mathrm{\λ}}_2-{\mathrm{\λ}}_3)\sin \mathrm{\θ}-({\mathrm{\λ}}_1-{\mathrm{\λ}}_3)\cos \mathrm{\θ}\]\end{array}$

X (t) is the actual run trace of rehabilitation ambulation training robot, and u (t) represents and controls input power, and M represents health The quality of multiple ambulation training robot, m represents the quality of rehabilitation clients, I₀Represent rotary inertia, M₀,K(θ),B (θ) is coefficient matrix；θ represents trunnion axis and robot center and first wheel line of centres Between angle, θ=θ₁, according to rehabilitation walking robot structure,θ₃=θ+π, l_iRepresent that system gravity is to each distance taking turns subcenter, r₀Expression center is to the distance of center of gravity, φ_iRepresent x ' axle The l corresponding with each wheel_iBetween angle, i=1,2,3,4；f₁,f₂,f₃,f₄Represent the control of four driving wheels respectively Input power processed, λ₁,λ₂,λ₃,λ₄Represent the center of gravity distance to each wheel, φ respectively₁,φ₂,φ₃,φ₄Represent that level is sat respectively Parameter and center of gravity are to the angle of each wheel line of centres；

Step 2) actual run trace X (t) of rehabilitation ambulation training robot, doctor specifies training track X_d(t), Movement locus and movement velocity tracking error e₁(t) and e₂T () is respectively

e₁(t)=X (t)-X_d(t) (2)

$e_2\left(t\right)=\stackrel{\·}{X}\left(t\right)-{\stackrel{\·}{X}}_d\left(t\right)---\left(3\right)$

Wherein X (t)=[x (t) y (t) θ (t)]^TExpression x, the actual motion track in y-axis and anglec of rotation direction, X_d(t)=[x_d(t) y_d(t) θ_d(t)] T represents x, the expectation pursuit movement track in y-axis and anglec of rotation direction； e₁(t)=[e₁₁(t) e₁₂(t) e₁₃(t)] T represents x, the track following error in y-axis and anglec of rotation direction, e₂(t)=[e₂₁(t) e₂₂(t) e₂₃(t)] T represents x, y-axis direction speed and the tracking error of angular velocity of rotation；

Step 3) gamma controller is set:

Wherein K_d=diag{K_d1,K_d2,K_d3, K_p=diag{K_p1,K_p2,K_p3,Represent the puppet of B (θ) Inverse matrix, K_d,K_pRepresent controller parameter matrix；

Controller formula (4) is substituted into system model formula (1),

${\stackrel{\·\·}{e}}_1\left(t\right)=K_d{e}_2\left(t\right)+K_p{e}_1\left(t\right)---\left(5\right)$

X, y-axis and anglec of rotation direction track following error system state equation can be obtained by formula (2), (3), (5) For

${\stackrel{\·}{e}}_{1i}\left(t\right)=e_{2i}\left(t\right)$

${\stackrel{\·}{e}}_{2i}\left(t\right)=K_{di}{e}_{2i}\left(t\right)+K_{pi}{e}_{1i}\left(t\right),i=1,2,3---\left(6\right)$

Step 4) Lyapunov function is set:

$\begin{array}{c}V\left(t\right)=V_1\left(t\right)+V_2\left(t\right)+V_3\left(t\right)\\ =\frac{1}{2}{e}_1^T\left(t\right){\mathrm{Pe}}_1\left(t\right)+\frac{1}{2}{e}_2^T\left(t\right){\mathrm{Qe}}_2\left(t\right)\end{array}---\left(7\right)$

Wherein

$V_i\left(t\right)=\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)---\left(8\right)$

Along track following error system (6) to formula (8) derivation, when following constraints is set up,

$\left[\begin{array}{cccc}{Q}_{ii}{K}_{di}& Q_{ii}{K}_{pi}& 0& 0\\ P_{ii}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& {\mathrm{\ϵ}}_i& 0\end{array}\right]\≤0---\left(9\right)$

Have

${\stackrel{\·}{V}}_i\≤-{\mathrm{\ϵ}}_i\left|e_{2i}\left(t\right)\right|---\left(10\right)$

Set up, i.e. x, the track following error system Asymptotic Stability of y-axis and three directions of motion of the anglec of rotation, wherein ε_iRepresent the little positive number being arbitrarily designated；

Step 5) to formula (10) two ends from 0 to t integration,

${\∫}_0^t{\stackrel{\·}{V}}_i\left(t\right)dt\≤{\∫}_0^t(-{\mathrm{\ϵ}}_i|e_{2i}\left(t\right)\left|\right)dt---\left(11\right)$

Arrange further

V_i(t)-V_i(0)≤-ε_i(|e_1i(t)|-|e_1i(0)|) (12)

When following condition is set up

$\left[\begin{array}{ccc}-\frac{1}{2}{Q}_{ii}& 0& 0\\ 0& -\frac{1}{2}{P}_{ii}& 0\\ 0& 0& V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\end{array}\right]\≤0---\left(13\right)$

Following formula is had to set up

$-\frac{1}{2}{e}_{2i}^T\left(t\right)Q_{ii}{e}_{2i}\left(t\right)-\frac{1}{2}{e}_{1i}^T\left(t\right)P_{ii}{e}_{1i}\left(t\right)+V_i\left(0\right)-{\mathrm{\ϵ}}_i^2\≤0---\left(14\right)$

Obtained by formula (8), (14), (12)

${\mathrm{\ϵ}}_i\left|e_{1i}\left(t\right)\right|\≤{\mathrm{\ϵ}}_i^2+{\mathrm{\ϵ}}_i\left|e_{1i}\left(0\right)\right|---\left(15\right)$

Then have

|e_1i(t)|≤ε_i+|e_1i(0)| (16)

The safety movement scope of actual path X (t) of rehabilitation ambulation training robot is

|x(t)-x_d(t)|≤ε₁+|x(0)-x_d(0)|

(17)

|y(t)-y_d(t)|≤ε₂+|y(0)-y_d(0)| (18)

|θ(t)-θ_d(t)|≤ε₂+|θ(0)-θ_d(0)| (19)。

The track following error constraints security control of rehabilitation ambulation training robot the most according to claim 1 Method, it is characterised in that output pwm signal is supplied to electric-motor drive unit based on MSP430 series monolithic, The track following error making rehabilitation ambulation training robot limits within the specified range；With MSP430 series monolithic Machine is master controller, and the input of master controller connects motor speed measuring module, output connects motor drive module；Motor Drive circuit is connected with direct current generator；Power-supply system gives each power electrical apparatus.

The track following error constraints security control of rehabilitation ambulation training robot the most according to claim 2 Method, it is characterised in that described main controller controls method is: read feedback signal and the master of motor encoder The control command signal X that controller is given_d(t) andCalculate error signal；According to error signal, main Controller calculates the controlled quentity controlled variable of motor according to predetermined control algolithm, gives electric-motor drive unit, and motor turns Dynamic drive wheel maintains Equilibrium and presses specific mode motion.