CN104842355B - The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of avoidance constraint - Google Patents

Abstract

The invention provides the MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of a kind of avoidance constraint, including: set up the kinetic model of free-floating space robot；Based on feedback linearization method, the kinetic model of step (1) is carried out linearisation；The control input of linear system is calculated: calculate the control input of former nonlinear system, for former nonlinear Space Robot System is controlled according to the input that controls of linear system according to the optimal control problem described.When robot for space under obstacle environment is completed task by the present invention, barrier is hidden the constraint being described as under optimal control problem.The characteristic that can change to obstacle distance heterogeneity with mechanical arm by considering avoidance to retrain, and the priority of all multiple constraints under the optimal control problem that describes in a systematic way, can effectively process in original method the constraints such as avoidance and be easily caused the infeasible shortcoming of optimal control problem.

Description

The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of avoidance constraint

[technical field]

The present invention relates to a kind of control method for kinematic redundancy robot for space, under particularly a kind of avoidance constraint The MIXED INTEGER forecast Control Algorithm of redundant space robot.

[background technology]

Robot for space plays an important role in On-orbit servicing, may be used for the maintenance of satellite in orbit, combustion Material filling, and to tasks such as inert satellite, space junk captures.But, in complicated spatial environments, robot for space is complete Generally require during one-tenth task and barrier is hidden.Wherein, kinematic redundancy robot for space is because avoiding kinesiology/dynamic Mechanics is unusual, and avoiding barrier aspect has obvious advantage, thus by the extensive concern of researcher.

Hiding barrier and can realize when path planning, this kind of method attempts finding robot for space by initial shape State arrives a secure path in expectation state, completes to hide barrier^[1][2][3], but exist computationally intensive, and past Toward being only applicable to the shortcomings such as static spatial environments, can not hide well for motion or emergent barrier Keep away.

In the control method that some redundant space robots are new, attempt robot for space under complex environment barrier Hide and be described as controlling lower constraint, thus realize barrier during control robot for space completes task simultaneously Hide, there is amount of calculation little, and the advantages such as dynamic barrier can be hidden.

Model predictive control method originates from the chemical field of the seventies in last century, because having aobvious in terms of constraint process The advantage write, thus in terms of the control and path planning of mechanical arm, attracted the interest of many researcheres in recent years, using should Barrier can be hidden the avoidance constraint being described as under control problem by robot for space by method easily^[4].But at this The application process of method there is also following clearly disadvantageous: realize robot for space to obstacle based on model predictive control method Hiding of thing may finally be described as an optimal control problem with all multiple constraints, and the existence retrained frequently can lead to Excellent control problem is infeasible, in existing method, to the optimization problem formed under model predictive control method, how to solve about Shu Keneng causes the infeasible shortcoming of optimal control problem not have good way.

List of references:

[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, and the present invention is by space Barrier is hidden the constraint being described as under optimal control problem by robot, uses 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:

The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of a kind of avoidance constraint, comprises the following steps:

Step (1): set up the kinetic model of free-floating space robot；

Step (2): the kinetic model of step (1) is carried out linearisation based on feedback linearization method；

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

Step (4): the control input of linear system step (3) obtained substitutes into below equation In, obtain the control input of former nonlinear system, for former nonlinear Space Robot System is controlled；

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

In object function, Δ U^*K (), for controlling the optimum input increment of time domain internal linear system, Y (k) is prediction time domain internal linear system Actual output, R (k) is the desired output of prediction time domain internal linear system, and Δ U (k) is variable to be optimized, and Q, T are constant-weight matrix, ε is slack variable, and S is the weight matrix that slack variable is corresponding, and ρ takesδ= [δ₁,δ₂,...δ_ε,δ_uf,δ_sf,δ_if] it is 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 avoidance constraint, meets $\&lsqb;{\mathrm{\&delta;}}_f=1\&rsqb;\&LeftRightArrow;d\&le;d_{sf},\&lsqb;{\mathrm{\&delta;}}_{si}=1\&rsqb;\&LeftRightArrow;d\&le;d_{if},\&lsqb;{\mathrm{\&delta;}}_{if}=1\&rsqb;\&LeftRightArrow;d>d_{if},$ δ_sf=(1-δ_uf)δ_si, symbolFor the equivalence relation in propositional logic, Γ_constFor constant,For Model Predictive Control Correlation matrix under method；

In constraints, the control time domain that Nc, Np are respectively under model predictive control method and prediction time domain, u (k+j | k) Represent that the control of k moment j backward step inputs, and x (k+j+1 | k) represent the status predication value that k moment j+1 backward walks, f_d(x, u) by The linear model discretization that feedback linearization obtains 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 take back Functional minimum value and maximum in bracket, G, g are respectively as follows:

$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;}}_{Nc}{u}_{\max }-{\mathrm{\&Omega;}}_{Nc}u\left(k-1\right)\\ -{\mathrm{\&Omega;}}_{Nc}{u}_{\min }+{\mathrm{\&Omega;}}_{Nc}u\left(k-1\right)\\ {\mathrm{\&Omega;}}_{Np}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{Np}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;}p}-D_{uf}\end{array}\right]$

