CN107309872B - 一种带有机械臂的飞行机器人及其控制方法 - Google Patents

Abstract

本发明公开了一种带有机械臂的飞行机器人及其控制方法，飞行机器人包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制系统，六自由度机械臂安装在多旋翼无人机飞行平台下方。将无人机与机械臂相结合，拓展其任务执行能力；对于机械臂，将其运动范围从地基拓宽到空中，同时对其控制方式加以改进，新的控制方式可以实现机械臂跟随人体手臂运动的功能，使其能够更加自然灵活地执行操作者的想法。该机器人及其控制方法扩大了无人机和机械臂作业范围，实现对机械臂的远程控制；机械臂随动人体手臂位姿，避免了复杂的模型检测与运算，响应效果更好；并且在尽可能保留舵机快速响应特性的同时减缓了舵机快速响应时对机械臂和飞行平台带来的冲击问题。

Description

一种带有机械臂的飞行机器人及其控制方法

技术领域

本发明属于作业型无人机技术领域，具体是一种带有机械臂的飞行机器人及其控制方法。

背景技术

目前多旋翼无人机技术自身已发展得较为成熟，其在日常生活中的应用一般需结合其它部件来完成，如和云台拍摄设备结合实现高空侦查拍摄功能、和喷洒装置结合实现农田植保的功能。目前也有相关学者和研究人员提出将旋翼无人机和手爪或机械臂结合来完成更复杂的功能，早期的这类飞行机器人多是将旋翼无人机和手爪相结合，但是这类机器人系统的作业能力十分有限，并且由于没有机械臂系统，作业能力受到很大限制。于是有学者提出飞行器结合多自由度机械臂的概念，但机械臂系统自身不具备主动作业的功能，目前在这一领域较先进的机械臂控制方式是通过外界视觉捕捉系统辅助定位来实现动态抓取目标，这样的控制方法很大程度上依赖于视觉捕捉功能，目前视觉捕捉技术发展不够成熟，且对于没有视觉捕捉系统的外界复杂环境这样的控制方式会束手无策，因而此类控制方式可靠程度不够高。

目前常见的机械臂多用于工业生产，旨在执行高精度、高复杂性的装配工作，从而替代人工，提高生产效率。这类机械臂一般造价高昂、体积较大，用途相对固定，运动范围有限，运动方式只能由预先编制的程序控制，难以动态响应外部的运动指令。与工业机械臂相对，另一类小型机械臂立足于桌面级应用，被用于教学演示、产品原型设计、科研验证等方面，这类机械臂大多采用步进电机作为驱动，步进电机带来较好的驱动效果的同时也因体积和重量较大使得机械臂系统使用范围受到限制，同时这类机械臂系统也有响应速度与力度有限、响应方式不灵活的局限性，在使用时需要与电脑和电源连接以获得运动指令。不管是工业级机械臂或者小型机械臂在人机交互方面均未能实现较理想的效果，即目前的机械臂技术一般只能按照预先编制的程序指令来控制，这种控制方式不够自然和灵活。

目前市面上销售的舵机控制方式即由使用者发送角度指令，而后舵机快速响应至目标位置，一般情况下无需操作者进行干预，但舵机快速响应的特性决定了其在响应时的角加速度很大，因此会对耦合装置造成较大的冲击，对于扭力较大的舵机这种现象尤为明显。

发明内容

本发明要解决的技术问题是提供一种带有机械臂的飞行机器人及其控制方法，该机器人及其控制方法能够扩大无人机和机械臂作业范围，实现对机械臂的远程控制；机械臂随动人体手臂位姿，避免了复杂的模型检测与运算，响应效果更好；并且在尽可能保留舵机快速响应特性的同时减缓舵机快速响应时的冲击问题。

本发明基本思路是将小型机械臂系统装配在多旋翼无人机下方，其上载有无线通讯设备来接收操作端的指令。在操作端的人体姿态采集装置捕捉人体关节与手势信息，然后经过程序运算处理，即时计算出机械臂的运动指令并发出，从而实现在无线信号可达范围内的三维灵活作业。

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

一种带有机械臂的飞行机器人，包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制系统，所述六自由度机械臂安装在多旋翼无人机飞行平台下方。

