CN103010491A - 一种气浮台抓捕试验机械臂控制方法 - Google Patents

Abstract

本发明公开了一种气浮台抓捕试验机械臂控制方法，包括基于运动学逆解迭代规划的机械臂抓捕和力闭环抓捕控制过程，基于运动学逆解迭代规划的机械臂抓捕过程根据不断更新的两气浮台相对距离和相对姿态变化，迭代更新两个多自由度机械臂各关节目标转角，在线规划轨迹直到可靠连接，从而提高了机械臂抓捕的自适应能力，使得目标特征部位抓捕冗余范围扩大；在触碰传感器检测到有效信号后，即由关节位置和速度闭环回路切换为机械臂力闭环控制过程，避免了机械臂对气浮台的硬触碰，引起弹离。本发明的控制方法能够保证机械臂抓捕连接的可靠性。

Description

一种气浮台抓捕试验机械臂控制方法

技术领域

本发明属于航天器地面物理试验技术领域，涉及一种气浮台抓捕试验机械臂控制方法。

背景技术

随着我国航天事业的发展，由于燃料耗尽等技术性原因，空间失效卫星增多，轨道资源和新航天任务需求的矛盾越来越突出，因此空间在轨服务成为未来航天事业发展的重要方向。由于失效卫星一般不能提供合作对接姿态，也没有合作对接机构，因此基于多自由度机械臂的空间捕获和操作控制是很好的选择。鉴于硬件和软件算法复杂程度高，需要在地面开展相关关键技术的试验验证。

在花岗岩平台开展的基于两个气浮台的超近距离交会和机械臂抓捕试验是验证超近距离相对运动导航、制导和精确控制技术的重要手段。然而，花岗岩台面的非理想状态和推力器的滞环效应，使得抓捕前两气浮台相对状态保持精度不高。其次，机械臂抓捕运动过程对气浮台带来干扰力和干扰力矩，使得两个气浮台相对状态在抓捕过程中不断变化，目标抓捕部位随之变化。上述两点都会引起两气浮台相对距离和相对姿态与标称状态相比存在较大偏差，如果采用地面一次规划机械臂轨迹的方法，则不能完成对目标气浮台的可靠抓捕，因此对抓捕机械臂的自适应性提出了很高要求。由于气浮台是漂浮基座且抓捕机构末端采用夹持连接方式，机械臂与目标气浮台接触后，如果径向不加控制，没有足够摩擦力，容易分离。此外，由于双臂运动不能完全同步，可能出现单臂与目标气浮台接触的现象，发生弹离或者反复碰撞，大大影响抓捕的可靠性。

发明内容

本发明所要解决的技术问题是提供一种气浮台抓捕试验机械臂控制方法，在两气浮台相对距离和相对姿态与标称状态相比存在较大偏差的条件下仍然能够保证对选定目标特征部位的可靠抓捕，并且能够保证不发生机械臂与目标气浮台结构的脱离。

本发明包括如下技术方案：

一种气浮台抓捕试验机械臂控制方法，所述机械臂为两个多自由度机械臂，步骤如下：

(1)根据相对测量敏感器输出的相对距离确定是否进行预抓捕，当追踪气浮台与目标气浮台之间的相对距离小于预抓捕设定距离时，两个机械臂进行预抓捕，将机械臂伸展到目标气浮台大包络范围；

(2)采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算每个机械臂目标特征抓捕部位坐标；根据每个机械臂目标特征抓捕部位坐标和运动学逆解算法解算每个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时；

(3)对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望运动轨迹运动；

(4)对于每个机械臂，根据安装在机械臂作用末端的触碰传感器输出的信号判断机械臂是否与目标气浮台接触，如果机械臂接触目标气浮台，则转入步骤(5)，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤(2)，如果没有到达第一时间间隔则转入步骤(3)；

(5)根据机械臂作用末端安装的压力传感器输出的信号进行力闭环控制，使得机械臂目标特征抓捕部位与目标气浮台的接触力恒定。

