CN110775288A - 一种基于仿生的飞行机械颈眼系统及控制方法 - Google Patents

Abstract

本发明公开了一种基于仿生的飞行机械颈眼系统及其控制方法，包括飞行器系统、机械颈眼系统、机载计算设备和飞控单元；机械颈眼系统包括安装在所述飞行器本体上的机械颈、安装在所述机械颈末端的双目视觉惯性系统、控制机械颈运动的机械颈主控；所述双目视觉惯性系统包括双目相机和惯性测量单元，所述双目视觉惯性系统用于进行主动观测并完成对机械颈末端的位姿估计；机械颈为六自由度串联机械颈，本发明通过将双鱼眼和惯性测量单元组成的双目视觉惯性系统放置于机械颈的末端形成感知系统，本系统的观测范围更大，相机位姿可变，具有主动感知环境的能力，可以一定程度上解决室内环境飞行器视野受限的问题，同时也可以对画面的晃动进行补偿。

Description

一种基于仿生的飞行机械颈眼系统及控制方法

技术领域

本发明涉及无人机领域，具体涉及一种基于仿生的飞行机械颈眼系统及控制方法。

背景技术

无人机具有大范围的空间移动能力和快速的反应能力，因此广泛应用于军事、农业、巡检等领域，近年来，人们开始了对飞行机械臂的研究，将机械爪甚至是多自由度机械臂安装在多旋翼无人飞行器机体上，从而实现更多更复杂多自由度的操作。

由于多旋翼飞行器在自主感知环境时所用的图像传感器通常是连接在飞行器机体上的，一般会将相机等设备安装于机载云台上以补偿机体的角度抖动。然而这种安装方式在对环境进行观测时常常受限于机体位置而导致观测视角有限，甚至在观测到某些场景时会导致感知失效，在室内狭窄场景中尤为如此。因此实现相机视角的稳定与不受限是一个至关重要的问题。

公开号为CN108279562A的发明专利公开了一种基于滑模PID控制的飞行机械臂的方法。考虑到四旋翼的欠驱动问题,充分利用系统的耦合特性,建立姿态角与位置和高度控制量之间的数学关系,从而通过控制姿态角和位置高度来实现轨迹跟踪的目的。

公开号为CN108248845A的发明专利公开了一种基于动态重心补偿的旋翼飞行机械臂系统及算法，所述系统包括旋翼飞行平台、图像传感器、连接架、机械臂系统、系统控制器和地面站控制装置；其中，图像传感器安装在所述旋翼飞行平台的前下方；一种基于动态重心补偿的旋翼飞行机械臂系统及算法，该算法在保证旋翼飞行机械臂系统稳定飞行的基础上，基于视觉伺服控制方法能更准确的自动定位在抓取目标上，提升了抓取的准确率和时间效率。

公开号为CN109816710A的发明专利公开了一种双目视觉系统高精度且无拖影的视差计算方法利用机载双目视觉系统计算出精确像素视差，且场景深度没有普遍存在的过度平滑现象，无需复杂的后处理，能直接用于下游任务。

上述专利中的相机都是接连在机体上的方法，上述系统在室内狭窄场景的飞行过程中的会受限于机体位置导致对环境的观察不完整，对某些失效环境无法进行躲避。

发明内容

为了解决上述技术问题，本发明目的之一是提供一种基于仿生的飞行机械颈眼系统，针对当前相机装在飞行器本体或上机器人本体上导致的在室内狭窄场景下观测受限等问题，通过一个六自由度的机械颈将双目相机和惯性测量单元组成的观测设备与飞行器本体相连接后，控制六自由度的机械颈达成观测设备相对于飞行器本体的运动，从而实现飞行器在室内狭窄场景的飞行过程中主动对环境进行多视角视觉观测，同时保持末端观测设备的稳定与图像稳定。

为实现上述发明目的，本发明采取的技术方案如下：

一种基于仿生的飞行机械颈眼系统，包括飞行器系统、机械颈眼系统、机载计算设备和飞控单元；

所述飞行器系统包括飞行器本体和安装在所述飞行器本体上的飞行动力系统；

所述机械颈眼系统包括安装在所述飞行器本体上的机械颈、安装在所述机械颈末端的双目视觉惯性系统、控制所述机械颈运动的机械颈主控；所述双目视觉惯性系统包括双目相机和惯性测量单元，所述双目视觉惯性系统用于进行主动观测并完成对机械颈末端的位姿估计；所述机械颈为六自由度串联机械颈，所述机械颈上设置六个舵机用于控制所述机械颈末端的双目视觉惯性系统到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统相对于所述飞行器本体的运动；六个舵机形成机械颈的6个关节；

所述机载计算设备设置在所述飞行器本体上，所述机械颈主控设置于所述所述机载计算设备和所述机械颈上的舵机之间，所述机械颈主控用于对所述机械颈上的舵机进行插补控制；插补控制的过程如下：机械颈主控接收机载计算设备下达的目标状态，目标状态通过逆运动学解算为各关节目标状态，目标状态包括关节的角度角速度与角加速度，根据当前关节状态和目标关节状态通过闭式法生成最小化加加速度的轨迹曲线，通过该曲线可以根据时间求得两点间的任意状态，并将对应状态发送到控制器中，从而完成机械颈的各个舵机的插补控制。