Wherein, ψ, Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_pThe correlation matrix introduced during for setting up constraint, u_max,u_minIt is respectively Maximum and minima, y that input can obtain is controlled under linear model_max,y_minFor exporting the maximum of permission under linear model And minima.

The kinetic model of described step (1) is to set up based on broad sense Jacobian matrix method, and this kinetic model is as follows:

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

Wherein, H^*Broad sense inertial tensor for free-floating space robot；θ∈Rⁿ, for broad sense joint coordinates；τ∈Rⁿ, For broad sense joint moment；Represent H^*Derivative to the 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}=Ax+Bu\\ y=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 definition state variable,For controlling input.

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

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

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

Compared with prior art, the present invention at least has the advantages that the present invention obtains first with propositional logic Avoidance retrains a kind of new describing mode, can effectively describe avoidance constraint with robot for space to barrier under which The change of distance, the characteristic that its character changes therewith, utilize propositional logic simultaneously, obtained under system description optimal control problem The method of all multiple constraint different priorities.Based on obtained MIXED INTEGER forecast Control Algorithm, by robot for space to barrier Hinder when hiding the constraint being described as under optimal control problem of thing, can be effectively prevented from retraining and easily cause optimal control problem Infeasible shortcoming.

[accompanying drawing explanation]

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

Fig. 2 illustrates and utilizes the MIXED INTEGER forecast Control Algorithm that obtains, to barrier when robot for space completes task Carry out the process hidden.

[detailed description of the invention]

The invention discloses the MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of a kind of avoidance constraint, according to Lower step is carried out:

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

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

θ∈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 (including satellite pedestal and mechanical arm) of system barycenter

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

a_i∈R³: connecting rod i vector representation under inertial system

c_i∈R³: record the vector representation under inertial system of the position coordinates of connecting rod i barycenter under the i of joint

z_i∈R³: represent the unit vector of joint i direction of rotation

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

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

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

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

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

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

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

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

f_r∈R³: the counteracting force that satellite pedestal is produced by manipulator motion

τ_r∈R³: the moment of reaction that satellite pedestal is produced by manipulator motion

The kinetic model of free-floating space robot is set up based on broad sense Jacobian matrix method.

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

Wherein,

