基于多路视觉融合的大型空间机械臂自主运动规划方法

摘要

基于多路视觉融合的大型空间机械臂自主运动规划方法，在多路视觉融合中，全局视觉系统的优先级低于局部视觉系统，局部视觉系统的优先级低于腕部视觉系统，即在只有全局视觉系统能够获取目标物体的位姿信息，而局部视觉系统和腕部视觉系统均未获取到目标物体的位姿信息，则采用全局视觉系统获取的目标位姿信息进行运动规划；随着机械臂本体的运动，如果局部视觉系统能够获取目标物体的位姿信息，在机械臂本体到达跟踪中间点之前，则采用局部视觉系统获取的目标位姿信息进行运动规划；当机械臂本体运动至跟踪中间点，此时腕部视觉系统已能获取到目标物体的位姿信息，则采用腕部视觉系统获取的目标位姿信息进行运动规划。本发明实现在大空间范围内的自主运动、自主跟踪和自主捕获目标物体的任务。

权利要求

1.基于多路视觉融合的大型空间机械臂自主运动规划方法，其特征在于：多路视觉系统包括全局视觉系统、局部视觉系统和腕部视觉系统，其中全局视觉系统安装于舱体基座上，局部视觉系统和腕部视觉系统安装于机械臂本体上，随机械臂本体一起移动；所述全局视觉系统和局部视觉系统用于对目标物体进行粗定位，目标物体上设计有大型标识点，全局视觉系统和局部视觉系统通过识别所述大型标识点获得目标物体的位姿信息，腕部视觉系统用于对目标物体进行精确定位，目标物体上设计有捕获杆，该捕获杆与机械臂本体的末端抓手匹配，捕获杆上设计有小型标识点，腕部视觉系统通过识别捕获杆的小型标识点获得目标物体的位姿信息，引导机械臂末端抓手抓取目标物的捕获杆；在所述自主运动规划中，为了保证机械臂本体对目标物体跟踪和捕获的平稳与准确，设定在目标物体正前方一定距离为跟踪中间点，可以根据需要设置多个跟踪中间点；运动规划时，首先以跟踪中间点为目标进行规划，到达跟踪中间点后则以目标物体为目标进行运动规划，直到到达目标物体，则可执行目标捕获；在所述多路视觉融合中，全局视觉系统的优先级低于局部视觉系统，局部视觉系统的优先级低于腕部视觉系统，即在只有全局视觉系统能够获取目标物体的位姿信息，而局部视觉系统和腕部视觉系统均未获取到目标物体的位姿信息，则采用全局视觉系统获取的目标位姿信息进行运动规划；随着机械臂本体的运动，如果局部视觉系统能够获取目标物体的位姿信息，则采用局部视觉系统获取的目标位姿信息进行运动规划，在机械臂本体到达跟踪中间点之前，均采用局部视觉系统获取的目标位姿信息；当机械臂本体运动至跟踪中间点，此时机械臂末端已经正对目标物体，腕部视觉系统已能获取到目标物体的位姿信息，则采用腕部视觉系统获取的目标位姿信息进行运动规划。

2.根据权利要求1所述的基于多路视觉融合的大型空间机械臂自主运动规划方法，其特征在于：所述的运动规划实现如下：

(1)将全局视觉系统或局部视觉系统或腕部视觉系统获得的目标物体位姿信息转换为相对于机械臂本体基准坐标系下的位姿信息；

(2)根据机械臂本体当前的关节角度，通过正运动学求得机械臂本体末端的位姿信息，计算出机械臂本体末端位姿信息与步骤(1)中转换后的目标物体位姿信息之间的差，通过运动规划驱动机械臂本体运动以逼近目标物体，消除该位姿差信息；在运动规划中，首先以跟踪中间点为目标进行跟踪，到达跟踪中间点后再以目标物体为目标进行精确跟踪，根据机械臂本体的运动速度条件在该路径上计算出一步运行的距离，求出机械臂本体下一步运动的目标位姿信息；

