CN109571484A - 一种用于自组装任务的空间机械臂柔顺装配控制方法 - Google Patents

Abstract

本发明实施例提供了一种用于自组装任务的空间机械臂柔顺装配控制方法，包括：获得漂浮基座空间机械臂正向动力学模型；依据所述漂浮基座空间机械臂正向动力学模型，获得舱体组装过程的封闭链结构动力学方程；依据所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程；依据所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型；依据所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律。根据本发明实施例提供的技术方案，可实现控制机械臂输出力矩的同时完成封闭链结构的柔顺阻抗控制，保证舱体‑机械臂‑底座组成的封闭链结构的位置和姿态的柔顺稳定，从而实现空间机械臂稳定可靠进行舱体自组装任务的目标。

Description

一种用于自组装任务的空间机械臂柔顺装配控制方法

【技术领域】

本发明涉及空间机器人柔顺装配控制技术，尤其涉及一种用于自组装任务的空间机械臂柔顺装配控制方法。

【背景技术】

目前，中国载人航天工程已全面转入空间站阶段，在空间站的构建、运行和维护过程中，需要利用空间机械臂辅助完成舱体组装、舱外货物搬运和安装、暴露平台实验载荷照料(主要是大型实验模组的插拔)以及悬停的飞行器辅助对接等操作任务。在上述在轨操作任务过程中，安装在空间站基座上的空间机械臂需要操作具有大质量和大惯量的待组装物体(如舱体、悬停飞行器、舱外货物及大型实验载荷等)与基座(用于安装机械臂的空间站本体)表面相应的接口进行装配，即空间机械臂需要完成大负载在轨自组装任务。空间机械臂在执行在轨自组装任务过程中，整个系统处于自由漂浮状态，当大负载目标与航天器基座发生碰撞时，较大的冲击力会造成舱体破坏，位置失控等影响，因此需要减小碰撞力，提供组装过程中的柔顺性。因此，面向大负载在轨自组装任务，开展自组装过程中的空间机械臂柔顺控制方法研究，对于未来进一步的空间探索，特别是空间机械臂的在轨应用具有十分重要的科学意义和理论价值。

现有空间舱体装配过程的空间机械臂柔顺控制算法，延用了开链式结构的柔顺控制方法。针对空间舱体装配过程中，基座-空间机械臂-待组装舱体组成的封闭链式结构，该方法没有考虑到装配过程中，空间机械臂末端的作用力会同时作用于基座，并通过基座对机械臂各个关节产生耦合力矩，影响控制精度。单纯的通过调整控制率的控制参数并不能解决控制模型不匹配带来的精度差的问题。

【发明内容】

有鉴于此，本发明实施例提供了一种用于自组装任务的空间机械臂柔顺装配控制方法，通过控制机械臂输出力矩的同时完成封闭链结构的柔顺阻抗控制，以实现空间机械臂稳定可靠进行舱体自组装任务。

本发明实施例提供了一种用于自组装任务的空间机械臂柔顺装配控制方法，包括：

获得漂浮基座空间机械臂正向动力学方程；

依据所述漂浮基座空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程；

依据所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程；

依据所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型；

依据所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律。

上述方法中，所述依据所述漂浮基座空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程为：

其中， 为基座的加速度项，为机械臂关节角速度，c_b,c_m分别为基座及机械臂速度依赖的非线性项，F_b是核心舱所受的扰动外力和外力矩，τ为机械臂关节输出力矩，J_b和J_m分别为描述与与映射关系的雅克比矩阵，F_T是舱体组装过程中所产生的碰撞力和摩擦力的合力，r_eh为接触点指向目标舱质心的向量表示。

上述方法中，利用所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程为：

其中，为空间机械臂相对于关节的惯性张量矩阵，为机械臂关节加速度，为关节速度依赖项，τ为机械臂关节输出力矩，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，F_e为机械臂和外界环境所产生的作用力，J_bm为基座-机械臂雅克比矩阵，F_b为核心舱所受的扰动外力。

上述方法中，利用所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型为：

其中，为空间机械臂相对于关节的惯性张量矩阵在操作空间中的表示， 为关节速度依赖项在操作空间中的表示，E为n阶单位阵，T为基座对机械臂的作用力的系数矩阵，F_e为机械臂和外界环境所产生的作用力。

上述方法中，利用所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律为：