所述飞控单元与所述地面站控制装置采用无线方式进行通讯，所述飞控单元用于所述飞行器本体的底层自稳和动力分配。

优选的，所述机载计算设备与所述飞控单元、所述机械颈主控和所述双目视觉惯性系统通过串口或USB方式进行通信连接。

优选的，所述双目相机包括两个鱼眼相机，两个所述鱼眼相机分别倾斜设置在所述惯性测量单元两侧，即所述鱼眼相机的中轴线与邻近所述惯性测量单元的一侧之间存在夹角，且夹角为锐角，且两个鱼眼相机中轴线之间的夹角为钝角，这样设置可以保证鱼眼相机前方半球中间大部分区域的立体视觉覆盖和全视野的单目视觉覆盖。

优选的，所述飞行器本体的上设置机械颈底座，具体的机械颈底座设置在所述飞行器本体底部，防止机械颈和飞行器本体上的旋转叶片产生碰撞，所述机械颈的首端安装在所述机械颈底座上，所述舵机包括舵机壳体、设置在所述舵机壳体一侧外壁上的舵盘、设置在所述舵机壳体中与所述舵盘连接的驱动组件，所述驱动组件驱动所述舵盘进行转动。所述驱动组件包括电机、减速齿轮组和舵机执行器，所述舵机执行器与电机和所述机械颈主控电连接，所述机械颈主控输出电压信号到所述舵机执行器，所述舵机执行器控制所述电机转动的方向和速度，电机的输出轴与减速齿轮组的连接，减速齿轮组的输出轴与所舵盘连接，电机转动带动减速齿轮组运动，经过减速齿轮组减速后将动力传输至所述舵盘完成舵盘的转动。

优选的，所述机械颈上沿首端至末端方向的舵机依次包括第一舵机、第二舵机、第三舵机、第四舵机、第五舵机和第六舵机；所述第二舵机、第三舵机和第五舵机均上的舵盘包括相对设置的主舵盘和副舵盘；所述主舵盘和副舵盘相对设置，且分别连接在减速齿轮组的输出轴的两端；

所述第一舵机的舵机壳体固定在所述机械颈底座上，所述第一舵机的舵机壳体的至少两个相对的外侧壁通过支撑架与所述机械颈底座固定连接，使第一舵机能够牢固的固定在所述机械颈底座上，所述第一舵机的舵盘设置在所述机械颈底座的正下方，所述第一舵机的舵盘的中轴线与所述机械颈底座互相垂直，所述第一舵机的舵盘通过轴承与所述第二舵机的未设置舵盘的舵机壳体外侧壁连接，所述第二舵机的主舵盘和副舵盘通过两个相互平行的第一连杆分别与所述第三舵机的两个设置舵盘的舵机壳体外侧壁连接；所述第三舵机的主舵盘和副舵盘通过两个相互平行的第二连杆分别与第四舵机的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第四舵机的舵盘与所述第五舵机未设置舵盘的一个舵机壳体外侧壁连接；所述第五舵机的主舵盘和副舵盘通过两个相互平行的第三连杆分别与第六舵机的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第六舵机的舵盘与安装所述双目视觉惯性系统的连接座底部固定连接。

优选的，所述连接座上设置安装鱼眼相机的安装架和安装所述惯性测量单元的安装部，在所述安装部的两侧各设置一个所述安装架。所述第六舵机驱动的所述连接座转动，从而使鱼眼相机和惯性测量单元一起转动，从而完成对鱼眼相机前方的半球区域内的立体视觉覆盖和全视野的单目视觉覆盖。

优选的，所述机械颈底座上设置轴承安装孔，所述轴承的外圈固定在所述轴承安装孔中，所述第一舵机的舵盘与安装在所述轴承内圈中的第一轴上端部固定连接，所述第一轴的下端部与第二轴固定连接，所述第二轴连接所述第二舵机未设置舵盘的舵机壳体外侧壁。所述第一舵机设置舵盘的舵机壳体外侧壁与所述第一轴之间还设置上垫片，所述上垫片在所述第一舵机和第一轴之间起到缓冲作用。

优选的，所述第二轴的底部设置舵机安装座，所述舵机安装座包括顶板和设置顶板两侧的两个连接板，所述顶板与所述第二轴的底部固定连接，两个所述连接板与所述第二舵机设置主舵盘和副舵盘的两个相对的舵机壳体外侧壁固定连接。所述第四舵机的舵盘通过舵机安装座固定连接，所述舵机安装座的两个连接板与所述第五舵机设置主舵盘和副舵盘的两个相对的舵机壳体外侧壁固定连接，所述舵机安装座的顶板与所述第四舵机的舵盘固定连接。

优选的，所述第一舵机和所述第二舵机的输出轴的设置方向互相垂直；所述第二舵机、第三舵机和第五舵机的旋转轴的设置方向互相平行，且与所述第一舵机的旋转轴的设置方向垂直相交；所述第四舵机的旋转轴与所述第一舵机、第二舵机的旋转轴的设置方向均垂直相交，所述第六舵机的旋转轴与所述第一舵机的旋转轴设置方向相同。

