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

权利要求书

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个匹配模板。