其中，τ_m为机械臂关节输出力矩，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，为空间机械臂相对于关节的惯性张量矩阵在操作空间的表示，M_i,B_i,K_i均为对角正定对称矩阵，分别代表在操作空间的期望惯量、期望阻尼和期望刚度，x_r代表操作环境的参考位置，代表机械臂末端的实际位置，为机械臂关节速度依赖项在操作空间的表示，T为基座对机械臂的作用力的系数矩阵，F_d为理想接触力，F_e′为机械臂对环境的作用力。

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，依据漂浮基空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程，进而依据舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程，进而依据获得空间机械臂自组装封闭链结构关节空间动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型，进而依据空间机械臂自组装封闭链结构操作空间阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律，通过控制机械臂的输出力矩，完成封闭链结构的柔顺阻抗控制，保证舱体-机械臂-底座组成的封闭链结构的位置和姿态的柔顺稳定，以实现空间机械臂稳定可靠进行舱体自组装任务的目标。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性和劳动性的前提下，还可以根据这些附图获得其它附图。

图1是本发明实施例所提供的用于自组装任务的空间机械臂柔顺装配控制方法的流程示意图；

图2是本发明实施例所提供的漂浮基座空间机械臂动力学模型示意图；

图3是利用本发明实施例所提供的方法对初始期望接触力为80N的碰撞进行控制的位置跟踪效果曲线图；

图4是利用本发明实施例所提供的方法对初始期望接触力为80N的碰撞进行控制的力跟踪效果曲线图；

图5是利用本发明实施例所提供的方法对初始期望接触力为80N的碰撞进行控制的关节角速度跟踪效果曲线图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例给出一种用于自组装任务的空间机械臂柔顺装配控制方法，请参考图1，其本发明实施例所提供的用于自组装任务的空间机械臂柔顺装配控制方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，首先，获得漂浮基座空间机械臂正向动力学方程。

具体的，针对漂浮基座的空间机械臂，获得如下动力学方程，用以描述漂浮基座空间机械臂关节力矩与关节运动的映射关系：

其中， 为基座的加速度项，为空间机械臂关节的角加速度项，c_b,c_m分别为基座及机械臂速度依赖的非线性项，τ_m为机械臂关节输出力矩。

步骤102，依据所述漂浮基座空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程。

具体的，考虑空间舱体组装过程中所产生的法向碰撞力和摩擦力模型，假设接纳锥的法向单位向量为对接球头移动方向的单位向量为依据力的合成原理，获得舱体组装过程中所产生碰撞力和摩擦力的合力为：

其中，F_N为接触点法向正压力，为法向单位向量，F_f为接触点切向摩擦力，为切向单位向量。

利用如下公式获得机械臂末端的作用力和力矩为：

其中，r_eh为接触点指向目标舱质心的向量表示，F_T是舱体组装过程中所产生的碰撞力和摩擦力的合力。

利用如下公式获得核心舱所受的扰动外力和外力矩为：

其中，r为接触点指向核心舱(基座)质心的向量表示，F_T是舱体组装过程中所产生的碰撞力和摩擦力的合力。

在自组装过程中，除碰撞力以外，相较于普通的空间机械臂，待组装舱体也会影响系统的动力学特征，可以获得如下待组装舱体运动带来的作用力为：

其中，H_L为待组装舱体的惯性矩阵，为待组装舱体的广义加速度。

利用如下公式获得作用在空间机械臂上的力矩为：

其中，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，H_L为待组装舱体的惯性矩阵，为待组装舱体的广义加速度。

将公式(1)、公式(3)结合公式(6)，获得舱体组装过程中的动力学方程为：

其中， 为基座的加速度项，为机械臂关节角速度，F_b是核心舱所受的扰动外力和外力矩，τ为机械臂关节输出力矩，J_b和J_m分别为描述与与映射关系的雅克比矩阵，F_T是舱体组装过程中所产生的碰撞力和摩擦力的合力，r_eh为接触点指向目标舱质心的向量表示。

步骤103，依据所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链关节空间动力学方程。

将公式(7)展开：

联立方程组消去得到：

将上式化简，获得自组装空间机械臂关节空间的动力学方程：

其中，为空间机械臂相对于关节的惯性张量矩阵，为机械臂关节加速度，为关节速度依赖项，τ为机械臂关节输出力矩，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，F_e为机械臂和外界环境所产生的作用力，J_bm为基座-机械臂雅克比矩阵，F_b为核心舱所受的扰动外力。

具体的，依据自组装空间机械臂关节空间的动力学方程，令F_e＝-F_b，获得组装状态下机械臂在关节空间的动力学方程：

