CN110298854B - 基于在线自适应与单目视觉的飞行蛇形臂协同定位方法 - Google Patents

Abstract

本发明涉及一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，该方法包括如下步骤：(1)搭建单目相机采集系统；(2)获取机载机械臂的CAD模型；(3)单目相机实时采集包含机载机械臂的图像；(4)处理CAD模型，创建机载机械臂各节的匹配模板集；(5)提取图像中的边缘信息，匹配得到机载机械臂各节的最优匹配模板；(6)根据最优匹配模板计算得到位于图像内且遮挡较少的关节的位姿；(7)基于机械臂各关节的拓扑关系计算得到其余关节的位姿，完成各个关节初步位姿的获取；(8)对各个关节的初步位姿进行协同粗到精优化得到各个关节的精确位姿。与现有技术相比，本发明方法定位准确、成本低、精度高、速度快、可快速自适应。

Description

基于在线自适应与单目视觉的飞行蛇形臂协同定位方法

技术领域

本发明涉及机器人视觉技术领域，尤其是涉及一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法。

背景技术

蛇形机器人是一种能够模仿生物蛇运动的新型仿生机器人，蛇体采用模块化设计思路，利用多关节来模拟蛇类生物的柔软身体。蛇形机器人作为一种特种机器人能够用于各种复杂狭小封闭环境作业以及高柔性操作等领域。当蛇形机器人一端固定，则变为蛇形机械臂，如安装于移动平台(移动机器人)上，可以用于管道检测以及公共场合的安检、排爆；安装于工业机器人末端可以用于飞机复杂箱体内的加工装配、发动机检测以及飞机壁板的涂胶等；蛇形机械臂还可以用于医疗，设计小型的移动机器人，进入人体器官(如肠道、血管)完成手术和定点给药是机器人研究的一个新的热门领域。

目前，多旋翼无人机正在广泛研究并制造中，可以用于高危险环境完成指定任务比如森林灭火或者人员救援，也可代替人工更高效地实现运输或者监测任务。无人机将进一步设计成高性价比、高机动性的作业机器人，各个研究机构正在开发能够快速响应作业的无人机。通常无人机是通过加载在无人机上的设备进行作业，如用于拍摄的摄像机，用于各种特定操作的机械臂。

定位是机械臂作业的前提之一，对于蛇形机械臂来说尤为重要，这是因为蛇形机械臂的控制大多是通过依次控制蛇形机械臂各节完成，控制的前提是知道蛇形机械臂各节的准确位姿。由于较多的关节，难以通过常规方法准确确定各关节位姿。视觉传感器由于信号探测范围广、信息量大等优点成为机械臂定位的一个主要研究方向常作为机械臂控制系统的重要组成部分，用于感知周围环境并指导蛇形机械臂进行作业。目前常用的依赖视觉进行机械臂定位的方法为：1)在机械臂尾端安装一个视觉系统，对目标进行识别与跟踪；2)在机械臂各部分上粘贴一张二维码，通过机载视觉系统获取二维码的位姿信息从而确定机械臂的位姿。对于第一种方法，蛇形机械臂作业时，尾端位姿变化较大较快，会引起尾端视觉系统采集到的图像变化较快，跟踪难度很大。对于第二种方法，随着蛇形机械臂关节的增加，通过二维码获取的位姿信息存在一定误差，由于累积效应会使蛇形机械臂的定位产生较大误差。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，该方法包括如下步骤：

(1)搭建单目相机采集系统；

(2)获取首尾双头蛇形机械臂的CAD模型并标定单目相机；

(3)单目相机实时采集包含首尾双头蛇形机械臂的图像；

(4)处理CAD模型，创建首尾双头蛇形机械臂各节的匹配模板集；

(5)提取图像中首尾双头蛇形机械臂的边缘信息，匹配得到首尾双头蛇形机械臂各节的最优匹配模板；

(6)根据最优匹配模板计算得到首尾双头蛇形机械臂中位于图像内且遮挡较少的关节的位姿信息；

