CN104842355A - Mixed-integer prediction control method for redundant space robot under obstacle avoidance restraint - Google Patents

Abstract

The invention provides a mixed-integer prediction control method for a redundant space robot under the obstacle avoidance restraint. The mixed-integer prediction control method for the redundant space robot under the obstacle avoidance restraint comprises the steps that a dynamical model of the freely-floating space robot is established, linearization is conducted on the dynamical model obtained in the first step based on the feedback linearization method, the control input of a linear system is calculated according to a described optimal control problem, and the control input of an original non-linear system is calculated according to the control input of the linear system so as to be used for controlling an original non-linear space robot system. According to the mixed-integer prediction control method for the redundant space robot under the obstacle avoidance restraint, the avoidance of the space robot to an obstacle in the obstacle environment when the space robot fulfill a task is described to the restraint under the optimum control problem. By considering the property that the obstacle avoidance restraint characteristic changes along with the change of the distance between a mechanical arm and an obstacle and systematically describing the priority of multiple restraints under the optimum control problem, the defect that according to an original method, the optimum control problem is infeasible under the restraints such as the obstacle avoidance restraints can be effectively overcome.

Description

Keep away the MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of barrier constraint

[technical field]

The present invention relates to a kind of control method for kinematic redundancy robot for space, particularly a kind of MIXED INTEGER forecast Control Algorithm of keeping away the lower redundant space robot of barrier constraint.

[background technology]

Robot for space plays an important role in On-orbit servicing, may be used for the maintenance of satellite in orbit, fuel adding, and the task such as to catch to inert satellite, space junk.But in complicated space environment, robot for space often needs when finishing the work to hide barrier.Wherein, kinematic redundancy robot for space is because avoiding kinematics/dynamic singularity, and avoiding barrier aspect has obvious advantage, is thus subject to the extensive concern of researcher.

Can realize when path planning hiding of barrier, these class methods attempt finding robot for space to arrive a secure path of expectation state by original state, complete hiding barrier ^{[1] [2] [3]}, but it is large to there is amount of calculation, and is often only applicable to the shortcomings such as static space environment, for motion or emergent barrier can not hide well.

In the control method that some redundant space robots are new, attempt by robot for space under complex environment to barrier hide be described as control under constraint, thus controlling to realize hiding barrier in the robot for space process of finishing the work simultaneously, there is amount of calculation little, and can the advantages such as dynamic barrier be hidden.

Model predictive control method originates from the chemical field of the seventies in last century, because there is significant advantage in constraint process, thus in the control and path planning of mechanical arm, attract the interest of many researchers in recent years, use the method to be retrained by the barrier of keeping away be described as under control problem of hiding of robot for space to barrier easily ^[4].But significantly not enough below also existing in the application process of the method: finally can be described as to hiding of barrier the optimal control problem that one has all multiple constraints based on model predictive control method implementation space robot, and the existence of constraint often causes optimal control problem infeasible, in existing method, to the optimization problem formed under model predictive control method, how to solve constraint and may cause the infeasible shortcoming of optimal control problem not good way.

Bibliography:

[1]Z.Shiller and S.Dubowsky,On Computing the Global Time-Optimal Motions of Robotic Manipulators in the Presence of Obstacles,IEEE Trans.Robotics and Automation,7(6),785-797(1991)。

[2]M.Galicki,Optimal Planning of a Collision-free Trajectory of Redundant Manipulators,Int.J.Robotics Research,11(6),549-559(1992)。

[3]M.Stilman,Global Manipulation Planning in Robot Joint Space with Task Constraints,IEEE Trans.Robotics,26(3),576-584(2010)。

[4]Mingming Wang,Jianjun Luo,Ulrich Walter.A Nonlinear Model Predictive Controller for Kinematically Redundant Space Manipulators,In 64th International Astronautical Congress,Beijing,China,2013。

[summary of the invention]

The present invention proposes a kind of MIXED INTEGER forecast Control Algorithm for redundant space robot, robot for space is hidden to barrier the constraint be described as under optimal control problem by the present invention, use MIXED INTEGER forecast Control Algorithm, effectively avoid constraint can cause the infeasible shortcoming of optimal control problem.