(3)获得机械臂本体下一步运动的目标位姿信息后，根据机械臂的运动学及逆运动学方程求解出各个关节下一步的目标角度，该目标角度将发送至相应关节，驱动各关节的运动，实现机械臂本体朝着目标位姿信息运动；

(4)每一步规划之前，都先判断机械臂本体末端是否到达跟踪中间点或目标物，到达跟踪中间点后则以目标物体为对象继续运动规划，直到到达目标物体，则退出运动规划及控制循环，执行目标捕获，如果未到达目标物，则跳至步骤(2)，继续工作。

说明书

基于多路视觉融合的大型空间机械臂自主运动规划方法

技术领域

[0001] 本发明涉及一种基于多路视觉融合的大型空间机械臂自主运动规划方法，属于载人航天领域。

背景技术

[0002] 随着空间科学技术的飞速发展，大量空间作业任务急需完成，如空间站的装配维修、辅助宇航员出舱、月球表面的取样等等，这些任务的完成均离不开机械臂这一必备工具，因此面向空间应用的机械臂技术成为了一个新的研究领域且成为热点研究问题。目前，国际上空间机械臂的研究开展的比较早并且已经得到了成功应用，如加拿大研制的航天飞机机械臂和国际空间站的机械臂均成功完成了预定任务，另外，欧洲、日本也研制了空间机械臂。国内在这一领域才刚刚起步。

[0003] 空间机械臂的研究主要是面向空间作业任务开展，针对不同的作业任务，机械臂的工作模式可以有多种，从运动的角度，可以分为预编程运动、单关节运动、手柄运动、自主运动等。预编程运动即预先确定机械臂的路径，提前规划并存储好规划数据，执行任务时，机械臂根据预编程的指令序号读取相应的规划数据，驱动机械臂运动即可。单关节运动是根据发送的单关节指令，包括关节号、运动方向、运动速度等，完成单关节的运动规划，驱动单关节运动。手柄运动是由操作者操控手柄，手柄的运动代表机械臂末端的运动轨迹，对该轨迹进行运动规划从而驱动机械臂沿着该轨迹运动。自主运动则是由机械臂完全自主规划、自主运动，不需要操作者的参与，这种运动模式需要视觉系统的参与，由视觉系统提供目标位姿信息，作为运动规划的输入。

[0004] 目前，基于视觉的自主运动规划技术已有研究，但一方面面向空间应用的研究还没有，另一方面已有的基于视觉的自主规划多采用单路视觉(单目或双目)，局限于小范围内的目标识别，对于总长度达十余米的大型空间机械臂，要完成自主运动和自主捕获，需要大范围内的目标识别、针对移动目标的识别以及近距离精确定位等功能，只通过单路视觉是不能完成任务的。

发明内容

[0005] 本发明的技术解决问题：克服现有技术的不足，提供一种基于多路视觉融合的大型空间机械臂自主运动规划方法，实现大型空间机械臂在大空间范围内的自主运动、自主跟踪和自主捕获目标物体的任务。