(7)基于首尾双头蛇形机械臂各关节的拓扑关系计算得到除步骤(6)中所述关节之外的其余关节的位姿信息，完成整个首尾双头蛇形机械臂各个关节的初步位姿信息的获取；

(8)对首尾双头蛇形机械臂各个关节的初步位姿信息进行协同优化得到各个关节的精确位姿信息。

步骤(1)中搭建单目相机采集系统具体为：将首尾双头蛇形机械臂安装在多旋翼无人机底部，同时将单目相机安装于多旋翼无人机底部且位于首尾双头蛇形机械臂首端附近，所述的单目相机进行3自由度运转：单目相机绕多旋翼无人机中心做圆周运动且自身在一定范围内做俯仰运动。

步骤(2)中CAD模型采用STL格式的三维模型，该CAD模型包含首尾双头蛇形机械臂各个关节的CAD模型及各关节CAD模型间的拓扑关系。

步骤(2)中标定单目相机包括：

(21)定义世界坐标系S，模型坐标系M₁,M₂…M_n、相机坐标系X和成像平面I；世界坐标系S固联于多旋翼无人机底部，以多旋翼无人机底部中心为原点；模型坐标系M₁,M₂…M_n固联于首尾双头蛇形机械臂各节CAD模型，分别以首尾双头蛇形机械臂各节模型中心为原点，其相对于世界坐标系S的运动即为首尾双头蛇形机械臂各节的位姿；相机坐标系X固联于单目相机，以光轴背离观察者方向为Z轴；成像平面I为过单目相机焦点且平行于相机坐标系X的xoy平面；

(22)获取单目相机外参数，该外参数用于将坐标从世界坐标系转为相机坐标系；

(23)获取单目相机内参数，该内参数用于将坐标从相机坐标系转为成像平面。

步骤(4)针对各个关节采用如下步骤创建首尾双头蛇形机械臂各节的匹配模板集：

(41)建立虚拟观察点：单目相机当前位置作为实际观察点，在实际观察点附近按经纬网格的方式均匀分布N个虚拟观察点X₁、X₂、…、X_N，定义虚拟观察点坐标系G₁,G₂…G_N，虚拟观察点坐标系G₁,G₂…G_N固联于各虚拟观察点，以光轴背离观察者方向为Z轴；

(42)针对各虚拟观察点对CAD模型进行处理，消除CAD模型中被遮挡的不可见部分，获取各虚拟观察点下的CAD模型文件；

(43)根据各虚拟观察点下的CAD模型文件更新索引表；

(44)根据更新的索引表提取各虚拟观察点下的CAD模型的轮廓；

(45)投影各虚拟观察点下的CAD模型的轮廓至各虚拟观察点对应的成像平面；

(46)将各成像平面中的线段离散成点，获取各虚拟观察点对应的点集，每个点集对应为一个匹配模板，进而各个关节形成N个匹配模板。

步骤(46)中将各成像平面中的线段离散成点具体操作为：当线段长度l大于采样间距s时，等间距采样；当线段长度l小于采样间距s时，顺序连接与该线段相邻的线段，直到两端跨度大于l，取中点为这些线段的采样点。

步骤(5)具体为：

(51)提取图像中首尾双头蛇形机械臂的边缘信息，得到首尾双头蛇形机械臂的边缘线段集；

(52)针对首尾双头蛇形机械臂每一个关节分别匹配选取最优匹配模板：获取该关节在所有虚拟观察点下的匹配模板点集，针对每个点集，计算该点集中各点与对应边缘线段集中最近线段的距离，若该距离大于阈值L，则舍弃该点，计算该点集中其余点与最近线段距离的平均值，选取平均值最小的匹配模板点集作为该关节的最优匹配模板。

步骤(6)具体为：

首先，判断首尾双头蛇形机械臂中位于图像内且遮挡较少的关节：步骤(52)进行最优匹配模板匹配时，若最优匹配模板对应的点集中小于阈值L的点的个数大于阈值M，则该关节为位于图像内且遮挡较少的关节；