所述多自由度机械臂为三自由度机械臂，该三自由度机械臂包括三个连杆和三个关节，其中第一连杆、第二连杆的长度为l1，l2；第三个连杆长度为0；所述步骤(2)中的运动学逆解算法如下：

其中，θ₁，θ₂，θ₃分别为第1关节、第2关节、和第3关节目标转角；(p_x，p_y)为机械臂目标特征抓捕部位坐标。

所述步骤(3)的力闭环控制方法如下：

首先由设定的抓捕末端与目标气浮台期望压力与力传感器输出压力值做差，通过两次积分及运动学逆解转换得到期望的关节转角；

然后由期望的关节转角与码盘测角装置获得的实际关节转角做差，并通过第一PID控制器滤波，得到期望的关节角速度；

最后对码盘测角装置获得的实际关节转角进行差分获得实际关节角速度，对期望关节角速度与实际关节角速度做差，通过第二PID控制器调整得到机械臂各关节电机的驱动指令；最后通过各关节电机驱动机械臂运动。

本发明与现有技术相比具有如下优点：

本发明提出了基于运动学逆解迭代规划的机械臂抓捕控制方法，根据不断更新的两气浮台相对距离和相对姿态变化，迭代更新两个多自由度机械臂各关节目标转角，在线规划轨迹直到可靠连接，从而提高了机械臂抓捕的自适应能力，使得目标特征部位抓捕冗余范围扩大。本发明设计了包括力传感器的闭环回路，在触碰传感器检测到有效信号后，即由关节位置和速度闭环回路切换为机械臂力闭环控制回路，避免了机械臂对气浮台的硬触碰，引起弹离。总之，本发明的控制方法能够保证机械臂抓捕连接的可靠性。

附图说明

图1为气浮台抓捕试验示意图。

图2为本发明气浮台抓捕试验机械臂控制方法的示意图。

图3为机械臂关节位置和速度双闭环控制回路示意图。

图4为力闭环控制回路示意图。

图5为单机械臂结构示意图。

图6为两个机械臂抓捕目标气浮台示意图。

具体实施方式

下面就结合附图对本发明做进一步介绍。

气浮台抓捕试验示意图如图1所示，在地面试验阶段用追踪气浮台模拟追踪星(在轨可控卫星)，用目标气浮台模拟目标星(失效卫星)；在追踪气浮台上安装两个对称多自由度机械臂。追踪及目标气浮台为均匀质量圆柱体，其质量m＝200Kg，底面半径r＝0.5m。追踪气浮台上设置控制器，在该控制器的控制下能够使得追踪气浮台向目标气浮台运动。在接近及抓捕过程中，追踪气浮台和目标气浮台之间的相对距离和相对姿态信息对可靠实施抓捕及连接至关重要，通过安装在追踪气浮台的相对测量敏感器获得上述相对距离和相对姿态信息。

由于气浮台运动为漂移基座，水平方向不受摩擦阻力影响，因此如果机械臂作用末端直接与目标气浮台外沿硬接触，由动量守恒，必将造成分离，不能可靠连接。为保证连接机构迅速、安全、可靠抓捕目标星，本发明在机械臂作用末端增加了触碰传感器和压力传感器，在触碰传感器检测到触碰信息后，立即将机械臂控制从末端位置闭环控制切换为力闭环控制，通过力闭环控制保证接触点压力恒定，从而确保抓捕连接的可靠性。

如图2所示，本发明的气浮台抓捕试验机械臂控制方法的控制过程如下：

一、进行预抓捕

由于机械臂初始处于收拢状态，为了避免机械臂大范围运动时间过长和对气浮台本体状态影响过大，根据相对测量敏感器输出的测量信息判断是否进行预抓捕；当目标气浮台进入追踪气浮台5m左右范围时，两个机械臂按程控指令进行预抓捕控制，将两个机械臂伸展到目标气浮台大包络范围。

二、判断追踪气浮台是否到达抓捕点(即由相对测量敏感器测量得到的相对位置和相对姿态判断是否进入有效抓捕范围)，如果到达则转入步骤三，如果没有到达则控制追踪气浮台接近目标气浮台，直至到达抓捕点；

