一种抓捕末阶段机械臂与操作平台协调控制方法
【技术领域】
[0001] 本发明属于空间机器人在轨维修维护领域，设及一种抓捕末阶段机械臂与操作平 台协调控制方法。
【背景技术】
[0002] 空间机器人系统包括至少一个机械臂和卫星基座（也称之为操作平台），由于空 间失重环境、空间机器人为非完整性约束，其操作平台和机械臂间存在动力学禪合，机械臂 的运动会对操作平台的位姿产生干扰，同时操作平台位姿的变化又影响机械臂的末端位 姿。尤其是操作平台的姿态发生一定的偏转会影响机械臂末端夹持工具抓捕目标。在有手 眼双目相机测量数据时，操作平台姿态的偏转不影响机械臂末端逼近目标可抓捕部位，其 末端的运动可W根据实时测量值来更新运动轨迹。但当机械臂末端距离待抓捕目标距离足 够近后（如小于10cm)，在该阶段，由于光学敏感器自身的限制（如相机安装、视场的限制 等），手眼双目相机无法继续提供有效的测量数据，运增加了机械臂末端工具抓捕目标的难 度。因此，在抓捕末阶段，即盲捕阶段，实现机械臂与操作平台之间的协调控制很有意义。
【发明内容】
[0003] 本发明的技术解决问题：克服现有技术的不足，提供一种抓捕末阶段机械臂与操 作平台协调控制方法，利用操作平台本体巧螺测量的姿态信息，并结合手眼相机提供的最 后一帖目标可抓捕点位姿信息，实时修正机械臂末端相对于目标可抓捕点的位姿，直至机 械臂末端执行器抓捕目标。
[0004] 本发明的技术解决方案是：一种抓捕末阶段机械臂与操作平台协调控制方法，步 骤如下：
[0005] 1)进入抓捕末阶段前，由机械臂末端手眼相机测量得到目标可抓捕部位的最后一 帖有效数据，确立在当前操作平台姿态构型A下，机械臂末端的抓捕期望位姿在机械臂基 坐标系下分别表示为Ατ。，Ap。，即At。表示期望姿态，Ap。表示期望位置.
[0006] 2)进入抓捕末阶段后，根据当前操作平台上巧螺测量到的操作平台姿态信息，计 算获得新的操作平台姿态构型B相对构型A的变化量：滚动角为、俯仰角为ΔΦ、偏航 角为A丫，结合最后一帖的测量数据，即机械臂末端在其基坐标系下的期望姿态At。和Αρ。， 则在新的操作平台姿态构型Β下机械臂末端的期望姿态将变为；
其中f贷是 由-ΔΦ和-Δ丫经过3-1-2坐标旋转得到的方向余弦阵，表示由构型A到构型B的变换；Bp。为操作平台在构型B下，机械臂末端期望位置在机械臂基坐标系下的位置平移 矢量，根据机械臂基坐标系与操作平台惯性系之间的坐标转换关系，得到如下关系式：
[0009] 其中J货为构型A到操作平台惯性系Q的方向余弦阵，f贷为构型B到操作平台惯 性系Q的方向余弦阵，中。为机械臂末端在操作平台的期望位置，中A和中e为安装偏差；
[0010] 结合上述两式，求得
W11]扣根据叫。和Bp。，由逆运动学计算得到机械臂各关节的修正控制量，驱动机械臂完 成对期望目标的抓捕。
[0012] 本发明与现有技术相比具有W下优点：
[0013] (1)在抓捕末阶段，本发明通过操作平台上的巧螺测量信息对机械臂末端位姿进 行补偿，进而解算机械臂关节控制量，完成对非合作目标的抓捕；
[0014] (2)本发明无需增加额外的测量装置，系统简单、有效；
[0015] (3)本发明能够大大提高了对非合作目标的抓捕成功率。
【附图说明】
[0016] 图1为坐标系变换关系；
[0017] 图2为机械臂末端位置变化曲线；
[0018] 图3为机械臂末端姿态变化曲线；
[0019] 图4为机械臂关节角变化曲线。
【具体实施方式】
[0020] 基于搭建的空间非合作目标抓捕操作地面试验系统，验证本发明设计的协调控制 算法，机械臂采用串并混联结构，末端测量设备为双目手眼相机，由相机测量机械臂末端与 可抓捕部位的相对期望位姿，解算得到期望位姿对应的机械臂的各关节构型，控制机械臂 末端移动到目标可抓捕部位，完成对星箭对接环的捕获。
[0021] 本发明一种抓捕末阶段机械臂与操作平台协调控制方法，步骤如下：
[0022] 1)进入抓捕末阶段前，由机械臂末端手眼相机测量得到目标可抓捕部位的最后一 帖有效数据，确立在当前操作平台姿态构型A下，机械臂末端的抓捕期望位姿在机械臂基 坐标系下分别表示为Ατ。，Ap。，即At。表示期望姿态，Ap。表示期望位置.
[0023] 2)进入抓捕末阶段后，根据当前操作平台上巧螺测量到的操作平台姿态信息，计 算获得新的操作平台姿态构型B相对构型A的变化量：滚动角为Δ声、俯仰角为ΔΦ、偏航 角为A丫，结合最后一帖的测量数据，即机械臂末端在其基坐标系下的期望姿态At。和Ap。， 则在新的操作平台姿态构型B下机械臂末端的期望姿态将变为：sj; = ，其中引?是 由-Δ科、-ΔΦ和-Δ丫经过3-1-2坐标旋转得到的方向余弦阵，表示由构型A到构型B的变换；Bp。为操作平台在构型B下，机械臂末端期望位置在机械臂基坐标系下的位置平移 矢量，根据机械臂基坐标系与操作平台惯性系之间的坐标转换关系，得到如下关系式：
[0026] 其中fi?为构型A到操作平台惯性系Q的方向余弦阵，f.孩为构型B到操作平台惯 性系Q的方向余弦阵，中。为机械臂末端在操作平台的期望位置，中A和中e为安装偏差；
[0027] 结合上述两式，求得
[0028] 3)根据叫。和Bp。，由逆运动学计算得到机械臂各关节的修正控制量，驱动机械臂完 成对期望目标的抓捕。
[0029] 本发明方法实施例所设及的试验系统包括一部安装于气浮平台上屯自由度机械 臂，机械臂末端安装双目手眼相机，为机械臂的抓捕操作提供测量信息。气浮平台上安装 IMU测量装置，为气浮平台提供测量信息。
[0030] 图1所示为各坐标系变换关系。由图1可知，在期望位姿解算过程中用到Ξ个坐标 系：操作平台惯性系、操作平台本体系和机械臂基座坐标系，运里操作平台本体系与机械臂 基座坐标系重合。机械臂末端期望位姿在操作平台惯性系下的状态是固定不变的，操作平 台本体系（机械臂基座坐标系）与操作平台惯性系的坐标转换关系可由IMU测量值得到。 因此，在已知构型A下机械臂末端期望位姿的情况下，可由操作平台本体系（机械臂基座坐 标系）与操作平台惯性系的坐标转换关系得到构型B下机械臂末端期望位置在操作平台本 体系（机械臂基座坐标系）下的表示。
[0031] 图2所示为机械臂末端位置变化曲线。由图2可知，在系统时间101340ms处，手 眼双目相机无法给出相对位姿的测量值。此时，由IMU测量平台的姿态信息反馈给机械臂 控制系统，对末端位置进行实时补偿，最终成功抓取星箭对接环。在101340ms之前，由于平 台姿态变化的扰动，机械臂末端期望位置在y、Z轴方向有轻微的变化，但此时双目手眼相 机能提供有效数据，因此机械臂末端位置渐近收敛到期望位置。机械臂末端X轴与平台的 运动解禪，因此该轴向机械臂末端期望位置保持恒值。
[0032] 图3所示为机械臂末端姿态变化曲线。由图3可知，在系统时间101340ms处，手 眼双目相机无法给出相对位姿的测量值。此时，由IMU测量平台的姿态信息反馈给机械臂 控制系统，对末端姿态进行实时补偿，最终成功抓取星箭对接环。在101340ms之前，由于平 台姿态变化的扰动，机械臂末端期望姿态在y、Z轴方向有轻微的变化，但此时双目手眼相 机能提供有效数据，因此机械臂末端姿态渐近收敛到期望姿态。机械臂末端X轴与平台的 运动解禪，因此该轴向机械臂末端期望姿态保持恒值。
[0033] 图4所示为机械臂关节角脉冲数变化曲线。由图4可知，机械臂各关节角运动平 滑。关节2为处于锁定状态，便于逆运动学解算。关节8为末端夹持工具的开合，初始为合 犹状态，进入抓捕末阶段，夹持工具打开，最终夹持工具合犹抓住目标。
[0034] 物理仿真试验结果表明，本发明提出的结合巧螺测量修正的抓捕末阶段机械臂与 操作平台协调控制方法能够有效的提高抓捕目标的成功率。本发明的主要技术内容可应用 于在空间机器人在轨维修维护任务，行星表面探测机器人机械臂抓取操作任务等。
[0035] 本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。
【主权项】
1. 一种抓捕末阶段机械臂与操作平台协调控制方法，其特征在于步骤如下： 1) 进入抓捕末阶段前，由机械臂末端手眼相机测量得到目标可抓捕部位的最后一帧有 效数据，确立在当前操作平台姿态构型A下，机械臂末端的抓捕期望位姿在机械臂基坐标 系下分别表示为AI；，APy即AI；表示期望姿态，表示期望位置； 2) 进入抓捕末阶段后，根据当前操作平台上陀螺测量到的操作平台姿态信息，计算获 得新的操作平台姿态构型B相对构型A的变化量：滚动角为Δρ、俯仰角为△φ、偏航角为 Δγ，结合最后一帧的测量数据，即机械臂末端在其基坐标系下的期望姿态Α?；和APe，则在 新的操作平台姿态构型B下机械臂末端的期望姿态将变为：5忍=忍，其中是由 -Δ炉、-Δφ和-Δγ经过3-1-2坐标旋转得到的方向余弦阵，表示由构型A到构型B的 变换；操作平台在构型B下，机械臂末端期望位置在机械臂基坐标系下的位置平移矢 量，根据机械臂基坐标系与操作平台惯性系之间的坐标转换关系，得到如下关系式：其中fi?为构型A到操作平台惯性系Q的方向余弦阵，fi?为构型B到操作平台惯性系Q的方向余弦阵，为机械臂末端在操作平台的期望位置，βΡΑ和βΡΒ为安装偏差； 结合上述两式，求得3尺=fiT1 ( + % - % ); 3) 根据BI；和BPy由逆运动学计算得到机械臂各关节的修正控制量，驱动机械臂完成对 期望目标的抓捕。
【专利摘要】一种抓捕末阶段机械臂与操作平台协调控制方法。进入抓捕末阶段前，由机械臂末端手眼相机测量得到目标可抓捕部位的最后一帧有效数据，确立在当前操作平台姿态构型A下，在机械臂基坐标系下的机械臂末端的抓捕期望位姿；进入抓捕末阶段后，根据当前操作平台上陀螺测量到的操作平台姿态信息，计算获得新的操作平台姿态构型B相对构型A的变化量，结合最后一帧的测量数据，求解在新的操作平台姿态构型B下机械臂末端的期望姿态；最终逆运动学计算得到机械臂各关节的修正控制量，驱动机械臂完成对期望目标的抓捕。本发明提出了一种抓捕末阶段机械臂与操作平台协调控制方法，保证机械臂在运动过程中平台的指向姿态与位置保持不变。
【IPC分类】B25J9/16, B25J13/08
【公开号】CN105382843
【申请号】CN201510860392
【发明人】张海博, 王勇, 何英姿, 魏春岭, 朱志斌, 唐强
【申请人】北京控制工程研究所
【公开日】2016年3月9日
【申请日】2015年11月30日