$H^{*}=H_{\mathrm{\&theta;}}-\left[\begin{array}{cc}{J}_{T\mathrm{\&omega;}}^T& H_{\mathrm{\&omega;}\mathrm{\&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}_{T\mathrm{\&omega;}}\\ H_{\mathrm{\&omega;}\mathrm{\&theta;}}\end{array}\right],$ Broad sense inertia for free-floating space robot is opened Amount.

$H_{\mathrm{\&theta;}}=\underset{i=1}{\overset{n}{\&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: connecting rod i is relative to the inertia matrix of self barycenter

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

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

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

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

The gross mass of m: 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}{\&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, kinetic model is carried out linearisation

Definition status variableControl inputThe linear model that then feedback linearization is later is:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=Ax+Bu\\ y=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\left(k+1\right)=A_{d}x\left(k\right)+B_{d}u\left(k\right)\\ y\left(k+1\right)=C_{d}x\left(k+1\right)\end{array}\right.$

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

And the control input of former nonlinear system is:

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

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

Utilize the MIXED INTEGER forecast Control Algorithm that obtains, when robot for space is completed in obstacle environment task pair Barrier hide the optimal control problem being described as shown in following formula:

subject to

$\left\{\begin{array}{c}x\left(k|k\right)=x\left(k\right)\\ u\left(k+j|k\right)=u\left(k+Nc|k\right),j\&GreaterEqual;Nc\\ x\left(k+j+1|k\right)=f_d\left(x,u\right),j\&le;Np\\ G\mathrm{\&Delta;}U\left(k\right)\&le;g+c\mathrm{\&epsiv;}\\ {\mathrm{\&epsiv;}}_{\min }\&le;\mathrm{\&epsiv;}\&le;diag\left({\mathrm{\&epsiv;}}_{\max }\right){\mathrm{\&delta;}}_{\mathrm{\&epsiv;}}\\ d>d_{uf}\\ {\mathrm{\&delta;}}_{sf}+{\mathrm{\&delta;}}_{uf}\&le;1,{\mathrm{\&delta;}}_{uf}-{\mathrm{\&delta;}}_{si}\&le;0,-{\mathrm{\&delta;}}_{uf}-{\mathrm{\&delta;}}_{sf}+{\mathrm{\&delta;}}_{si}\&le;0\\ {\mathrm{\&delta;}}_{sf}+{\mathrm{\&delta;}}_{uf}+{\mathrm{\&delta;}}_{if}=1\\ d_{sf}+L\left(d-d_{sf}\right){\mathrm{\&delta;}}_{uf}\&le;d\&le;d_{sf}+U\left(d-d_{df}\right)\left(1-{\mathrm{\&delta;}}_{uf}\right)\\ d_{if}+L\left(d-d_{if}\right)\left(1-{\mathrm{\&delta;}}_{if}\right)<d<d_{if}+U\left(d-d_{if}\right){\mathrm{\&delta;}}_{if}\\ d_{if}+L\left(d-d_{if}\right){\mathrm{\&delta;}}_{si}\&le;d\&le;d_{if}+U\left(d-d_{if}\right)\left(1-{\mathrm{\&delta;}}_{si}\right)\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,For variable to be optimized,For the correlation matrix under model predictive control method, ε is slack variable, and S is the weight matrix that slack variable is corresponding, and ρ takesδ =[δ₁,δ₂,...δ_ε,δ_uf,δ_sf,δ_if] it is the logical variable introduced, δ₁,δ₂,...δ_εLogic for corresponding each slack variable becomes Amount, meetsδ_uf,δ_sf,δ_if,δ_siThe logical variable introduced during for setting up avoidance constraint, meets $\&lsqb;{\mathrm{\&delta;}}_{uf}=1\&rsqb;\&LeftRightArrow;d\&le;d_{sf},\&lsqb;{\mathrm{\&delta;}}_{si}=1\&rsqb;\&LeftRightArrow;d\&le;d_{if},\&lsqb;{\mathrm{\&delta;}}_{if}=1\&rsqb;\&LeftRightArrow;d>d_{if},$ δ_sf=(1-δ_uf)δ_si.Wherein, symbolFor proposition Equivalence relation in logic, M_pFor the corresponding weight coefficient of each logical variable.In constraint, Nc, Np are respectively Model Predictive Control Control time domain under method and prediction time domain, and u (k+j | k) represent that the control of k moment j backward step inputs, x (k+j+1 | k) represent k The status predication value of moment j+1 backward step, f_d(x, linear model discretization u) obtained by feedback linearization obtains, and c is constant Matrix, d is mechanical arm to the nearest distance of barrier, d_uf,d_sf,d_ifFor the dangerous distance defined near barrier, safety away from From and affect distance.L, U represent back functional minimum value and maximum respectively, and G, g are respectively as follows:

$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;}}_{Nc}{u}_{\max }-{\mathrm{\&Omega;}}_{Nc}u\left(k-1\right)\\ -{\mathrm{\&Omega;}}_{Nc}{u}_{\min }+{\mathrm{\&Omega;}}_{Nc}u\left(k-1\right)\\ {\mathrm{\&Omega;}}_{Np}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{Np}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;}p}-D_{uf}\end{array}\right]$

