一种基于力反馈装置和VR感知的飞行机械臂系统及控制方法与流程

本发明涉及无人机领域，具体涉及一种基于力反馈装置和vr感知的飞行机械臂系统及控制方法。

背景技术：

近年来，伴随着机器人技术的迅速发展，无论在军事应用还是民用服务行业领域中，无人机都已经发挥了重要的作用。现有的无人机应用中，无人机运输仍占据着主要的部分，但这无法满足人们日益增长的需求。在无人机上装载机械臂，使机器人能够对环境中的目标进行抓取和移动，两者结合可以大大扩展传统无人机的应用范围，比如可以代替人类完成复杂环境下的高空交互操作任务。

无人机机械臂的控制已成为一大研究热点，但同时也存在着巨大的挑战和难点。其中之一就是实现自动控制，即让无人机自动识别物体并实现自动抓取，但该技术实现难度大，尚不成熟。相比之下，手动控制就显得更加简单和实用，即通过人眼或设立摄像头准确获取无人机以及目标物的状态，然后操作机械臂完成抓取目标的任务。现有的飞行机械臂在目标识别方面,大多采用通过第三方视角直接观察飞行机械臂和目标的位置,观察精度和效率低下。

公开号为cn106444810a的申请公开了借助虚拟现实的无人机机械臂空中作业系统及其控制方法，该发明中采用了将摄像头搭载于三轴自稳云台，并通过测量头部姿态信息解算三轴自稳云台控制指令，虽然可以扩展操作机械臂的可视范围，降低了云台的控制难度，但是同时这增加了载荷的体积和成本。且只是运用单目相机采集图像，复制两个相同的画面，并未真正实现立体视觉。

技术实现要素：

为了解决上述技术问题，本发明的目的之一是提供一种基于vr的飞行机械臂系统、vr感知显示技术和飞行机械臂抓取控制的基于力反馈装置和vr感知的飞行机械臂系统。

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

一种基于力反馈装置和vr感知的飞行机械臂系统，包括无人机部分和地面站部分，所述无人机部分包括了无人机机体、双目相机、机载电脑、机械臂、控制系统和视觉标签，所述视觉标签用于与所要抓取的物体进行匹配识别，所述机载电脑和所述控制系统安装在所述无人机机体上；所述地面站部分包括vr头显、力反馈装置和地面站主机，所述机载电脑将从所述地面站主机接收到的飞行控制指令以及机械臂控制指令发送至所述控制系统；

所述双目相机固定于所述无人机机体前端的上方位置，用于采集所述无人机机体周边的运动环境图像，并将图像信息传送回所述机载电脑的图传节点；

所述机械臂固定于所述无人机机体的下方且与所述控制系统中的机械臂主控相连，所述力反馈装置将操作杆控制指令通过所述地面站主机发送给所述机载电脑，所述机载电脑将所述操作杆控制指令转换为机械臂控制信息后传送给所述机械臂主控；

所述vr头显为虚拟现实头戴式显示设备，所述vr头显戴在操作者头部用于接收从所述双目相机传回的所述无人机机体前端的第一视角信息，并收集操作者头部的姿态信息。

优选的，所述机载电脑为unc电脑。

优选的，所述vr头显包含视频显示器和头部姿态测量模块，所述视频显示器实时显示从双目相机传回的无人机前端的第一视角；所述头部姿态测量模块包括加速度计、陀螺仪和磁力计，用于实时测量头部姿态信息，并传回所述机载电脑的数传节点。

优选的，所述机械臂末端搭载末端执行器，所述末端执行器为执行器柔性爪，所述执行器柔性爪用于抓取目标物体。

优选的，所述双目相机通过usb将采集的图像传送到所述机载电脑上，所述机载电脑运用opencv压缩图像，经过压缩处理过后的图像从所述机载电脑由5gwifi传输到所述地面站主机的windows环境下，所述地面站主机调用opencv解压缩图像，并调用opengl进行渲染，所述地面站主机将解压和渲染后的图像传递至vr头显中，最终在vr头显中显示双目相机采集到的实时视频图像，即无人机机体前端的第一视角空间立体图。在目标识别方面，通过双目相机和vr头显直观的观察到无人机前方的场景，从第一视角观察、定位目标物体，便于后续通过力反馈装置操控机械臂进行抓取任务。

