避障约束下冗余空间机器人的混合整数预测控制方法

摘要

本发明提供了一种避障约束下冗余空间机器人的混合整数预测控制方法，包括：建立自由漂浮空间机器人的动力学模型；基于反馈线性化方法对步骤(1)的动力学模型进行线性化；根据描述的最优控制问题计算线性系统的控制输入：根据线性系统的控制输入计算原非线性系统的控制输入，用于对原非线性的空间机器人系统进行控制。本发明将障碍物环境下空间机器人完成任务时对障碍物的躲避描述为最优控制问题下的约束。通过考虑避障约束随机械臂到障碍物距离不同性质会发生改变的特性，以及系统地描述最优控制问题下诸多约束的优先级，可以有效地处理原有方法中避障等约束容易导致最优控制问题不可行的缺点。

说明书

【技术领域】

本发明涉及一种用于运动学冗余空间机器人的控制方法，特别是一种避障约束下冗余空间机器人的混合整数预测控制方法。

【背景技术】

空间机器人在空间在轨服务中发挥着重要的作用，可以用于在轨卫星的维修，燃料加注，以及对失效卫星、空间碎片捕获等任务。然而，复杂的空间环境中，空间机器人在完成任务时往往需要对障碍物进行躲避。其中，运动学冗余空间机器人因为在避免运动学/动力学奇异，以及躲避障碍物方面具有明显的优势，因而受到研究者的广泛关注。

对障碍物的躲避可以在路径规划时实现，这类方法尝试找到空间机器人由初始状态到达期望状态的一条安全路径，完成对障碍物的躲避^[1][2][3]，然而存在计算量大，以及往往只适用于静态的空间环境等缺点，对于运动的或者突然出现的障碍物并不能很好地躲避。

在一些冗余空间机器人新的控制方法中，尝试将复杂环境下空间机器人对障碍物的躲避描述为控制下的约束，从而在控制空间机器人完成任务的过程中同时实现对障碍物的躲避，具有计算量小，以及可以躲避动态的障碍物等优点。

模型预测控制方法起源于上世纪70年代的化工领域，因为在约束处理方面具有显著的优势，因而近年来在机械臂的控制和路径规划方面吸引了诸多研究者的兴趣，使用该方法可以方便地将空间机器人对障碍物的躲避描述为控制问题下的避障约束^[4]。然而在该方法的应用过程中还存在以下明显的不足：基于模型预测控制方法实现空间机器人对障碍物的躲避最终可以描述为一个具有诸多约束的最优控制问题，而约束的存在往往会导致最优控制问题不可行，在已有的方法中，对模型预测控制方法下形成的优化问题，如何解决约束可能导致最优控制问题不 可行的缺点并没有很好的办法。

参考文献： 

[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。

【发明内容】

本发明提出了一种用于冗余空间机器人的混合整数预测控制方法，本发明将空间机器人对障碍物的躲避描述为最优控制问题下的约束，使用混合整数预测控制方法，有效地避免约束会导致最优控制问题不可行的缺点。

本发明采用以下技术方案：

一种避障约束下冗余空间机器人的混合整数预测控制方法，包括以下步骤：

步骤(1)：建立自由漂浮空间机器人的动力学模型；

步骤(2)：基于反馈线性化方法对步骤(1)的动力学模型进行线性化；

步骤(3)：根据描述的最优控制问题计算步骤(2)中线性系统的控制输入：

步骤(4)：将步骤(3)得到的线性系统的控制输入代入以下公式 中，得到原非线性系统的控制输入，用于对原非线性的空间机器人系统进行控制；

所述步骤(3)中，最优控制问题描述为：

优化的目标函数为：

优化的约束条件为：

$\left(\begin{array}{c}x\left(k\right|k)=x\left(k\right)\\ u(k+j|k)=u(k+\mathrm{Nc}|k),j\ge \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ϵ}\\ {ϵ}_{\min }\le ϵ\le \mathrm{diag}\left({ϵ}_{\max }\right){\delta }_{ϵ}\\ d>d_{\mathrm{uf}}\\ {\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{uf}}\le 1,{\delta }_{\mathrm{uf}}-{\delta }_{\mathrm{si}}\le 0,-{\delta }_{\mathrm{uf}}-{\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{si}}\le 0\\ {\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{uf}}+{\delta }_{\mathrm{if}}=1\\ d_{\mathrm{sf}}+L(d-d_{\mathrm{sf}}){\delta }_{\mathrm{uf}}\le d\le d_{\mathrm{sf}}+U(d-d_{\mathrm{sf}})(1-{\delta }_{\mathrm{uf}})\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}})(1-{\delta }_{\mathrm{if}})<d<d_{\mathrm{if}}+U(d-d_{\mathrm{if}}){\delta }_{\mathrm{if}}\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}}){\delta }_{\mathrm{si}}\le d\le d_{\mathrm{if}}+U(d-d_{\mathrm{if}})(1-{\delta }_{\mathrm{si}})\end{array}\right)$

