一种基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制的制作方法

本发明涉及一种基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制。

背景技术：

在多旋翼飞行器的应用发展日益广泛的今天，根据不同的任务需求，多旋翼飞行器的有效载荷种类也多种多样。其中，利用多旋翼飞行器的机载机械臂进行目标捕获和运输已经成为新的国内外研究热点，例如亚马逊公司推出的PrimeAir多旋翼快递无人机。

但是大多数现有的此类具有抓捕功能的多旋翼无人机在抓捕过程中，多为定点抓捕，且被捕获目标也多为定尺寸定形状的已知目标。而实际情况往往是：目标尺寸不固定位置不统一。在这种情况下，近距离的视觉位置伺服会由于视场中的目标较大而给出较大的位置调整信号；而这种近距离的大位置调整信号输入到机载机械臂上则会有一个相应的大角度控制输出，这会使得多旋翼无人机的机体本身产生较大的晃动。所以近距离的视觉伺服可能会导致一些列的超调控制。在现有的关于多旋翼控制的发明专利多集中在单独的旋翼无人机控制，例如申请号：CN201410164108.4的发明专利公开了一整套包括故障模式下的控制系统，但是此控制算法只能适用于多旋翼控制本身。申请号：CN201230276176.1的发明专利公开了一个带有三自由度机械臂的多旋翼飞行器的外形设计，但是对于其控制系统，和不同任务

模式下的具体控制算法都未有涉及。

携带机载机械臂的多旋翼无人机，无论是作为一种取代人力的可能完成日常快递投送，还是在抢险救灾、战场物资配送，或者在一些极端环境下完成特殊任务，都有着极大优势。所以近年来，携带有机械臂的多旋翼无人机成为国内外各大研究机构以及公司的研究热点。

而此类多旋翼飞行器在进行抓捕任务时，多采用视觉伺服来完成对目标的定位。所以基于视觉伺服的多旋翼飞行器和机载机械臂的联合姿态控制是一个重点的研究内容，主要原因是：首先，单一的基于近距离视觉伺服的飞行器控制或者机载机械臂控制会导致大幅度超调控制，同时也会使得整个系统由于单独部位的突然大幅控制而失去瞬间平衡，而系统的联合控制则可以避免此问题的发生；其次，多旋翼飞行器和机载机械臂的联合控制可以最大程度上的节省电池消耗，这将为长距离飞行或者大载荷飞行提供更长的续航时间。

技术实现要素：

本发明的目的是针对多旋翼飞行器利用自身携带的机载机械臂在近距离瞄准被抓捕目标时，提出一种基于视觉伺服的控制多旋翼飞行器与机载机械臂的联合姿态控制算法，利用视觉伺服的传感器信息，对机载机械臂和多旋翼飞行器的姿态进行联合协调控制，从而使多旋翼飞行器和其机载机械臂，既可以完成对被抓捕目标的瞄准，又可以节省电池消耗延长续航时间，该方法可以广泛应用于不同种类的多旋翼飞行器利用机载机械臂对各类目标的抓捕任务。

附图说明

图1为本发明的系统结构示意图。

图2为本发明的系统的坐标系示意图。

图3为本发明的被抓捕目标在相机中的构图。

图4为本发明的控制算法框图。

其中图2的O-XYZ为地面坐标系，Cq-xqyqzq为多旋翼飞行器坐标系，Cm-xmymzm为机载机械臂坐标系，Cc-xcyczc为相机坐标系；图3-(a)为被抓捕目标完全符合被抓捕条件，3-(b)为被抓捕目标与机载机械臂的抓捕爪有姿态偏差的情况，3-(c)为被抓捕目标与机载机械臂的抓捕爪同时有姿态偏差和位置偏差的情况。

具体实施方式

下面结合附图对本发明的做进一步详细的说明：

本发明主要包括以下步骤：

第一步：明确多旋翼飞行器及其机载机械臂的位置、速度、姿态角以及待抓捕目标的位置；

第二步：如图1所示，根据系统的坐标系示意图和Newton-Euler法建立多旋翼飞行器和其机载机械臂的联合动力学方程(8)；

第三步：根据图2和图3所示，将视觉伺服得到的相机坐标系下的目标位置信息转换为机载机械臂操作平台坐标系下信息；

第四步：根据图4的控制律，利用第二步中得到的系统动力学方程和第三步中视觉伺服得到的位置差和角度差，完成多旋翼飞行器和机载机械臂的联合控制。