然后，对于首尾双头蛇形机械臂中位于图像内且遮挡较少的第i个关节，利用上一时刻蛇形机械臂第i个关节在相机坐标系中的位姿进行坐标变换得到当前时刻蛇形机械臂第i个关节在世界坐标系中的位姿，首尾双头蛇形机械臂第i个关节在世界坐标系中的位姿即为第i个关节的位姿信息。

步骤(7)具体为：提取首尾双头蛇形机械臂中与待定位的关节直接相连的前后两个关节的位姿信息，根据前后两个关节的位姿信息以及首尾双头蛇形机械臂各个关节的活动范围计算得到待定位的关节的位姿信息。

步骤(8)具体为：

(81)根据首尾双头蛇形机械臂第i个关节的初步位姿信息

和第i+1个关节的初步位姿信息

获取第i个关节末端和第i+1个关节首端距离d_i，i＝1,2,……n-1，n为关节总数；

(82)建立代价方程

α和β为常参数，b_i为第i个关节位姿变化的距离；

(83)调整首尾双头蛇形机械臂各个关节的位姿使得代价方程ε取值最小，输出首尾双头蛇形机械臂各个关节的最终位姿即为各个关节对应的精确位姿信息。

与现有技术相比，本发明具有如下优点：

(1)本发明方法结合了蛇形机械臂的CAD模型和单目相机视觉信息，通过单目相机实时采集包含首尾双头蛇形机械臂的图像，实现了首尾双头蛇形机械臂的准确定位，成本低、精度高、速度快、可快速自适应、可获得首尾双头蛇形机械臂所有关节位姿；

(2)本发明方法针对各实际观察点建立虚拟观察点，从而建立匹配模板，通过模板匹配的方式获取最优匹配模板，快速确定蛇形机械臂当前时刻的位姿状态，定位速度快；

(3)本发明首先通过匹配模板实现首尾双头蛇形机械臂的粗定位，进而通过首尾双头蛇形机械臂各节的拓扑关系协同优化机械臂各节位姿信息完成细定位，使得输出的最终位姿信息更加准确，即使存在部分或其全部遮挡甚至处于图像外，依然且可以准确输出首尾双头蛇形机械臂所有关节位姿。

附图说明

图1为本发明单目相机采集系统的结构示意图；

图2为本发明实施例中基于在线自适应与单目视觉的飞行蛇形臂协同定位方法的流程框图；

图3为本发明实施例中建立顶点索引表的流程框图；

图4为本发明实施例中建立线段索引表的流程框图；

图5为本发明实施例中对线段进行采样的示意图(l>s)；

图6为本发明实施例中对线段进行采样的示意图(l<s)。

图中，1为多旋翼无人机，2为首尾双头蛇形机械臂，3为单目相机，4为对接模块。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。注意，以下的实施方式的说明只是实质上的例示，本发明并不意在对其适用物或其用途进行限定，且本发明并不限定于以下的实施方式。

实施例

一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，该方法包括如下步骤：

(1)搭建单目相机采集系统；

(2)获取首尾双头蛇形机械臂的CAD模型并标定单目相机；

(3)单目相机实时采集包含首尾双头蛇形机械臂的图像；

(4)处理CAD模型，创建首尾双头蛇形机械臂各节的匹配模板集；

(5)提取图像中首尾双头蛇形机械臂的边缘信息，匹配得到首尾双头蛇形机械臂各节的最优匹配模板；

(6)根据最优匹配模板计算得到首尾双头蛇形机械臂中位于图像内且遮挡较少的关节的位姿信息；

(7)基于首尾双头蛇形机械臂各关节的拓扑关系计算得到除步骤(6)中所述关节之外的其余关节的位姿信息，完成整个首尾双头蛇形机械臂各个关节的初步位姿信息的获取；

(8)对首尾双头蛇形机械臂各个关节的初步位姿信息进行协同优化得到各个关节的精确位姿信息。