进一步的，所述多旋翼无人机飞行平台，包括六轴无人机、飞控系统、GPS模块、电池组、图像采集系统，所述飞控系统、GPS模块、电池组和图像采集系统位于六轴无人机上方。

另一种改进，所述六自由度机械臂包括底座、肘部和腕部，所述底座和肘部之间采用大臂连接，同时在底座和肘部之间设置与大臂平行的连杆，底座、大臂、连杆和肘部组成四连杆机构；所述肘部和腕部之间采用小臂连接；所述六自由度机械臂通过与飞行器接合结构安装到多旋翼无人机飞行平台下方。

更进一步，所述底座上安装有三个舵机，分别为第一、二、三舵机；所述第一舵机位于上方，所述第二、三舵机位于第一舵机下方，并分列于第一舵机两侧；第一舵机用于驱动机械臂和飞行器平台之间的平面内转动，第二舵机用于驱动大臂相对于底座的转动，第三舵机用于驱动小臂相对于大臂的转动；所述大臂呈工字型，所述大臂上固接有第一、二、三法兰轴承，大臂侧面开孔以便于舵机电源及信号走线；所述肘部是由四片板材和舵机结合而形成的立体结构；所述小臂由前、后连接件和圆管组成；所述连杆包括第二连杆和第三连杆；所述第二连杆头部短轴与第一法兰轴承内径装配实现大臂和第二连杆的相对转动，第二连杆尾部开孔与第三连杆铰接实现相互转动；所述腕部包括第一部分和第二部分；所述第一部分的左挡板开孔装配有法兰轴承，后挡板的四个开孔与小臂的前端连接件上的孔利用螺栓螺母固接，右挡板安装有第五舵机，舵机转轴和左挡板上的法兰轴承转轴共轴；所述第二部分的左挡板固接有短轴，与第一部分左挡板上法兰轴承内径配合，前侧挡板安装有第六舵机，右侧挡板的四个孔与第一部分右侧挡板上的舵机舵盘利用螺栓螺母固接。

再进一步，所述底座与大臂间连接的实现方式是：第二舵机舵盘处的四个圆形孔和大臂上部左侧的四个螺纹孔用螺栓螺母固接，第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用螺栓螺母固接。通过这样的连接方式，第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转动，第三舵机的转动将直接驱动第二连杆绕其转轴转动，而不对大臂产生影响。

另一种改进，所述操作端控制系统包括Kinect和Leap Motion设备；所述LeapMotion用于检测手掌的运动并定位人手的各主要关节；所述Kinect用于实现对其可视范围内物体深度数据的获取，对人体全身主要关节进行捕捉和定位。

一种带有机械臂的飞行机器人的控制方法，包括基于Kinect的正向控制方式、基于Leap Motion的逆向控制方式和七段S曲线舵机平滑运动控制方法；所述基于Kinect的正向控制方式，通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度，通过将求解得到的各个关节的角度映射到机械臂的关节间夹角实现对其控制。

进一步的，所述基于Leap Motion的逆向控制方式，通过Leap Motion获取手掌的6自由度信息：3个位移量、3个姿态角；利用这6自由度信息作为机械臂末端关节位置姿态条件，逆向求解出各个关节之间的夹角；采用DH法确定各关节坐标系；将相邻坐标系i-1和i的齐次变换记为T_i，如以T₁表示坐标系1相对于坐标系0的位置姿态，T_i由参数a_i、α_i、d_i、θ_i决定：

由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换T_all为：

T_all＝T₁T₂T₃T₄T₅T₆ (13)

设更新间隔时间为Δt，Δt间隔内程序检测到的手掌位姿信息变化记为A_Δt：

A_Δt＝[Δx Δy Δz Δα Δβ Δγ]^T

其中x、y、z为手掌在Leap Motion坐标系下的相对位移，α、β、γ为手掌在LeapMotion坐标系下的回转角、俯仰角、偏转角；

记时刻t机械臂末端在底座坐标系下的位置姿态为为A_t′：

A_t'＝[x_t' y_t' z_t' α_t' β_t' γ_t']^T

