[发明专利]空间机械臂抗干扰姿态稳定的微分几何非线性控制方法

摘要

本发明公开了一种空间机械臂抗干扰姿态稳定的微分几何非线性控制方法，使用微分几何理论将非线性姿态动力学模型进行线性化转换，得到一种类线性空间的形式，接着利用LQR对线性化后的姿态稳定系统进行控制器设计，得到非线性控制律。基于微分几何理论与LQR控制方法的空间机械臂姿态稳定控制器不但可以保证系统对各种不确定性以及非合作干扰的鲁棒稳定性，并且采用了较为简单的线性控制理论，控制结构简便，减少计算量，节约星上资源，因此特别适用于需要实时解算且计算能力有限的空间机械臂姿态稳定任务。

主权项

空间机械臂抗干扰姿态稳定的微分几何非线性控制方法，其特征在于，包括以下步骤：步骤一、建立自由飞行空间机械臂仿射非线性状态空间模型通过空间机械臂的姿态运动学方程和系统总动能方程，代入Lagrange方程，得到空间机械臂的姿态动力学模型为：$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}---\left(1\right)$式中：$C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\stackrel{\·}{M}\stackrel{\·}{\mathrm{\θ}}-\frac{\∂}{\∂\mathrm{\θ}}\[\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}\]---\left(2\right)$其中，θ为空间机械臂在惯性坐标系下的姿态角，为空间机械臂在惯性坐标系下的姿态角速度；M(θ)为空间机械臂系统的广义质量阵，是空间机械臂姿态角的矩阵函数；选取状态变量$x={\left[\begin{array}{cccccc}{\mathrm{\θ}}_0& {\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\θ}}_1& {\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\θ}}_2& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]}^T$输入变量与输出变量u＝[τ₀ τ₁ τ₂]^T，y＝[θ₀ θ₁ θ₂]^T得到空间机械臂系统的状态空间表达式为：$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_4\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ g_{21}& g_{22}& g_{23}\\ 0& 0& 0\\ g_{41}& g_{42}& g_{43}\\ 0& 0& 0\\ g_{61}& g_{62}& g_{63}\end{array}\right]\left[\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right]$$\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_0\\ {\mathrm{\θ}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\mathrm{\θ}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(3\right)$式中：$\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=-M^{-1}\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]+\frac{1}{2}{M}^{-1}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$$\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]=M^{-1}---\left(4\right)$记$f=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right],g_1=\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right],g_2=\left[\begin{array}{c}0\\ g_{22}\\ 0\\ g_{42}\\ 0\\ g_{62}\end{array}\right],g_3=\left[\begin{array}{c}0\\ g_{23}\\ 0\\ g_{43}\\ 0\\ g_{63}\end{array}\right]---\left(5\right)$h₁＝θ₀,h₂＝θ₁,h₃＝θ₂则得到空间机械臂系统的仿射非线性状态空间形式$\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}---\left(6\right)$步骤二、利用微分几何理论建立类线性空间形式的姿态动力学模型设计如下状态反馈控制律$\begin{array}{c}u=\mathrm{\α}\left(x\right)+\mathrm{\β}\left(x\right)v\\ =A^{-1}\left(x\right)\[-b\left(x\right)+v\]\end{array}---\left(7\right)$其中$A^{-1}\left(x\right)={\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]}^{-1}=M\left(x\right)$$b\left(x\right)=\left[\begin{array}{c}{L}_f^2{h}_1\\ L_f^2{h}_2\\ L_f^2{h}_3\end{array}\right]=\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]---\left(8\right)$$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$β(x)＝A^‑1(x)＝M(x)将式(7)(8)代入(6)，得到空间机械臂姿态动力学模型的线性空间表达形式为：$\begin{array}{c}\stackrel{\·}{x}=Ax+bv\\ y=cx\end{array}---\left(9\right)$其中，称为李导数，为一种计算因子，v为针对线性系统(9)所设计的控制律，u为实际非线性系统(6)所设计的控制律；步骤三、基于线性空间系统的LQR最优控制器设计将线性系统(9)视为$\left\{\begin{array}{c}\stackrel{\·}{x}=\tilde{A}x+\tilde{B}v+\tilde{B}d\\ y=\tilde{C}x+\tilde{D}v\end{array}\right.$则基于线性二次型最优调节问题设计线性系统的控制律为$v=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)---\left(10\right)$式中，R为最优控制性能指标中对输入变量的加权矩阵，P为满足以下Riccati代数方程的解矩阵，PA+A^TP‑PBR^‑1B^TP+Q＝0最终得到基于微分几何的航天器非线性姿态跟踪控制器为u＝α(x)+β(x)ν   (11)式中：$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$β(x)＝A^‑1(x)＝M(x)$v_{LQR}=-R^{-1}{\tilde{B}}^{T}Px\left(t\right).$