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

摘要

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

主权项

一种避障约束下冗余空间机器人的混合整数预测控制方法，其特征在于：步骤(1)：建立自由漂浮空间机器人的动力学模型；步骤(2)：基于反馈线性化方法对步骤(1)的动力学模型进行线性化；步骤(3)：根据描述的最优控制问题计算步骤(2)中线性系统的控制输入：步骤(4)：将步骤(3)得到的线性系统的控制输入代入以下公式中，得到原非线性系统的控制输入，用于对原非线性的空间机器人系统进行控制；所述步骤(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^*对时间的导数，为关节角速度，为关节角加速度；所述步骤(3)中，最优控制问题描述如下：优化的目标函数为：优化的约束条件为：$\left\{\begin{array}{c}x\left(k\right|k)=x\left(k\right)\\ u(k+j|k)=u(k+Nc|k),j\ge Nc\\ x(k+j+1|k)=f_d(x,u),j\le Np\\ G\Delta U\left(k\right)\le g+cϵ\\ {ϵ}_{\min }\le ϵ\le diag\left({ϵ}_{\max }\right){\delta }_{ϵ}\\ d>d_{uf}\\ {\delta }_{sf}+{\delta }_{uf}\le 1,{\delta }_{uf}-{\delta }_{si}\le 0,-{\delta }_{uf}-{\delta }_{sf}+{\delta }_{si}\le 0\\ {\delta }_{sf}+{\delta }_{uf}+{\delta }_{if}=1\\ d_{sf}+L(d-d_{sf}){\delta }_{uf}\le d\le d_{sf}+U(d-d_{sf})(1-{\delta }_{uf})\\ d_{if}+L(d-d_{if})(1-{\delta }_{if})<d\le d_{if}+U(d-d_{if}){\delta }_{if}\\ d_{if}+L(d-d_{if}){\delta }_{si}\le d\le d_{if}+U(d-d_{if})(1-{\delta }_{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 }_{uf}=1]\leftrightarrow d\le d_{sf},[{\delta }_{si}=1]\leftrightarrow d\le d_{if},[{\delta }_{if}=1]\leftrightarrow d>d_{if},$δ_sf＝(1‑δ_uf)δ_si，符号为命题逻辑中的等价关系，Γ_const为常数，M,为模型预测控制方法下的相关矩阵；约束条件中，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分别为：$\begin{array}{cc}G=\left[\begin{array}{c}\Psi \\ -\Psi \\ \Theta \\ -\Theta \\ -{\Theta }_{\upsilon }\end{array}\right]& g=\left[\begin{array}{c}{\Omega }_{Nc}{u}_{\max }-{\Omega }_{Nc}u(k-1)\\ -{\Omega }_{Nc}{u}_{\min }+{\Omega }_{Nc}u(k-1)\\ {\Omega }_{Np}{y}_{\max }-Y_p\\ -{\Omega }_{Np}{y}_{\min }+Y_p\\ D_{\upsilon p}-D_{uf}\end{array}\right]\end{array}$其中，ψ,Θ_υ,Ω_Nc,Ω_Np,D_υp,D_uf,Y_p为建立约束时引入的相关矩阵，u_max,u_min分别为线性模型下控制输入能取得的最大值和最小值，y_max,y_min为线性模型下输出允许的最大值和最小值。