所述抓捕点根据机械臂关节结构尺寸确定；例如抓捕点应为追踪气浮台与目标气浮台距离为0.75m±0.05m的点。

三、采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算两个机械臂目标特征抓捕部位坐标；根据两个机械臂目标特征抓捕部位坐标和运动学逆解算法解算两个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时；所述第一时间间隔例如可以是2s。

目标特征抓捕部位坐标是机械臂末端与目标气浮台接触部位的坐标。

以双臂三自由度机械臂为例，关节期望运动轨迹的确定方法如下：令当前两个机械臂关节转角为

抓捕部位对应的目标关节转角为

其中θ₁、θ₂、θ₃为第一三自由度机械臂三个关节的转角；θ₄、θ₅、θ₆为第二三自由度机械臂三个关节的转角；机械臂关节转角q随时间t变化，其运动轨迹可以用一个平滑插值函数表示。这里用时间t的3阶多项式插值来逼近各关节转角的函数。

${\mathrm{\θ}}_1(b_1,t)=\underset{l=1}{\overset{3}{\mathrm{\Σ}}}{b}_{1l}{t}^i$

      .

     .

    .

${\mathrm{\θ}}_6(b_6,t)=\underset{l=1}{\overset{3}{\mathrm{\Σ}}}{b}_{6l}{t}^l$

使得 ${\mathrm{\θ}}_j(b_j,t_0)={\mathrm{\θ}}_j^0,$ ${\mathrm{\θ}}_j(b_j,t_f)={\mathrm{\θ}}_j^f,$ 两点边值条件成立。

四、对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望轨迹运动。

具体为，每隔1ms采集各关节测角装置信号，得到实际关节转角(角位置)，并差分得到关节角速度，根据采样时刻t和步骤三中的各关节转角的函数计算当前时刻的指令关节转角，并差分获得指令关节角速度，分别与实际关节转角和角速度做差，构成机械臂关节位置和速度双闭环回路，如图3所示。

五、监测触碰传感器信号，如果机械臂接触目标气浮台，则转入步骤六，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤三，如果没有到达第一时间间隔则转入步骤四；

六、进行力闭环控制，将压力传感器信号接入，在机械臂关节位置和速度闭环外增加压力闭环回路，使得抓取末端与目标台体间压力恒定，从而保证抓捕效果，不发生分离现象。如图4所示，在力闭环控制回路中包括外框压力闭环、中框位置闭环和内框速度闭环三个回路。首先由设定的抓捕末端与目标气浮台期望压力与力传感器测值做差，由于力对应加速度，因此通过两次积分及运动学逆解转换，得到期望的关节转角；然后由期望的关节转角与码盘测角装置获得的实际关节转角做差，并通过第一PID控制器滤波，得到期望的关节角速度；第三步由期望关节角速度与实际转角差分得到的关节角速度做差，通过第二PID控制器调整，得到机械臂各关节电机的驱动指令(一般为电压形式)；最后通过关节电机驱动机械臂运动。压力传感器和码盘测角装置分别完成接触部位压力和机械臂各关节转角的测量。

当机械臂与目标星接触时，触碰传感器反馈接触信息，机械臂降低驱动速度、减少接触冲击，同时压力传感器反馈连接压力信息，机械臂根据这些传感器信号闭环控制，保证足够的抓捕摩擦力。

如果连续10个周期双臂末端压力传感器读数稳定，则认为抓捕成功，结束机械臂抓捕控制过程。

通过上述控制方法，保证了无论哪个臂与目标气浮台接触，均自动转入力闭环模式，保持接触部位压力恒定，即机械臂与目标气浮台不发生分离，通过测试双臂不同步运动时差可以控制在3s以内(即双臂同时从初始位置开始运动，到达目标位置的时间差)，由于接触部位压力相对气浮台质量很小，目标气浮台加速度有限，即使单臂接触，也可使气浮台短时间运动在可控范围内，同时，通过上述迭代规划控制，使得另一只臂末端到达指定抓捕部位，构成双臂组合体连接。因此可以克服双臂运动不同步问题。