其中，τ＝τ_m+τ_mL为空间机械臂的控制力矩，τ_ext为装配过程中舱体间碰撞引起的关节力矩如下：

结合公式(12)、公式(11)同时左乘获得空间机械臂操作空间的动力学方程：

其中，为空间机械臂相对于关节的惯性张量矩阵在操作空间中的表示，为关节速度依赖项在操作空间中的表示，F_e为机械臂和外界环境所产生的作用力，J_f为自由漂浮空间机械臂雅克比矩阵，T为基座对机械臂的作用力的系数矩阵，τ_m为机械臂关节输出力矩。

步骤104，依据所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型。

具体的，首先，依据如下的二阶线性阻抗函数：

其中，M、B、K分别为目标阻抗的惯性矩阵、阻尼矩阵、刚度矩阵，加速度一般通过测得的力间接获得。X和X_r分别代表机械臂末端的实际和参考运动轨迹。F_e为机械臂和外界环境所产生的作用力。阻抗函数是通过控制加速度、速度和位置偏差以达到控制外界作用力的效果。

然后，依据所述自组装空间机械臂操作空间的动力学方程并结合二阶线性阻抗函数，获得自组装空间机械臂操作空间阻抗控制模型：

其中，为空间机械臂相对于关节的惯性张量矩阵在操作空间中的表示， 为关节速度依赖项在操作空间中的表示，E为n阶单位阵，T为基座对机械臂的作用力的系数矩阵，F_e为机械臂和外界环境所产生的作用力。

步骤105，依据所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律。

具体的，依据阻抗模型获得如下方程：

其中，e＝(x_r-x_e),在对移动目标进行跟踪时，x_t可以作为环境参考位置替代x_r，F_e′为机械臂对环境的作用力，与F_e互为相反力。

当需要跟踪理想的接触力F_d时，需要将式(16)修改成如下形式：

联立公式(17)、公式(15)，获得自组装空间机械臂的操作空间的阻抗控制规律：

其中，τ_m为机械臂关节输出力矩，为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，为空间机械臂相对于关节的惯性张量矩阵在操作空间的表示，M_i,B_i,K_i均为对角正定对称矩阵，分别代表在操作空间的期望惯量、期望阻尼和期望刚度，x_r代表操作环境的参考位置，代表机械臂末端的实际位置，为机械臂关节速度依赖项在操作空间的表示，T为基座对机械臂的作用力的系数矩阵，F_d为理想接触力，F_e′为机械臂对环境的作用力。

依据本发明实施例提供的上述方法，对空间机械臂的自组装阻抗控制过程进行了仿真，对初始期望接触力为80N的组装碰撞控制开展仿真试验研究，并分别记录其位置跟踪效果曲线、力跟踪效果曲线和关节角速度跟踪效果曲线。

请参考图2，其为七自由度空间机械臂的运动学模型，其中，本文中提及的关节角度均以该构型为参考零位。为了与空间算子代数方法中坐标系一致，从基座到末端的关节编号依次为关节7至关节1。此外，七自由度机械臂的D-H坐标系在图5中给出，图中相应的尺寸如下：

d₇＝1.2m，d₆＝0.53m，d₅＝0.53m

a₅＝5.8m，d₄＝0.52m，a₄＝5.8m

d₃＝0.53m，d₂＝0.53m，d₁＝1.2m

Σ₈(即机械臂安装坐标系)在基座坐标系下位姿坐标为[-0.2m,0m,2m,0°,0°,0°]，负载操作任务起始时刻基座坐标系与惯性参考系Σ_I重合。机械臂D-H参数如表1所示，航天器基座和机械臂的质量特性参数如表2所示。

表1七自由度空间机械臂D-H参数表

连杆i	θ<sub>i</sub>/(°)	d<sub>i</sub>/(m)	a<sub>i-1</sub>/(m)	α<sub>i-1</sub>/(°)
						θ<sub>7</sub>(0)	d<sub>7</sub>	0	90
6	θ<sub>6</sub>(90)	d<sub>6</sub>	0	-90
					5	θ<sub>5</sub>(0)	0	a<sub>5</sub>	0
4	θ<sub>4</sub>(0)	d<sub>3</sub>+d<sub>4</sub>+d<sub>5</sub>	a<sub>4</sub>	0
					3	θ<sub>3</sub>(0)	0	0	90
2	θ<sub>2</sub>(-90)	d<sub>2</sub>	0	-90
					1	θ<sub>1</sub>(0)	d<sub>1</sub>	0	0