其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标，α′、β′、γ′为机械臂末端在底座坐标系下的回转角、俯仰角、偏转角，将Leap Motion检测到的人手的位姿变化映射到机械臂末端的位姿，则t+Δt时刻机械臂末端位置A_t+Δt′为：

其中k₁、k₂分别为手掌位置、姿态的控制灵敏度系数，通过变化k₁、k₂的值可改变手掌运动映射到机械臂末端的幅度大小。T为一3×3矩阵，是Leap Motion坐标系到机械臂底座坐标系的方向变换。将A_t+Δt′转化为与T_all对应的齐次形式，其中姿态表示方法采用X-Y-Z固定角坐标系，记转化后的A_t+Δt″为：

其中：

由此得到机械臂随动手掌的空间位置和姿态，即：

T_all＝A_t+Δt″ (16)

结合式(2)，即可解出各关节角度θ₁～θ₆。

进一步的，所述七段S曲线舵机平滑运动控制方法过程如下：

为了使机械臂的运动足够平滑，需要保证舵机的角加速度α(t)连续没有突变。将舵机运动至目标值的整个运动过程分为7段：加加速、匀加速、减加速，匀速，加减速，匀减速，减减速，从原始的控制变量着手，推算出舵机在每一个时刻的位置θ(t)，在控制时以此为基准来改变输出的PWM脉宽从而达到平滑控制舵机运动的目的，在这个过程中的θ(t)曲线即为S型曲线。

对于舵机转角为α、转动时间为T的一次运动，取边界条件为ω(0)＝0、ω(T)＝0、θ(0)＝0，θ(T)＝α，即运动始末角速度为0，运动角位移为α，设7段运动的时间分别为：t₀～t₆。S曲线在给定每一段的时间长度和整条曲线的起始位置后便可唯一确定，其中最核心的两个参数是加速段的最大加速度a和减速段的最小减速度b，通过积分推导出S曲线的形式，进而由边界条件得到a和b。对于每段的加速度，表示为：

其中，每一行与7段运动过程的每一段相对应。p_i为：

对角加速度积分得到二次形式的角速度：

由角速度的连续性，上式中的ω(p_i)可根据上一时间段末的角速度值依次求出，对角速度积分得到三次形式的角位移曲线：

其中，ω[i][j]为式(8)中ω矩阵中的i行、j列元素，θ_i为：

由角度的连续性，上式中的θ(p_i)可根据上一时间段末的角度值依次求出。最终，根据终点的位移边界条件，推导得出两个重要参数a、b如下：

其中M(p_i，t_i)可由时间量p_i和t_i得到。可以看出，一旦给定每段的运动时间，即可确定两个加速度量a、b，将a、b进一步代入速度矩阵与位移矩阵，便可得到满足加速度不突变的平滑的运动轨迹。

本发明的一种带有机械臂的飞行机器人及其控制方法，具有以下有益效果：

(1)使得无人机和机械臂作业范围广，受地理位置限制小，可实现远程操作，使得操作者无需达到作业环境，避免了危险与不便(如危险地区、地质灾害地区、水面、高空等)；

(2)机械臂的控制方式更加自然灵活，根据人体姿态做出响应，实现机械臂随动人体手臂位姿的效果，充分体现操作者的决策和灵活性，同时避免了复杂的模型检测与运算，可提供更好的响应效果；

(3)在舵机控制方式上，通过7段S曲线的平滑控制方法，减缓了舵机快速响应时的冲击问题。

附图说明

图1是本发明的多旋翼无人机飞行平台和六自由度机械臂结构示意图；

图2是图1所示的多旋翼无人机飞行平台和六自由度机械臂另一角度结构示意图；

图3是本发明六自由度机械臂结构示意图；

图4是图3所示的六自由度机械臂另一角度结构示意图；

图5是底座示结构意图；

图6是图5所示的底座的另一角度结构示意图；

图7是肘部示结构意图；

图8是本发明控制方法示意图；

图9是机械臂DH坐标系；

图10是舵机在1s内转动100°其角位移、角速度、角加速度随时间的变化示意图。

具体实施方式