更优选的，双目相机通过usb3.0将采集的图像传送到所述机载电脑上，vr头显中的虚拟现实系统运行在windows环境下，而无人机的上层控制在ros环境下运行，通过撰写基于tcp的信号传输协议，将双目相机传递的经过压缩处理过后的图像，从机载电脑由5gwifi传输到地面站的windows环境下。

优选的，所述机械臂上设置舵机，所述力反馈装置包括操作杆，所述力反馈装置通过usb将所述操作杆的末端位置信息和末端执行器控制信息发送到所述地面站主机，所述地面站主机通过映射和逆运动求解将所述末端位置信息转化为所述舵机实际转角命令。

优选的，所述执行器控制信息经过地面站主机转换为末端执行器开闭状态命令，所述舵机实际转角命令和末端执行器开闭状态命令基于udp的信号传输协议编码并通过5gwifi发送至所述机载电脑，所述机载电脑通过串口将舵机实际转角命令传递至所述机械臂主控用于控制所述舵机进行相应转动，机械臂主控接收相应的舵机转角命令和末端执行器开闭状态指令后产生对应的pwm波控制舵机进行相应的转动。

更优选的，所述力反馈装置通过usb3.0将所述操作杆的末端位置信息和末端执行器控制信息发送到所述地面站主机。

优选的，所述机载电脑通过串口将末端执行器开闭状态命令传递至所述机械臂主控，所述机械臂主控经过i/o口并通过mosfet驱动控制所述末端执行器。

优选的，所述力反馈装置依次通过获取所述力反馈操作柄和所述机械臂的映射关系、建立所述机械臂末端位置的正逆运动学解算、理论模型与实际模型的映射、机械臂角度三次多项式差值算法来控制所述机械臂关节空间的轨迹规划。

本发明的目的之二是提供一种基于力反馈装置和vr感知的飞行机械臂系统的控制方法，包括如下步骤：

(1).操作者带上vr头显后，无人机机体上的双目相机通过usb将采集的图像传送到机载电脑上，机载电脑运用opencv压缩图像，经过压缩处理过后的图像从机载电脑由5gwifi传输到地面站主机的windows环境下，地面站主机调用opencv解压缩图像，并调用opengl进行渲染，地面站主机将解压和渲染后的图像传递至vr头显中，vr头显实时显示以无人机前端为第一视角观察到的周围环境；

(2).地面站主机发出的飞行控制指令通过5gwifi传递至机载电脑，机载电脑将飞行控制指令传递至控制系统中的飞行主控上，通过飞行主控控制无人机的飞行，操作者同时通过移动vr头显寻找目标物，vr头显中的头部姿态测量模块实时测量头部姿态信息，并传回所述机载电脑的数传节点，机载电脑通过头部姿态信息控制无人机机体与操作者头部同步旋转，时刻保持操作者的视野为无人机机体前端的第一视角，视野中呈现当前视角的立体视觉图；

(3).地面站主机发出的飞行控制指令操作无人机机体至距目标物合适的位置以及姿态，准备开始执行机械臂抓取目标物的任务；

(4).操作者操作力反馈传感器，所述力反馈装置依次通过获取所述力反馈操作柄和所述机械臂的映射关系、建立所述机械臂末端位置的正逆运动学解算、建立舵机理论模型与实际模型的映射、机械臂角度三次多项式差值算法来控制所述机械臂关节空间的轨迹规划，操作者通过反馈传感器控制机械臂调整至合适的位姿，调整末端执行器的姿态，执行抓取。

优选的，步骤(4)中的所述力反馈装置控制所述机械臂关节空间的轨迹规划包括如下步骤：

1)获取力反馈装置和机械臂的映射关系：

获取力反馈装置中操作杆位置和机械臂末端位置的映射关系，测取力反馈装置的末端位置在xyz三个方向上的取值范围，以及机械臂末端位置在xyz方向上的取值范围，从而建立了两者的映射关系；

2)建立机械臂末端位置的正逆运动学解算

基于dh参数法建立机械臂的正运动学模型，建立关系变量与末端执行器位置和运动方向之间的函数：

其中，符号ci…j，si…j分别表示cos(θi+…+θj)和sin(θi+…+θj)，ai表示机械臂第i个杆的长度，表示从坐标系j到坐标系i的变换矩阵。