具体步骤如下：

(1)利用Newton-Euler法推导“多旋翼飞行器-机载机械臂”复合体的动力学方程

携带机载机械臂的多旋翼飞行器，如图1所示，m1，m2分别为多旋翼飞行器、多旋翼飞行器携带的机载机械臂的质量；地面坐标系O-XYZ的坐标原点O在地面指挥站，X轴在当地水平面内并指向水平面内任意方向，Z轴的负方向垂直于水平面并指向地心，Y轴也在水平面内并垂直于X轴，其方向按照右手定则确定；多旋翼飞行器坐标系Cq-xqyqzq与多旋翼飞行器固连，其坐标原点Cq为不包括机载机械臂的多旋翼飞行器质心，xq轴处在多旋翼飞行器对称平面内并且指向机身前方，yq轴垂直于机身对称平面且指向多旋翼飞行器机体的左方，zq轴也在多旋翼飞行器对称平面内且满足右手定则指向机身的上方；机载机械臂坐标系Cm-xmymzm的坐标原点在主连杆与多旋翼飞行器的交点，zm轴沿机载机械臂的主连接臂，并指向机载机械臂与多旋翼飞行器的连接点的反方向，xm轴和ym轴所构成平面与zm轴垂直，xm轴沿平面并指向平面内任意方向，ym轴沿平面并垂直于xm轴，且满足右手定则；相机坐标系Cc-xcyczc与机载机械臂坐标系Cm-xmymzm重合。

根据以上的坐标系定义和任务需求，现对系统定义如下：多旋翼飞行器具有6个自由度，包括三个位置x，y，z和三个姿态角即滚转角φ、俯仰角θ和偏航角ψ；机载机械臂包括两个自由度，分别为俯仰角α和偏航角。具体各个姿态角的定义如下：

多旋翼飞行器滚转角φ：多旋翼飞行器机体轴Cqzq与通过机体轴Cqxq的铅垂面之间的夹角，并定义多旋翼飞行器右滚转为正；

多旋翼飞行器俯仰角θ：多旋翼飞行器机体轴Cqxq与水平面之间的夹角，并定义多旋翼飞行器抬头为正；

多旋翼飞行器偏航角ψ：多旋翼飞行器机体轴Cqxq在水平面上的投影与地面坐标系OX之间的夹角，并定义多旋翼飞行器右偏航为正；

机载机械臂俯仰角α：机载机械臂体轴Cmxm与水平面之间的夹角，并定义机载机械臂抬头为正；

机载机械臂偏航角β：机载机械臂体轴Cmxm在水平面上的投影与地面坐标系OX之间的夹角，并定义机载机械臂右偏航为正。

为了推导整个系统的动力学公式，在本专利中对研究对象做如下合理假设：

①.多旋翼飞行器、机载机械臂和被抓捕目标均视为刚体，忽略其弹性变形；

②.多旋翼飞行器的质心位置始终保持在机体的结构纵轴上，机载机械臂的质心位置始终保持在机载机械臂主连杆与副连杆的连接点上；

③.多旋翼飞行器、机载机械臂的外形结构和质量分布均匀，质心与坐标系原点重合，且结构完全对称；

④.被抓捕目标为外形规则的正立方体或长方体，且最长边距不超过机载机械臂的可抓捕范围，且摆放位置平行于当地水平面。

根据Newton-Euler，系统的动力学方程为：

本专利中，右下角标1和2分别表示多旋翼飞行器和机载机械臂，左上角标1和2分别表示在多旋翼飞行器坐标系和机载机械臂坐标系下。所以有，¹V1＝(u1,v1,w1)^T和¹V2＝(u2,v2,w2)^T分别是多旋翼飞行器和机载机械臂在多旋翼飞行器坐标系各坐标轴下的线速度分量，而和为线加速度；¹Ω1＝(p1,q1,r1)^T和¹Ω2＝(p1,q1,r1)^T分别是多旋翼飞行器和机载机械臂在多旋翼飞行器坐标系各坐标轴下的角速度分量，而和为角加速度；J1和J2分别是多旋翼飞行器和机载机械臂在坐标系轴向的转动惯量，由于假设条件③，可以得到惯性积I1xy＝I1yz＝I1zx＝0，I2xy＝I2yz＝I2zx＝0；Ftotal和Mtotal分别为系统所受的合外力和合外力矩。