[0006] 本发明的技术解决方案：基于多路视觉融合的大型空间机械臂自主运动规划方法，其特征在于：所述的多路视觉系统包括全局视觉系统、局部视觉系统和腕部视觉系统，其中全局视觉系统安装于舱体基座上，局部视觉系统和腕部视觉系统安装于机械臂本体上，随机械臂本体一起移动；所述全局视觉系统和局部视觉系统用于对目标物体进行粗定位，目标物体上设计有大型标识点，全局视觉系统和局部视觉系统通过识别所述大型标识点获得目标物体的位姿信息，腕部视觉系统用于对目标物体进行精确定位，目标物体上设计有捕获杆，该捕获杆与机械臂本体的末端抓手匹配，捕获杆上设计有小型标识点，腕部视觉系统通过识别捕获杆的小型标识点获得目标物体的位姿信息，引导机械臂末端抓手抓取目标物的捕获杆；在所述自主运动规划中，为了保证机械臂本体对目标物体跟踪和捕获的平稳与准确，设定在目标物体正前方一定距离为跟踪中间点，可以根据需要设置多个跟踪中间点；运动规划时，首先以跟踪中间点为目标进行规划，到达中间点后则以目标物体为目标进行运动规划，直到到达目标物体，则可执行目标捕获；在所述多路视觉融合中，全局视觉系统的优先级低于局部视觉系统，局部视觉系统的优先级低于腕部视觉系统，即在只有全局视觉系统能够获取目标物体的位姿信息，而局部视觉系统和腕部视觉系统均未获取到目标物体的位姿信息，则采用全局视觉系统获取的目标位姿信息进行运动规划；随着机械臂本体的运动，如果局部视觉系统能够获取目标物体的位姿信息，则采用局部视觉系统获取的目标位姿信息进行运动规划，在机械臂本体到达跟踪中间点之前，均采用局部视觉系统获取的目标位姿信息；当机械臂本体运动至跟踪中间点，此时机械臂末端已经正对目标物体，腕部视觉系统已能获取到目标物体的位姿信息，则采用腕部视觉系统获取的目标位姿信息进行运动规划。

[0007] 所述的全局视觉系统、局部视觉系统和腕部视觉系统运动规划实现如下：

[0008] (1)通过视觉系统获得的目标物体位姿信息是相对于视觉系统坐标系的，首先需要转换为相对于机械臂本体基准坐标系下的位姿。

[0009] (2)然后根据机械臂本体当前的关节角，通过正运动学求得机械臂本体末端的位姿，计算出机械臂本体末端位姿与转换后的目标物体位姿之间的差，通过运动规划驱动机械臂本体运动以逼近目标物体，消除该位姿差。在运动规划中，首先以跟踪中间点为目标进行跟踪，到达中间点后再以目标物体为目标进行精确跟踪，根据机械臂本体的运动速度条件在该路径上计算出一步运行的距离，求出机械臂本体下一步运动的目标位姿。

[0010] (3)获得机械臂本体下一步运动的目标位姿后，根据机械臂的运动学及逆运动学方程求解出每个关节下一步的目标角度，该目标角度发送至相应关节，驱动各关节运动，实现机械臂本体朝着目标位姿的运动。

[0011] (4)每一步规划之前，都先判断机械臂本体末端是否到达中间点或目标物，到达中间点后则以目标物体为对象继续运动规划，直到到达目标物体，则退出运动规划及控制循环，执行目标捕获，如果未到达目标物，则跳至步骤(2)，继续工作。

[0012] 本发明与现有技术相比的优点在于：

[0013] (1)现有的对机器人运动规划技术通常局限于单路视觉，不具备多路视觉融合功能，固然不能实现大范围内的目标识别，不能满足大型空间机械臂的空间任务需求，而本发明的基于多路视觉融合系统，当距离目标物体较远时，能够通过全局视觉系统和局部视觉系统实现对目标物体的识别以及粗定位，当到达设定的中间点接近目标时，可以通过腕部视觉系统实现对目标物体的精确定位以及捕获。实现了大范围内对目标物体的粗定位、精确定位以及精确捕获，以及远距离目标的粗定位和近距离目标的精确定位。因此，本发明可以实现大型空间机械臂自主运动、自主跟踪、自主捕获目标等任务。

[0014] (2)本发明基于多路视觉融合的大型空间机械臂自主运动规划方法通过设定跟踪中间点，避免了由于直接跟踪目标物发生机械臂本体末端和目标物碰撞的问题，并且通过设置多个中间点保证了捕获的平稳和准确，而且不仅能够实现对静止目标物体的捕获，还有效解决了对运动目标捕获困难的问题，扩展了空间机械臂的应用范围。

附图说明

[0015] 图1为本发明的空间机械臂坐标系关系图。