目标函数中，ΔU^*(k)为控制时域内线性系统的最优输入增量，Y(k)为预测时域内线性系统的实际输出，R(k)为预测时域内线性系统的期望输出，ΔU(k)为要优化的变量，Q，T为常数权重矩阵，ε为松弛变量，S为松弛变量对应的权重矩阵，ρ取 δ＝[δ₁,δ₂,...δ_ε,δ_uf,δ_sf,δ_if]为引入的逻辑变量，M_p为各逻辑变量相应的权重系数，δ_uf,δ_sf,δ_if,δ_si为建立避障约束时引入的逻辑变量，满足 $[{\delta }_{\mathrm{uf}}=1]\leftrightarrow d\le d_{\mathrm{sf}},$$[{\delta }_{\mathrm{si}}=1]\leftrightarrow d\le d_{\mathrm{if}},$$[{\delta }_{\mathrm{if}}=1]\leftrightarrow d\le d_{\mathrm{if}},$δ_sf＝(1-δ_uf)δ_si，符号“”为命题逻辑中的等价关系，Γ_const为常数，为模型预测控制方法下的相关矩阵；

约束条件中，Nc,Np分别为模型预测控制方法下的控制时域和预测时域，u(k+j|k)表示k时刻往后j步的控制输入，x(k+j+1|k)表示k时刻往后j+1步的状态预测值，f_d(x,u)由反馈线性化得到的线性模型离散化得到，c是常数矩阵，d为机械臂到障碍物最近的距离，d_uf,d_sf,d_if为障碍物附近定义的不安全距离，安全距离以及影响距离，L,U分别表示取后边括号中函数的最小值和最大值，G,g分别为：

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

其中，ψ,Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_p为建立约束时引入的相关矩阵，u_max,u_min分别为线性模型下控制输入能取得的最大值和最小值，y_max,y_min为线性模型下输出允许的最大值和最小值。

所述步骤(1)的动力学模型是基于广义雅克比矩阵方法建立，该动力学模型如下：

$H^{*}\stackrel{··}{\theta }+{\stackrel{·}{H}}^{*}\stackrel{·}{\theta }-\frac{\partial }{\partial \theta }\left\{\frac{1}{2}{\stackrel{·}{\theta }}^T{H}^{*}\stackrel{·}{\theta }\right\}=\tau$

其中，H^*为自由漂浮空间机器人的广义惯性张量；θ∈Rⁿ，为广义关节坐标；τ∈Rⁿ，为广义关节力矩；表示H^*对时间的导数，为关节角速度，为关节角加速度。

所述步骤(2)进行线性化得到的线性模型为：

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

其中，$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}$

为定义的状态变量，为控制输入。 

所述步骤(2)中，建立线性模型后，将线性模型的连续系统离散化，得到：

$\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),$

其中，A_d＝e^Ah,C_d＝C(kh)。

与现有技术相比，本发明至少具有以下有益效果：本发明首先利用命题逻辑得到了避障约束一种新的描述方式，该方式下可以有效地描述避障约束随空间机器人到障碍物距离的变化，其性质随之改变的特性，同时利用命题逻辑，得到了系统描述最优控制问题下诸多约束不同优先级的方法。基于所得到的混合整数预测控制方法，在将空间机器人对障碍物的躲避描述为最优控制问题下的约束时，可以有效地避免约束容易引起最优控制问题不可行的缺点。

【附图说明】

图1给出了空间机器人系统的示意图；

图2展示了利用得到的混合整数预测控制方法，空间机器人完成任务时对障碍物进行躲避的过程。

【具体实施方式】

本发明公开了一种避障约束下冗余空间机器人的混合整数预测控制方法，按照以下步骤进行：

步骤一、建立自由漂浮空间机器人的动力学模型

空间机器人系统如图1所示。各符号的物理意义分别为：

θ∈Rⁿ:广义关节坐标

τ∈Rⁿ:广义关节力矩