优选地，步骤(1)中搭建单目相机采集系统具体为：将首尾双头蛇形机械臂安装在多旋翼无人机底部，同时将单目相机安装于多旋翼无人机底部且位于首尾双头蛇形机械臂首端附近，所述的单目相机进行3自由度运转：单目相机绕多旋翼无人机中心做圆周运动且自身在一定范围内做俯仰运动。

具体地，如图1所示，首尾双头蛇形机械臂2安装于多旋翼无人机1底部，可向任意方向作业，单目相机3通过对接模块4安装于多旋翼无人机1底部，单目相机3可以3自由度运转：首先，单目相机3能以对接模块4为轴在一定范围内做俯仰运动，其次对接模块4可绕多旋翼无人机1中心轴做圆周运动，因此单目相机3可实时采集含有首尾双头蛇形机械臂的图像、

如图2所示为本发明基于在线自适应与单目多关节协同的蛇形机械臂定位方法的具体流程框图，包括离线准备阶段和在线定位阶段两部分。应说明的是，本发明实施例的所述方法并不限于图2所示的流程图中的步骤及顺序。根据不同的实施例，图2所示的流程图中的步骤可以增加、移除、或者改变顺序。

首先，离线准备阶段：

步骤101，获取首尾双头蛇形机械臂的CAD模型，CAD模型包含首尾双头蛇形机械臂各节的CAD模型及各节CAD模型间的拓扑关系，CAD模型采用STL格式的三维模型；

在本实施方式中，三维CAD模型可由多种软件创建，但需要转换为通用格式STL，使用ASCⅡ码存储几何体表面的细分三角网格来表示封闭的面或者体；

具体地，ASCⅡ码格式的STL文件逐行给出三角形的几何信息，每一行以1个或2个关键字开头，以此存储有三角形的法向量、三个顶点的坐标，各坐标均为在模型坐标系中的坐标。

步骤102，标定单目相机包括：

(21)定义世界坐标系S，模型坐标系M₁,M₂…M_n、相机坐标系X和成像平面I；世界坐标系S固联于多旋翼无人机底部，以多旋翼无人机底部中心为原点；模型坐标系M₁,M₂…M_n固联于首尾双头蛇形机械臂各节CAD模型，分别以首尾双头蛇形机械臂各节模型中心为原点，其相对于世界坐标系S的运动即为首尾双头蛇形机械臂各节的位姿；相机坐标系X固联于单目相机，以光轴背离观察者方向为Z轴；成像平面I为过单目相机焦点且平行于相机坐标系X的xoy平面；

(22)获取单目相机外参数，该外参数用于将坐标从世界坐标系转为相机坐标系；

(23)获取单目相机内参数，该内参数用于将坐标从相机坐标系转为成像平面。

其中外参数用于将坐标从世界坐标系转为相机坐标系(或虚拟观察点坐标系)，这是一个三维点到三维点的转换，包括R和t，R代表旋转矩阵，t代表平移向量,共同组成了单目相机坐标系X相对于世界坐标系S的变换矩阵：

其中内参数用于将坐标从相机坐标系转为成像平面，这是一个三维点到二维点的转换，包括K，其中：

其中，dx和dy代表相机单个感光单元芯片的长度和宽度，是一个物理尺寸，Cx和Cy分别代表相机感光芯片的中心点在x和y方向上可能存在的偏移，f代表相机的焦距。设空间中一向量P在世界坐标系下的坐标P^S＝[X；Y；Z],则其在相机坐标系中的坐标P^X＝[X′；Y′；Z′]可以利用下式进行计算：

(注意这里利用了齐次坐标)；其在成像平面中的坐标P^I＝[u,v]可以利用下式进行计算：Z*[P^I；1]＝KP^X，

即：

已知单目相机相对于世界坐标系的初始位姿

从t-1时刻到t时刻相机的运动十分简单且可以准确控制，故可以实时知道单目相机相对于世界坐标系的初始位姿X^S，也就能实时知道单目相机坐标系X相对于世界坐标系S的变换矩阵

在线定位阶段从步骤103开始。

