CN106003104B - 一种适用于多约束条件下视觉信息导引的机械臂规划方法 - Google Patents

Abstract

本发明涉及一种适用于多约束条件下视觉信息导引的机械臂规划方法，包括如下步骤：(1)判断机械臂当前所处的工作阶段；(2)获取捕获目标位置，并计算目标速度；(3)判断是否满足捕获条件，如果是，则完成抓捕操作，如果否，则执行步骤(4)；(4)判断手眼相机是否稳定捕获，如果是，则执行捕获段机械臂规划指令，如果否，则执行步骤(5)；(5)判断机械臂是否进入危险区，如果是，则执行搜索接近段机械臂规划指令，如果否，则执行快速接近段机械臂规划指令。该方法采用双目相机和手眼相机作为视觉传感器，保证机械臂规划输入信息有效性，并加入视觉信息缺失情况下的机械臂运动预测方法，使得方案完整性好且适应性强。

Description

一种适用于多约束条件下视觉信息导引的机械臂规划方法

技术领域

本发明涉及一种包含多约束条件、基于视觉信息导引的机械臂规划方法，属于空间操作领域。

背景技术

人类长期从事太空活动，完全失效的或由于任务结束被放弃的卫星停留在太空成为太空垃圾，空间碎片数量不断增加，不仅危及在轨运行航天器的安全，而且对有限的空间轨道资源也构成了严重的威胁。为了尽可能净化轨道环境，各国正在研究以卫星维修和太空垃圾清除为目的的在轨服务技术。由于空间机械臂具有控制精度高、灵活性强以及任务适应性强等优点，基于机械臂操作的主动清除技术已成为清除废弃卫星和空间碎片的主要手段。

废弃卫星及空间碎片等被清除对象姿态并未受控，因此为解决面向运动目标的捕获问题，需要进行机械臂在线运动规划，文献(贾庆轩，陈钢，孙汉旭，郑双奇.基于A*算法的空间机械臂避障路径规划.机械工程学报，2010，13)公开了利用A*算法在空间机械臂的自由工作空间进行无碰撞路径搜索，实现了空间机械臂的避障路径规划，在应用时并未涉及视觉信息输入，未构造机械臂与双目相机视场间的约束条件。文献(CN200910073472，空间机器人视觉测量、路径规划、GNC一体化仿真***)中提出了一种空间机器人跟踪、接近、捕获目标过程的自主路径规划方法，利用手眼相机的相对位置与姿态测量值实现规划空间机器人的运动，并未涉及机械臂规划的各种约束条件，也未考虑视觉信息导引和双目相机、手眼相机切换的机械臂规划分段策略，难以满足多约束条件下视觉信息引导的机械臂在线规划需求。

发明内容

本发明的目的在于解决现有技术的不足，提供了一种适用于视场约束、碰撞约束和运动约束等多约束条件下的视觉信息导引的机械臂规划方法，该方法采用双目相机和手眼相机作为视觉传感器，充分利用两类相机的优势，提出了基于视觉信息导引的机械臂分段运动及切换策略，保证机械臂规划输入信息的有效性，并考虑视觉信息缺失情况下的机械臂运动预测方法，有效提升算法运算效率又保证了机械臂操作的安全性。

为了解决背景技术中存在的问题，本发明提供了一种适用于多约束条件下视觉信息导引的机械臂规划方法，包括如下步骤：

(1)计算机械臂末端位置,判断机械臂当前所处的工作阶段，所述工作阶段包括快速接近段、搜索接近段和捕获段；

(2)获取捕获目标位置，并用该位置计算目标速度；

(3)判断是否满足捕获条件，如果是，则完成抓捕操作，如果否，则执行步骤(4)；

(4)判断手眼相机是否稳定捕获，如果是，则执行捕获段机械臂规划指令，如果否，则执行步骤(5)；

(5)判断机械臂是否进入危险区，如果符合危险区条件，则执行搜索接近段机械臂规划指令，如果不符合危险区条件，则执行快速接近段机械臂规划指令。

所述快速接近段机械臂规划指令包括如下步骤：