r_i∈R³:刚体i质心的位置向量

r_c∈R³:系统质心的位置向量(包括卫星基座和机械臂)

r_e∈R³:机械臂末端的位置向量

a_i∈R³:连杆i在惯性系下的向量表示

c_i∈R³:关节i下测得连杆i质心的位置坐标在惯性系下的向量表示

z_i∈R³:表示关节i旋转方向的单位向量

v_i∈R³:连杆i质心的线速度 

ω_i∈R³:连杆i的角速度

v₀∈R³:卫星基座的线速度

ω₀∈R³:卫星基座的角速度

v_e∈R³:机械臂末端的线速度

ω_e∈R³:机械臂末端的角速度

f_e∈R⁶:作用在机械臂末端的外力和外力矩

f₀∈R⁶:作用在卫星基座的外力和外力矩

f_r∈R³:机械臂运动对卫星基座产生的反作用力

τ_r∈R³:机械臂运动对卫星基座产生的反作用力矩

基于广义雅克比矩阵方法建立自由漂浮空间机器人的动力学模型。

$H^{*}\stackrel{··}{\theta }+{\stackrel{·}{H}}^{*}\stackrel{·}{\theta }-\frac{\partial }{\partial \theta }\left\{\frac{1}{2}{\stackrel{·}{\theta }}^T{H}^{*}\stackrel{·}{\theta }\right\}=\tau ---\left(0\right)$

其中，

$H^{*}=H_{\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}& \omega {\tilde{r}}_{0g}^T\\ \omega {\tilde{r}}_{0g}& H_{\omega }\end{array}\right)}^{-1}\left(\begin{array}{c}{J}_{\mathrm{T\omega }}\\ H_{\mathrm{\omega \theta }}\end{array}\right),$为自由漂浮空间机器人的广义惯性张量。

$H_{\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})\in R^{n\times n}:$机械臂的广义惯量矩阵

I_i∈R^3×3:连杆i相对自身质心的惯量矩阵

$J_{R_i}=\left(\begin{array}{cccccc}{z}_1& z_2& ...& z_i& 0& ...\end{array}\right)\in R^{3\times n}:$刚体i角速度的雅克比矩阵

$J_{T_i}=\left(\begin{array}{cccccc}{z}_1\times {\rho }_{c1}& z_2\times {\rho }_{c2}& ...& z_i\times {\rho }_{\mathrm{ci}}& 0& ...\end{array}\right)\in R^{3\times n}:$刚体i线速度的雅克比矩阵

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

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

m:空间机器人系统的总质量

m_i:刚体i的质量

E∈R^3×3：单位矩阵 

$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):$向量z_i的反对称矩阵

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

ρ_cj∈R³:关节j到刚体j的位置向量 

步骤二、基于反馈线性化方法对动力学模型进行线性化

定义状态变量控制输入则反馈线性化以后的线性模型为：

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

其中，

$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}$

将连续系统离散化得到：

$\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)$

其中，A_d＝e^Ah,C_d＝C(kh)。

而原非线性系统的控制输入为：

$\tau \left(k\right)=H^{*}u\left(k\right)+{\stackrel{·}{H}}^{*}\stackrel{·}{\theta }-\frac{\partial }{\partial \theta }\{\frac{1}{2}{\stackrel{·}{\theta }}^T{H}^{*}\stackrel{·}{\theta }\}---\left(3\right)$

步骤三、利用得到的混合整数预测控制方法，计算线性系统的控制输入u

利用得到的混合整数预测控制方法，将空间机器人在障碍物环境中完成任务时对障碍物的躲避描述为下式所示的最优控制问题：

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\ge \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ϵ}\\ {ϵ}_{\min }\le ϵ\le \mathrm{diag}\left({ϵ}_{\max }\right){\delta }_{ϵ}\\ d>d_{\mathrm{uf}}\\ {\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{uf}}\le 1,{\delta }_{\mathrm{uf}}-{\delta }_{\mathrm{si}}\le 0,-{\delta }_{\mathrm{uf}}-{\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{si}}\le 0\\ {\delta }_{\mathrm{sf}}+{\delta }_{\mathrm{uf}}+{\delta }_{\mathrm{if}}=1\\ d_{\mathrm{sf}}+L(d-d_{\mathrm{sf}}){\delta }_{\mathrm{uf}}\le d\le d_{\mathrm{sf}}+U(d-d_{\mathrm{sf}})(1-{\delta }_{\mathrm{uf}})\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}})(1-{\delta }_{\mathrm{if}})<d<d_{\mathrm{if}}+U(d-d_{\mathrm{if}}){\delta }_{\mathrm{if}}\\ d_{\mathrm{if}}+L(d-d_{\mathrm{if}}){\delta }_{\mathrm{si}}\le d\le d_{\mathrm{if}}+U(d-d_{\mathrm{if}})(1-{\delta }_{\mathrm{si}})\end{array}\right)$