步骤103，初始化：将首尾双头蛇形机械臂和单目相机移至初始位置，得到首尾双头蛇形机械臂各节的初始位姿

在本实施方式中，为保证后续步骤的准确性，将首尾双头蛇形机械臂和单目相机的极限位置作为初始位置；

具体地，由于首尾双头蛇形机械臂的初始位置已知，可获得各模型坐标系M₁,M₂…M_n在世界坐标系中的初始位姿

也即首尾双头蛇形机械臂各节的初始位姿

步骤104，首尾双头蛇形机械臂作业过程中，实时移动单目相机，保证首尾双头蛇形机械臂的主体部分时刻处于单目相机视野内。

步骤105，建立虚拟观察点：单目相机前一帧位置作为实际观察点，在实际观察点附近按经纬网格的方式均匀分布N个虚拟观察点X₁、X₂、…、X_N，定义虚拟观察点坐标系G₁,G₂…G_N，虚拟观察点坐标系G₁,G₂…G_N固联于各虚拟观察点，以光轴背离观察者方向为Z轴，由于虚拟观察点在世界坐标系中相对于单目相机的相对位置已知，故可获得各虚拟观察点在世界坐标系中的坐标

可得到固联于各虚拟观察点即各虚拟单目相机上的各虚拟观察点坐标系相对于世界坐标系的变换矩阵

步骤106，针对各虚拟观察点对首尾双头蛇形机械臂CAD模型进行处理，消除CAD模型中被遮挡的不可见部分，获取各虚拟观察点下的CAD模型文件。

具体地，从观察点观察首尾双头蛇形机械臂各节，会有部分面和线段因被遮挡而不可见，由于在stl格式文件通过几何体表面的细分三角网格来表示模型，故只需删除这些不可见的三角形即可达到消除模型中被遮挡的不可见部分的目的；

进一步地，不可见的三角形分为两类，一类由于位于首尾双头蛇形机械臂各节背面，被本身遮挡而不可见，另一类由于首尾双头蛇形机械臂其他部分遮挡而不可见；

进一步地，对于第一类，可计算某一观察点下，三角形法向量与观察方向的夹角，若为锐角或直角，则该三角形位于首尾双头蛇形机械臂各节正面，若为钝角，则该三角形位于首尾双头蛇形机械臂各节背面，被遮挡而不可见，保留原模型后，在模型中删除这些三角形；

进一步地，对于第二类，相较于第一类中被遮挡的三角形，数量较少，且后续可利用首尾双头蛇形机械臂各节之间的拓扑关系减弱影响，故此处不处理。

步骤107，根据各虚拟观察点下的首尾双头蛇形机械臂CAD模型文件更新索引表。

具体地，参考图3，依次读取stl文件中首尾双头蛇形机械臂各节的各个三角形的法向量、三顶点坐标，建立三角形索引表，包含各个三角形的法向量及三个顶点坐标信息；参考图4，建立线段索引表，包含线段端点坐标、线段临面法向量、线段中点坐标信息。

步骤108，根据更新的索引表提取各虚拟观察点下的首尾双头蛇形机械臂各节的CAD模型的轮廓。

具体地，轮廓包括两类：棱边线段和外轮廓线段；

进一步地，棱边线段是几何体上两相临面的公共线段，故这些线段的两个临面呈一定夹角，计算每条线段临面法向量夹角，当夹角大于某一阈值时，判定该线段为棱边线段，反之判定其不是棱边线段；

进一步地，外轮廓线段是在某一观察点下，所看到的模型最外缘的线段，对于处理过的CAD模型，判断每条线段是否只有一个临面，若只有一个临面，判定其为外轮廓线段，若有两个临面，判定其不是外轮廓线段。

步骤109，投影各虚拟观察点下的首尾双头蛇形机械臂各节的CAD模型的轮廓至各虚拟观察点对应的成像平面。

具体地，已知固联于第i个虚拟观察点的虚拟观察点坐标系相对于世界坐标系的变换矩阵

