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

技术特征：

1.一种基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制方法，其特征在于，包括以下步骤：

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

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

3)设计多旋翼飞行器和机载机械臂联合位姿控制器；

所述的步骤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.根据权利要求1所述的基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制方法，其特征在于：所述的步骤2)中，需要将视觉伺服得到的相机坐标系下的目标位置信息转换为机载机械臂操作平台坐标系下信息：

由于机载机械臂的结构和抓捕方式的限制，需要机载机械臂的抓捕爪与被抓捕目标的某一对边缘平行，即机载机械臂的偏航角与被抓捕目标在机载机械臂坐标系下的偏航角一致，所以，最重要的环节就是确定被抓捕目标与机载机械臂偏航角的差值；

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

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

3.根据权利要求2所述的基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制方法，其特征在于：所述的步骤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)。