The present invention is by the following technical solutions:

Keep away a MIXED INTEGER forecast Control Algorithm for the lower redundant space robot of barrier constraint, comprise the following steps:

Step (1): the kinetic model setting up free-floating space robot;

Step (2): carry out linearisation based on the kinetic model of feedback linearization method to step (1);

Step (3): the control inputs according to optimal control problem calculation procedure (2) Linear System described:

Step (4): the control inputs of linear system step (3) obtained substitutes into following formula in, obtain the control inputs of former nonlinear system, for controlling former nonlinear Space Robot System;

In described step (3), optimal control problem is described as:

The object function optimized is:

The constraints optimized is:

$\left\{\begin{array}{c}x\left(k\right|k)=x\left(k\right)\\ u(k+j|k)=u(k+\mathrm{Nc}|k),j\&GreaterEqual;\mathrm{Nc}\\ x(k+j+1|k)=f_d(x,u),j\&le;\mathrm{Np}\\ \mathrm{G\&Delta;U}\left(k\right)\&le;g+\mathrm{c\&epsiv;}\\ {\mathrm{\&epsiv;}}_{\min }\&le;\mathrm{\&epsiv;}\&le;\mathrm{diag}\left({\mathrm{\&epsiv;}}_{\max }\right){\mathrm{\&delta;}}_{\mathrm{\&epsiv;}}\\ d>d_{\mathrm{uf}}\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}\&le;1,{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0,-{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}+{\mathrm{\&delta;}}_{\mathrm{if}}=1\\ d_{\mathrm{sf}}+L(d-d_{\mathrm{sf}}){\mathrm{\&delta;}}_{\mathrm{uf}}\&le;d\&le;d_{\mathrm{sf}}+U(d-d_{\mathrm{sf}})(1-{\mathrm{\&delta;}}_{\mathrm{uf}})\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{if}})<d<d_{\mathrm{if}}+U(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{if}}\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{si}}\&le;d\&le;d_{\mathrm{if}}+U(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{si}})\end{array}\right.$