固联于首尾双头蛇形机械臂第j节的模型坐标系相对于世界坐标系的变换矩阵

可得到模型坐标系相对于相机坐标系的变换矩阵

进一步地，可根据小孔成像原理，结合相机内参数，求出各线段端点在成像平面I上投影的坐标，从而得到各线段在成像平面I上的投影。

步骤110，将各成像平面中的线段离散成点，获取各虚拟观察点对应的n个点集，每个点集对应为一个匹配模板，这里n表示首尾双头蛇形机械臂共有n节。

具体地，如图5所示当线段长度l大于采样间距s时，等间距采样；如图6所示，当线段长度l小于采样间距s时，顺序连接与该线段相邻的线段，直到两端跨度大于l，取中点为这些线段的采样点。

步骤111，提取图像中首尾双头蛇形机械臂的边缘信息，得到图像中首尾双头蛇形机械臂的边缘线段集。具体地，一种可行的方法包括边缘检测和边缘编组两个步骤。其中，边缘检测的任务是检测出图像中灰度值、颜色或纹理不连续或突变的地方，即边缘，边缘编组的任务是按照某种规则，将边缘顺次组合成知觉上显著的轮廓，如可使用边缘连接、动态规划和轮廓编组等方法。

步骤112，将图像中首尾双头蛇形机械臂的边缘线段集分别与各匹配模板的点集进行匹配，选取最优匹配模板。

具体地，对于N个虚拟观察点与n个模型坐标系，每个虚拟观察点都会产生n个匹配模板，每个匹配模板都对应首尾双头蛇形机械臂的一节，针对首尾双头蛇形机械臂每一个关节分别匹配选取最优匹配模板：获取该关节在所有虚拟观察点下的匹配模板点集，针对每个点集，计算该点集中各点与对应边缘线段集中最近线段的距离，若该距离大于阈值L，则舍弃该点，计算该点集中其余点与最近线段距离的平均值，选取平均值最小的匹配模板点集作为该关节的最优匹配模板。

步骤113，针对首尾双头蛇形机械臂中位于图像内且遮挡较少的关节，根据最优匹配模板获取位姿信息，具体地：首先，判断首尾双头蛇形机械臂中位于图像内且遮挡较少的关节：步骤(52)进行最优匹配模板匹配时，若最优匹配模板对应的点集中小于阈值L的点的个数大于阈值M，则该关节为位于图像内且遮挡较少的关节；

然后，对于首尾双头蛇形机械臂中位于图像内且遮挡较少的第i个关节，利用上一时刻蛇形机械臂第i个关节在相机坐标系中的位姿进行坐标变换得到当前时刻蛇形机械臂第i个关节在世界坐标系中的位姿，首尾双头蛇形机械臂第i个关节在世界坐标系中的位姿即为第i个关节的位姿信息。

用蛇形机械臂其中一节来说明：

因为t-1时刻的蛇形机械臂各节在t-1时刻的相机坐标系中的位姿P^X已知，又因为最优模板对应的虚拟观察点坐标系相对于t-1时刻相机坐标系的变换矩阵

已知，所以蛇形机械臂在最优模板对应的虚拟观察点坐标系中的位姿

已知，又因为蛇形机械臂在最优模板对应的虚拟观察点坐标系中的位姿与在t时刻单目相机坐标系中的位姿相同，所以蛇形机械臂在t时刻的相机坐标系中的位姿P^X＝P^G已知，又因为单目相机坐标系X相对于世界坐标系S的变换矩阵

实时知道，所以t时刻时蛇形机械臂在世界坐标系中的位姿P^S可利用

计算，即

步骤114，基于各节的拓扑关系计算出被完全遮挡以及位于图像外的首尾双头蛇形机械臂各节的位姿。

进一步地，计算出的第i节机械臂的位姿

对应的模型坐标系M_i的坐标原点位于第i节模型中心，由于模型尺寸已知，可计算得到固联于第i节模型末端的坐标系在世界坐标系下的位姿；同理由第i+1节模型中心位姿