下面结合附图，对本发明提出的一种带有机械臂的飞行机器人及其控制方法进行详细说明。在本发明的描述中，需要理解的是，术语“左侧”、“右侧”、“上部”、“下部”、“底部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，“第一”、“第二”等并不表示零部件的重要程度，因此不能理解为对本发明的限制。本实施例中采用的具体尺寸只是为了举例说明技术方案，并不限制本发明的保护范围。

如图1和2所示，本发明的一种带有机械臂的飞行机器人，包括多旋翼无人机飞行平台1、六自由度机械臂2和操作端控制系统，六自由度机械臂安装在多旋翼无人机飞行平台下方。

多旋翼无人机飞行平台1，包括六轴无人机、飞控系统、GPS模块、电池组、图像采集系统，所述飞控系统、GPS模块、电池组和图像采集系统位于六轴无人机上方。

如图3至4所示，六自由度机械臂2包括底座21、肘部24和腕部26，所述底座21和肘部24之间采用大臂22连接，同时在底座21和肘部24之间设置与大臂22平行的连杆23，底座21、大臂22、连杆23和肘部24组成四连杆机构；所述肘部24和腕部26之间采用小臂25连接；六自由度机械臂2通过与飞行器接合结构27安装到多旋翼无人机飞行平台下方腹部。

如图3至7所示，下面将详细说明六自由度机械臂2不同部分的结构及相邻部分间的连接形式。

与飞行器接合结构27：

自上而下分别为与飞行器固接部分、平面轴承、与底座固接部分。

与飞行器固接部分：与飞行器固接依靠三个孔使用螺栓螺母固接。

平面轴承：实现底座和飞行器之间的相互平面转动的功能，平面轴承较小内径的部分位于上侧。

与飞行器固接部分、与底座固接部分侧面开螺纹孔可通过螺钉进一步锁紧平面轴承。

与底座固接部分与底座依靠两个孔通过螺栓螺母连接。

底座21：底座21结构组成如图5和6所示。底座上固接有三个舵机，舵机和舵盘相连接，在图6中，由上至下、由左至右分别第一、二、三舵机，本发明中舵机安装的方法均为在安装位置开孔通过螺栓螺母连接。第一舵机用来驱动机械臂和飞行器平台之间的平面内转动，第二舵机用来驱动大臂相对于底座的转动，第三舵机用来驱动小臂相对于大臂的转动。其中第一舵机上通过舵盘固接了轴，轴的直径和平面轴承内径关系是大于其较小的内径而小于其较大的内径，轴与平面轴承上部的平面垫圈形成过盈配合而固接，当第一舵机转动时，带动轴、平面轴承上部垫圈转动，实现平面轴承的平面内转动，即机械臂相对于飞行器平台的平面内转动。

大臂22：大臂22上固接有三个法兰轴承，大臂侧面开孔以便于舵机电源及信号走线。

大臂22本身作为大臂22小臂25之间的四连杆机构的连杆之一，四连杆机构的第二连杆头部短轴可与第一法兰轴承内径装配实现大臂和第二连杆的相对转动，第二连杆尾部开孔与第三连杆铰接实现相互转动。大臂、第二连杆、第三连杆装配后如图3和4所示。

底座22与大臂23间连接的实现方式是：第二舵机舵盘处的四个圆形孔和大臂上部左侧的四个螺纹孔用螺栓螺母固接、第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用螺栓螺母固接；通过这样的连接方式，第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转动，第三舵机的转动将直接驱动第二连杆绕其转轴转动，而不对大臂产生影响。底座、大臂、第二连杆、第三连杆连接后如图3和4所示。

如图7所示，肘部24是由四片板材和舵机结合而形成立体结构。其中第一部件241上开孔，孔径相比小臂外径略大；第一部件241通过螺纹孔使用螺钉与第二、三部件242、243固接配合，第四舵机安装于第三部件243上，第四舵机转轴与第一部件241上的孔共轴；第二、四部件242、244上的大孔用来与大臂22末端的第二、三法兰轴承内孔通过法兰短轴形成共轴配合，从而使得腕部相对大臂22可绕大臂22末端的法兰轴承转轴转动；第四部件后部开孔可实现与第三连杆铰接，实现相对转动，即肘部24同时也是大臂22和小臂25之间的四连杆机构的第四根连杆。