[0016] 图2为本发明的基于多路视觉融合的自主运动规划流程图。

具体实施方式

[0017] 本发明的多路视觉系统包括全局视觉系统、局部视觉系统和腕部视觉系统，其中全局视觉系统安装于舱体基座上，局部视觉系统安装于机械臂本体如局部或臂杆上，腕部视觉系统安装于机械臂末端，局部视觉系统和腕部视觉系统都随机械臂本体一起移动。多路视觉系统均是由摄像机、图像压缩模块和位姿信息处理模块组成，只是全局、局部和腕部的摄像机视场不同，这些为成熟技术。

[0018] 在本发明中，全局视觉系统中的全局摄像机的焦距为8mm，在距离目标[5m，20m]时，位置测量精度优于120mm，姿态测量精度优于5°；局部视觉系统中的局部摄像机的焦距为20mm，在距离目标[10m，20m]时，位置测量精度优于40mm，姿态测量精度优于2°；腕部视觉系统中的腕部摄像机的焦距为12mm，在距离目标[1m，5m]时，位置测量精度优于20mm，姿态测量精度优于1.5°，在距离目标[0.1m，1m]时，位置测量精度优于15mm，姿态测量精度测量优于1.5°。

[0019] 本发明的多路视觉融合的实现过程如下：

[0020] 步骤1，当多路视觉系统启动后，全局视觉系统始终能观察到目标物体，实现对目标物体的识别以及位姿解算，确定目标物体的位姿信息，即位置和姿态信息，由于全局摄像机的测量精度低于局部摄像机和腕部摄像机，因此相当于对目标的粗定位，基于该位姿信息进行运动规划。运动规划以机械臂本体当前位姿为起始点，以获取的目标物体的位姿信息为目标点，规划出中间过程机械臂本体各关节的关节角数据，驱动机械臂本体朝着目标物体运动；

[0021] 步骤2，随着机械臂本体的运动，当目标物体到达局部视觉系统的视野范围内，且能够通过局部视觉系统获取所述目标物体位姿信息时，局部视觉系统的优先级高于全局视觉系统，根据局部视觉系统获取的目标物体位姿信息进行运动规划，驱动机械臂本体朝着设定的跟踪中间点运动，设定目标物体正前方一定距离为跟踪中间点，本发明中选取与目标物固连的坐标系的Z轴反向延长线上距离目标物ds的点为跟踪中间点(如图2)，在机械臂的实际调试过程中，设定距目标物正前方0.8m为跟踪中间点；

[0022] 步骤3，在局部视觉系统的引导下，机械臂运动到跟踪中间点，此时，腕部视觉系统正对目标物体，能够识别捕获杆的标识点，获取到目标物体的精确位姿信息，腕部视觉系统的优先级最高，则根据腕部视觉系统获取的目标物体位姿信息进行运动规划，驱动机械臂本体朝着目标物体运动，为了保证捕获的平稳和准确，根据需要再设置三个跟踪中间点，在机械臂的实际调试过程中，设置了分别距目标物0.3m、0.2m和0.1m三个中间点，基于腕部视觉系统获取的目标物体位姿信息，分别以中间点为目标点进行运动规划，随后机械臂本体的末端分别通过三个中间点，然后以目标物体为目标点进行运动规划，当到达目标物体时则可以执行自主捕获。

[0023] 本发明中三个视觉系统的运动规划过程相同，每个运动规则如下：

[0024] (1)首先需要将通过视觉系统获得的目标物体位姿信息转换为相对于机械臂本体基准坐标系下的位姿信息。

[0025] (2)然后根据机械臂本体当前的关节角，通过正运动学求得机械臂本体末端的位姿信息，计算出机械臂本体末端位姿与转换后的目标物体位姿之间的差，通过运动规划驱动机械臂本体运动以逼近目标物体，消除该位姿差。在运动规划中，首先以跟踪中间点为目标进行跟踪，到达中间点后再以目标物体为目标进行精确跟踪，根据机械臂本体的运动速度条件在该路径上计算出一步运行的距离，求出机械臂本体下一步运动的目标位姿信息。