表2七自由度空间机械臂质量特性参数

设置关节角度为初始位姿[0.997m,3.995m,1.003m,0.000rad,0.001rad,-3.139rad]为起始构型，规划一段直线，起始点和目标点分别为：x_ini＝[1,4,1]^Tm,x_end＝[1,4.25,1]^Tm，引入末端期望操作力F_d＝[0,80,0]N，环境刚度K_env＝[1000,1000,1000]N/m，并调节控制参数为M_i＝30E，B_i＝50E，K_i＝0。使用本发明实施例的技术方案对上述任务进行仿真，仿真结果如图3～图5所示。

从仿真结果可以看出，随着任务的进行，机械臂末端y方向上输出力逐渐趋近于期望值，最终力误差为3×10^-3N。机械臂末端y方向上的位置误差也稳定在0.010m，达到了对机械臂操作空间的力控制效果。在此要对K_i参数的量化进行特别说明，在构造的操作空间阻抗方程时，若环境位置刚度参数确知情况下，可根据任务需求调整末端位置期望值，使得位置控制精度和接触力控制精度均满足任务需求；当环境位置刚度参数未确知情况下，末端位置误差的存在，会使对末端接触力的控制产生稳态误差，若要保证接触力的控制精度，需要将惯性增益K_i置为0。

本发明实施例的技术方案具有以下有益效果：

建立的封闭链式的自组装动力学模型，并以此做构型优化，能更加真实的反映实际情况；提出的自组装封闭链的阻抗控制方法，可实现大负载舱体组装过程中闭合链结构的稳定控制，为空间机械臂的保护方法提供了新思路；提出的控制方法可进一步应用于其他在轨操作任务及研究领域中。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (5)

1.一种用于自组装任务的空间机械臂柔顺装配控制方法，其特征在于,所述方法包括：

获得漂浮基座空间机械臂正向动力学方程；

依据所述漂浮基座空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程；

依据所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程；

依据所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型；

依据所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律。

2.根据权利要求1所述的方法，其特征在于，所述依据所述漂浮基座空间机械臂正向动力学方程，获得舱体组装过程的封闭链结构动力学方程，包括：

其中， 为基座的加速度项，为机械臂关节角速度，c_b,c_m分别为基座及机械臂速度依赖的非线性项，F_b是核心舱所受的扰动外力和外力矩，τ为机械臂关节输出力矩，J_b和J_m分别为描述与与映射关系的雅克比矩阵，F_T是舱体组装过程中所产生的碰撞力和摩擦力的合力，r_eh为接触点指向目标舱质心的向量表示。

3.根据权利要求2所述的方法，其特征在于，依据所述舱体组装过程的封闭链结构动力学方程，获得空间机械臂自组装封闭链结构关节空间动力学方程，包括：

其中，为空间机械臂相对于关节的惯性张量矩阵，为机械臂关节加速度，为关节速度依赖项，τ为机械臂关节输出力矩，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，F_e为机械臂和外界环境所产生的作用力，J_bm为基座-机械臂雅克比矩阵，F_b为核心舱所受的扰动外力。

4.根据权利要求3所述的方法，其特征在于，依据所述空间机械臂自组装封闭链关节空间的动力学方程，获得空间机械臂自组装封闭链结构操作空间阻抗控制模型，包括：

其中，为空间机械臂相对于关节的惯性张量矩阵在操作空间中的表示， 为关节速度依赖项在操作空间中的表示，E为n阶单位阵，T为基座对机械臂的作用力的系数矩阵，F_e为机械臂和外界环境所产生的作用力。

5.根据权利要求4所述的方法，其特征在于，依据所述自组装空间机械臂阻抗控制模型，获得空间机械臂自组装封闭链结构的阻抗控制规律，包括：

利用自组装空间机械臂操作空间阻抗控制模型，获得自组装空间机械臂的操作空间的阻抗控制规律：

其中，τ_m为机械臂关节输出力矩，J_f为自由漂浮空间机械臂雅克比矩阵，也称为广义雅克比矩阵，为空间机械臂相对于关节的惯性张量矩阵在操作空间的表示，M_i,B_i,K_i均为对角正定对称矩阵，分别代表在操作空间的期望惯量、期望阻尼和期望刚度，x_r代表操作环境的参考位置，代表机械臂末端的实际位置，为机械臂关节速度依赖项在操作空间的表示，T为基座对机械臂的作用力的系数矩阵，F_d为理想接触力，F_e′为机械臂对环境的作用力。