可计算得到固联于第i+1节模型首端的坐标系在世界坐标系下的位姿；由于首尾双头蛇形机械臂第i节的末端与第i+1节相连，故这两个坐标系的原点应该重合。

进一步地，首尾双头蛇形机械臂各节的活动范围一定，即第i+1节模型末端的坐标系原点与第i+1节模型首端的坐标系原点的连线、第i节模型末端的坐标系原点与第i节模型首端的坐标系原点的连线的夹角不会超过某一定值。

具体地，对于被完全遮挡以及位于图像外的各节首尾双头蛇形机械臂的位姿，获取与被完全遮挡以及位于图像外的机械臂直接相连的前后两节机械臂的位姿，根据前后两节机械臂的位姿以及首尾双头蛇形机械臂各节的活动范围计算得到被完全遮挡以及位于图像外的机械臂的位姿；进一步地，对于首尾双头蛇形机械臂首端附近的机械臂，始终位于图像外，首端的位置始终已知，可转化为上述情况。

步骤115，基于各节的拓扑关系协同优化首尾双头蛇形机械臂各节的位姿，具体为：根据步骤113获取的首尾双头蛇形机械臂第i节中点位姿

以及第i+1节中点位姿

获取首尾双头蛇形机械臂第i节末端坐标系位姿以及第i+1节首端坐标系位姿，计算两坐标系原点的距离d_i，建立代价方程

调整步骤113获取首尾双头蛇形机械臂各节的位姿

使得代价方程最小，其中b_i为第i节模型位姿原点变化的距离，α与β为参数。

重复步骤104至115，实时输出首尾双头蛇形机械臂各节在世界坐标系S中的位姿。

上述实施方式仅为例举，不表示对本发明范围的限定。这些实施方式还能以其它各种方式来实施，且能在不脱离本发明技术思想的范围内作各种省略、置换、变更。

Claims (10)

1.一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，该方法包括如下步骤：

(1)搭建单目相机采集系统；

(2)获取首尾双头蛇形机械臂的CAD模型并标定单目相机；

(3)单目相机实时采集包含首尾双头蛇形机械臂的图像；

(4)处理CAD模型，创建首尾双头蛇形机械臂各节的匹配模板集；

(5)提取图像中首尾双头蛇形机械臂的边缘信息，匹配得到首尾双头蛇形机械臂各节的最优匹配模板；

(6)根据最优匹配模板计算得到首尾双头蛇形机械臂中位于图像内且遮挡较少的关节的位姿信息；

(7)基于首尾双头蛇形机械臂各关节的拓扑关系计算得到除步骤(6)中所述关节之外的其余关节的位姿信息，完成整个首尾双头蛇形机械臂各个关节的初步位姿信息的获取；

(8)对首尾双头蛇形机械臂各个关节的初步位姿信息进行协同优化得到各个关节的精确位姿信息。

2.根据权利要求1所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(1)中搭建单目相机采集系统具体为：将首尾双头蛇形机械臂安装在多旋翼无人机底部，同时将单目相机安装于多旋翼无人机底部且位于首尾双头蛇形机械臂首端附近，所述的单目相机进行3自由度运转：单目相机绕多旋翼无人机中心做圆周运动且自身在一定范围内做俯仰运动。

3.根据权利要求1所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(2)中CAD模型采用STL格式的三维模型，该CAD模型包含首尾双头蛇形机械臂各个关节的CAD模型及各关节CAD模型间的拓扑关系。

4.根据权利要求2所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(2)中标定单目相机包括：

(21)定义世界坐标系S，模型坐标系M₁,M₂…M_n、相机坐标系X和成像平面I；世界坐标系S固联于多旋翼无人机底部，以多旋翼无人机底部中心为原点；模型坐标系M₁,M₂…M_n固联于首尾双头蛇形机械臂各节CAD模型，分别以首尾双头蛇形机械臂各节模型中心为原点，其相对于世界坐标系S的运动即为首尾双头蛇形机械臂各节的位姿；相机坐标系X固联于单目相机，以光轴背离观察者方向为Z轴；成像平面I为过单目相机焦点且平行于相机坐标系X的xoy平面；