[0026] (3)获得机械臂本体下一步运动的目标位姿后，根据机械臂的运动学及逆运动学方程求解出每个关节下一步的目标角度，该目标角度发送至相应关节，驱动各关节运动，实现机械臂本体朝着目标位姿的运动。

[0027] (4)每一步规划之前，都先判断机械臂本体末端是否到达中间点或目标物，到达中间点后则以目标物体为对象继续运动规划，直到到达目标物体，则退出运动规划及控制循环，执行目标捕获，如果未到达目标物，则跳至步骤(2)，继续工作。

[0028] 上述运动规划的具体实现如下：如图1所示，设基坐标系为I(IO-IXIYIZ)，机械臂末T T T T W W W W端坐标系为T(O-XYZ)，目标点坐标系为W(O-XYZ)，本发明的自主运动规划具体流程如图2所示：

[0029] 1、视觉系统采集目标点W相对于机械臂末端坐标系T的6维坐标其中 为位置坐标， 为姿态坐标，
通常用欧拉角表示。得到目标点W相对于机械臂末端坐标系T的变换矩阵

[0030] 2、由机械臂本体当前关节角度θnow，根据运动学正解计算末端T相对于基坐标系I的位姿矩阵 并由此得到末端T相对于基坐标系I的位姿坐标

[0031] 3、由步骤1和2所得的 和 计算目标点W相对于基坐标系I的变换矩阵并由此得到目标W相对于基坐标系I的位姿坐标

[0032] 4、中间点和目标物体跟踪

[0033] 考虑到直接跟踪目标物，可能会发生抓手和目标物碰撞的问题，因此先以与目标物固连的坐标系的Z轴反向延长线上距离目标物ds的点作为跟踪中间点。为保证捕获平稳准确，可根据需要设置多个中间点。在机械臂的实际调试过程中，设置了距目标物0.8m、0.3m、0.2m和0.1m四个中间点。首先以中间点进行规划，然后以目标物体为目标点进行规划，具体运动规划如下：

[0034] (1)以跟踪0.8m处的中间点为例，ds＝0.8m。

[0035] 计算出WOp的位姿坐标

[0036] 其中W

[0037] (2)然后以 Op为目标物，按下列方法进行位姿跟踪。W

[0038] a.计算目标点 Op相对于末端抓手T在基坐标系I下的位姿坐标差D′oe为位姿坐标差向量，ΔP′为位置差向量，
ΔEul′为姿态差向量，并计算抓手与WOp之间的距离dv；

[0039] b.计算下一步位移量Send＝vel*dt*D′oe/dv，其中vel为设定的机械臂末端运动速度，dt为运动控制周期；

[0040] c.针对末端位移量Send，由运动学逆解计算始末端对应的两组关节角度值θnow，θnext，其中θnow为当前关节角度值，θnext为下一个周期开始时的关节角度值；

[0041] d.考虑到关节角速度约束条件，则每个周期关节实际转角有约束范围，当(θnext-θnow)不超过约束范围时，以(θnext-θnow)作为下一步实际动作的关节角度θplan，若(θnext-θnow)使某些关节转角超过约束范围时，取θmax＝max(θnext_i-θnow_i)，则其中ang为关节最大转角，则 为下一步第i个关节实际关节角度；

[0042] e.判断此时抓手与WOp之间的距离dv＜d′min是否成立，d′min为设定的距离值，以及此时末端姿态欧拉角与目标物姿态差ΔEul′是否在给定的误差范围内，若在，则该中间点跟踪结束；否则，返回到第1)步继续跟踪，直到满足条件为止；

[0043] f.按上述步骤，依次跟踪所有中间点，最终跟踪目标物体，到达后捕获目标物体。

[0044] 本发明未详细阐述部分属于本领域技术人员公知技术。