Wherein, ψ, Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_pThe correlation matrix introduced during for setting up constraint, u_max,u_minIt is respectively Maximum and minima, y that input can obtain is controlled under linear model_max,y_minFor exporting the maximum of permission under linear model And minima.

Correlation matrix value in optimal control problem is respectively as follows:

ε (k)=R (k)-Φ x (k)-Υ u (k-1)

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

M=Θ^TQΘ+T

Ω_Nc=[E_n…E_n]^T∈R^nNc×n

Y_p=Φ x (k)+Υ u (k-1)

Ω_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_vd B_d 0_n … 0_n]∈R^n×nNc

${\mathrm{\&Theta;}}_{vj}=\left[\begin{array}{c}\left(1-{\mathrm{\&delta;}}_{1_{if}}\right){n_1}^T{J}_{p_1}\\ .\\ .\\ .\\ \left(1-{\mathrm{\&delta;}}_{i_{if}}\right){n_i}^T{J}_{p_i}\\ .\\ .\\ .\\ {{(1-{\mathrm{\&delta;}}_{N_{k_{\mathrm{if}}}})n}_{N_k}}^T{J}_{p_{N_k}}\end{array}\right],D_{uf}=\left[\begin{array}{c}\left(1-{\mathrm{\&delta;}}_{1_{if}}\right)<{\stackrel{\&CenterDot;}{r}}_{p_1},n_1>-\mathrm{\&xi;}\frac{d_{{11}^{\&prime;}}-d_{uf}}{d_{if}-d_{uf}}\\ .\\ .\\ .\\ \left(1-{\mathrm{\&delta;}}_{i_{if}}\right)<{\stackrel{\&CenterDot;}{r}}_{p_i},n_i>-\mathrm{\&xi;}\frac{d_{{\mathrm{ii}}^{\&prime;}}-d_{uf}}{d_{if}-d_{uf}}\\ .\\ .\\ .\\ \left(1-{\mathrm{\&delta;}}_{N_{k_{if}}}\right)<{\stackrel{\&CenterDot;}{r}}_{p_{{N_k}^{\&prime;}}},n_{N_k}>-\mathrm{\&xi;}\frac{d_{N_k{N}_{k^{\&prime;}}}-d_{uf}}{d_{if}-d_{uf}}\end{array}\right]$

Wherein,

p_i,p_i'For one group of point the most nearest 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 input u of linear system.

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

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

For former nonlinear Space Robot System is controlled.

The invention has the beneficial effects as follows: propose a kind of MIXED INTEGER PREDICTIVE CONTROL side for redundant space robot Method.Based on the method, when robot for space under obstacle environment is completed task, barrier is hidden and be described as optimum control Constraint under problem.The characteristic that can change to obstacle distance heterogeneity with mechanical arm by considering avoidance to retrain, with And the priority of all multiple constraints under the optimal control problem that describes in a systematic way, can effectively process in original method the constraints such as avoidance It is easily caused the infeasible shortcoming of optimal control problem.

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

As in figure 2 it is shown, utilize the MIXED INTEGER forecast Control Algorithm obtained, the environment exist barrier controls 7 certainly Expected angle is arrived by each joint of degree redundant space robot.Initial time, each joint rotation angle is respectively, it is desirable to angle of arrival.1 Second time, each joint setting in motion, barrier detected when 5.1 seconds, regulate the speed with posterior joint 4,5, barrier is hided Keep away, the most each joint continue motion arrive expected angle, dotted line gives the reference locus of each joint motions, solid line give into The actual path passed by each joint of avoiding barrier.Example absolutely proves that the MIXED INTEGER forecast Control Algorithm of proposition is for redundancy Robot for space controls, it is possible to achieve barrier is hidden during completing task by robot for space.