其中，Y(k)为预测时域内线性系统的实际输出，R(k)为预测时域内线性系统的期望输出，ΔU(k)＝[Δu(kk), ..., Δu(k+Nc-1k)]^T为要优化的变量，为模型预测控制方法下的相关矩 阵,ε为松弛变量，S为松弛变量对应的权重矩阵，ρ取δ＝[δ₁,δ₂,...δ_ε,δ_uf,δ_sf,δ_if]为引入的逻辑变量，δ₁,δ₂,...δ_ε为对应每个松弛变量的逻辑变量，满足 δ_uf,δ_sf,δ_if,δ_si为建立避障约束时引入的逻辑变量，满足 $[{\delta }_{\mathrm{uf}}=1]\leftrightarrow d\le d_{\mathrm{sf}},$$[{\delta }_{\mathrm{si}}=1]\leftrightarrow d\le d_{\mathrm{if}},$$[{\delta }_{\mathrm{if}}=1]\leftrightarrow d\le d_{\mathrm{if}},$δ_sf＝(1-δ_uf)δ_si。其中，符号“”为命题逻辑中的等价关系，M_p为各逻辑变量相应的权重系数。约束中，Nc,Np分别为模型预测控制方法下的控制时域和预测时域，u(k+j|k)表示k时刻往后j步的控制输入，x(k+j+1|k)表示k时刻往后j+1步的状态预测值，f_d(x,u)由反馈线性化得到的线性模型离散化得到，c是常数矩阵，d为机械臂到障碍物最近的距离，d_uf,d_sf,d_if为障碍物附近定义的不安全距离，安全距离以及影响距离。L,U分别表示后边函数的最小值和最大值，G,g分别为：

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

其中，ψ,Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_p为建立约束时引入的相关矩阵，u_max,u_min分别为线性模型下控制输入能取得的最大值和最小值，y_max,y_min为线性模型下输出允许的最大值和最小值。

最优控制问题中的相关矩阵取值分别为：

Γ_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_d 0_n … 0_n]∈R^n×nNc

其中，

p_i,p_i'为机械臂和障碍物上相距最近的一组点，n_i为p_i'指向p_i的单位向量。

为障碍物p_i'的线速度，为机械臂上p_i点线速度对应的雅克比矩阵。

ξ为速度阻尼系数。

求解如上最优控制问题，得到线性系统的控制输入u。

步骤四、将步骤三得到的线性系统的控制输入u代入公式(3)，得到原非线性系统的控制输入

$\tau \left(k\right)=H^{*}u\left(k\right)+{\stackrel{·}{H}}^{*}\stackrel{·}{\theta }-\frac{\partial }{\partial \theta }\{\frac{1}{2}{\stackrel{·}{\theta }}^T{H}^{*}\stackrel{·}{\theta }\}$

用于对原非线性的空间机器人系统进行控制。

本发明的有益效果是：提出了一种用于冗余空间机器人的混合整数预测控制方法。基于该方法，将障碍物环境下空间机器人完成任务时对障碍物的躲避描述为最优控制问题下的约束。通过考虑避障约束随机械臂到障碍物距离不同性质会发生改变的特性，以及系统地描述最优控制问题下诸多约束的优先级，可以有效地处理原有方法中避障等约束容易导致最优控制问题不可行的缺点。

以下结合一种实施例对本发明方法进行阐述：

如图2所示，利用得到的混合整数预测控制方法，在存在障碍物的环境中控制7自由度冗余空间机器人各关节到达期望角度。初始时刻，各关节转角分别为，期望到达角度。1秒钟时，各关节开始运动，5.1秒时检测到障碍物，随后关节4、5调整速度，对障碍物进行躲避，之后各关节继续运动到达期望角度，虚线给出了各关节运动的参考轨迹，实线给出了为躲避障碍物各关节走过的实际轨迹。实例充分说明提出的混合整数预测控制方法用于冗余空间机器人控制，可以实现空间机器人在完成任务的过程中对障碍物进行躲避。