小臂25：小臂结构较为简单，其由前、后连接件和圆管组成，圆管前后端打孔利用细长螺栓和螺母与前后连接件固接。后端连接件上的四个圆孔与4号舵机舵盘固接，4号舵机由此能驱动小臂绕其自身转轴转动。

腕部26：

腕部26主要由两大部分组成。

第一部分左挡板开孔装配有法兰轴承，后挡板的四个开孔可与小臂的前端连接件上的孔利用螺栓螺母固接，右挡板安装有第五舵机，舵机转轴和左挡板上的法兰轴承转轴共轴。

第二部分左挡板固接有短轴，可与第一部分左挡板上第四法兰轴承内径配合，前侧挡板安装有第六舵机，右侧挡板的四个孔可与第一部分右侧挡板上的舵机舵盘利用螺栓螺母固接。

作业部说明：

作业部安装于第六舵机的舵盘上，可依据任务需求安装执行装置，常见的如机械爪类的抓取装置或真空吸附装置等等。

系统工作原理

本发明可利用人体手掌或者手臂的位置和姿态信息来控制机械臂运动，人体手掌的位置和姿态信息表现为三个方向的位移量和三个方向(偏航、俯仰、滚转)的转动量，手臂的姿态表现为各关节之间的夹角。人体姿态捕捉设备可以是例如购买自Leap Motion的Leap Motion装置，或者是购买自Microsoft的Kinect装置。

本发明中，使用Leap Motion控制是通过捕捉到的人体手掌位置和姿态6个自由度的信息作为机械臂末端的位姿信息，通过逆向运动学方法反解出各个关节的角度，对应到舵机的转动角度。使用Kinect控制是通过将捕捉到的手臂关节间的夹角映射到机械臂关节夹角，对应到舵机的转动角度。

整套系统如图8所示。由如前所述的空中飞行平台及机械臂和地基操作端组成。地基操作端由计算机、人体姿态捕捉装置、相应的控制软件包组成。操作端通过无线讯号向空中端发送机械臂运动指令，空中端通过无线讯号向操作端传送即使拍摄到的图像数据，操作端的人依据图像对飞行平台进行控制，同时操作端的人利用手掌或者手臂位置姿态对机械臂进行控制，控制均通过无线讯号远程实现。通过这样的控制方法，将人自身作为主要环节引入到控制当中去，使得控制方式更加灵活自然。

Leap Motion和Kinect手势、人体姿态检测设备：

在对机械臂的控制测试中对比分析了目前较易购买到的Kinect和Leap Motion设备，其中Leap Motion可检测手掌的运动并定位人手的各主要关节，可以达到0.01mm的识别精度，Kinect则可实现对其可视范围内物体深度数据的获取，特别地还能对人体全身主要关节进行捕捉和定位，常被用于人体动作识别和场景建模。

基于Kinect的正向控制方式：

由于Kinect检测的是全身骨骼关节空间坐标，可直接获取手臂的运动，因此基于Kinect的控制采用的是正向控制方式，即通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度，如求解大臂和小臂之间的角度时，可由肩部、肘部、腕部的空间坐标得到沿大臂和小臂的单位向量方向，进而得到两向量间的夹角。通过将求解得到的各个关节的角度映射到机械臂的关节间夹角实现对其控制，这种控制方式思路简单，在程序上易于实现。

基于Leap Motion的逆向控制方式:

与Kinect不同，Leap Motion是对手掌运动进行检测，通过Leap Motion可以获取手掌的6自由度信息：3个位移量、3个姿态角。利用这6自由度信息作为机械臂末端关节位置姿态条件，可逆向求解出各个关节之间的夹角。考虑到机械臂连杆机构只是作为动力传动机构，在推导机械臂的运动学逆解时不考虑连杆。采用DH法确定各关节坐标系，如图9所示。机械臂DH参数如表1所示。

表1 机械臂DH参数

将相邻坐标系i-1和i的齐次变换记为T_i，如以T₁表示坐标系1相对于坐标系0的位置姿态，T_i由参数a_i、α_i、d_i、θ_i决定：

