一种适用于多约束条件下视觉信息导引的机械臂规划方法
【专利摘要】本发明涉及一种适用于多约束条件下视觉信息导引的机械臂规划方法，包括如下步骤：(1)判断机械臂当前所处的工作阶段；(2)获取捕获目标位置，并计算目标速度；(3)判断是否满足捕获条件，如果是，则完成抓捕操作，如果否，则执行步骤(4)；(4)判断手眼相机是否稳定捕获，如果是，则执行捕获段机械臂规划指令，如果否，则执行步骤(5)；(5)判断机械臂是否进入危险区，如果是，则执行搜索接近段机械臂规划指令，如果否，则执行快速接近段机械臂规划指令。该方法采用双目相机和手眼相机作为视觉传感器，保证机械臂规划输入信息有效性，并加入视觉信息缺失情况下的机械臂运动预测方法，使得方案完整性好且适应性强。
【专利说明】
一种适用于多约束条件下视觉信息导引的机械臂规划方法
技术领域
[0001] 本发明涉及一种包含多约束条件、基于视觉信息导引的机械臂规划方法，属于空 间操作领域。
【背景技术】
[0002] 人类长期从事太空活动，完全失效的或由于任务结束被放弃的卫星停留在太空成 为太空垃圾，空间碎片数量不断增加，不仅危及在轨运行航天器的安全，而且对有限的空间 轨道资源也构成了严重的威胁。为了尽可能净化轨道环境，各国正在研究以卫星维修和太 空垃圾清除为目的的在轨服务技术。由于空间机械臂具有控制精度高、灵活性强以及任务 适应性强等优点，基于机械臂操作的主动清除技术已成为清除废弃卫星和空间碎片的主要 手段。
[0003] 废弃卫星及空间碎片等被清除对象姿态并未受控，因此为解决面向运动目标的捕 获问题，需要进行机械臂在线运动规划，文献（贾庆轩，陈钢，孙汉旭，郑双奇.基于A*算法 的空间机械臂避障路径规划.机械工程学报，2010,13)公开了利用A*算法在空间机械臂的 自由工作空间进行无碰撞路径搜索，实现了空间机械臂的避障路径规划，在应用时并未涉 及视觉信息输入，未构造机械臂与双目相机视场间的约束条件。文献（CN200910073472,空 间机器人视觉测量、路径规划、GNC -体化仿真系统）中提出了一种空间机器人跟踪、接近、 捕获目标过程的自主路径规划方法，利用手眼相机的相对位置与姿态测量值实现规划空间 机器人的运动，并未涉及机械臂规划的各种约束条件，也未考虑视觉信息导引和双目相机、 手眼相机切换的机械臂规划分段策略，难以满足多约束条件下视觉信息引导的机械臂在线 规划需求。
【发明内容】
[0004] 本发明的目的在于解决现有技术的不足，提供了一种适用于视场约束、碰撞约束 和运动约束等多约束条件下的视觉信息导引的机械臂规划方法，该方法采用双目相机和手 眼相机作为视觉传感器，充分利用两类相机的优势，提出了基于视觉信息导引的机械臂分 段运动及切换策略，保证机械臂规划输入信息的有效性，并考虑视觉信息缺失情况下的机 械臂运动预测方法，有效提升算法运算效率又保证了机械臂操作的安全性。
[0005] 为了解决【背景技术】中存在的问题，本发明提供了一种适用于多约束条件下视觉信 息导引的机械臂规划方法，包括如下步骤：
[0006] (1)计算机械臂末端位置，判断机械臂当前所处的工作阶段，所述工作阶段包括 快速接近段、搜索接近段和捕获段；
[0007] (2)获取捕获目标位置，并用该位置计算目标速度；
[0008] (3)判断是否满足捕获条件，如果是，则完成抓捕操作，如果否，则执行步骤（4);
[0009] (4)判断手眼相机是否稳定捕获，如果是，则执行捕获段机械臂规划指令，如果否， 则执行步骤（5);
[0010] (5)判断机械臂是否进入危险区，如果符合危险区条件，则执行搜索接近段机械臂 规划指令，如果不符合危险区条件，则执行快速接近段机械臂规划指令。
[0011] 所述快速接近段机械臂规划指令包括如下步骤：
[0012] (1)根据目标位置和机械臂末端位置与姿态，计算相对位置偏差与指向偏差，从而 计算期望的末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到六个关节角的期望角速 度；
[0013] ⑵根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算 三次多项式系数。
[0014] 所述搜索接近段机械臂规划指令包括如下步骤：
[0015] (1)初始化最优代价函数值，随机生成一组关节角，对随机关节角可行性进行判 断，判断是否到达最大搜索次数；
[0016] (2)根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算 三次多项式系数。
[0017] 所述捕获段机械臂规划指令包括如下步骤：
[0018] (1)根据三维目标位置和机械臂末端位置与姿态，计算相对位置偏差与指向偏差， 从而计算期望的末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到六个关节角的期望 角速度；
[0019] (2)根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算 三次多项式系数。
[0020] 所述获取捕获目标位置通过视觉信息获取捕获目标的位置，具体步骤为：
[0021] 若所处阶段为快速接近段，则优先使用双目相机，如果双目相机失效则使用手眼 相机；若所处阶段为搜索接近段，优先使用双目相机，如果双目相机失效则使用手眼相机； 若所处阶段为捕获段，则优先使用手眼相机，如果手眼相机失效则使用双目相机。
[0022] 进一步的，当双目相机、手眼相机均失效时，采用预测算法将双目相机与手眼相机 失效前保存的数据进行线性外插。
[0023] 所述危险区判断包括如下步骤：
[0024] (1)判断各关节角是否超过危险边界；
[0025] (2)如果未超出危险边界，继续判断机械臂是否与本体发生碰撞；
[0026] (3)如果未发生碰撞，继续判断机械臂是否遮挡双目相机视场；
[0027] 进一步的，所述危险边界为150°~180°，机械臂与本体的最大安全距离为 100mm~300mm，机械臂各关节距离双目相机视场的最小距离为100mm~300mm。
[0028] 本发明与现有技术相比具有如下有益效果：
[0029] (1)采用分段式的视觉信息导引和切换策略，充分发挥双目相机和手眼相机的优 势，并且考虑工程应用中可能出现的视觉信息缺失情况，通过视觉信息融合处理，保障方法 可行性和有效性，既提升算法运算效率又保证了操作安全性；
[0030] (2)在机械臂规划算法中考虑机械臂与双目相机视线间的约束，有效防止机械臂 运动过程中遮挡视场；加入关节角限位约束，有效避免机械臂运动超出关节运动范围；对 碰撞约束进行检测，从而提升机械臂操作的安全性。
【附图说明】
[0031] 图1是基于视觉信息导引和切换的机械臂规划分段策略示意图；
[0032] 图2是视觉信息缺失条件下的机械臂运动预测方法示意图；
[0033] 图3是快速接近段机械臂规划算法流程图；
[0034] 图4是搜索接近段机械臂规划算法流程图；
[0035] 图5是捕获段机械臂规划算法流程图；
[0036] 图6是蒙特卡洛打靶仿真试验结果统计图。
【具体实施方式】
[0037] 下面结合附图和【具体实施方式】对本发明的技术方案做进一步详细说明。显然，所 描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例， 本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明要 求保护的范围。
[0038] 考虑空间运动目标抓捕的快速性和安全性，将在线捕获段分成三个阶段：快速接 近段、搜索接近段和捕获段，如图1所示。其中，快速接近段和搜索接近段是指机械臂通过 双目相机向目标接近并使手眼相机捕获目标的过程；捕获段是指手眼相机观测到目标之后 指引机械臂末端捕获目标的过程。
[0039] 视觉信息缺失情况下的机械臂运动预测方法，如图2所示。捕获段之前，优先使用 双目相机；捕获段开始，优先使用手眼相机；当优先使用的相机失效时，采用另一相机；当 两套相机均失效时，采用预测算法估计目标位置及速度。
[0040] 针对双目相机视场约束、平台碰撞约束和关节最大转角约束，即危险区判断，主要 采取如下措施：
[0041] (1)双目相机视线约束
[0042] 定义目标和双目三点组成的平面为视线平面，机械臂根据需要在该平面的一侧运 动，选取位于机械臂运动侧且与视线平面距离为d的平面为危险区边界，两平面之间的区 域为危险区。根据机械臂的结构，通过计算机械臂关节和末端距离视线平面的距离可以判 断机械臂是否进入危险区。
[0043] (2)平台碰撞约束
[0044] 对于平台上的障碍物，可以选择平台上最高的部件，固定耳片作为基准建立障碍 平面。同样选择一定的安全裕度确定危险区边界，危险区边界和障碍平面之间的空间即为 危险区。根据机械臂的结构，通过计算机械臂关节和机械臂末端与障碍平面的距离可以判 断机械臂是否进入危险区。
[0045] (3)关节最大转角约束
[0046] 考虑机械臂关节转角限制，确定危险转角边界，当机械臂某一关节转角达到该边 界值时，即认为进入了危险区。
[0047] 当机械臂运动到危险区内后，采用捕获段机械臂规划指令考虑各种约束向目标接 近，直至手眼相机能够稳定捕获目标。
[0048] 适用于多约束条件下视觉信息导引的机械臂规划具体方法如下：
[0049] (1)通过机械臂运动学计算机械臂末端位置pe，
[0050] (2)判断当前所属的工作阶段是否满足捕获条件，如果符合，执行步骤（7)，如果 不符合，执行步骤（3);
[0051] (3)通过视觉信息使用策略计算目标速度与位置具体步骤如下：
[0052] (a)若所处阶段为快速接近段，则优先使用双目相机，进入步骤（b)，如果双目相 机失效则使用手眼相机，进入步骤（c)，仍失效则进入步骤（d);若所处阶段为搜索接近段， 优先使用双目相机，进入步骤（b)，如果双目相机失效则使用手眼相机，进入步骤（c)，仍失 效则进入步骤（d);若所处阶段为捕获段，则优先使用手眼相机，进入步骤（c)，如果手眼相 机失效则使用双目相机，进入步骤（b)，仍失效则进入步骤（d);
[0053] (b)利用双目相机观测值计算目标位置rt_SV，并用该位置估算目标速度vt，
将该速度值写入目标速度估算量Vt，并将最近目标位置rt = rt_ SV加入序列，返回步骤（4);
[0054] (c)利用手眼相机观测值计算目标位置rt_HE，并将最近目标位置计算 值rt = Fe (1:3, 1:3) ? rt_HE+Fe (1:3, 4)加入序列，并用该位置估算目标速度vt，
将该速度值写入目标速度估算量Vt，返回步骤（4);
[0055] (d)采用预测算法将双目相机与手眼相机失效前保存的数据进行线性外插，返回 步骤（4);
[0056] (4)计算指向偏差角 aimangle = acos([0 ;0 ;1] ?rt/norm(rt))，根据机械臂末端 与目标相对距离dret = rt-pe(rt为目标位置）和指向偏差角aimangle判断手眼相机是 否稳定捕获，若|(1代1：|〈1'1：_1：11代811〇1(1，且3；[1]^1^16〈3；[111_1：11代81101(1，则进入步骤（5);否则 进入步骤（6);
[0057] (5)判断是否进入危险区，具体步骤如下：
[0058] (e)判断各关节角是否达到危险边界safeangle，若if | JiRsafeangle(设定 safeangle=150° )，则认为未达到危险边界，进入步骤（f);若if|j」彡safeangle，则认 为达到危险边界，执行搜索接近段规划算法；
[0059] (f)判断机械臂是否与本体发生碰撞：若if | Pi (x) |〈safedist (设定safedist = 150mm)，则认为未发生碰撞，则进入步骤（g);若if | Pi (x) |彡safedist，则认为发生碰撞， 执行搜索接近段规划算法；
[0060] (g)判断机械臂是否遮挡双目相机视场：
[0061] 首先计算
，（i = 3, 4, 5, 6, e)，若 dXsafedist (设定safedist = 150mm)，则认为未发生遮挡，执行快速接近段规划算法；否 则认为发生遮挡，执行搜索接近段规划算法；
[0062] (6)将当前时间代入三次多项式，完成一次机械臂规划指令组求解并发送给机械 臂，执行步骤（1)-步骤（2);
[0063] (7)完成抓捕操作，流程结束。
[0064] 如图3所示，快速接近段规划算法具体步骤如下：
[0065] (16)、根据三维目标位置rt和机械臂末端位置pe，计算相对位置偏差DeltaR : A R = rt-pe-Fe (1:3, l:3)>!<[CapPosX ；CapPosY ；CapPosZ]T；
[0066] (17)、根据机械臂末端位置与姿态矩阵Fe和相对位置偏差DeltaR，计算指向偏差 DeltaPhi
ved^\ 「Ai?
[0067] (18)、计算期望的末端速度和角速度：=^2 其中A R = *- -* L. _J
[DeltaRX, DeltaRY, DeltaRZ]1；
[0068] (19)、通过矩阵求逆计算雅可比矩阵Jm的逆Jmlnv ;
[0069] (20)、根据ved，wed和Jmlnv，计算六个关节角的期望角速度：
[0071] (21)、根据指令关节角速度DCJ1~DCJ6和角速度、角加速度约束，调整期望的关 节角速度DCJ1~DCJ6 ;
[0072] (22)、根据期望的关节角速度DCJ1~DCJ6,计算指令关节角CJ1~CJ6 ;
[0073] (23)、根据上一时刻存储的指令角速度DJ1~DJ6和指令关节角CJ1~CJ6、指令 角速度DCJ1-DCJ6,计算三次多项式系数ai3~aiO, i = 1~6,返回步骤（6);
[0074] 如图4所示，搜索接近段规划算法具体步骤如下：
[0075] (24)、最优代价函数值置为OxFFFFFFFF，并将最优关节角BJ1~BJ6置为当前关节 角J1~J6 ;
[0076] (25)、随机生成一组关节角RJ1~RJ6 ;
[0077] (26)、根据当前关节角J1~J6和生成的关节角RJ1~RJ6,计算期望的关节角速 度 DCJ1 ~DCJ6 ;
[0078] (27)、根据最大角速度和角加速度约束调整期望关节角速度DCJ1~DCJ6 ;
[0079] (28)、根据关节角速度DCJ1~DCJ6,重新计算随机关节角RJ1~RJ6 ;
[0080] (29)、按照步骤（5)的方法判断各关节角是否可行；
[0081] (30)、若可行，则计算代价函数值，进入步骤（31);若不可行，进入步骤（32);
[0082] (31)、比较代价函数值TempVal是否小于目前最优代价函数值BestVal，若是，则 将最优代价函数值置为当前代价函数值，并将最优关节角BJ1~BJ6置为当前随机关节角 RJ1~RJ6 ;若否，进入步骤（32);
[0083] (32)、判断是否到达最大搜索次数SearchNum(可根据硬件处理速度在30~300 内选择），若到达，则进入步骤（33);若未到达，则返回至步骤（25);
[0084] (33)、利用最优关节角BJ1~BJ6和当前关节角J1~J6计算各关节角速度 DCJ1~DCJ6,并根据最大角速度和角加速度调整期望关节角速度DCJ1~DCJ6 ;
[0085] (34)、根据期望的关节角速度DCJ1~DCJ6,计算指令关节角CJ1~CJ6 ;
[0086] (35)、根据上一时刻存储的指令角速度DJ1~DJ6和指令关节角CJ1~CJ6、指令 角速度DCJ1-DCJ6,计算三次多项式系数ai3~aiO, i = 1~6,返回步骤（6);
[0087] 如图5所示，捕获段规划算法具体步骤如下：
[0088] (36)、根据三维目标位置rt和机械臂末端位置pe，计算相对位置偏差DeltaR : A R = rt-pe-Fe (1:3, l:3)>!<[CapPosX ；CapPosY ；CapPosZ]T；
[0089] (37)、根据机械臂末端位置与姿态矩阵Fe和相对位置偏差DeltaR，计算指向偏 差：DeltaPhi
ved~\ . f
[0090] (38)、计算期望的末端速度和角速度：D & 其中A R = [DeltaRX, DeltaRY, DeltaRZ]1；
[0091] (39)、通过矩阵求逆计算雅可比矩阵Jm的逆Jmlnv ;
[0092] (40)、根据ved，wed和Jmlnv，计算六个关节角的期望角速度：
[0093] :細―、ed
[wed\ ^
[0094] (41)、根据指令关节角速度DCJ1~DCJ6和角速度、角加速度约束，调整期望的关 节角速度DCJ1~DCJ6 ;
[0095] (42)、根据上一时刻存储的指令角速度DJ1~DJ6和指令关节角CJ1~CJ6、指令 角速度DCJ1-DCJ6,计算三次多项式系数ai3~aiO, i = 1~6,返回步骤（6)。
[0096] 如图6所示为蒙特卡洛打靶仿真试验结果统计图。为验证本文提出的机械臂规划 方法的有效性，搭建了抓捕可靠性仿真系统，综合考虑机械臂控制误差、视觉探测误差以及 视觉信息延迟，采用蒙特卡洛打靶法，进行有效性及可靠性验证。运行2000次仿真，经统 计，抓捕成功率为99. 95%，验证了本方法的可行性和有效性。
[0097] 对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。 对这些实施例的多种修改对本领域的技术人员来说是显而易见的，本文中所定义的一般原 理可以在不脱离本发明范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于 本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽范围。
【主权项】
1. 一种适用于多约束条件下视觉信息导引的机械臂规划方法，其特征在于，包括如下 步骤： (1) 计算机械臂末端位置，判断机械臂当前所处的工作阶段，所述工作阶段包括快速 接近段、搜索接近段和捕获段； (2) 获取捕获目标位置，并用该位置计算目标速度； (3) 判断是否满足捕获条件，如果是，则完成抓捕操作，如果否，则执行步骤（4); (4) 判断手眼相机是否稳定捕获，如果是，则执行捕获段机械臂规划指令，如果否，则执 行步骤（5); (5) 判断机械臂是否进入危险区，如果符合危险区条件，则执行搜索接近段机械臂规划 指令，如果不符合危险区条件，则执行快速接近段机械臂规划指令。2. 根据权利要求1所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述快速接近段机械臂规划指令包括如下步骤： (1) 根据目标位置和机械臂末端位置与姿态，计算相对位置偏差与指向偏差，从而计算 期望的末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到关节角的期望角速度； (2) 根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次 多项式系数。3. 根据权利要求1所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述搜索接近段机械臂规划指令包括如下步骤： (1) 初始化最优代价函数值，随机生成一组关节角，对随机关节角可行性进行判断，判 断是否到达最大搜索次数； (2) 根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次 多项式系数。4. 根据权利要求1所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述捕获段机械臂规划指令包括如下步骤： (1) 根据三维目标位置和机械臂末端位置与姿态，计算相对位置偏差，从而计算期望的 末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到关节角的期望角速度； (2) 根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次 多项式系数。5. 根据权利要求1所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述步骤（2)通过视觉信息获取捕获目标的位置，具体步骤为： 若所处阶段为快速接近段，则优先使用双目相机，如果双目相机失效则使用手眼相 机；若所处阶段为搜索接近段，优先使用双目相机，如果双目相机失效则使用手眼相机； 若所处阶段为捕获段，则优先使用手眼相机，如果手眼相机失效则使用双目相机。6. 根据权利要求5所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，当双目相机、手眼相机均失效时，采用预测算法将双目相机与手眼相机失效前 保存的数据进行线性外插。7. 根据权利要求1所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述危险区判断包括如下步骤： (1)判断各关节角是否超过危险边界； (2) 如果未超出危险边界，继续判断机械臂是否与本体发生碰撞； (3) 如果未发生碰撞，继续判断机械臂是否遮挡双目相机视场。8.根据权利要求7所述的一种适用于多约束条件下视觉信息导引的机械臂规划方法， 其特征在于，所述危险边界为150°~180°，机械臂与本体的最大安全距离为100mm~ 300mm，机械臂各关节距离双目相机视场的最小距离为100mm~300mm。
【文档编号】B25J13/08GK106003104SQ201510387748
【公开日】2016年10月12日
【申请日】2015年7月3日
【发明人】焉宁, 张烽, 李扬, 申麟, 张旭辉, 高朝辉, 唐庆博, 陈蓉, 唐超
【申请人】中国运载火箭技术研究院