所述第一舵机驱动所述第二舵机在所述飞行器本体的底部沿第一舵机舵盘的中轴线做旋转运动，第一舵机舵盘的中轴线为所处空间内的Z轴，所述第二舵机驱动所述第一连杆和第二舵机共同绕着第二舵机的主舵盘和副舵盘的中轴线转动，所述第三舵机驱动所述第二连杆和第四舵机共同绕着第三舵机的主舵盘和副舵盘的中轴线转动，所述第四舵机驱动所述第五舵机绕着第四舵机舵盘的中轴线做旋转运动，所述第五舵机驱动所述第三连杆和第六舵机共同绕着第五舵机的主舵盘和副舵盘的中轴线转动。所述机械颈上的六个舵机在机械颈主控的控制下转动，实现机械颈末端的双目视觉惯性系统在所处空间内完成六自由度的运动，通过机载计算设备和机械颈主控的共同控制下，在飞行器滞空悬停过程中不仅可以对相机翻滚、俯仰和偏航角进行补偿，而且机体的移动也可以得到相应的补偿。使飞行颈眼系统获得的图像相对于捷联式或者云台式系统获得的图像更加稳定且能够实现飞行器在室内狭窄场景的飞行过程中的主动观测。

本发明的目的之二是提供一种基于仿生的飞行机械颈眼系统的控制方法，通过采用的六自由度机械颈在飞行器本体滞空悬停过程中不仅可以对双目相机翻滚、俯仰和偏航角进行补偿，而且飞行器本体的移动也可以得到相应的补偿，使飞行颈眼系统获得的图像相对于捷联式或者云台式系统获得的图像更加稳定且能够实现飞行器在室内狭窄场景的飞行过程中的主动观测，具体包括如下步骤：

步骤(1).所述飞控单元与所述地面站控制装置采用无线方式进行通讯并控制飞行器本体飞行，飞行器本体在所需观测环境中飞行悬停过程中，通过机械颈主控控制所述机械颈上的舵机进行转动，并对舵机进行插补控制，机械颈通过舵机的转动带动所述双目视觉惯性系统中的双目相机进行六自由度的运动并到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统相对于所述飞行器本体的运动，所述双目视觉惯性系统在运动过程中对所处环境进行主动视觉观测并完成对机械颈末端的位姿估计和飞行器本体的位姿估计；

步骤(2).由所述机载计算设备处理所述机械颈眼系统的主动视觉规划、运动学解算、飞行器本体轨迹规划以及双目视觉惯性里程计位姿估计的计算任务；

双目视觉惯性系统采用紧耦合的方式融合双目相机和所述惯性测量单元数据形成双目视觉惯性里程计，视觉里程计是通过所述双目相机运动所引起的图像变化以逐步估计机械颈末端位姿的过程，通过双目相机捕捉周围环境中的信息并结合惯性测量单元数据完成双目视觉惯性里程计位姿估计，并同时得到所述机械颈末端状态中的位姿估计，根据所述机械颈末端的位姿估计结合运动学解算反推得到飞行器本体的位姿估计，然后完成对飞行器本体轨迹规划和主动视觉规划。

其中惯性测量单元通过测量三轴姿态角速度和加速度并进行预积分得到惯性测量单元的相对姿态信息，所述惯性测量单元与所述视觉里程计采用紧耦合的融合方式形成双目视觉惯性里程计，将视觉里程计采集的图像特征信息加入到系统的状态向量中进行联合优化，用预积分技术来处理惯性测量单元数据，将惯性测量单元的运动模型的参考系由固定的初始参考系转为不断变化的相对参考系，所述惯性测量单元的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图进行全局优化。

优选的，所述运动学解算具体包括如下步骤：通过标定后的机械颈参数，计算其正运动学关系，利用正运动学的坐标转换关系逆将机械颈末端位姿转换成飞行器本体位姿，完成对飞行器本体的位姿估计。

优选的，所述步骤(2)中在完成机械颈末端状态中的位姿估计过程中，通过滤波器对视觉里程计与惯性测量单元输出的位姿进行优化估计，并实时监控滤波器中的特征点收敛状态，当某一特征点未收敛时，会对该特征点的逆深度重新计算；在每帧图像的滤波器计算完成之后，在滤波器后端添加一个滑动窗口，所述滑动窗口随着飞行器本体同时运动，利用局部的光束平差法对滑窗口内的特征点位姿与双目相机位姿进一步优化。光束平差法建立在滑动窗口上。滑动窗口中的内容包括：相机关键帧的位姿、关键帧中观测到的二维点位置、所有观测到的特征点已估计的3D位置。该滑动窗口随着相机运动，会不断地维护，加入最新关键帧，删除最旧关键帧与相关特征点和观测。优化过程中的目标函数是所有特征点在所有观测关键帧中的重投影误差的二范数之和，重投影误差即已估计的3D点的位置在各关键帧上的理论2D投影与关键帧中实际观测到的2D位置之间的误差。数值优化方法可利用LM等常见非线性优化算法。在优化过程中的每次迭代需要利用该误差函数对各变量的雅可比矩阵。变量包括滑窗内的所有变量，每次优化迭代，都会对这些变量进行更新。

优选的，所述主动视觉规划具体包括如下步骤：

(1).对飞行器本体所处环境进行建模生成点云地图，评估环境中的特征点丰富程度，并基于实时更新的地图进行机械颈运动路径规划控制，使机械颈末端双目相机始终朝向特征丰富的场景，通过机械颈运动路径规划来实现所述主动视觉规划；避免基于视觉的状态估计器在诸如白墙等无纹理场景下的失效问题；

