一种空间机器人在轨抓捕过程的柔顺与协调控制方法与流程

技术特征：

1.一种空间机器人在轨抓捕过程的柔顺与协调控制方法，其特征在于，包括：

根据动力学和运动学方程，建立面向控制的空间机器人模型；

根据所述面向控制的空间机器人模型，建立机械臂柔顺抓捕控制律；

确定基于基座喷气装置的基座位姿控制律；

根据所述机械臂柔顺抓捕控制律，建立机械臂柔顺控制回路；

根据所述基座位姿控制律和PSR伪速率脉冲调制器，建立基座位姿控制回路；

将所述机械臂柔顺控制回路和基座位姿控制回路计算得到的控制量分别作为空间机器人系统的机械臂和基座喷气装置的控制输入；同时，将基座的控制力作为前馈项，输入给机械臂控制律，以补偿基座控制对机械臂末端柔顺控制的干扰。

2.根据权利要求1所述的方法，其特征在于，所述根据动力学和运动学方程，建立面向控制的空间机器人模型，包括：

将空间机器人系统作为典型多体结构，根据空间机器人系统的自由度，应用第二类拉格朗日方程，得到如下动力学方程：

$\left[\begin{array}{cc}{H}_b& H_{bm}\\ H_{bm}^T& H_m\end{array}\right]\left[\begin{array}{c}{\stackrel{\·\·}{x}}_b\\ \stackrel{\·\·}{\mathrm{\φ}}\end{array}\right]+\left[\begin{array}{c}{c}_b\\ c_m\end{array}\right]=\left[\begin{array}{c}{F}_b\\ \mathrm{\τ}\end{array}\right]---\left(1\right)$

其中，H_b∈R^6×6，表示基座惯性矩阵；H_m∈R^6×6，表示机械臂惯性矩阵；H_bm∈R^6×n，表示基座与机械臂的耦合惯性矩阵；c_b∈R⁶，表示基座非线性项；c_m∈Rⁿ，表示机械臂非线性项；F_b∈R⁶，表示基座喷气装置作用在基座质心上的力和力矩；τ∈Rⁿ，表示机械臂的关节力矩；x_b∈R⁶，表示基座的位姿；φ∈Rⁿ，表示机械臂的关节角；

消去基座运动方程，对上述动力学方程(1)进行整理，得到：

$H^{*}\stackrel{\·\·}{\mathrm{\φ}}+c^{*}=\mathrm{\τ}+J_b^{*}{F}_b---\left(2\right)$

其中，H^*∈R^n×n为广义惯性矩阵；

$H^{*}=H_m-H_{bm}^T{H}_b{H}_{bm}---\left(3\right)$

c^*和定义如下：

$c^{*}=c_m-H_{bm}^T{H}_b^{-1}{c}_b---\left(4\right)$

$J_b^{*}=-H_{bm}^T{H}_b^{-1}---\left(5\right)$

相对于基座质心，系统的线动量和角动量为：

$\left[\begin{array}{c}{P}_0\\ L_0\end{array}\right]=H_b{\stackrel{\·}{x}}_b+H_{bm}\stackrel{\·}{\mathrm{\φ}}---\left(6\right)$

其中，P₀表示线动量，L₀表示角动量；

机械臂末端速度与基座速度、机械臂关节角速度的运动学关系如下：

${\stackrel{\·}{x}}_h=J_b{\stackrel{\·}{x}}_b+J_m\stackrel{\·}{\mathrm{\φ}}---\left(7\right)$

其中，x_h表示机械臂末端位姿；J_b∈R^6×6，表示基座雅克比矩阵；J_m∈R^6×n，表示机械臂雅克比矩阵；

将上述式(6)代入式(7)，可得：

${\stackrel{\·}{x}}_h=J^{*}\stackrel{\·}{\mathrm{\φ}}+{\stackrel{\·}{x}}_{gh}---\left(8\right)$

其中，J^*∈R^6×n为广义雅克比矩阵，定义如下：

$J^{*}=J_m-J_b{H}_b^{-1}{H}_{bm}---\left(9\right)$

定义中间变量

${\stackrel{\·}{x}}_{gh}=J_b{H}_b^{-1}\left[\begin{array}{c}{P}_0\\ L_0\end{array}\right]---\left(10\right).$

3.根据权利要求2所述的方法，其特征在于，所述根据所述面向控制的空间机器人模型，建立机械臂柔顺抓捕控制律，包括：