多旋翼飞行器的角速度¹Ω1和航向角ξ1＝(ψ,θ,φ)^T之间的转换关系如下：

转换矩阵Φ1(ξ1)为绕机体轴的三轴角速率到欧拉角速率的转换矩阵，其表达式如下：

并且有det(Φ1(ξ1))＝secθ。因此，当俯仰角θ满足θ≠2(2k-1)/π,k∈Z时，矩阵Φ1(ξ1)是可逆的。

同样可以得到机载机械臂的角速度¹Ω2和其姿态角ξ2＝(α,β,0)^T之间的转换关系为：

根据牛顿第二定律可以得到系统的运动学方程：

其中，p1＝(x1,y1,z1)^T和分别为多旋翼飞行器在地面坐标系中的位置和线速度；p2＝(x2,y2,z2)^T和分别为机载机械臂在地面坐标系中的位置和线速度；g为重力加速度；ze＝(0,0,1)^T表示在地面坐标系中的单位向量；F为Ftotal是除重力以外作用在多旋翼飞行器机体上的合外力向量；R是多旋翼飞行器坐标系到地面坐标系的转换矩阵，具体表示为：

联立方程(1)，(2)，(4)，和(6)即可得到整个系统的非线性动力学方程为：

(2)将视觉伺服得到的相机坐标系下的目标位置信息转换为机载机械臂操作平台坐标系下信息

由于机载机械臂的结构和抓捕方式的限制，需要机载机械臂的抓捕爪与被抓捕目标的某一对边缘平行，即机载机械臂的偏航角与被抓捕目标在机载机械臂坐标系下的偏航角一致。所以，在对像极坐标系下的目标位置信息进行处理的过程中，最重要的环节就是确定被抓捕目标与机载机械臂偏航角的差值。

如图3所示，根据假设④，被抓捕目标的六面体当中，如果平行于且不接触当地水平面的平面，其四个顶点在相机坐标系下的坐标分别为(xc1,yc1)，(xc2,yc2)，(xc3,yc3)，(xc4,yc4)，则其任意两点间连线的斜率分别为：

若任意两个kij之间的误差小于0.01，则可以认为此斜率所对应的两条边线平行。在此规则下，即可将被抓捕目标在相机中呈现的平面重构，这个平面叫做抓捕平面。然后即可计算重构的抓捕平面的某一对边缘与机载机械臂前段的抓捕爪之间的夹角，具体方法如下：假设斜率k12＝k34，k14＝k23，则可确定k12和k34所对应的一对边缘平行，而另一对边缘平行；选取斜率较大的一对边缘，在此假设k12＞k14，则可以得到视觉伺服下得到的机载机械臂和多旋翼飞行器需要补偿的位置差和偏航角度差为：

(3)利用第二步中得到的系统动力学方程和第三步中视觉伺服得到的位置差和角度差，设计多旋翼飞行器和机载机械臂的联合控制律。

在这一步的位姿控制器设计中，最重要的就是位置差和角度差在多旋翼飞行器和机载机械臂上的分配。合理的分配可以让多旋翼飞行器和机载机械臂在完成位置差和姿态差输入的情况下，同时二者之间的相互扰动最小。

首先，将动力学公式(8)重新写为标准形式：

其中，f(x)，g(x)，和p(x,d)均为和系统有关的非线性函数；u是系统输入。

根据经验，在工作点附近对系统进行展开线性化，可以将非线性动力学公式(11)改写为工作点附近的线性化动力学公式：

其中，A为线性化后的系统状态矩阵，B为控制矩阵，E是干扰矩阵，Δ为非线性系统线性化后的高阶余项。

定义系统的状态差向量为：

e＝xd-x (13)

其中，xd为期望的状态变量，在本专利中，即为视觉伺服下的被抓捕目标状态。对上式求导，可得：

对上式重新整理，可以得到关于状态差向量的状态方程为：

而理想的输入ud可以使得期望状态的状态方程表示为：

设实际的输入控制状态表示为：

u＝ud-δ (17)

则将上式代入公式(15)可以得到：

令性能指标为：

其中，Q和R为给定的正定矩阵，且通过求取Riccati方程

PA+A^TP-PBR^-1B^TP+Q＝0，得到正定矩阵P，从而可以得到满足性能指标(19)下的控制输入：

δ＝-R^-1BPe (20)。