(2).在机械颈末端状态估计过程中实时维护一个基于特征点的点云地图，并统计环境中各个特征点的平均观测方向与距离，对于每一个机械颈的状态X，通过正运动学推算双目相机的位姿，并将符合平均观测方向与距离的特征点投影到相机平面，根据理论投影得到的特征点的数目、理论观测角度与平均观测角度差、理论观测深度与平均观测深度的差值来计算场景丰富度得分F(X)；

(3).根据双目视觉惯性系统前进方向与任务目标来设定机械颈观测目标位置，利用运动规划RRT*算法在机械颈安装舵机的关节空间内进行机械颈运动路径规划，在生长路径时计算当前状态的场景丰富度F(Xi)，只有当F(Xi)满足达到一个设定的阈值才连接此路径；规划完成后得到离散的期望状态点，在状态点之间利用五次多项式进行轨迹插补，保证状态点前后的加速度平滑，最终将插补后的轨迹输出至机械颈中的各个舵机完成机械颈运动路径规划。

相对于现有技术，本发明取得了有益的技术效果：

(1).本发明采用了六自由度串联机械颈实现双目相机的位移和旋转，可以实现主动移动双目相机和主动视觉规划，相对于通过飞行器运动来实现视角移动的传统方法更加简便，主动视觉可以强化感知效率，增强VIO(视觉惯性里程计)鲁棒性，实现对环境的完整观察，也能够避免基于视觉的状态估计器在诸如白墙等无纹理场景下的失效问题，对潜在失效场景进行躲避；

(2).通过机械颈辅助控制，有利于稳定画面或机体姿态；

(3).有利于实现特定的任务场景，有利于实现室内狭窄受限环境的无人机移动和特定的任务场景，当飞机位置受限时通过机械颈的运动有效探索环境，趋利避害。

附图说明

图1为本发明基于仿生的飞行机械颈眼系统的整体结构示意图；

图2为本发明基于仿生的飞行机械颈眼系统的机械颈结构示意图；

图3为本发明基于仿生的飞行机械颈眼系统的机械颈局部结构示意图；

图4为本发明基于仿生的飞行机械颈眼系统的系统框架图；

图5为本发明基于仿生的飞行机械颈眼系统控制方法中视觉惯性里程计设计过程中的光束平差法的算法流程图；

图6为本发明基于仿生的飞行机械颈眼系统控制方法中视觉里程计经过光束平差法优化后的方法和原方法在EuRoC数据集上的绝对定位误差对比。

附图标记：

1、飞行器系统；11、机械颈底座；111、轴承安装孔；2、机械颈眼系统；21、机械颈；211、第一舵机；2110、轴承；2111、第一轴；2112、第二轴；2113、上垫片；2114、舵机安装座；21141、顶板；21142、连接板；2115、支撑架；212、第二舵机；2121、第一连杆；213、第三舵机；2131、第二连杆；214、第四舵机；215、第五舵机；2151、第三连杆；216、第六舵机；217、连接座；218、主舵盘；219、副舵盘；22、双目视觉惯性系统；221、鱼眼相机；3、滑动窗口；4、关键帧图像平面；5、特征点。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明，但本发明要求保护的范围并不局限于下述具体实施例。

系统实施例

如图1和4所示，一种基于仿生的飞行机械颈眼系统2，包括飞行器系统1、机械颈眼系统2、机载计算设备和飞控单元；

所述飞行器系统1包括飞行器本体和安装在所述飞行器本体上的飞行动力系统；

所述机械颈眼系统2包括安装在所述飞行器本体上的机械颈21、安装在所述机械颈21末端的双目视觉惯性系统22、控制所述机械颈21运动的机械颈主控；所述双目视觉惯性系统22包括双目相机和惯性测量单元，所述双目视觉惯性系统22用于进行主动观测并完成对机械颈21末端的位姿估计；所述机械颈21为六自由度串联机械颈，所述机械颈21上设置六个舵机用于控制所述机械颈21末端的双目视觉惯性系统22到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统22相对于所述飞行器本体的运动；

所述机载计算设备设置在所述飞行器本体上，所述机械颈主控设置于所述所述机载计算设备和所述机械颈21上的舵机之间，所述机械颈主控用于对所述机械颈21上的舵机进行插补控制；插补控制的过程如下：机械颈主控接收机载计算设备下达的目标状态，目标状态通过逆运动学解算为各关节目标状态，目标状态包括关节的角度角速度与角加速度，根据当前关节状态和目标关节状态通过闭式法生成最小化加加速度的轨迹曲线，通过该曲线可以根据时间求得两点间的任意状态，并将对应状态发送到控制器中，从而完成机械颈21的各个舵机的插补控制。

所述飞控单元与所述地面站控制装置采用无线方式进行通讯，所述飞控单元用于所述飞行器本体的底层自稳和动力分配。

所述机载计算设备与所述飞控单元、所述机械颈主控和所述双目视觉惯性系统22通过串口或USB方式进行通信连接。

所述双目相机包括两个鱼眼相机221，两个所述鱼眼相机221分别倾斜设置在所述惯性测量单元两侧，即所述鱼眼相机221的中轴线与邻近所述惯性测量单元的一侧之间存在夹角，且夹角为锐角，且两个鱼眼相机221中轴线之间的夹角为钝角，这样设置可以保证鱼眼相机221前方半球中间大部分区域的立体视觉覆盖和全视野的单目视觉覆盖。