由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换T_all为：

T_all＝T₁T₂T₃T₄T₅T₆ (24)

对于本文构型的机械臂，已知其末端关节的位置和姿态，可逆向推导出各关节间夹角的解析解，但解的个数不唯一，对于某一给定位姿有八组逆解，在实际应用中结合舵机特性和机械臂安装方式，选择了一组最合适的解。

Leap Motion自身的数据更新频率在测试用的计算机上约为120Hz，在控制程序中，需以低于此频率的更新速率计算求解。设更新间隔时间为Δt，Δt间隔内程序检测到的手掌位姿信息变化记为A_Δt：

A_Δt＝[Δx Δy Δz Δα Δβ Δγ]^T

其中x、y、z为手掌在Leap Motion坐标系下的相对位移，α、β、γ为手掌在LeapMotion坐标系下的回转角、俯仰角、偏转角。

记时刻t机械臂末端在底座坐标系下的位置姿态为为A_t′：

A_t'＝[x_t' y_t' z_t' α_t' β_t' γ_t']^T

其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标，α′、β′、γ′为机械臂末端在底座坐标系下的回转角、俯仰角、偏转角，将Leap Motion检测到的人手的位姿变化映射到机械臂末端的位姿，则t+Δt时刻机械臂末端位置A_t+Δt′为：

其中k₁、k₂分别为手掌位置、姿态的控制灵敏度系数，通过变化k₁、k₂的值可改变手掌运动映射到机械臂末端的幅度大小。T为一3×3矩阵，是Leap Motion坐标系到机械臂底座坐标系的方向变换。将A_t+Δt′转化为与T_all对应的齐次形式，其中姿态表示方法采用X-Y-Z固定角坐标系，记转化后的A_t+Δt″为：

其中：

由此得到机械臂随动手掌的空间位置和姿态，即：

T_all＝A_t+Δt″ (27)

结合式(2)，即可解出各关节角度θ₁～θ₆。

七段S曲线舵机平滑运动控制方法：

用于舵机的近似调速方法：

为尽量减小机械臂装置重量并提高响应速度，选用舵机作为其动力装置。在实际测试中发现：机械臂过于快速的响应会造成自身运动的不稳定，且由机械臂运动导致的对飞行器的耦合影响使得飞控系统难以处理。因此为维持系统的稳定性，有必要寻求一种能够使机械臂响应平滑、减缓其对飞行器冲击的控制方式。

舵机自身的动力控制特性决定了难以直接对其工作时的角加速度和角速度进行干预，所以可以采取一种近似的方式：即先让舵机以固有的角速度速度转动t₁个单位时间，然后再停下t₂个单位时间，这样在dt＝t₁+t₂这个时间段内的平均速度即可通过t₂来调节，若dt取得足够小(不小于舵机控制型号周期)，则可近似地用平均角速度代替舵机在dt时间内的真实角速度。

七段S型曲线调速的实现：

为了使机械臂的运动足够平滑，需要保证舵机的角加速度α(t)连续没有突变。将舵机运动至目标值的整个运动过程分为7段：加加速、匀加速、减加速，匀速，加减速，匀减速，减减速，从原始的控制变量着手，推算出舵机在每一个时刻的位置θ(t)，在控制时以此为基准来改变输出的PWM脉宽从而达到平滑控制舵机运动的目的，在这个过程中的θ(t)曲线即为S型曲线。

对于舵机转角为α、转动时间为T的一次运动，取边界条件为ω(0)＝0、ω(T)＝0、θ(0)＝0，θ(T)＝α，即运动始末角速度为0，运动角位移为α，设7段运动的时间分别为：t₀～t₆。S曲线在给定每一段的时间长度和整条曲线的起始位置后便可唯一确定，其中最核心的两个参数是加速段的最大加速度a和减速段的最小减速度b，通过积分推导出S曲线的形式，进而由边界条件得到a和b。对于每段的加速度，表示为：

其中，每一行与7段运动过程的每一段相对应。p_i为：

对角加速度积分得到二次形式的角速度：

由角速度的连续性，上式中的ω(p_i)可根据上一时间段末的角速度值依次求出，对角速度积分得到三次形式的角位移曲线：