In object function, Δ U ^*k () is for controlling the optimum input increment of time domain internal linear system, Y (k) is the actual output of prediction time domain internal linear system, R (k) is the desired output of prediction time domain internal linear system, the variable of Δ U (k) for optimizing, Q, T are constant weight matrix, and ε is slack variable, S is weight matrix corresponding to slack variable, and ρ gets δ=[δ ₁, δ ₂... δ _ε, δ _uf, δ _sf, δ _if] be the logical variable introduced, M _pfor the corresponding weight coefficient of each logical variable, δ _uf, δ _sf, δ _if, δ _sifor the logical variable introduced when setting up and keep away barrier constraint, meet $[{\mathrm{\&delta;}}_{\mathrm{uf}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{sf}},$ $[{\mathrm{\&delta;}}_{\mathrm{si}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},$ $[{\mathrm{\&delta;}}_{\mathrm{if}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},$ δ _sf=(1-δ _uf) δ _si, symbol " " be the equivalence relation in propositional logic, Γ _constfor constant, for the correlation matrix under model predictive control method;

In constraints, Nc, Np are respectively control time domain under model predictive control method and prediction time domain, and u (k+j|k) represents the control inputs of k moment j step backward, and x (k+j+1|k) represents the status predication value of k moment j+1 step backward, f _dthe linear model discretization that (x, u) is obtained by feedback linearization obtains, and c is constant matrices, and d is mechanical arm to the nearest distance of barrier, d _uf, d _sf, d _iffor the dangerous distance defined near barrier, safe distance and affect distance, L, U represent respectively and get functional minimum value and maximum in the bracket of back, and G, g are respectively:

$G=\left[\begin{array}{c}\mathrm{\&Psi;}\\ -\mathrm{\&Psi;}\\ \mathrm{\&Theta;}\\ -\mathrm{\&Theta;}\\ {-\mathrm{\&Theta;}}_{\mathrm{\&upsi;}}\end{array}\right]g=\left[\begin{array}{c}{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\max }-{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ -{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\min }+{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ {\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;p}}-D_{\mathrm{uf}}\end{array}\right]$

Wherein, ψ, Θ _υ, Ω _nc, Ω _np, D _{υ p}, D _uf, Y _pfor the correlation matrix introduced when setting up constraint, u _max, u _minbe respectively maximum and minimum of a value that control inputs under linear model can obtain, y _max, y _minfor exporting maximum and the minimum of a value of permission under linear model.

The kinetic model of described step (1) is that this kinetic model is as follows based on broad sense Jacobian matrix method establishment:

$H^{*}\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+{\stackrel{\&CenterDot;}{H}}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\frac{\&PartialD;}{\&PartialD;\mathrm{\&theta;}}\left\{\frac{1}{2}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}^T{H}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right\}=\mathrm{\&tau;}$

Wherein, H ^*for the broad sense inertial tensor of free-floating space robot; θ ∈ R ⁿ, be broad sense joint coordinates; τ ∈ R ⁿ, be broad sense joint moment; represent H ^*to the derivative of time, for joint angle speed, for joint angle acceleration.

Described step (2) carries out the linear model that linearisation obtains:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}\\ y=\mathrm{Cx}\end{array}\right.$

Wherein, $A=\left[\begin{array}{cc}{0}_n& E_n\\ 0_n& 0_n\end{array}\right],B=\left[\begin{array}{c}{0}_n\\ E_n\end{array}\right],C=E_{2n}$

for the state variable of definition, for control inputs.

In described step (2), after setting up linear model, by the discrete model construction of linear model, obtain:

$\left\{\begin{array}{c}x(k+1)=A_{d}x\left(k\right)+B_{d}u\left(k\right)\\ y(k+1)=C_{d}x(k+1)\end{array}\right.,$

Wherein, A _d=e ^ah, c _d=C (kh).

Compared with prior art, the present invention at least has following beneficial effect: first the present invention utilizes propositional logic to obtain and keep away a kind of new describing mode of barrier constraint, effectively can describe under which and keep away barrier constraint with the change of robot for space to obstacle distance, the characteristic that its character changes thereupon, utilize propositional logic, the method for the system of obtaining all multiple constraint different priorities under describing optimal control problem simultaneously.Based on obtained MIXED INTEGER forecast Control Algorithm, by robot for space to barrier hide the constraint be described as under optimal control problem time, can effectively avoid constraint easily cause the infeasible shortcoming of optimal control problem.

[accompanying drawing explanation]

Fig. 1 gives the schematic diagram of Space Robot System;

Fig. 2 illustrates the MIXED INTEGER forecast Control Algorithm utilizing and obtain, to the process that barrier is hidden when robot for space is finished the work.

[detailed description of the invention]

The invention discloses a kind of MIXED INTEGER forecast Control Algorithm of keeping away the lower redundant space robot of barrier constraint, carry out according to following steps:

Step one, set up the kinetic model of free-floating space robot

Space Robot System as shown in Figure 1.The physical significance of each symbol is respectively:

θ ∈ R ⁿ: broad sense joint coordinates

τ ∈ R ⁿ: broad sense joint moment

R _i∈ R ³: the position vector of rigid body i barycenter

R _c∈ R ³: the position vector (comprising satellite pedestal and mechanical arm) of system barycenter

R _e∈ R ³: the position vector of mechanical arm tail end

A _i∈ R ³: the vector representation of connecting rod i under inertial system

C _i∈ R ³: the vector representation of position coordinates under inertial system recording connecting rod i barycenter under the i of joint

Z _i∈ R ³: the unit vector representing joint i direction of rotation

V _i∈ R ³: the linear velocity of connecting rod i barycenter

ω _i∈ R ³: the angular speed of connecting rod i

V ₀∈ R ³: the linear velocity of satellite pedestal

ω ₀∈ R ³: the angular speed of satellite pedestal

V _e∈ R ³: the linear velocity of mechanical arm tail end

ω _e∈ R ³: the angular speed of mechanical arm tail end

F _e∈ R ⁶: the external force and the moment of face that act on mechanical arm tail end

F ₀∈ R ⁶: the external force and the moment of face that act on satellite pedestal

F _r∈ R ³: the reaction force that manipulator motion produces satellite pedestal

τ _r∈ R ³: the moment of reaction that manipulator motion produces satellite pedestal

Based on the kinetic model of broad sense Jacobian matrix method establishment free-floating space robot.

$H^{*}\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+{\stackrel{\&CenterDot;}{H}}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\frac{\&PartialD;}{\&PartialD;\mathrm{\&theta;}}\left\{\frac{1}{2}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}^T{H}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right\}=\mathrm{\&tau;}---\left(0\right)$

Wherein,

$H^{*}=H_{\mathrm{\&theta;}}-\left[\begin{array}{cc}{J}_{\mathrm{T\&omega;}}^T& H_{\mathrm{\&omega;\&theta;}}^T\end{array}\right]{\left[\begin{array}{cc}\mathrm{\&omega;E}& \mathrm{\&omega;}{\tilde{r}}_{0g}^T\\ \mathrm{\&omega;}{\tilde{r}}_{0g}& H_{\mathrm{\&omega;}}\end{array}\right]}^{-1}\left[\begin{array}{c}{J}_{\mathrm{T\&omega;}}\\ H_{\mathrm{\&omega;\&theta;}}\end{array}\right],$ For the broad sense inertial tensor of free-floating space robot.

$H_{\mathrm{\&theta;}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(J_{R_i}^T{I}_i{J}_{R_i}+m_i{J}_{T_i}^T{J}_{T_i})\&Element;R^{n\&times;n}:$ The broad sense inertia matrix of mechanical arm

I _i∈ R ^{3 × 3}: the inertia matrix of connecting rod i self barycenter relative

$J_{R_i}=\left[\begin{array}{cccccc}{z}_1& z_2& ...& z_i& 0& ...\end{array}\right]\&Element;R^{3\&times;n}:$ The Jacobian matrix of rigid body i angular speed

$J_{T_i}=\left[\begin{array}{cccccc}{z}_1\&times;{\mathrm{\&rho;}}_{c1}& z_2\&times;{\mathrm{\&rho;}}_{c2}& ...& z_i\&times;{\mathrm{\&rho;}}_{\mathrm{ci}}& 0& ...\end{array}\right]\&Element;R^{3\&times;n}:$ The Jacobian matrix of rigid body i linear velocity

$J_{T_{\mathrm{\&omega;}}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{m}_i{J}_{T_i}\&Element;R^{3\&times;n}$

$H_{\mathrm{\&omega;\&theta;}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(I_i{J}_{R_i}+m_i{Z}_i{J}_{T_i})\&Element;R^{3\&times;n}$

M: the gross mass of Space Robot System

M _i: the quality of rigid body i

E ∈ R ^{3 × 3}: unit matrix

$Z_i=\left[\begin{array}{ccc}0& -z_i\left(3\right)& z_i\left(2\right)\\ z_i\left(3\right)& 0& -z_i\left(1\right)\\ -z_i\left(2\right)& z_i\left(1\right)& 0\end{array}\right]:$ Vector z _iantisymmetric matrix

$H_{\mathrm{\&omega;}}=(\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{I}_i+m_i{r}_{0i}^T{r}_{0i})+I_0\&Element;R^{3\&times;3}$

ρ _cj∈ R ³: joint j is to the position vector of rigid body j

Step 2, based on feedback linearization method, linearisation is carried out to kinetic model

Definition status variable control inputs the linear model that then feedback linearization is later is:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}\\ y=\mathrm{Cx}\end{array}\right.---\left(2\right)$

Wherein,

$A=\left[\begin{array}{cc}{0}_n& E_n\\ 0_n& 0_n\end{array}\right],B=\left[\begin{array}{c}{0}_n\\ E_n\end{array}\right],C=E_{2n}$

Discrete model construction is obtained:

$\left\{\begin{array}{c}x(k+1)=A_{d}x\left(k\right)+B_{d}u\left(k\right)\\ y(k+1)=C_{d}x(k+1)\end{array}\right.$

Wherein, A _d=e ^ah, c _d=C (kh).

And the control inputs of former nonlinear system is:

$\mathrm{\&tau;}\left(k\right)=H^{*}u\left(k\right)+{\stackrel{\&CenterDot;}{H}}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\frac{\&PartialD;}{\&PartialD;\mathrm{\&theta;}}\{\frac{1}{2}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}^T{H}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\}---\left(3\right)$

The MIXED INTEGER forecast Control Algorithm that step 3, utilization obtain, calculates the control inputs u of linear system

Utilize the MIXED INTEGER forecast Control Algorithm that obtains, hide to barrier when being finished the work in obstacle environment by robot for space the optimal control problem be described as shown in following formula:

subject to

$\left\{\begin{array}{c}x\left(k\right|k)=x\left(k\right)\\ u(k+j|k)=u(k+\mathrm{Nc}|k),j\&GreaterEqual;\mathrm{Nc}\\ x(k+j+1|k)=f_d(x,u),j\&le;\mathrm{Np}\\ \mathrm{G\&Delta;U}\left(k\right)\&le;g+\mathrm{c\&epsiv;}\\ {\mathrm{\&epsiv;}}_{\min }\&le;\mathrm{\&epsiv;}\&le;\mathrm{diag}\left({\mathrm{\&epsiv;}}_{\max }\right){\mathrm{\&delta;}}_{\mathrm{\&epsiv;}}\\ d>d_{\mathrm{uf}}\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}\&le;1,{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0,-{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}+{\mathrm{\&delta;}}_{\mathrm{if}}=1\\ d_{\mathrm{sf}}+L(d-d_{\mathrm{sf}}){\mathrm{\&delta;}}_{\mathrm{uf}}\&le;d\&le;d_{\mathrm{sf}}+U(d-d_{\mathrm{sf}})(1-{\mathrm{\&delta;}}_{\mathrm{uf}})\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{if}})<d<d_{\mathrm{if}}+U(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{if}}\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{si}}\&le;d\&le;d_{\mathrm{if}}+U(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{si}})\end{array}\right.$

Wherein, Y (k) is the actual output of prediction time domain internal linear system, and R (k) is the desired output of prediction time domain internal linear system, Δ U (k)=[Δ u (kk) ..., Δ u (k+Nc-1k)] ^tfor the variable that will optimize, for the correlation matrix under model predictive control method, ε is slack variable, and S is weight matrix corresponding to slack variable, and ρ gets δ=[δ ₁, δ ₂... δ _ε, δ _uf, δ _sf, δ _if] be the logical variable introduced, δ ₁, δ ₂... δ _εfor the logical variable of each slack variable of correspondence, meet δ _uf, δ _sf, δ _if, δ _sifor the logical variable introduced when setting up and keep away barrier constraint, meet $[{\mathrm{\&delta;}}_{\mathrm{uf}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{sf}},$ $[{\mathrm{\&delta;}}_{\mathrm{si}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},$ $[{\mathrm{\&delta;}}_{\mathrm{if}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},$ δ _sf＝(1-δ _uf)δ _si。Wherein, symbol " " be the equivalence relation in propositional logic, M _pfor the corresponding weight coefficient of each logical variable.About intrafascicular, Nc, Np are respectively control time domain under model predictive control method and prediction time domain, and u (k+j|k) represents the control inputs of k moment j step backward, and x (k+j+1|k) represents the status predication value of k moment j+1 step backward, f _dthe linear model discretization that (x, u) is obtained by feedback linearization obtains, and c is constant matrices, and d is mechanical arm to the nearest distance of barrier, d _uf, d _sf, d _iffor the dangerous distance defined near barrier, safe distance and affect distance.L, U represent back functional minimum value and maximum respectively, and G, g are respectively:

$G=\left[\begin{array}{c}\mathrm{\&Psi;}\\ -\mathrm{\&Psi;}\\ \mathrm{\&Theta;}\\ -\mathrm{\&Theta;}\\ {-\mathrm{\&Theta;}}_{\mathrm{\&upsi;}}\end{array}\right]g=\left[\begin{array}{c}{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\max }-{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ -{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\min }+{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ {\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;p}}-D_{\mathrm{uf}}\end{array}\right]$

Wherein, ψ, Θ _υ, Ω _nc, Ω _np, D _{υ p}, D _uf, Y _pfor the correlation matrix introduced when setting up constraint, u _max, u _minbe respectively maximum and minimum of a value that control inputs under linear model can obtain, y _max, y _minfor exporting maximum and the minimum of a value of permission under linear model.

Correlation matrix value in optimal control problem is respectively:

Γ _const＝ε(k) ^TQε(k)

M＝Θ ^TQΘ+T

Ω _Nc＝[E _n… E _n] ^T∈R ^nNc×n

Ω _Np＝[E _2n… E _2n] ^T∈R ^2nNp×2n

D _vp＝Θ _vjy _vp,Θ _v＝Θ _vjΘ _vb,C _vd＝[0 _n,E _n]

y _vp＝C _vdA _dx(k)+C _vdB _du(k-1),Θ _vb＝[C _vdB _d0 _n… 0 _n]∈R ^n×nNc

Wherein,

P _i, p _i'for one group of point nearest apart on mechanical arm and barrier, n _ifor p _i'point to p _iunit vector.

for barrier p _i'linear velocity, for p on mechanical arm _ithe Jacobian matrix that some linear velocity is corresponding.

ξ is speed damped coefficient.

Solve as above optimal control problem, obtain the control inputs u of linear system.

Step 4, the control inputs u of linear system step 3 obtained substitute into formula (3), obtain the control inputs of former nonlinear system

$\mathrm{\&tau;}\left(k\right)=H^{*}u\left(k\right)+{\stackrel{\&CenterDot;}{H}}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\frac{\&PartialD;}{\&PartialD;\mathrm{\&theta;}}\{\frac{1}{2}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}^T{H}^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\}$

For controlling former nonlinear Space Robot System.

The invention has the beneficial effects as follows: propose a kind of MIXED INTEGER forecast Control Algorithm for redundant space robot.Based on the method, will hide to barrier when robot for space is finished the work under obstacle environment the constraint be described as under optimal control problem.By considering that keeping away barrier retrains the characteristic that can change to obstacle distance heterogeneity with mechanical arm, and the priority of all multiple constraints under the optimal control problem that describes in a systematic way, effectively can process in original method constraints such as keeping away barrier and easily cause the infeasible shortcoming of optimal control problem.

Below in conjunction with a kind of embodiment, the inventive method is set forth:

As shown in Figure 2, utilize the MIXED INTEGER forecast Control Algorithm obtained, in the environment that there is barrier, control the 7 each joints of DOF (degree of freedom) redundance robot for space arrive expected angle.Initial time, each joint rotation angle is respectively, and expects angle of arrival.During 1 second, each joint setting in motion, barrier is detected when 5.1 seconds, regulate the speed with posterior joint 4,5, barrier is hidden, each joint is continued motion and is arrived expected angle afterwards, and dotted line gives the reference locus of each joint motions, and solid line gives the actual path of passing by for each joint of avoiding barrier.Example absolutely proves that the MIXED INTEGER forecast Control Algorithm of proposition is for redundant space robot controlling, can hide barrier by implementation space robot in the process of finishing the work.

Claims (4)

1. keep away a MIXED INTEGER forecast Control Algorithm for the lower redundant space robot of barrier constraint, it is characterized in that:

Step (1): the kinetic model setting up free-floating space robot;

Step (2): carry out linearisation based on the kinetic model of feedback linearization method to step (1);

Step (3): the control inputs according to optimal control problem calculation procedure (2) Linear System described:

Step (4): the control inputs of linear system step (3) obtained substitutes into following formula in, obtain the control inputs of former nonlinear system, for controlling former nonlinear Space Robot System;

In described step (3), optimal control problem is described below:

The object function optimized is:

$\begin{array}{c}\mathrm{\&Delta;}{U}^{*}\left(k\right)\underset{\mathrm{\&Delta;U}}{\min }{\left|\right|Y\left(k\right)-R\left(k\right)\left|\right|}_Q^2+{\left|\right|\mathrm{\&Delta;U}\left(k\right)\left|\right|}_T^2+{\mathrm{\&epsiv;}}^T\mathrm{S\&epsiv;}+\mathrm{\&rho;}{M}_p\mathrm{\&delta;}\\ ={\mathrm{\&Gamma;}}_{\mathrm{const}}+\underset{\mathrm{\&Delta;U}}{\min }{\mathrm{\&Delta;U}\left(k\right)}^T\mathrm{M\&Delta;U}\left(k\right)+{\mathrm{\&theta;}}^T\mathrm{\&Delta;U}\left(k\right)+{\mathrm{\&epsiv;}}^T\mathrm{S\&epsiv;}+\mathrm{\&rho;}{M}_p\mathrm{\&delta;}\end{array}$

The constraints optimized is:

$\left\{\begin{array}{c}x\left(k\right|k)=x\left(k\right)\\ u(k+j|k)=u(k+\mathrm{Nc}|k),j\&GreaterEqual;\mathrm{Nc}\\ x(k+j+1|k)=f_d(x,u),j\&le;\mathrm{Np}\\ \mathrm{G\&Delta;U}\left(k\right)\&le;g+\mathrm{c\&epsiv;}\\ {\mathrm{\&epsiv;}}_{\min }\&le;\mathrm{\&epsiv;}\&le;\mathrm{diag}\left({\mathrm{\&epsiv;}}_{\max }\right){\mathrm{\&delta;}}_{\mathrm{\&epsiv;}}\\ d>d_{\mathrm{uf}}\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}\&le;1,{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0,-{\mathrm{\&delta;}}_{\mathrm{uf}}-{\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{si}}\&le;0\\ {\mathrm{\&delta;}}_{\mathrm{sf}}+{\mathrm{\&delta;}}_{\mathrm{uf}}+{\mathrm{\&delta;}}_{\mathrm{if}}=1\\ d_{\mathrm{sf}}+L(d-d_{\mathrm{sf}}){\mathrm{\&delta;}}_{\mathrm{uf}}\&le;d\&le;d_{\mathrm{sf}}+U(d-d_{\mathrm{sf}})(1-{\mathrm{\&delta;}}_{\mathrm{uf}})\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{if}})<d<d_{\mathrm{if}}+U(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{if}}\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}}){\mathrm{\&delta;}}_{\mathrm{si}}\&le;d\&le;d_{\mathrm{if}}+U(d-d_{\mathrm{if}})(1-{\mathrm{\&delta;}}_{\mathrm{si}})\end{array}\right.$