如图2所示，所述飞行器本体的上设置机械颈底座11，具体的机械颈底座11设置在所述飞行器本体底部，防止机械颈21和飞行器本体上的旋转叶片产生碰撞，所述机械颈21的首端安装在所述机械颈底座11上，所述舵机包括舵机壳体、设置在所述舵机壳体一侧外壁上的舵盘、设置在所述舵机壳体中与所述舵盘连接的驱动组件，所述驱动组件驱动所述舵盘进行转动。所述驱动组件包括电机、减速齿轮组和舵机执行器，所述舵机执行器与电机和所述机械颈主控电连接，所述机械颈主控输出电压信号到所述舵机执行器，所述舵机执行器控制所述电机转动的方向和速度，电机的输出轴与减速齿轮组的连接，减速齿轮组的输出轴与所舵盘连接，电机转动带动减速齿轮组运动，经过减速齿轮组减速后将动力传输至所述舵盘完成舵盘的转动。

所述机械颈21上沿首端至末端方向的舵机依次包括第一舵机211、第二舵机212、第三舵机213、第四舵机214、第五舵机215和第六舵机216；所述第二舵机212、第三舵机213和第五舵机215上的舵盘包括相对设置的主舵盘218和副舵盘219；所述主舵盘218和副舵盘219相对设置，且分别连接在减速齿轮组的输出轴的两端；

所述第一舵机211的舵机壳体固定在所述机械颈底座11上，所述第一舵机211的舵机壳体的至少两个相对的外侧壁通过支撑架2115与所述机械颈底座11固定连接，使第一舵机211能够牢固的固定在所述机械颈底座11上，所述第一舵机211的舵盘设置在所述机械颈底座11的正下方，所述第一舵机211的舵盘的中轴线与所述机械颈底座11互相垂直，所述第一舵机211的舵盘通过轴承2110与所述第二舵机212的未设置舵盘的舵机壳体外侧壁连接，所述第二舵机212的主舵盘218和副舵盘219通过两个相互平行的第一连杆2121分别与所述第三舵机213的两个设置舵盘的舵机壳体外侧壁连接；所述第三舵机213的主舵盘218和副舵盘219通过两个相互平行的第二连杆2131分别与第四舵机214的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第四舵机214的舵盘与所述第五舵机215未设置舵盘的一个舵机壳体外侧壁连接；所述第五舵机215的主舵盘218和副舵盘219通过两个相互平行的第三连杆2151分别与第六舵机216的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第六舵机216的舵盘与安装所述双目视觉惯性系统22的连接座217底部固定连接。

所述连接座217上设置安装鱼眼相机221的安装架和安装所述惯性测量单元的安装部，在所述安装部的两侧各设置一个所述安装架。所述第六舵机216驱动的所述连接座217转动，从而使鱼眼相机221和惯性测量单元一起转动，从而完成对鱼眼相机221前方的半球区域内的立体视觉覆盖和全视野的单目视觉覆盖。

如图3所示，所述机械颈底座11上设置轴承安装孔111，所述轴承2110的外圈固定在所述轴承安装孔111中，所述第一舵机211的舵盘与安装在所述轴承2110内圈中的第一轴2111上端部固定连接，所述第一轴2111的下端部与第二轴2112固定连接，所述第二轴2112连接所述第二舵机212。所述第一舵机211设置舵盘的舵机壳体外侧壁与所述第一轴2111之间还设置上垫片2113，所述上垫片2113在所述第一舵机211和第一轴2111之间起到缓冲作用。

所述第二轴2112的底部设置舵机安装座2114，所述第二舵机212安装在舵机安装座2114上，所述舵机安装座2114包括顶板21141和设置顶板21141两侧的两个连接板21142，所述顶板21141与所述第二轴2112的底部固定连接，两个所述连接板21142与所述第二舵机212设置主舵盘218和副舵盘219的两个相对的舵机壳体外侧壁固定连接。所述第四舵机214的舵盘通过舵机安装座2114固定连接，所述舵机安装座2114的两个连接板21142与所述第五舵机215设置主舵盘218和副舵盘219的两个相对的舵机壳体外侧壁固定连接，所述舵机安装座2114的顶板21141与所述第四舵机214的舵盘固定连接。

所述第一舵机211和所述第二舵机212的输出轴的设置方向互相垂直；所述第二舵机212、第三舵机213和第五舵机215的旋转轴的设置方向互相平行，且与所述第一舵机211的旋转轴的设置方向垂直相交；所述第四舵机214的旋转轴与所述第一舵机211、第二舵机212的旋转轴的设置方向均垂直相交，所述第六舵机216的旋转轴与所述第一舵机211的旋转轴设置方向相同。