其中，ω[i][j]为式(8)中ω矩阵中的i行、j列元素，θ_i为：

由角度的连续性，上式中的θ(p_i)可根据上一时间段末的角度值依次求出。最终，根据终点的位移边界条件，推导得出两个重要参数a、b如下：

其中M(p_i，t_i)可由时间量p_i和t_i得到。可以看出，一旦给定每段的运动时间，即可确定两个加速度量a、b，将a、b进一步代入速度矩阵与位移矩阵，便可得到满足加速度不突变的平滑的运动轨迹。

以舵机在1s内转动100°为例，图10给出了基于上述方式得到的舵机角位移、角速度、角加速度随时间的变化，其中t₀＝80ms、t₁＝140ms、t₂＝80ms，t₃＝400ms，t₄＝80ms、t₅＝140ms、t₆＝80ms。由第三幅图中的角加速度随时间的变化可清晰地看出整个运动分为加加速、匀加速、减加速，匀速，加减速，匀减速，减减速7个阶段，角加速度连续没有突变。

基于对本发明优选实施方式的描述，应该清楚，由所附的权利要求书所限定的本发明并不仅仅局限于上面说明书中所阐述的特定细节，未脱离本发明宗旨或范围的对本发明的许多显而易见的改变同样可能达到本发明的目的。

Claims (1)

1.一种带有机械臂的飞行机器人的控制方法，其特征在于，包括基于Kinect的正向控制方式、基于Leap Motion的逆向控制方式和七段S曲线舵机平滑运动控制方法；所述基于Kinect的正向控制方式，通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度，通过将求解得到的各个关节的角度映射到机械臂的关节间夹角实现对其控制；

所述带有机械臂的飞行机器人，包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制系统，所述六自由度机械臂安装在多旋翼无人机飞行平台下方；

所述多旋翼无人机飞行平台，包括六轴无人机、飞控系统、GPS模块、电池组、图像采集系统，所述飞控系统、GPS模块、电池组和图像采集系统位于六轴无人机上方；

所述六自由度机械臂包括底座、肘部和腕部，所述底座和肘部之间采用大臂连接，同时在底座和肘部之间设置与大臂平行的连杆，底座、大臂、连杆和肘部组成四连杆机构；所述肘部和腕部之间采用小臂连接；所述六自由度机械臂通过与飞行器接合结构安装到多旋翼无人机飞行平台下方；

所述底座上安装有三个舵机，分别为第一、二、三舵机；所述第一舵机位于上方，所述第二、三舵机位于第一舵机下方，并分列于第一舵机两侧；第一舵机用于驱动机械臂和飞行器平台之间的平面内转动，第二舵机用于驱动大臂相对于底座的转动，第三舵机用于驱动小臂相对于大臂的转动；

所述大臂呈工字型，所述大臂上固接有第一、二、三法兰轴承，大臂侧面开孔以便于舵机电源及信号走线；

所述肘部是由四片板材和舵机结合而形成的立体结构；

所述小臂由前、后连接件和圆管组成；所述连杆包括第二连杆和第三连杆；所述第二连杆头部短轴与第一法兰轴承内径装配实现大臂和第二连杆的相对转动，第二连杆尾部开孔与第三连杆铰接实现相互转动；

所述腕部包括第一部分和第二部分；所述第一部分的左挡板开孔装配有法兰轴承，后挡板的四个开孔与小臂的前端连接件上的孔利用螺栓螺母固接，右挡板安装有第五舵机，舵机转轴和左挡板上的法兰轴承转轴共轴；所述第二部分的左挡板固接有短轴，与第一部分左挡板上法兰轴承内径配合，前侧挡板安装有第六舵机，右侧挡板的四个孔与第一部分右侧挡板上的舵机舵盘利用螺栓螺母固接；

所述底座与大臂间连接的实现方式是：第二舵机舵盘处的四个圆形孔和大臂上部左侧的四个螺纹孔用螺栓螺母固接，第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用螺栓螺母固接；通过这样的连接方式，第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转动，第三舵机的转动将直接驱动第二连杆绕其转轴转动，而不对大臂产生影响；