基于代数法以及机械臂抓取的实际操作范围,进行逆运动学求解，获得机体坐标系到机械臂的末端执行器之间的转换关系，从而建立机械臂末端位置和理想舵机转动角度的对应关系；

3)建立舵机理论模型与实际模型映射

机械臂上设置三个关节，三个关节在转动过程中存在着联动，根据带轮的齿数转换联系，建立起合理的理想舵机转动角度和实际舵机转动角度的对应关系；

4)机械臂角度三次多项式差值算法

机械臂关节空间的轨迹规划包括机械臂从起始位姿到终止位姿去取放物体，利用三次多项式差值法计算机械臂起始点和目标点的角度，三次多项式的表达式为：

θ＝a0t³+a1t²+θ1

其中θ1和θ2分别是机械臂起始点和目标点的角度，ttotal是所设定完成从起始点到目标点角度的时间长度。

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

本发明借助虚拟现实感知以及力反馈传感器实现对无人机机械臂的控制，在目标识别方面，通过vr感知及显示技术，双目相机作为图像采集装置实现以第一视角实时观察到无人机前方的立体场景，更为精准地观察、定位目标物体，降低了后续机械臂控制的难度。采用力反馈设备控制机械臂运动，运用飞行机械臂抓取技术，实时、精确的调整机械臂位置。

附图说明

图1为本发明一种基于力反馈装置和vr感知的飞行机械臂系统的原理示意图；

图2为本发明一种基于力反馈装置和vr感知的飞行机械臂系统的结构示意图；

图3为本发明一种基于力反馈装置和vr感知的飞行机械臂系统中vr头显感知的流程图；

图4为本发明一种基于力反馈装置和vr感知的飞行机械臂系统中力反馈装置操控机械臂的流程图；

图5为本发明一种基于力反馈装置和vr感知的飞行机械臂系统控制方法中的机械臂三个关节角度变化的示意图；

图6为本发明一种基于力反馈装置和vr感知的飞行机械臂系统控制方法的流程图。

附图标记：

1.无人机机体；2.双目相机；3.机械臂；4.视觉标签；5.目标物体；6.地面站主机；7.vr头显；8.力反馈装置；9.飞行控制键盘。

具体实施方式

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

系统实施例

如图1和2所示，一种基于力反馈装置8和vr感知的飞行机械臂系统，包括无人机部分和地面站部分，所述无人机部分包括了无人机机体1、双目相机2、机载电脑、机械臂3、控制系统和视觉标签4，所述机载电脑为unc小电脑。所述视觉标签4用于与所要抓取的物体进行匹配识别，控制系统包括飞行主控和机械臂主控，飞行主控包括mcu、imu、mag、bqro和超声波传感器，飞行主控和机械臂主控分别安装在飞行控制板和机械臂控制板上，所述机载电脑和所述控制系统安装在所述无人机机体1上；所述地面站部分包括vr头显7、力反馈装置8、飞行控制键盘9和地面站主机6，飞行控制键盘9发出飞行控制指令，力反馈装置8发出机械臂控制指令，所述机载电脑将从所述地面站主机6接收到的飞行控制指令以及机械臂控制指令发送至所述控制系统。

所述双目相机2固定于所述无人机机体1前端的上方位置，用于采集所述无人机机体1周边的运动环境图像，并将图像信息传送回所述机载电脑的图传节点。

所述机械臂3固定于所述无人机机体1的下方且与所述控制系统中的机械臂主控相连，所述力反馈装置8将操作杆控制指令通过所述地面站主机6发送给所述机载电脑，所述机载电脑将所述操作杆控制指令转换为机械臂控制信息后传送给所述机械臂主控。

所述vr头显7为虚拟现实头戴式显示设备，所述vr头显7戴在操作者头部用于接收从所述双目相机2传回的所述无人机机体1前端的第一视角信息，第一视角信息为双目相机2正前方的视角信息，并收集操作者头部的姿态信息。所述vr头显7包含视频显示器和头部姿态测量模块，所述视频显示器实时显示从双目相机2传回的无人机前端的第一视角；所述头部姿态测量模块包括加速度计、陀螺仪和磁力计，用于实时测量头部姿态信息，并传回所述机载电脑的数传节点。