所述第一舵机211驱动所述第二舵机212在所述飞行器本体的底部沿第一舵机211舵盘的中轴线做旋转运动，第一舵机211舵盘的中轴线为所处空间内的Z轴，所述第二舵机212驱动所述第一连杆2121和第二舵机212共同绕着第二舵机212的主舵盘218和副舵盘219的中轴线转动，所述第三舵机213驱动所述第二连杆2131和第四舵机214共同绕着第三舵机213的主舵盘218和副舵盘219的中轴线转动，所述第四舵机214驱动所述第五舵机215绕着第四舵机214舵盘的中轴线做旋转运动，所述第五舵机215驱动所述第三连杆2151和第六舵机216共同绕着第五舵机215的主舵盘218和副舵盘219的中轴线转动。所述机械颈21上的六个舵机在机械颈主控的控制下转动，实现机械颈末端的双目视觉惯性系统22在所处空间内完成六自由度的运动，通过机载计算设备和机械颈主控的共同控制下，在飞行器滞空悬停过程中不仅可以对相机翻滚、俯仰和偏航角进行补偿，而且机体的移动也可以得到相应的补偿。使飞行颈眼系统获得的图像相对于捷联式或者云台式系统获得的图像更加稳定且能够实现飞行器在室内狭窄场景的飞行过程中的主动观测。

该基于仿生的飞行机械颈眼系统2通过将双鱼眼和惯性测量单元组成的双目视觉惯性系统22放置于机械颈21的末端形成感知系统。相较于将相机固定在机体上的传统捷联式感知系统而言，本系统的观测范围更大，相机位姿可变，具有主动感知环境的能力，可以一定程度上解决室内环境飞行器视野受限的问题，同时也可以对画面的晃动进行补偿。通过控制六自由度的机械颈21能使观测设备相对于机体的运动，最终实现飞行器在室内狭窄场景的飞行过程中的主动观测与图像稳定。

控制方法实施例：

如图4所示，

一种基于仿生的飞行机械颈眼系统2的控制方法，通过采用的六自由度机械颈21在飞行器本体滞空悬停过程中不仅可以对双目相机翻滚、俯仰和偏航角进行补偿，而且飞行器本体的移动也可以得到相应的补偿，使飞行颈眼系统获得的图像相对于捷联式或者云台式系统获得的图像更加稳定且能够实现飞行器在室内狭窄场景的飞行过程中的主动观测，具体包括如下步骤：

步骤(1).所述飞控单元与所述地面站控制装置采用无线方式进行通讯并控制飞行器本体飞行，飞行器本体在所需观测环境中飞行悬停过程中，通过机械颈主控控制所述机械颈21上的舵机进行转动，并对舵机进行插补控制，机械颈21通过舵机的转动带动所述双目视觉惯性系统22中的双目相机进行六自由度的运动并到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统22相对于所述飞行器本体的运动，所述双目视觉惯性系统22在运动过程中对所处环境进行主动视觉观测并完成对机械颈末端的位姿估计和飞行器本体的位姿估计；

步骤(2).由所述机载计算设备处理所述机械颈眼系统2的主动视觉规划、运动学解算、飞行器本体轨迹规划以及双目视觉惯性里程计位姿估计的计算任务；

双目视觉惯性系统采用紧耦合的方式融合双目相机和所述惯性测量单元数据形成双目视觉惯性里程计，视觉里程计是通过所述双目相机运动所引起的图像变化以逐步估计机械颈末端位姿的过程，通过双目相机捕捉周围环境中的信息并结合惯性测量单元数据完成双目视觉惯性里程计位姿估计，并同时得到所述机械颈末端状态中的位姿估计，根据所述机械颈末端的位姿估计结合运动学解算反推得到飞行器本体的位姿估计，然后完成对飞行器本体轨迹规划和主动视觉规划。

其中惯性测量单元通过测量三轴姿态角速度和加速度并进行预积分得到惯性测量单元的相对姿态信息，所述惯性测量单元与所述视觉里程计采用紧耦合的融合方式形成双目视觉惯性里程计，将视觉里程计采集的图像特征信息加入到系统的状态向量中进行联合优化，用预积分技术来处理惯性测量单元数据，将惯性测量单元的运动模型的参考系由固定的初始参考系转为不断变化的相对参考系，所述惯性测量单元的姿态信息与所述视觉里程计的视觉信息融合用于更新姿态图进行全局优化。

所述运动学解算具体包括如下步骤：通过标定后的机械颈参数，计算其正运动学关系，利用正运动学的坐标转换关系逆将机械颈末端位姿转换成飞行器本体位姿，完成对飞行器本体的位姿估计。

所述步骤(2)中在完成机械颈末端状态中的位姿估计过程中，结合基于优化的方法，对基于滤波器的位姿估计方法进行了一系列改进，在系统运行过程中，由于各种因素影响，如特征点5模糊、光线变化和传感器噪声等影响，特征点5在滤波器中的逆深度可能不会快速收敛。因此步骤(2)中针对机械颈末端状态估计中的位姿估计，系统通过滤波器对视觉里程计与惯性测量单元输出的位姿进行优化估计，并实时监控滤波器中的特征点5收敛状态，当某一特征点5未收敛时，会对该特征点5的逆深度重新计算；

如图5所示，由于系统的线性度和噪声的高斯分布程度对基于滤波器的视觉里程计的表现至关重要，但是在实际工程中，这两点很难被满足，为了进一步消除这些因素的影响，采用状态量的局部光束平差优化方法，具体的包括如下步骤：在每帧图像的滤波器计算完成之后，在滤波器后端添加一个滑动窗口3，所述滑动窗口3随着飞行器本体同时运动，利用局部的光束平差法对滑窗口内的特征点位姿与双目相机位姿进一步优化。光束平差法建立在滑动窗口3上。在滑动窗口3上形成关键帧图像平面5，滑动窗口3中的内容包括：相机关键帧的位姿、关键帧中观测到的二维点位置、所有观测到的特征点5已估计的3D位置。该滑动窗口3随着相机运动，会不断地维护，加入最新关键帧，删除最旧关键帧与相关特征点5和观测。优化过程中的目标函数是所有特征点5在所有观测关键帧中的重投影误差的二范数之和，重投影误差即已估计的3D点的位置在各关键帧上的理论2D投影与关键帧中实际观测到的2D位置之间的误差。数值优化方法可利用LM等常见非线性优化算法。在优化过程中的每次迭代需要利用该误差函数对各变量的雅可比矩阵。变量包括滑窗内的所有变量，每次优化迭代，都会对这些变量进行更新。