(1)根据目标位置和机械臂末端位置与姿态，计算相对位置偏差与指向偏差，从而计算期望的末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到六个关节角的期望角速度；

(2)根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次多项式系数。

所述搜索接近段机械臂规划指令包括如下步骤：

(1)初始化最优代价函数值，随机生成一组关节角，对随机关节角可行性进行判断，判断是否到达最大搜索次数；

(2)根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次多项式系数。

所述捕获段机械臂规划指令包括如下步骤：

(1)根据三维目标位置和机械臂末端位置与姿态，计算相对位置偏差与指向偏差，从而计算期望的末端速度和角速度，通过矩阵求逆计算雅可比矩阵得到六个关节角的期望角速度；

(2)根据指令关节角速度和角速度、角加速度约束，调整期望的关节角速度，计算三次多项式系数。

所述获取捕获目标位置通过视觉信息获取捕获目标的位置，具体步骤为：

若所处阶段为快速接近段，则优先使用双目相机，如果双目相机失效则使用手眼相机；若所处阶段为搜索接近段，优先使用双目相机，如果双目相机失效则使用手眼相机；若所处阶段为捕获段，则优先使用手眼相机，如果手眼相机失效则使用双目相机。

进一步的，当双目相机、手眼相机均失效时，采用预测算法将双目相机与手眼相机失效前保存的数据进行线性外插。

所述危险区判断包括如下步骤：

(1)判断各关节角是否超过危险边界；

(2)如果未超出危险边界，继续判断机械臂是否与本体发生碰撞；

(3)如果未发生碰撞，继续判断机械臂是否遮挡双目相机视场；

进一步的，所述危险边界为150°～180°，机械臂与本体的最大安全距离为100mm～300mm，机械臂各关节距离双目相机视场的最小距离为100mm～300mm。

本发明与现有技术相比具有如下有益效果：

(1)采用分段式的视觉信息导引和切换策略，充分发挥双目相机和手眼相机的优势，并且考虑工程应用中可能出现的视觉信息缺失情况，通过视觉信息融合处理，保障方法可行性和有效性，既提升算法运算效率又保证了操作安全性；

(2)在机械臂规划算法中考虑机械臂与双目相机视线间的约束，有效防止机械臂运动过程中遮挡视场；加入关节角限位约束，有效避免机械臂运动超出关节运动范围；对碰撞约束进行检测，从而提升机械臂操作的安全性。

附图说明

图1是基于视觉信息导引和切换的机械臂规划分段策略示意图；

图2是视觉信息缺失条件下的机械臂运动预测方法示意图；

图3是快速接近段机械臂规划算法流程图；

图4是搜索接近段机械臂规划算法流程图；

图5是捕获段机械臂规划算法流程图；

图6是蒙特卡洛打靶仿真试验结果统计图。

具体实施方式

下面结合附图和具体实施方式对本发明的技术方案做进一步详细说明。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明要求保护的范围。

考虑空间运动目标抓捕的快速性和安全性，将在线捕获段分成三个阶段：快速接近段、搜索接近段和捕获段，如图1所示。其中，快速接近段和搜索接近段是指机械臂通过双目相机向目标接近并使手眼相机捕获目标的过程；捕获段是指手眼相机观测到目标之后指引机械臂末端捕获目标的过程。

视觉信息缺失情况下的机械臂运动预测方法，如图2所示。捕获段之前，优先使用双目相机；捕获段开始，优先使用手眼相机；当优先使用的相机失效时，采用另一相机；当两套相机均失效时，采用预测算法估计目标位置及速度。

针对双目相机视场约束、平台碰撞约束和关节最大转角约束，即危险区判断，主要采取如下措施：

(1)双目相机视线约束

定义目标和双目三点组成的平面为视线平面，机械臂根据需要在该平面的一侧运动，选取位于机械臂运动侧且与视线平面距离为d的平面为危险区边界，两平面之间的区域为危险区。根据机械臂的结构，通过计算机械臂关节和末端距离视线平面的距离可以判断机械臂是否进入危险区。

(2)平台碰撞约束