Claims (3)

1. the MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of avoidance constraint, it is characterised in that:

Step (1): set up the kinetic model of free-floating space robot；

Step (2): the kinetic model of step (1) is carried out linearisation based on feedback linearization method；

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

Step (4): the control input of linear system step (3) obtained substitutes into below equation In, obtain the control input of former nonlinear system, for former nonlinear Space Robot System is controlled；

The kinetic model of described step (1) is to set up based on broad sense Jacobian matrix method, and this kinetic model is as follows:

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

Wherein, H^*Broad sense inertial tensor for free-floating space robot；θ∈Rⁿ, for broad sense joint coordinates；τ∈Rⁿ, for extensively Justice joint moment；Represent H^*Derivative to the time,For joint angle speed,For joint angle acceleration；

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

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

In object function, Δ U^*K (), for controlling the optimum input increment of time domain internal linear system, Y (k) is prediction time domain internal linear The actual output of system, R (k) is the desired output of prediction time domain internal linear system, and Δ U (k) is for controlling time domain internal linear system Input increment, Q and T is constant-weight matrix, and ε is slack variable, and S is the weight matrix that slack variable is corresponding, and ρ takesδ=[δ₁,δ₂,...δ_ε,δ_uf,δ_sf,δ_if] it is the logical variable introduced, M_p For the corresponding weight coefficient of each logical variable, δ_uf,δ_sf,δ_if,δ_siThe logical variable introduced during for setting up avoidance constraint, meets $\&lsqb;{\mathrm{\&delta;}}_{uf}=1\&rsqb;\&LeftRightArrow;d\&le;d_{sf},\&lsqb;{\mathrm{\&delta;}}_{si}=1\&rsqb;\&LeftRightArrow;d\&le;d_{if},\&lsqb;{\mathrm{\&delta;}}_{if}=1\&rsqb;\&LeftRightArrow;d>d_{if},$ δ_sf=(1-δ_uf)δ_si, symbolFor in propositional logic Equivalence relation, Γ_constFor constant, M,For the correlation matrix under model predictive control method；

In constraints, the control time domain that Nc, Np are respectively under model predictive control method and prediction time domain, u (k+j | k) represent The control input of k moment j backward step, and x (k+j+1 | k) represent the status predication value that k moment j+1 backward walks, f_d(x, u) by feeding back The linear model discretization that linearisation obtains 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 take back bracket Middle functional minimum value and maximum, G, g are respectively as follows:

$\begin{array}{cc}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;}}_{Nc}{u}_{\max }-{\mathrm{\&Omega;}}_{Nc}u(k-1)\\ -{\mathrm{\&Omega;}}_{Nc}{u}_{\min }+{\mathrm{\&Omega;}}_{Nc}u(k-1)\\ {\mathrm{\&Omega;}}_{Np}{y}_{\max }-Y_p\\ -{\mathrm{\&Omega;}}_{Np}{y}_{\min }+Y_p\\ D_{\mathrm{\&upsi;}p}-D_{uf}\end{array}\right]\end{array}$

Wherein, ψ, Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_pThe correlation matrix introduced during for setting up constraint, u_max,u_minIt is respectively linear mould Maximum and minima, y that input can obtain is controlled under type_max,y_minFor exporting maximum and the minimum of permission under linear model Value.

The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of a kind of avoidance the most according to claim 1 constraint, It is characterized in that: described step (2) carries out the linear model that linearisation obtains and is:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=Ax+Bu\\ y=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 definition state variable,For controlling input.

The MIXED INTEGER forecast Control Algorithm of the lower redundant space robot of a kind of avoidance the most according to claim 2 constraint, It is characterized in that: 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, $B_d={\&Integral;}_0^h{e}^{Ah}Bdt,C_d=C\left(kh\right).$