[发明专利]双视觉飞行机械臂系统及动态目标位姿估计与主动重建方法

权利要求书

1.一种双视觉飞行机械臂系统，其特征在于，包括：

无人机，所述无人机用于执行空中移动任务，所述无人机上设置搭载平台(1)和机械臂(4)基座(2)；

机械臂(4)，所述机械臂(4)的首端安装在所述无人机的机械臂(4)基座(2)上，所述机械臂(4)的末端安装用于执行空中抓取任务的夹爪；

机体相机(3)，所述机体相机(3)设置在所述搭载平台(1)上用于观测静态环境，所述机体相机(3)从静态环境中获得的图像信息，并提取静态特征点用于估计机体相机(3)相对于世界坐标系的位姿，以获取无人机相对于世界坐标系的位姿；

颈眼相机(5)，所述颈眼相机(5)安装在所述机械臂(4)的末端，所述颈眼相机(5)用于对动态目标进行追踪，并提取动态特征点用于估计颈眼相机(5)与动态目标的相对位姿。

2.一种面向飞行机械臂的动态目标位姿估计与主动重建方法，其特征在于，包括如下步骤：

步骤S1.位姿估计过程：在无人机在执行飞行任务时，无人机上安装的机械臂用于调整机械臂末端的颈眼相机和夹爪位置，同时无人机和机械臂工作过程时，构建松耦合式位姿估计系统，其中位姿估计系统的位姿估计量包括无人机机体世界位姿、动态目标世界位姿及颈眼相机与动态目标间的相对位姿，通过无人机上的机体相机平视前方，获得来源于静态环境的图像信息并提取静态特征点，提取的静态特征点用于估计机体相机相对于世界坐标系的位姿；并通过安装在机械臂末端的颈眼相机结合机械臂基座位姿来获取动态目标的世界位姿；再者通过对动态目标进行追踪，以估计颈眼相机与动态目标的相对位姿；

步骤S2.动态目标的主动三维重建：基于步骤S1的位姿估计量，将颈眼相机的原始相机点云经转换并提取得到相对静止的动态目标点云，实现对动态目标的信息增益更新，从候选视角中优化得到最佳探索视角；

步骤S3.对动态目标进行主动追踪：在无人机和机械臂工作过程中，通过协调控制无人机与机械臂，分别控制无人机机体的位置与机械臂的关节角，以完成连续的位姿估计与三维重建；对于无人机的机体，在伺服最佳探索视角的同时，需要追踪动态目标的世界位姿，从而使最佳探索视角的相对位姿不变，同时结合动态目标的旋转，对无人机的机体位置进行修正；且通过基于图像的视觉伺服主动追踪策略，以保证对动态目标的观测质量；综上，对动态目标进行主动追踪的算法如下：

其中为无人机的世界位置，为目标物的世界位置，为当前视角与下一个最佳探索视角的路点插值，ΔP_r为考虑目标物旋转的位置修正项，ΔP_l为视觉伺服的位置修正量；通过上述算法实现对动态目标追踪进行；对于机械臂的关节角控制而言，当到达最佳探索位置时，控制机械臂伺服最佳探索方位。

3.根据权利要求2所述的面向飞行机械臂的动态目标位姿估计与主动重建方法，其特征在于，所述步骤S1中，动态目标的世界位姿获取方法如下：

a1.首先通过手眼标定得到颈眼相机与机械臂末端的相对位姿变换；

a2.再通过机械臂正运动学得到机械臂末端相对于机械臂基座的座标系的位姿变换，同时将机械臂基坐标系与机体相机的相对位姿视作平移变换；

a3.结合步骤a2的变换关系，将动态目标的位姿表示在世界坐标系下，以实现通过机械臂运动学建立基于机体相机的视觉里程计和基于颈眼相机的视觉里程计的联系，进而获得动态目标的世界位姿。

4.根据权利要求2或3所述的面向飞行机械臂的动态目标位姿估计与主动重建方法，其特征在于，所述步骤S1中对动态目标进行追踪的方法如下：

b1.前端采用深度学习网络检测目标，得到动态目标的2D包围框，2D包围框中包含动态目标和静态背景，提取2D包围框中的动态目标和静态背景的特征点，所述特征点沿着动态目标分布；

b2.基于2D PCA提取2D包围框中特征分布的主轴与次轴，通过判断特征点到主、次轴的加权距离，有效分割2D包围框内近景与远景的特征，将2D包围框内中远景的特征删除，以实现对静态背景的特征点的删除，然后保留提取到的动态特征点用于估计颈眼相机相对动态目标的位姿；

b3.在经将2D包围框中属于静态背景的特征噪点后剔除及位姿估计结束后，对由于数据存在而产生的噪点，通过卡方检验进一步剔除。