如图3所示，所述双目相机2通过usb将采集的图像传送到所述机载电脑上，所述机载电脑运用opencv压缩图像，经过压缩处理过后的图像从所述机载电脑由5gwifi传输到所述地面站主机6的windows环境下，为提高图像传送效率，所述地面站主机6调用opencv解压缩图像，并调用opengl进行渲染，所述地面站主机6将解压和渲染后的图像经过oculussdk传递至vr头显7中，最终在vr头显7中显示双目相机2采集到的实时视频图像，即无人机机体1前端的第一视角空间立体图。在目标识别方面，通过双目相机2和vr头显7直观的观察到无人机前方的场景，从第一视角观察、定位目标物体5，便于后续通过力反馈装置8操控机械臂3进行抓取任务。

如图6所示，本发明优选的双目相机2通过usb3.0将采集的图像传送到所述机载电脑上，vr头显7中的虚拟现实系统运行在windows环境下，而无人机的上层控制在ros环境下运行，通过撰写基于tcp的信号传输协议，将双目相机2传递的经过压缩处理过后的图像，从机载电脑由5gwifi传输到地面站的windows环境下。

如图4所示，所述机械臂3末端搭载末端执行器，所述末端执行器为执行器柔性爪，执行器柔性爪由形状记忆合金制成，且执行器柔性爪数量至少为一个，所述执行器柔性爪用于抓取目标物体5。所述机械臂3上设置舵机，所述力反馈装置8包括操作杆，所述力反馈装置8采集操作杆末端位置信息，所述力反馈装置8通过usb将所述操作杆的末端位置信息和末端执行器控制信息发送到所述地面站主机6，末端执行器控制信息包括末端执行器运动指令，所述地面站主机6接收到信息后，通过映射和逆运动求解将所述末端位置信息转化为所述舵机实际转角命令，并将末端执行器控制信息转为末端执行器开闭状态命令。

所述舵机实际转角命令和末端执行器开闭状态形成机械臂控制指令，所述舵机实际转角命令和末端执行器开闭状态命令基于udp的信号传输协议编码并通过5gwifi发送至所述机载电脑，所述舵机实际转角命令和末端执行器开闭状态命令形成机械臂控制指令。所述机载电脑通过usb串口将舵机实际转角命令和末端执行器开闭状态命令传递至所述机械臂主控用于控制所述舵机进行相应转动，机械臂主控接收相应的舵机转角命令后产生对应的pwm波控制舵机进行相应的转动，所述机载电脑通过串口将末端执行器开闭状态命令传递至所述机械臂主控，所述机械臂主控经过i/o口并通过mosfet驱动控制所述末端执行器，使末端执行器处于不同的末端执行器状态。本发明优选的所述力反馈装置8通过usb3.0将所述操作杆的末端位置信息和末端执行器控制信息发送到所述地面站主机6。

由于机械臂抓取控制技术中包含多种复杂的机械机构，特别依赖于机械臂3与力反馈传感器。因此所述力反馈装置8依次通过获取所述力反馈操作柄和所述机械臂3的映射关系、建立所述机械臂末端位置的正逆运动学解算、理论模型与实际模型的映射、机械臂角度三次多项式差值算法来控制所述机械臂关节空间的轨迹规划。首先需要获取准确对力反馈装置8中的操作柄和机械臂3的映射关系，测取力反馈装置8的末端位置和机械臂末端位置在xyz三个方向上的取值范围，基于dh参数法建立正运动学模型建立关节变量与末端执行器位置和方向之间的关系，基于代数法的逆运动学求解建立机械臂末端和理想舵机转动角度关系，基于舵机上的带轮的齿轮关系转换完成舵机的理论模型与实际模型的映射，最终基于三次多项式差值算法实现机械臂关节空间轨迹规划即舵机的平滑控制。

控制方法实施例：

如图6所示，一种基于力反馈装置8和vr感知的飞行机械臂系统的控制方法，包括如下步骤：

(1).操作者带上vr头显7后，无人机机体1上的双目相机2通过usb将采集的图像传送到机载电脑的图传节点上，机载电脑运用opencv压缩图像，经过压缩处理过后的图像从机载电脑由5gwifi传输到地面站主机6的windows环境下，地面站主机6调用opencv解压缩图像，并调用opengl进行渲染，地面站主机6将解压和渲染后的图像经过oculussdk传递至vr头显7中，vr头显7实时显示以无人机前端为第一视角观察到的周围环境；