所述操作端控制系统包括Kinect和Leap Motion设备；所述Leap Motion用于检测手掌的运动并定位人手的各主要关节；所述Kinect用于实现对其可视范围内物体深度数据的获取，对人体全身主要关节进行捕捉和定位；

所述基于Leap Motion的逆向控制方式，通过Leap Motion获取手掌的6自由度信息：3个位移量、3个姿态角；利用这6自由度信息作为机械臂末端关节位置姿态条件，逆向求解出各个关节之间的夹角；采用DH法确定各关节坐标系；将相邻坐标系i-1和i的齐次变换记为T_i，如以T₁表示坐标系1相对于坐标系0的位置姿态，T_i由机械臂结构参数a_i、α_i、d_i、θ_i决定：

由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换T_all为：

T_all＝T₁T₂T₃T₄T₅T₆ (2)

设Leap Motion更新间隔时间为Δt，Δt间隔内程序检测到的手掌位姿信息变化记为A_Δt：

A_Δt＝[Δx Δy Δz Δα Δβ Δγ]^T

其中x、y、z为手掌在Leap Motion坐标系下的相对位移，α、β、γ为手掌在Leap Motion坐标系下的回转角、俯仰角、偏转角；

记时刻t机械臂末端在底座坐标系下的位置姿态为A_t′：

A_t'＝[x_t' y_t' z_t' α_t' β_t' γ_t']^T

其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标，α′、β′、γ′为机械臂末端在底座坐标系下的回转角、俯仰角、偏转角，将Leap Motion检测到的人手的位姿变化映射到机械臂末端的位姿，则t+Δt时刻机械臂末端位置A_t+Δt′为：

其中k₁、k₂分别为手掌位置、姿态的控制灵敏度系数，通过变化k₁、k₂的值可改变手掌运动映射到机械臂末端的幅度大小；T为一3×3矩阵，是Leap Motion坐标系到机械臂底座坐标系的方向变换；将A_t+Δt′转化为与T_all对应的齐次形式，其中姿态表示方法采用X-Y-Z固定角坐标系，记转化后的A_t+Δt″为：

其中：

由此得到机械臂随动手掌的空间位置和姿态，即：

T_all＝A_t+Δt″ (5)

结合式(2)，即可解出各关节角度θ₁～θ₆；

所述七段S曲线舵机平滑运动控制方法过程如下：

为了使机械臂的运动足够平滑，需要保证舵机的角加速度α(t)连续没有突变；将舵机运动至目标值的整个运动过程分为7段：加加速、匀加速、减加速，匀速，加减速，匀减速，减减速，从原始的控制变量着手，推算出舵机在每一个时刻的位置θ(t)，在控制时以此为基准来改变输出的PWM脉宽从而达到平滑控制舵机运动的目的，在这个过程中的θ(t)曲线即为S型曲线；

对于舵机转角为α、转动时间为T的一次运动，取边界条件为ω(0)＝0、ω(T)＝0、θ(0)＝0，θ(T)＝α，即运动始末角速度为0，运动角位移为α，设7段运动的时间分别为：t₀～t₆；S曲线在给定每一段的时间长度和整条曲线的起始位置后便可唯一确定，其中最核心的两个参数是加速段的最大加速度a和减速段的最小减速度b，通过积分推导出S曲线的形式，进而由边界条件得到a和b；对于每段的加速度，表示为：

其中，每一行与7段运动过程的每一段相对应；p_i为：

对角加速度积分得到二次形式的角速度：

由角速度的连续性，上式中的ω(p_i)可根据上一时间段末的角速度值依次求出，对角速度积分得到三次形式的角位移曲线：

其中，ω[i][j]为式(8)中ω矩阵中的i行、j列元素，θ_i为：

由角度的连续性，上式中的θ(p_i)可根据上一时间段末的角度值依次求出；最终，根据终点的位移边界条件，推导得出两个重要参数a、b如下：

其中M(p_i，t_i)可由时间量p_i和t_i得到；可以看出，一旦给定每段的运动时间，即可确定两个加速度量a、b，将a、b进一步代入速度矩阵与位移矩阵，便可得到满足加速度不突变的平滑的运动轨迹。

Images