[发明专利]一种基于视觉伺服的多旋翼飞行器和机载机械臂联合位姿控制

摘要

本发明公开了一种机载机械臂的多旋翼飞行器在捕获未知目标时，根据视觉伺服来对多旋翼飞行器和机载机械臂的位置姿态进行联合控制的方法，该方法在多旋翼飞行器靠近抓捕未知目标时，利用多旋翼飞行器自身携带的相机，通过对目标获取视觉信息，完成多旋翼飞行器和机载机械臂的位置姿态的联合控制。首先，根据Newton‑Euler法对多旋翼飞行器‑机载机械臂系统进行动力学建模。然后，根据不同情况下的视觉伺服信息，对机载相机获取的视觉信息进行分析归类。最后，在视觉伺服信息的基础上，完成多旋翼飞行器和机载机械臂的位置姿态联合控制。

主权项

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分别表示在多旋翼飞行器坐标系和机载机械臂坐标系下；所以有，¹V₁＝(u₁,v₁,w₁)^T和¹V₂＝(u₂,v₂,w₂)^T分别是多旋翼飞行器和机载机械臂在多旋翼飞行器坐标系各坐标轴下的线速度分量，而和为线加速度；¹Ω₁＝(p₁,q₁,r₁)^T和¹Ω₂＝(p₁,q₁,r₁)^T分别是多旋翼飞行器和机载机械臂在多旋翼飞行器坐标系各坐标轴下的角速度分量，而和为角加速度；J₁、J₂分别是多旋翼飞行器坐标系轴向的转动惯量、机载机械臂坐标系轴向的转动惯量，由于假设条件③，可以得到惯性积I_1xy＝I_1yz＝I_1zx＝0，I_2xy＝I_2yz＝I_2zx＝0；F_total和M_total分别为系统所受的合外力和合外力矩；多旋翼飞行器的角速度1Ω1和航向角ξ1＝(ψ,θ,φ)T之间的转换关系如下：转换矩阵Φ1(ξ1)为绕机体轴的三轴角速率到欧拉角速率的转换矩阵，其表达式如下：并且有det(Φ1(ξ1))＝secθ，因此，当俯仰角θ满足θ≠2(2k‑1)/π,k∈Z时，矩阵Φ1(ξ1)是可逆的；同样可以得到机载机械臂的角速度1Ω2和其姿态角ξ2＝(α,β,0)T之间的转换关系为：根据牛顿第二定律可以得到系统的运动学方程：其中，p₁＝(x₁,y₁,z₁)^T和分别为多旋翼飞行器在地面坐标系中的位置和线速度；p₂＝(x₂,y₂,z₂)^T和分别为机载机械臂在地面坐标系中的位置和线速度；g为重力加速度；z_e＝(0,0,1)^T表示在地面坐标系中的单位向量；F为F_total是除重力以外作用在多旋翼飞行器机体上的合外力向量；R是多旋翼飞行器坐标系到地面坐标系的转换矩阵，具体表示为：联立方程(1)，(2)，(4)，和(6)即可得到整个系统的非线性动力学方程为：