(2).地面站主机6发出的飞行控制指令通过5gwifi传递至机载电脑，机载电脑将飞行控制指令传递至控制系统中的飞行主控上，通过飞行主控控制无人机的飞行，操作者同时通过移动vr头显7寻找目标物，vr头显7中的头部姿态测量模块实时测量头部姿态信息，并传回所述机载电脑的数传节点，机载电脑通过头部姿态信息控制无人机机体1与操作者头部同步旋转，时刻保持操作者的视野为无人机机体1前端的第一视角，视野中呈现当前视角的立体视觉图；

(3).地面站主机6发出的飞行控制指令操作无人机机体1至距目标物合适的位置以及姿态，准备开始执行机械臂3抓取目标物的任务；

(4).操作者操作力反馈传感器，所述力反馈装置8依次通过获取所述力反馈操作柄和所述机械臂3的映射关系、建立所述机械臂末端位置的正逆运动学解算、建立舵机理论模型与实际模型的映射、机械臂角度三次多项式差值算法来控制所述机械臂关节空间的轨迹规划，操作者操作力反馈传感器，并将舵机实际转角命令和末端执行器开闭状态命令基于udp的信号传输协议编码并通过5gwifi发送至所述机载电脑的数传节点，机载电脑的主控节点中将机械臂通信节点传递至机械臂控制板，操作者通过反馈传感器控制机械臂调整至合适的位姿，调整末端执行器的姿态，执行位置平衡模式、立即生效模式和抓取任务中的一种。

步骤(4)中的所述力反馈装置8控制所述机械臂关节空间的轨迹规划包括如下步骤：

1)获取力反馈装置8和机械臂3的映射关系：

获取力反馈装置8中操作杆位置和机械臂末端位置的映射关系，测取力反馈装置8的末端位置在xyz三个方向上的取值范围，以及机械臂末端位置在xyz方向上的取值范围，从而建立了两者的映射关系；

2)建立机械臂末端位置的正逆运动学解算

如图5所示，基于dh参数法建立机械臂的正运动学模型，建立关系变量与末端执行器位置和运动方向之间的函数：

其中，符号ci…j，si…j分别表示cos(θi+…+θj)和sin(θi+…+θj)，ai表示第i个杆的长度，表示从坐标系j到坐标系i的变换矩阵。

基于代数法以及机械臂3抓取的实际操作范围,进行逆运动学求解，获得机体坐标系到机械臂3的末端执行器之间的转换关系，从而建立机械臂末端位置和理想舵机转动角度的对应关系；

3)建立舵机理论模型与实际模型映射

由于机械臂三个关节在转动过程中存在着联动，本发明在寻找合适的理论模型映射到实际模型的过程中，机械臂3上设置三个关节，三个关节在转动过程中存在着联动，根据带轮的齿数转换联系，建立起合理的理想舵机转动角度和实际舵机转动角度的对应关系，调高了对舵机的控制效率；

4)机械臂角度三次多项式差值算法

机械臂关节空间的轨迹规划包括机械臂从起始位姿到终止位姿去取放物体，利用三次多项式差值法计算机械臂3起始点和目标点的角度，三次多项式的表达式为：

θ＝a0t³+a1t²+θ1

其中θ1和θ2分别是机械臂起始点和目标点的角度，ttotal是所设定完成从起始点到目标点角度的时间长度，θ是机械臂在任一时刻t的角度。

机械臂关节空间的轨迹规划解决机械臂3从起始位姿到终止位姿去取放物体的问题。机械臂末端移动的过程并不重要，只要求运动是平滑的，防止舵机速度与加速度突变，减小对无人机运动的影响。对于无路径的要求，在关节空间进行轨迹规划。本发明利用三次多项式差值法可以保证起始点和目标点角速度连续的要求。对于点位作业实现平滑控制，即舵机角度转动和加速度在运动过程中可以保持轨迹平滑，从而保证舵机可以较为平稳地达到预期的角度。

本发明借助虚拟现实感知以及力反馈传感器实现对无人机机械臂的控制，在目标识别方面，通过vr感知及显示技术，双目相机2作为图像采集装置实现以第一视角实时观察到无人机前方的立体场景，更为精准地观察、定位目标物体5，降低了后续机械臂控制的难度。采用力反馈设备控制机械臂运动，运用飞行机械臂抓取技术，实时、精确的调整机械臂位置。

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