对于平台上的障碍物，可以选择平台上最高的部件，固定耳片作为基准建立障碍平面。同样选择一定的安全裕度确定危险区边界，危险区边界和障碍平面之间的空间即为危险区。根据机械臂的结构，通过计算机械臂关节和机械臂末端与障碍平面的距离可以判断机械臂是否进入危险区。

(3)关节最大转角约束

考虑机械臂关节转角限制，确定危险转角边界，当机械臂某一关节转角达到该边界值时，即认为进入了危险区。

当机械臂运动到危险区内后，采用捕获段机械臂规划指令考虑各种约束向目标接近，直至手眼相机能够稳定捕获目标。

适用于多约束条件下视觉信息导引的机械臂规划具体方法如下：

(1)通过机械臂运动学计算机械臂末端位置pe，

(2)判断当前所属的工作阶段是否满足捕获条件，如果符合，执行步骤(7)，如果不符合，执行步骤(3)；

(3)通过视觉信息使用策略计算目标速度与位置具体步骤如下：

(a)若所处阶段为快速接近段，则优先使用双目相机，进入步骤(b)，如果双目相机失效则使用手眼相机，进入步骤(c)，仍失效则进入步骤(d)；若所处阶段为搜索接近段，优先使用双目相机，进入步骤(b)，如果双目相机失效则使用手眼相机，进入步骤(c)，仍失效则进入步骤(d)；若所处阶段为捕获段，则优先使用手眼相机，进入步骤(c)，如果手眼相机失效则使用双目相机，进入步骤(b)，仍失效则进入步骤(d)；

(b)利用双目相机观测值计算目标位置rt_SV，并用该位置估算目标速度vt，将该速度值写入目标速度估算量Vt，并将最近目标位置rt＝rt_SV加入序列，返回步骤(4)；

(c)利用手眼相机观测值计算目标位置rt_HE，并将最近目标位置计算值rt＝Fe(1:3,1:3)·rt_HE+Fe(1:3,4)加入序列，并用该位置估算目标速度vt，将该速度值写入目标速度估算量Vt，返回步骤(4)；

(d)采用预测算法将双目相机与手眼相机失效前保存的数据进行线性外插，返回步骤(4)；

(4)计算指向偏差角aimangle＝acos([0；0；1]·rt/norm(rt))，根据机械臂末端与目标相对距离dret＝rt-pe(rt为目标位置)和指向偏差角aimangle判断手眼相机是否稳定捕获，若|dret|<rt_threshold，且aimangle<aim_threshold，则进入步骤(5)；否则进入步骤(6)；

(5)判断是否进入危险区，具体步骤如下：

(e)判断各关节角是否达到危险边界safeangle，若if|J_i|<safeangle(设定safeangle＝150°)，则认为未达到危险边界，进入步骤(f)；若if|J_i|≥safeangle，则认为达到危险边界，执行搜索接近段规划算法；

(f)判断机械臂是否与本体发生碰撞：若if|p_i(x)|<safedist(设定safedist＝150mm)，则认为未发生碰撞，则进入步骤(g)；若if|p_i(x)|≥safedist，则认为发生碰撞，执行搜索接近段规划算法；

(g)判断机械臂是否遮挡双目相机视场：

首先计算(i＝3,4,5,6,e)，若d_i<safedist(设定safedist＝150mm)，则认为未发生遮挡，执行快速接近段规划算法；否则认为发生遮挡，执行搜索接近段规划算法；

(6)将当前时间代入三次多项式，完成一次机械臂规划指令组求解并发送给机械臂，执行步骤(1)-步骤(2)；

(7)完成抓捕操作，流程结束。

如图3所示，快速接近段规划算法具体步骤如下：

(16)、根据三维目标位置rt和机械臂末端位置pe，计算相对位置偏差DeltaR：△R＝rt-pe-Fe(1:3,1:3)*[CapPosX；CapPosY；CapPosZ]^T；

(17)、根据机械臂末端位置与姿态矩阵Fe和相对位置偏差DeltaR，计算指向偏差DeltaPhi：

(18)、计算期望的末端速度和角速度：其中△R＝[DeltaRX,DeltaRY,DeltaRZ]^T；