下面对步骤三进行详细介绍：

对包括多个连杆和关节的平面机械臂系统，设连杆i的坐标系{i}置于i+1关节上，并固定于连杆i上，坐标系{i}与连杆i无相对运动，o_iz_i轴与关节i+1的轴线重合，o_ix_i沿连杆方向，o_iy_i符合右手定则。坐标系{i-1}向坐标系{i}的齐次变换矩阵可以写为：

$A_{i-1}=\left[\begin{array}{cc}{R}_{i-1}& p_{i-1}\\ 000& 1\end{array}\right]$

其中R_i-1为坐标系{i}相对坐标系{i-1}的姿态转换矩阵，p_i-1为坐标系{i}原点相对坐标系{i-1}原点的位置矢量。进一步可以写为：

A_i＝Rot(z_i-1，θ_i)Trans(0，0，d_i)Trans(a_i，0，0)Rot(x_i，α_i)

$A_i=\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_i& -{\sin \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& {\sin \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& a_i{\cos \mathrm{\θ}}_i\\ i{\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& a_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

其中θ_i为关节i绕转动轴o_i-1z_i-1转角，d_i为关节i和关节i+1沿o_i-1z_i-1轴的距离，a_i为连杆i的长度，α_i为关节i+1转轴o_iz_i绕连杆方向o_ix_i转动的偏转角。以此类推，第n个连杆相对于机械臂安装基座坐标系的转换矩阵为

T_n＝A₁A₂…A_n

下面以单臂为三自由度平面关节机械臂为例进行说明，示意图如图5。其中坐标系OX₀Y₀为机械臂安装基座坐标系，与气浮台质心本体系方向一致，两者的原点相差一个气浮台半径。机械臂第一个和第二个连杆长度分别为l₁，l₂，连杆坐标系均在关节处，则矩阵A的参数是：

连杆编号	关节变量	αi	ai	di	cosαi	sinαi
							1	θ1	0deg	l1	0	1	0
2	θ2	0deg	l2	0	1	0
							3	θ3	0deg	0	0	1	0

则有

$T_3=A_1{A}_2{A}_3=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0& l_1{c}_1+l_2{c}_{12}\\ s_{123}& c_{123}& 0& l_1{s}_1+l_2{s}_{12}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

其中 $\left\{\begin{array}{c}{c}_i={\cos \mathrm{\θ}}_i\\ s_i={\sin \mathrm{\θ}}_i\\ i=\mathrm{1,2,3}\end{array}\right.,$ $\left\{\begin{array}{c}{c}_{12}=c_1{c}_2-s_1{s}_2\\ c_{123}=c_{12}{c}_3-s_{12}{s}_3\end{array}\right.,$ $\left\{\begin{array}{c}{s}_{12}=c_1{s}_2-s_1{c}_2\\ s_{123}=c_{12}{s}_3-s_{12}{c}_3\end{array}\right.$ T₃中第四列是机械臂末端相对于机械臂安装基座坐标系的矢量位置

令 $p=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\\ 1\end{array}\right]=\left[\begin{array}{c}{l}_1{c}_1+l_2{c}_{12}\\ l_1{s}_1+l_2{s}_{12}\\ 0\\ 1\end{array}\right]$

逆向求解可得

$\left\{\begin{array}{c}{\tan \mathrm{\θ}}_1=\frac{l_1{p}_y+l_2{\cos \mathrm{\θ}}_2-p_x{l}_2{\sin \mathrm{\θ}}_2}{l_1{p}_x+l_2{p}_x{\cos \mathrm{\θ}}_2+l_2{p}_y{\sin \mathrm{\θ}}_2}\\ {\mathrm{\θ}}_2=\arccos [(p_x^2+p_y^2-l_1^2-l_2^2)/\left({2l}_1{l}_2\right)]\end{array}\right.---\left(1\right)$

如图6所示，安装在追踪气浮台上的两个机械臂都为三自由度机械臂，其中一个机械臂包括连杆1、2和3；另一个机械臂包括连杆4、5和6。两个机械臂目标特征抓捕部位的位置设为：两个圆心连接线相垂直的与圆外围相接的两个点，即(X₁，Y₁)，(X₂，Y₂)，由相对测量敏感器给定的偏转角(坐标系取追踪气浮台本体坐标系)，两星之间的距离d，可以推出(X₀，Y₀)的坐标，进而可以推导出两个抓取点(X₁，Y₁)，(X₂，Y₂)的坐标。设气浮台半径为r，求解的(X₀，Y₀)坐标：

以右臂为例，左臂类似求解，关节3抓取位置与虚线垂直，所以关节3的坐标为：

对公式(1)设p_x＝X₂，p_y＝Y₂，解得θ₁，θ₂，进而求得θ₃

根据相对测量敏感器获得的相对姿态和相对距离信息，在特定步长下更新和d，进而重新解算目标关节转角θ₁，θ₂，θ₃。从而构成基于运动学逆解迭代规划的机械臂抓捕控制方法。

本发明所提出的基于迭代规划和力闭环的机械臂控制方法已经成功应用于气浮台相对运动地面试验。试验结果表明通过采用基于运动学逆解迭代规划的机械臂抓捕控制方法，使得机械臂能够在距离误差0.3m，姿态误差5deg范围内实现对目标气浮台特征部位的可靠抓捕。待任一臂末端触碰传感器检测到信号后，切换到力闭环控制，保证机械臂末端施加压力恒定，试验结果表明机械臂接触目标气浮台外沿后，没有发生碰撞分离现象。物理试验结果表明，本发明方法切实有效保证了可靠抓捕和连接。本发明的主要技术内容可推广应用于在轨服务、空间站等机械臂抓捕地面物理仿真试验中。

本发明未详细说明部分属本领域技术人员公知常识。

Claims (3)

1.一种气浮台抓捕试验机械臂控制方法，所述机械臂为两个多自由度机械臂，其特征在于，步骤如下：

(1)根据相对测量敏感器输出的相对距离确定是否进行预抓捕，当追踪气浮台与目标气浮台之间的相对距离小于预抓捕设定距离时，两个机械臂进行预抓捕，将机械臂伸展到目标气浮台大包络范围；

(2)采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算每个机械臂目标特征抓捕部位坐标；根据每个机械臂目标特征抓捕部位坐标和运动学逆解算法解算每个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时；

(3)对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望运动轨迹运动；

(4)对于每个机械臂，根据安装在机械臂作用末端的触碰传感器输出的信号判断机械臂是否与目标气浮台接触，如果机械臂接触目标气浮台，则转入步骤(5)，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤(2)，如果没有到达第一时间间隔则转入步骤(3)；

(5)根据机械臂作用末端安装的压力传感器输出的信号进行力闭环控制，使得机械臂目标特征抓捕部位与目标气浮台的接触力恒定。

2.根据权利要求1所述的控制方法，其特征在于，所述多自由度机械臂为三自由度机械臂，该三自由度机械臂包括三个连杆和三个关节，其中第一连杆、第二连杆的长度为l₁，l₂；第三个连杆长度为0；所述步骤(2)中的运动学逆解算法如下：

其中，θ₁，θ₂，θ₃分别为第1关节、第2关节、和第3关节目标转角；(p_x，p_y)为机械臂目标特征抓捕部位坐标。

3.根据权利要求1所述的控制方法，其特征在于，所述步骤(3)的力闭环控制方法如下：

首先由设定的抓捕末端与目标气浮台期望压力与力传感器输出压力值做差，通过两次积分及运动学逆解转换得到期望的关节转角；

然后由期望的关节转角与码盘测角装置获得的实际关节转角做差，并通过第一PID控制器滤波，得到期望的关节角速度；

最后对码盘测角装置获得的实际关节转角进行差分获得实际关节角速度，对期望关节角速度与实际关节角速度做差，通过第二PID控制器调整得到机械臂各关节电机的驱动指令；最后通过各关节电机驱动机械臂运动。

Images