In object function, Δ U ^*k () is for controlling the optimum input increment of time domain internal linear system, Y (k) is the actual output of prediction time domain internal linear system, R (k) is the desired output of prediction time domain internal linear system, Δ U (k) is for controlling the input increment of time domain internal linear system, Q and T is constant weight matrix, ε is slack variable, and S is weight matrix corresponding to slack variable, and ρ gets max{ Δ U (k) ^tm Δ U (k)+θ ^tΔ U (k)+ε ^ts ε }, δ=[δ ₁, δ ₂... δ _ε, δ _uf, δ _sf, δ _if] be the logical variable introduced, M _pfor the corresponding weight coefficient of each logical variable, δ _uf, δ _sf, δ _if, δ _sifor the logical variable introduced when setting up and keep away barrier constraint, meet $[{\mathrm{\&delta;}}_{\mathrm{uf}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{sf}},[{\mathrm{\&delta;}}_{\mathrm{si}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},[{\mathrm{\&delta;}}_{\mathrm{if}}=1]\&LeftRightArrow;d\&le;d_{\mathrm{if}},{\mathrm{\&delta;}}_{\mathrm{sf}}=(1-{\mathrm{\&delta;}}_{\mathrm{uf}}){\mathrm{\&delta;}}_{\mathrm{si}},$ Symbol for the equivalence relation in propositional logic, Γ _constfor constant, for the correlation matrix under model predictive control method;

In constraints, Nc, Np are respectively control time domain under model predictive control method and prediction time domain, and u (k+j|k) represents the control inputs of k moment j step backward, and x (k+j+1|k) represents the status predication value of k moment j+1 step backward, f _dthe linear model discretization that (x, u) is obtained by feedback linearization obtains, and c is constant matrices, and d is mechanical arm to the nearest distance of barrier, d _uf, d _sf, d _iffor the dangerous distance defined near barrier, safe distance and affect distance, L, U represent respectively and get functional minimum value and maximum in the bracket of back, and G, g are respectively:

$G=\left[\begin{array}{c}\mathrm{\&Psi;}\\ -\mathrm{\&Psi;}\\ \mathrm{\&Theta;}\\ -\mathrm{\&Theta;}\\ {-\mathrm{\&Theta;}}_{\mathrm{\&upsi;}}\end{array}\right],g=\left[\begin{array}{c}{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\max }-{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ -{\mathrm{\&Omega;}}_{\mathrm{Nc}}{u}_{\min }+{\mathrm{\&Omega;}}_{\mathrm{Nc}}u(k-1)\\ {\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{\mathrm{Np}}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;p}}-D_{\mathrm{uf}}\end{array}\right]$

Wherein, ψ, Θ _υ, Ω _nc, Ω _np, D _{υ p}, D _uf, Y _pfor the correlation matrix introduced when setting up constraint, u _max, u _minbe respectively maximum and minimum of a value that control inputs under linear model can obtain, y _max, y _minfor exporting maximum and the minimum of a value of permission under linear model.

2. a kind of MIXED INTEGER forecast Control Algorithm of keeping away the lower redundant space robot of barrier constraint according to claim 1, it is characterized in that: the kinetic model of described step (1) is that this kinetic model is as follows based on broad sense Jacobian matrix method establishment:

$H^{*}\stackrel{..}{\mathrm{\&theta;}}+{\stackrel{.}{H}}^{*}\stackrel{.}{\mathrm{\&theta;}}-\frac{\&PartialD;}{\&PartialD;\mathrm{\&theta;}}\left\{\frac{1}{2}{\stackrel{.}{\mathrm{\&theta;}}}^T{H}^{*}\stackrel{.}{\mathrm{\&theta;}}\right\}=\mathrm{\&tau;}$

Wherein, H ^*for the broad sense inertial tensor of free-floating space robot; θ ∈ R ⁿ, be broad sense joint coordinates; τ ∈ R ⁿ, be broad sense joint moment; represent H ^*to the derivative of time, for joint angle speed, for joint angle acceleration.

3. a kind of MIXED INTEGER forecast Control Algorithm of keeping away the lower redundant space robot of barrier constraint according to claim 1, is characterized in that: described step (2) is carried out the linear model that linearisation obtains and is:

$\left\{\begin{array}{c}\stackrel{.}{x}=\mathrm{Ax}+\mathrm{Bu}\\ y=\mathrm{Cx}\end{array}\right.$

Wherein, $A=\left[\begin{array}{cc}{0}_n& E_n\\ 0_n& 0_n\end{array}\right],B=\left[\begin{array}{c}{0}_n\\ E_n\end{array}\right],$ C＝E _2n

for the state variable of definition, for control inputs.

4. a kind of MIXED INTEGER forecast Control Algorithm of keeping away the lower redundant space robot of barrier constraint according to claim 3, is characterized in that: in described step (2), after setting up linear model, by the discrete model construction of linear model, obtains:

$\left\{\begin{array}{c}x(k+1)=A_{d}x\left(k\right)+B_{d}u\left(k\right)\\ y(k+1)=C_{d}x(k+1)\end{array}\right.,$

Wherein, $A_d=e^{\mathrm{Ah}},B_d={\&Integral;}_0^h{e}^{\mathrm{Ah}}\mathrm{Bdt},C_d=C\left(\mathrm{kh}\right)$

。