(19)、通过矩阵求逆计算雅可比矩阵Jm的逆JmInv；

(20)、根据ved，wed和JmInv，计算六个关节角的期望角速度：

(21)、根据指令关节角速度DCJ1～DCJ6和角速度、角加速度约束，调整期望的关节角速度DCJ1～DCJ6；

(22)、根据期望的关节角速度DCJ1～DCJ6，计算指令关节角CJ1～CJ6；

(23)、根据上一时刻存储的指令角速度DJ1～DJ6和指令关节角CJ1～CJ6、指令角速度DCJ1-DCJ6，计算三次多项式系数ai3～ai0，i＝1～6，返回步骤(6)；

如图4所示，搜索接近段规划算法具体步骤如下：

(24)、最优代价函数值置为0xFFFFFFFF，并将最优关节角BJ1～BJ6置为当前关节角J1～J6；

(25)、随机生成一组关节角RJ1～RJ6；

(26)、根据当前关节角J1～J6和生成的关节角RJ1～RJ6，计算期望的关节角速度DCJ1～DCJ6；

(27)、根据最大角速度和角加速度约束调整期望关节角速度DCJ1～DCJ6；

(28)、根据关节角速度DCJ1～DCJ6，重新计算随机关节角RJ1～RJ6；

(29)、按照步骤(5)的方法判断各关节角是否可行；

(30)、若可行，则计算代价函数值，进入步骤(31)；若不可行，进入步骤(32)；

(31)、比较代价函数值TempVal是否小于目前最优代价函数值BestVal，若是，则将最优代价函数值置为当前代价函数值，并将最优关节角BJ1～BJ6置为当前随机关节角RJ1～RJ6；若否，进入步骤(32)；

(32)、判断是否到达最大搜索次数SearchNum(可根据硬件处理速度在30～300内选择)，若到达，则进入步骤(33)；若未到达，则返回至步骤(25)；

(33)、利用最优关节角BJ1～BJ6和当前关节角J1～J6计算各关节角速度DCJ1～DCJ6，并根据最大角速度和角加速度调整期望关节角速度DCJ1～DCJ6；

(34)、根据期望的关节角速度DCJ1～DCJ6，计算指令关节角CJ1～CJ6；

(35)、根据上一时刻存储的指令角速度DJ1～DJ6和指令关节角CJ1～CJ6、指令角速度DCJ1-DCJ6，计算三次多项式系数ai3～ai0，i＝1～6，返回步骤(6)；

如图5所示，捕获段规划算法具体步骤如下：

(36)、根据三维目标位置rt和机械臂末端位置pe，计算相对位置偏差DeltaR：△R＝rt-pe-Fe(1:3,1:3)*[CapPosX；CapPosY；CapPosZ]^T；

(37)、根据机械臂末端位置与姿态矩阵Fe和相对位置偏差DeltaR，计算指向偏差：DeltaPhi：

(38)、计算期望的末端速度和角速度：其中△R＝[DeltaRX,DeltaRY,DeltaRZ]^T；

(39)、通过矩阵求逆计算雅可比矩阵Jm的逆JmInv；

(40)、根据ved，wed和JmInv，计算六个关节角的期望角速度：

(41)、根据指令关节角速度DCJ1～DCJ6和角速度、角加速度约束，调整期望的关节角速度DCJ1～DCJ6；

(42)、根据上一时刻存储的指令角速度DJ1～DJ6和指令关节角CJ1～CJ6、指令角速度DCJ1-DCJ6，计算三次多项式系数ai3～ai0，i＝1～6，返回步骤(6)。

如图6所示为蒙特卡洛打靶仿真试验结果统计图。为验证本文提出的机械臂规划方法的有效性，搭建了抓捕可靠性仿真***，综合考虑机械臂控制误差、视觉探测误差以及视觉信息延迟，采用蒙特卡洛打靶法，进行有效性及可靠性验证。运行2000次仿真，经统计，抓捕成功率为99.95％，验证了本方法的可行性和有效性。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的技术人员来说是显而易见的，本文中所定义的一般原理可以在不脱离本发明范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽范围。

Claims (8)

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。