(22)获取单目相机外参数，该外参数用于将坐标从世界坐标系转为相机坐标系；

(23)获取单目相机内参数，该内参数用于将坐标从相机坐标系转为成像平面。

5.根据权利要求4所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(4)针对各个关节采用如下步骤创建首尾双头蛇形机械臂各节的匹配模板集：

(41)建立虚拟观察点：单目相机当前位置作为实际观察点，在实际观察点附近按经纬网格的方式均匀分布N个虚拟观察点X₁、X₂、…、X_N，定义虚拟观察点坐标系G₁,G₂…G_N，虚拟观察点坐标系G₁,G₂…G_N固联于各虚拟观察点，以光轴背离观察者方向为Z轴；

(42)针对各虚拟观察点对CAD模型进行处理，消除CAD模型中被遮挡的不可见部分，获取各虚拟观察点下的CAD模型文件；

(43)根据各虚拟观察点下的CAD模型文件更新索引表；

(44)根据更新的索引表提取各虚拟观察点下的CAD模型的轮廓；

(45)投影各虚拟观察点下的CAD模型的轮廓至各虚拟观察点对应的成像平面；

(46)将各成像平面中的线段离散成点，获取各虚拟观察点对应的点集，每个点集对应为一个匹配模板，进而各个关节形成N个匹配模板。

6.根据权利要求5所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(46)中将各成像平面中的线段离散成点具体操作为：当线段长度l大于采样间距s时，等间距采样；当线段长度l小于采样间距s时，顺序连接与该线段相邻的线段，直到两端跨度大于l，取中点为这些线段的采样点。

7.根据权利要求5所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(5)具体为：

(51)提取图像中首尾双头蛇形机械臂的边缘信息，得到首尾双头蛇形机械臂的边缘线段集；

(52)针对首尾双头蛇形机械臂每一个关节分别匹配选取最优匹配模板：获取该关节在所有虚拟观察点下的匹配模板点集，针对每个点集，计算该点集中各点与对应边缘线段集中最近线段的距离，若该距离大于阈值L，则舍弃该点，计算该点集中其余点与最近线段距离的平均值，选取平均值最小的匹配模板点集作为该关节的最优匹配模板。

8.根据权利要求7所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(6)具体为：

首先，判断首尾双头蛇形机械臂中位于图像内且遮挡较少的关节：步骤(52)进行最优匹配模板匹配时，若最优匹配模板对应的点集中小于阈值L的点的个数大于阈值M，则该关节为位于图像内且遮挡较少的关节；

然后，对于首尾双头蛇形机械臂中位于图像内且遮挡较少的第i个关节，利用上一时刻蛇形机械臂第i个关节在相机坐标系中的位姿进行坐标变换得到当前时刻蛇形机械臂第i个关节在世界坐标系中的位姿，首尾双头蛇形机械臂第i个关节在世界坐标系中的位姿即为第i个关节的位姿信息。

9.根据权利要求5所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(7)具体为：提取首尾双头蛇形机械臂中与待定位的关节直接相连的前后两个关节的位姿信息，根据前后两个关节的位姿信息以及首尾双头蛇形机械臂各个关节的活动范围计算得到待定位的关节的位姿信息。

10.根据权利要求5所述的一种基于在线自适应与单目视觉的飞行蛇形臂协同定位方法，其特征在于，步骤(8)具体为：

(81)根据首尾双头蛇形机械臂第i个关节的初步位姿信息P_i ^S和第i+1个关节的初步位姿信息

获取第i个关节末端和第i+1个关节首端距离d_i，i＝1,2,……n-1，n为关节总数；

(82)建立代价方程

α和β为常参数，b_i为第i个关节位姿变化的距离；

(83)调整首尾双头蛇形机械臂各个关节的位姿使得代价方程ε取值最小，输出首尾双头蛇形机械臂各个关节的最终位姿即为各个关节对应的精确位姿信息。

Images