将优化后的状态估计器在公开数据集EuRoC上做与原方法的对比试验，状态估计器是视觉惯性系统中用于估计系统的位置、角度、速度状态信息的估计器，该估计器为利用相机和惯性测量单元数据，在机载计算设备上运行的程序。两种方法估计的绝对位置的误差如下图6所示，可以看到经过光束平差优化的方法比未使用光束平差的原方法拥有更小的绝对位置误差。

所述主动视觉规划具体包括如下步骤：

(1).对飞行器本体所处环境进行建模生成点云地图，评估环境中的特征点丰富程度，并基于实时更新的地图进行机械颈运动路径规划控制，使机械颈末端双目相机始终朝向特征丰富的场景，通过机械颈运动路径规划来实现所述主动视觉规划；避免基于视觉的状态估计器在诸如白墙等无纹理场景下的失效问题；

(2).在机械颈末端状态估计过程中实时维护一个基于特征点的点云地图，并统计环境中各个特征点的平均观测方向与距离，对于每一个机械颈的状态X，通过正运动学推算双目相机的位姿，并将符合平均观测方向与距离的特征点投影到相机平面，根据理论投影得到的特征点的数目、理论观测角度与平均观测角度差、理论观测深度与平均观测深度的差值来计算场景丰富度得分F(X)；

(3).根据双目视觉惯性系统前进方向与任务目标来设定机械颈观测目标位置，利用运动规划RRT*算法在机械颈安装舵机的关节空间内进行机械颈运动路径规划，在生长路径时计算当前状态的场景丰富度F(Xi)，只有当F(Xi)满足达到一个设定的阈值才连接此路径；规划完成后得到离散的期望状态点，在状态点之间利用五次多项式进行轨迹插补，保证状态点前后的加速度平滑，最终将插补后的轨迹输出至机械颈中的各个舵机完成机械颈运动路径规划。

这种方法保证了最终规划出的机械颈运动路径始终在丰富度强的场景中，实现了主动视觉功能，并增强了系统的观测鲁棒性。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对发明构成任何限制。

Claims (10)

1.一种基于仿生的飞行机械颈眼系统，其特征在于，包括飞行器系统、机械颈眼系统、机载计算设备和飞控单元；

所述飞行器系统包括飞行器本体和安装在所述飞行器本体上的飞行动力系统；

所述机械颈眼系统包括安装在所述飞行器本体上的机械颈、安装在所述机械颈末端的双目视觉惯性系统、控制所述机械颈运动的机械颈主控；所述双目视觉惯性系统包括双目相机和惯性测量单元，所述双目视觉惯性系统用于进行主动观测并完成对机械颈末端的位姿估计；所述机械颈为六自由度串联机械颈，所述机械颈上设置六个舵机用于控制所述机械颈末端的双目视觉惯性系统到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统相对于所述飞行器本体的运动；

所述机载计算设备设置在所述飞行器本体上，所述机械颈主控设置于所述所述机载计算设备和所述机械颈上的舵机之间，所述机械颈主控用于对所述机械颈上的舵机进行插补控制；

所述飞控单元与地面站控制装置采用无线方式进行通讯，所述飞控单元用于所述飞行器本体的底层自稳和动力分配。

2.根据权利要求1所述的一种基于仿生的飞行机械颈眼系统，其特征在于，所述机载计算设备与所述飞控单元、所述机械颈主控和所述双目视觉惯性系统通过串口或USB方式进行通信连接。

3.根据权利要求1所述的一种基于仿生的飞行机械颈眼系统，其特征在于，所述双目相机包括两个鱼眼相机，两个所述鱼眼相机分别倾斜设置在所述惯性测量单元两侧，且两个鱼眼相机中轴线之间的夹角为钝角。

4.根据权利要求1所述的一种基于仿生的飞行机械颈眼系统，其特征在于，所述飞行器本体的上设置机械颈底座，所述机械颈的首端安装在所述机械颈底座上，所述舵机包括舵机壳体、设置在所述舵机壳体一侧外壁上的舵盘、设置在所述舵机壳体中与所述舵盘连接的驱动组件，所述驱动组件驱动所述舵盘进行转动。

5.根据权利要求4所述的一种基于仿生的飞行机械颈眼系统，其特征在于，所述机械颈上沿首端至末端方向的舵机依次包括第一舵机、第二舵机、第三舵机、第四舵机、第五舵机和第六舵机；所述第二舵机、第三舵机和第五舵机均上的舵盘包括相对设置的主舵盘和副舵盘；

