专利名称：一种气浮台抓捕试验机械臂控制方法
技术领域：
本发明属于航天器地面物理试验技术领域，涉及一种气浮台抓捕试验机械臂控制方法。
背景技术：
随着我国航天事业的发展，由于燃料耗尽等技术性原因，空间失效卫星增多，轨道资源和新航天任务需求的矛盾越来越突出，因此空间在轨服务成为未来航天事业发展的重要方向。由于失效卫星一般不能提供合作对接姿态，也没有合作对接机构，因此基于多自由度机械臂的空间捕获和操作控制是很好的选择。鉴于硬件和软件算法复杂程度高，需要在地面开展相关关键技术的试验验证。在花岗岩平台开展的基于两个气浮台的超近距离交会和机械臂抓捕试验是验证超近距离相对运动导航、制导和精确控制技术的重要手段。然而，花岗岩台面的非理想状态和推力器的滞环效应，使得抓捕前两气浮台相对状态保持精度不高。其次，机械臂抓捕运动过程对气浮台带来干扰力和干扰力矩，使得两个气浮台相对状态在抓捕过程中不断变化，目标抓捕部位随之变化。上述两点都会引起两气浮台相对距离和相对姿态与标称状态相比存在较大偏差，如果采用地面一次规划机械臂轨迹的方法，则不能完成对目标气浮台的可靠抓捕，因此对抓捕机械臂的自适应性提出了很高要求。由于气浮台是漂浮基座且抓捕机构末端采用夹持连接方式，机械臂与目标气浮台接触后，如果径向不加控制，没有足够摩擦力，容易分离。此外，由于双臂运动不能完全同步，可能出现单臂与目标气浮台接触的现象，发生弹离或者反复碰撞，大大影响抓捕的可靠性。
发明内容
本发明所要解决的技术问题是提供一种气浮台抓捕试验机械臂控制方法，在两气浮台相对距离和相对姿态与标称状态相比存在较大偏差的条件下仍然能够保证对选定目标特征部位的可靠抓捕，并且能够保证不发生机械臂与目标气浮台结构的脱离。本发明包括如下技术方案一种气浮台抓捕试验机械臂控制方法，所述机械臂为两个多自由度机械臂，步骤如下(I)根据相对测量敏感器输出的相对距离确定是否进行预抓捕，当追踪气浮台与目标气浮台之间的相对距离小于预抓捕设定距离时，两个机械臂进行预抓捕，将机械臂伸展到目标气浮台大包络范围；(2)采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算每个机械臂目标特征抓捕部位坐标；根据每个机械臂目标特征抓捕部位坐标和运动学逆解算法解算每个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时；
(3)对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望运动轨迹运动；(4)对于每个机械臂，根据安装在机械臂作用末端的触碰传感器输出的信号判断机械臂是否与目标气浮台接触，如果机械臂接触目标气浮台，则转入步骤(5)，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤(2)，如果没有到达第一时间间隔则转入步骤(3)；(5)根据机械臂作用末端安装的压力传感器输出的信号进行力闭环控制，使得机械臂目标特征抓捕部位与目标气浮台的接触力恒定。所述多自由度机械臂为三自由度机械臂，该三自由度机械臂包括三个连杆和三个关节，其中第一连杆、第二连杆的长度为11，12 ;第三个连杆长度为O ;所述步骤(2)中的运
动学逆解算法如下
权利要求
1.一种气浮台抓捕试验机械臂控制方法，所述机械臂为两个多自由度机械臂，其特征在于，步骤如下 (1)根据相对测量敏感器输出的相对距离确定是否进行预抓捕，当追踪气浮台与目标气浮台之间的相对距离小于预抓捕设定距离时，两个机械臂进行预抓捕，将机械臂伸展到目标气浮台大包络范围； (2)采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算每个机械臂目标特征抓捕部位坐标；根据每个机械臂目标特征抓捕部位坐标和运动学逆解算法解算每个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时； (3)对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望运动轨迹运动； (4)对于每个机械臂，根据安装在机械臂作用末端的触碰传感器输出的信号判断机械臂是否与目标气浮台接触，如果机械臂接触目标气浮台，则转入步骤(5)，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤(2)，如果没有到达第一时间间隔则转入步骤(3)； (5)根据机械臂作用末端安装的压力传感器输出的信号进行力闭环控制，使得机械臂目标特征抓捕部位与目标气浮台的接触力恒定。
2.根据权利要求1所述的控制方法，其特征在于，所述多自由度机械臂为三自由度机械臂，该三自由度机械臂包括三个连杆和三个关节，其中第一连杆、第二连杆的长度为I1,I2 ;第三个连杆长度为O ;所述步骤(2)中的运动学逆解算法如下
3.根据权利要求1所述的控制方法，其特征在于，所述步骤(3)的力闭环控制方法如下 首先由设定的抓捕末端与目标气浮台期望压力与力传感器输出压力值做差，通过两次积分及运动学逆解转换得到期望的关节转角； 然后由期望的关节转角与码盘测角装置获得的实际关节转角做差，并通过第一 PID控制器滤波，得到期望的关节角速度； 最后对码盘测角装置获得的实际关节转角进行差分获得实际关节角速度，对期望关节角速度与实际关节角速度做差，通过第二 PID控制器调整得到机械臂各关节电机的驱动指令；最后通过各关节电机驱动机械臂运动。
全文摘要
本发明公开了一种气浮台抓捕试验机械臂控制方法，包括基于运动学逆解迭代规划的机械臂抓捕和力闭环抓捕控制过程，基于运动学逆解迭代规划的机械臂抓捕过程根据不断更新的两气浮台相对距离和相对姿态变化，迭代更新两个多自由度机械臂各关节目标转角，在线规划轨迹直到可靠连接，从而提高了机械臂抓捕的自适应能力，使得目标特征部位抓捕冗余范围扩大；在触碰传感器检测到有效信号后，即由关节位置和速度闭环回路切换为机械臂力闭环控制过程，避免了机械臂对气浮台的硬触碰，引起弹离。本发明的控制方法能够保证机械臂抓捕连接的可靠性。
文档编号B64G4/00GK103010491SQ20121050443
公开日2013年4月3日 申请日期2012年11月30日 优先权日2012年11月30日
发明者朱志斌, 何英姿, 魏春岭, 唐强, 张勇智 申请人:北京控制工程研究所