根据机械臂末端相对于惯性系的动态稳定性，以及抓捕机构的旋转角速度，结合面向控制的空间机器人模型，定义期望动态特性：

$M_i{\stackrel{\·\·}{x}}_h+D_i\mathrm{\Δ}{\stackrel{\·}{x}}_h+K_i{\mathrm{\Δx}}_h=0---\left(11\right)$

$T_s{\stackrel{\·\·}{\mathrm{\φ}}}_n+{\stackrel{\·}{\mathrm{\φ}}}_n={\stackrel{\·}{\mathrm{\φ}}}_{ref}---\left(12\right)$

其中，M_i∈R^6×6，表示质量阵，D_i∈R^6×6，表示阻尼阵，K_i∈R^6×6，表示刚度阵，Δx_h表示机械臂末端在惯性系下的位姿相对于参考点的摄动量；T_s表示抓捕机构旋转角速度的闭环时间常数，表示期望角速度；

根据式(11)和式(12)，建立机械臂柔顺抓捕控制律：

$\mathrm{\τ}=H^{*}{\left[\begin{array}{c}{J}^{*}\\ T_s{S}_n\end{array}\right]}^{-1}(\left[\begin{array}{c}{M}_i^{-1}(-D_i\mathrm{\Δ}{\stackrel{\·}{x}}_h-K_i\mathrm{\Δ}{x}_h)\\ -\mathrm{\Δ}{\stackrel{\·}{\mathrm{\φ}}}_n+{\stackrel{\·}{\mathrm{\φ}}}_{ref}\end{array}\right]-\left[\begin{array}{c}{\stackrel{\·}{J}}^{*}\stackrel{\·}{\mathrm{\φ}}+{\stackrel{\·\·}{x}}_{gh}\\ 0\end{array}\right])+c^{*}-J_b^{*}{F}_b---\left(19\right).$

4.根据权利要求3所述的方法，其特征在于，所述根据机械臂末端相对于惯性系的动态稳定性，以及抓捕机构的旋转角速度，结合面向控制的空间机器人模型，定义期望动态特性，包括：

对式(8)微分得到：

${\stackrel{\·\·}{x}}_h={\stackrel{\·}{J}}^{*}\stackrel{\·}{\mathrm{\φ}}+J^{*}\stackrel{\·\·}{\mathrm{\φ}}+{\stackrel{\·\·}{x}}_{gh}---\left(13\right)$

定义中间变量v，令

$\mathrm{\τ}=H^{*}v+c^{*}-J_b^{*}{F}_b---\left(14\right)$

将式(14)代入式(2)，得：

$\stackrel{\·\·}{\mathrm{\φ}}=v---\left(15\right)$

定义中间变量f，令

$v={\left[\begin{array}{c}{J}^{*}\\ S_n\end{array}\right]}^{-1}(f-\left[\begin{array}{c}{\stackrel{\·}{J}}^{*}\stackrel{\·}{\mathrm{\φ}}+{\stackrel{\·\·}{x}}_{gh}\\ 0\end{array}\right])---\left(16\right)$

其中，S_n＝[0,0,...,0,1]∈R^1×n；

根据式(13)可得：

$\left[\begin{array}{c}{\stackrel{\·\·}{x}}_h\\ {\stackrel{\·\·}{\mathrm{\φ}}}_h\end{array}\right]=f---\left(17\right)$

令

$f=\left[\begin{array}{c}{M}_i^{-1}(-D_i\mathrm{\Δ}{\stackrel{\·}{x}}_h-K_i\mathrm{\Δ}{x}_h)\\ -\mathrm{\Δ}{\stackrel{\·}{\mathrm{\φ}}}_n+{\stackrel{\·}{\mathrm{\φ}}}_{ref}\end{array}\right]---\left(18\right)$

则，得到表征期望动态特性的式(11)和式(12)。

5.根据权利要求1所述的方法，其特征在于，所述基座位姿控制律如下：

$e=K_P{\mathrm{\Δx}}_b+K_i\∫{\mathrm{\Δx}}_{b}dt+K_D\mathrm{\Δ}{\stackrel{\·}{x}}_b---\left(21\right)$

其中，Δx_b表示基座在惯性系下的位姿相对于参考点r的摄动量，K_P为比例控制增益，K_i为积分控制增益，K_D为微分控制增益，Δx_b＝r-x_b。