所述第一舵机的舵机壳体固定在所述机械颈底座上，所述第一舵机的舵盘设置在所述机械颈底座的正下方，所述第一舵机的舵盘的中轴线与所述机械颈底座互相垂直，所述第一舵机的舵盘通过轴承与所述第二舵机的未设置舵盘的舵机壳体外侧壁连接，所述第二舵机的主舵盘和副舵盘通过两个相互平行的第一连杆分别与所述第三舵机的两个设置舵盘的舵机壳体外侧壁连接；所述第三舵机的主舵盘和副舵盘通过两个相互平行的第二连杆分别与第四舵机的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第四舵机的舵盘与所述第五舵机未设置舵盘的一个舵机壳体外侧壁连接；所述第五舵机的主舵盘和副舵盘通过两个相互平行的第三连杆分别与第六舵机的两个相对的未设置舵盘的舵机壳体外侧壁连接；所述第六舵机的舵盘与安装所述双目视觉惯性系统的连接座底部固定连接。

6.根据权利要求5所述的一种基于仿生的飞行机械颈眼系统，其特征在于，所述机械颈底座上设置轴承安装孔，所述轴承的外圈固定在所述轴承安装孔中，所述第一舵机的舵盘与安装在所述轴承内圈中的第一轴上端部固定连接，所述第一轴的下端部与第二轴固定连接，所述第二轴连接所述第二舵机未设置舵盘的舵机壳体外侧壁。

7.一种应用权利要求1-6任一项所述的基于仿生的飞行机械颈眼系统的控制方法，其特征在于，包括如下步骤：

步骤(1).所述飞控单元与所述地面站控制装置采用无线方式进行通讯并控制飞行器本体飞行，飞行器本体在所需观测环境中飞行悬停过程中，通过机械颈主控控制所述机械颈上的舵机进行转动，并对舵机进行插补控制，机械颈通过舵机的转动带动所述双目视觉惯性系统中的双目相机进行六自由度的运动并到达其工作空间中指定的任意位置和姿态，完成所述双目视觉惯性系统相对于所述飞行器本体的运动，所述双目视觉惯性系统在运动过程中对所处环境进行主动视觉观测并完成对机械颈末端的位姿估计和飞行器本体的位姿估计；

步骤(2).由所述机载计算设备处理所述机械颈眼系统的主动视觉规划、运动学解算、飞行器本体轨迹规划以及双目视觉惯性里程计位姿估计的计算任务；

双目视觉惯性系统采用紧耦合的方式融合双目相机和所述惯性测量单元数据形成双目视觉惯性里程计，视觉里程计是通过所述双目相机运动所引起的图像变化以逐步估计机械颈末端位姿的过程，通过双目相机捕捉周围环境中的信息并结合惯性测量单元数据完成双目视觉惯性里程计位姿估计，并同时得到所述机械颈末端状态中的位姿估计，根据所述机械颈末端的位姿估计结合运动学解算反推得到飞行器本体的位姿估计，然后完成对飞行器本体轨迹规划和主动视觉规划。

8.根据权利要求7所述的一种基于仿生的飞行机械颈眼系统的控制方法，其特征在于，所述运动学解算具体包括如下步骤：通过标定后的机械颈参数，计算其正运动学关系，利用正运动学的坐标转换关系逆将机械颈末端位姿转换成飞行器本体位姿，完成对飞行器本体的位姿估计。

9.根据权利要求7所述的一种基于仿生的飞行机械颈眼系统的控制方法，其特征在于，所述步骤(2)中在完成机械颈末端状态中的位姿估计过程中，通过滤波器对视觉里程计与惯性测量单元输出的位姿进行优化估计，并实时监控滤波器中的特征点收敛状态，当某一特征点未收敛时，会对该特征点的逆深度重新计算；在每帧图像的滤波器计算完成之后，在滤波器后端添加一个滑动窗口，所述滑动窗口随着飞行器本体同时运动，利用局部的光束平差法对滑窗口内的特征点位姿与双目相机位姿进一步优化。

10.根据权利要求7所述的一种基于仿生的飞行机械颈眼系统的控制方法，其特征在于，所述主动视觉规划具体包括如下步骤：

(1).对飞行器本体所处环境进行建模生成点云地图，评估环境中的特征点丰富程度，并基于实时更新的地图进行机械颈运动路径规划控制，使机械颈末端双目相机始终朝向特征丰富的场景，通过机械颈运动路径规划来实现所述主动视觉规划；避免基于视觉的状态估计器在诸如白墙等无纹理场景下的失效问题；

(2).在机械颈末端状态估计过程中实时维护一个基于特征点的点云地图，并统计环境中各个特征点的平均观测方向与距离，对于每一个机械颈的状态X，通过正运动学推算双目相机的位姿，并将符合平均观测方向与距离的特征点投影到相机平面，根据理论投影得到的特征点的数目、理论观测角度与平均观测角度差、理论观测深度与平均观测深度的差值来计算场景丰富度得分F(X)；

(3).根据双目视觉惯性系统前进方向与任务目标来设定机械颈观测目标位置，利用运动规划RRT*算法在机械颈安装舵机的关节空间内进行机械颈运动路径规划，在生长路径时计算当前状态的场景丰富度F(Xi)，只有当F(Xi)满足达到一个设定的阈值才连接此路径；规划完成后得到离散的期望状态点，在状态点之间利用五次多项式进行轨迹插补，保证状态点前后的加速度平滑，最终将插补后的轨迹输出至机械颈中的各个舵机完成机械颈运动路径规划。

Images