CN108453738B - 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 - Google Patents

Abstract

本发明提出了一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法。四旋翼飞行器作业***由四旋翼飞行器和作业装置两部分组成，其中作业装置由对称安装于四旋翼飞行器下方的机械臂和重心平衡机构构成。本发明利用四旋翼飞行器上的摄像头获取被抓目标及人工标志的图像，利用Opencv处理图像并解算摄像头位姿信息，从而得到四旋翼飞行器的位姿信息，根据四旋翼飞行器的位姿信息分级控制四旋翼飞行器、机械臂及重心平衡机构。本发明拓宽了四旋翼飞行器的应用领域，既可在室内也可在周边建筑密集的室外环境下进行空中作业，提高了空中作业精度。

Description

一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业 的控制方法

技术领域

本发明涉及空中机器人控制技术领域，特别是涉及一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法。

背景技术

四旋翼飞行器凭借机动灵活、便于携带等特点一直是军事和商业领域的研究热点,在航拍勘测、农药喷洒植保、目标跟踪定位等领域应用广泛。相对于地面移动机器人的移动作业，具有空中作业能力的无人飞行器将作业空间扩展到了三维，这拓宽了无人机的应用领域，在复杂环境下危险物体的搬运、勘测设备的放置与回收、空中目标的抓取等方面有着广阔的应用前景。

在对四旋翼飞行器的研究中，位姿估计问题即定位问题一直是研究的核心问题。四旋翼飞行器自身位姿的精确估计是四旋翼飞行器控制、避障、路径规划、暂栖、抓取等研究方向的重要基础和前提。目前四旋翼飞行器的定位主要有两类方法：一类是基于四旋翼飞行器自身视觉获得位置信息。另一类是基于外部设备提供位置信息，如全球卫星定位***(GPS)，运动捕捉***。但GPS易受周边环境与气候的影响，在室内或周边建筑密集的情况下，无人机难以依靠GPS获取准确位置信息。而运动捕捉***只能用于实验室等特定的研究场所，这限制了四旋翼飞行器的应用范围。

发明内容

为了解决上述存在的问题，针对四旋翼飞行器利用自身携带的机械臂抓取物体时要求抓取精确高，作业过程稳定性强以及操作简单、灵活等特点，本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法，拓宽了四旋翼飞行器的应用领域，既可在室内也可在周边建筑密集的室外环境下进行空中作业，提高了空中作业精度，为达此目的，本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法，具体步骤如下：

步骤一：构建***实施平台，所述***实施平台由四旋翼飞行器和空中作业装置两部分组成，其中空中作业装置包括一个三自由度的机械臂和一个单自由度的重心平衡机构；分别对空中作业装置和四旋翼飞行器推导运动学和动力学方程；

步骤二：标定摄像头后，选用ArUco库中的任一标志，使用基于OpenCV_ArUco的识别算法识别定位标志，将标志在摄像头坐标系下的位置和姿态信息转换为摄像头在世界坐标系的位姿信息；

步骤三：整个控制***分为三个级别，分别是飞行器控制、机械臂控制、重心平衡机构控制；设定一个空中作业范围值S，根据由图像处理得到四旋翼飞行器位姿信息判断四旋翼飞行器的分级控制，如果四旋翼飞行器未进入空中作业范围，控制四旋翼飞行器靠近或远离人工标志直至进入空中作业范围，否则，四旋翼飞行器悬停，规划机械臂各关节角度并计算出重心平衡机构关节运动角度以减小机械臂运动对四旋翼飞行器的扰动，控制机械臂自主抓取目标。

本发明的进一步改进，步骤一中对作业装置和四旋翼飞行器推导运动学和动力学方程步骤如下：

步骤1：假设四旋翼飞行器、机械臂和重心平衡机构都是刚体，对相关坐标系做了如下定义：机体坐标系{O_b}-x_by_bz_b以四旋翼飞行器的形心为原点，坐标轴x_b与四旋翼飞行器前进方向一致，坐标轴z_b垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向，{O_c}-x_cy_cz_c和{O_i}-x_iy_iz_i分别表示重心平衡机构坐标系和机械臂坐标系，摄像头固定在四旋翼飞行器上，位于机械臂上方且镜头朝下；被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体，大小不超出机械手可抓取的尺寸范围且水平摆放于地面上；

步骤2：由D-H法，对机械臂和重心平衡机构进行运动学建模，建模参数如下：

其中，θ_i,d_i,α_i-1,a_i-1为D-H法建模参数，i＝1,2,3是机械臂的关节编号，c为重心平衡机构关节编号，0、0’为安装参考系编号，H＝[θ₁,θ₂,θ₃]^T为机械臂关节角，θ_c为重心平衡机构关节角。可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为：

⁰T₃＝A₁A₂A₃(1)；

其中，A_i的表达式如下：

对正运动方程逆运动学求解后能确定每个关节的值，从而使机械臂到达期望的位姿；

步骤3：对四旋翼飞行器进行运动学建模：

世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下：

其中，P＝[x,y,z]^T表示坐标系{O_b}的原点在坐标系{O_w}中的位置，Φ＝[φ,θ,ψ]^T表示坐标系{O_b}在坐标系{O_w}中所测量的横滚角，俯仰角和偏航角，V＝[u,v,w]^T表示坐标系{O_b}相对于坐标系{O_w}的线速度，Ω＝[p,q,r]^T表示四旋翼飞行器的角速度，R为按照Z-Y-X顺序旋转的旋转矩阵，Q为机体角速度与欧拉角变化率之间的转换矩阵；

步骤4：根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模：

对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***，假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量。通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程，可得动力学方程组：

其中，m表示复合***的质量，G＝[0,0,-mg]^T表示复合***重力矢量，I_m表示复合***重心处惯性张量，重心偏移向量r_G＝[x_G,y_G,z_G]^T表示复合***的重心到机体坐标系{O_b}的偏移。F_p＝[0,0,f_p]^T表示桨叶作用于复合***上的推动力矢量，τ_p＝[τ_x,τ_y,τ_z]T为作用于复合***推动力产生的力矩的矢量，

为***与环境交互力产生的力矩，考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态，复合***与环境的交互力F_e＝0。

本发明的进一步改进，步骤二中人工标志的检测及坐标系之间转换过程步骤如下：

人工标志选用ArUco库中相应的标志，摄像头成像使用小孔成像模型：

P＝K[R|T]Q(5)；

其中，Q是三维空间中标志的坐标，[R|T]是摄像头外参矩阵，用于将世界坐标中的标志变换到摄像头坐标内，K是摄像头内参，用于将摄像头坐标中的某点投影到像平面上。P即为标志投影后的像素坐标，使用基于OpenCV_ArUco的识别算法识别定位人工标志，人工标志的检测及坐标系之间的转换有如下步骤：

步骤1：采用棋盘对摄像头进行标定，确定摄像头的内参矩阵K；

步骤2：检测标志，包括：

步骤2.1：搜索图像中的所有候选标志，利用自适应性阈值来分割标志，然后从阈值化的图像中提取外形轮廓，并且舍弃那些非凸多边形的，以及那些不是方形的，还使用了一些额外的滤波；

步骤2.2：分析候选标志的内部编码来确定它们是否确实是标志，包括：

步骤2.2.1：对图像进行透视变换，来得到它规范的形态；

步骤2.2.2：对规范的图像用Ossu阈值化以分离白色和黑色位，提取每个标志的标志位并分析出标志的编号；

步骤3：角点处的亚像素级的细化后，根据小孔成像模型，利用ArUco标志的四个顶点的像素坐标，通过solvePnP方法即可求取摄像机的旋转矩阵R和平移矩阵T；

步骤4：根据摄像头坐标系与世界坐标系之间的欧式变换关系，获取摄像头位置姿态信息，求解公式如下：

P_c＝-R^-1T

yaw(ψ)＝atan(r₂₁/r₁₁)(6)；

pitch(θ)＝asin(-r₃₁)

roll(φ)＝atan(r₃₂/r₃₃)

其中，P_c表示摄像头在世界坐标系的坐标。

本发明的进一步改进，步骤三中分级控制四旋翼飞行器和作业装置步骤如下：

假设四旋翼飞行器抓取坐标已知目标的作业范围是S，四旋翼飞行器位姿信息由图4所示的图像处理后解算摄像头位姿信息得到，如果四旋翼飞行器未进入作业范围S，四旋翼飞行器则靠近或远离人工标志，目标轨迹生成器把目标位置P^*＝[x^*,y^*,z^*]^T和偏航角ψ_d分别输出给位置控制器和姿态稳定控制器，位置控制器主体为PD控制器，其输出通过***动力学方程计算得到姿态控制所需的姿态角θ_d,φ_d，姿态稳定控制器采用反演法进行设计，其输出经过动力分配模型解耦出电机转速ω_i(i＝1,2,3,4)，输出到电机，直至四旋翼飞行器进入作业范围S；

如果四旋翼飞行器进入作业范围S，四旋翼飞行器悬停，四旋翼飞行器位姿信息输出到机械臂轨迹生成器，机械臂轨迹生成器首先将四旋翼飞行器位姿信息转换为目标位姿信息然后规划各个关节运动角度θ_i(i＝1,2,3)并输出到重心平衡控制器，重心平衡控制器根据重心平衡方程公式计算重心平衡机构关节运动角度θ_c，最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取，重心平衡方程公式：

∑m_ix_i+m_cx_c+m_tx_t＝0(7)；

其中，m_i、m_c、m_t分别机械臂各部分、重心平衡机构以及被抓取物体的质量，x_i、x_c、x_t分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{O_b}中的位置。

本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法，有益效果如下：

1、克服GPS和运动捕捉***的应用局限；

2、提高瞄准目标的速度和抓取精度，降低电池消耗，延长续航时间；

3、分级控制四旋翼飞行器和作业装置，既保证四旋翼飞行器***的飞行稳定性，也保证抓取作业过程的平稳性。

附图说明

图1是本发明的***结构示意图；

图2是四旋翼飞行器坐标系示意图；

图3是被抓目标及人工标志的世界坐标系示意图；

图4是Opencv图像处理流程图；

图5是控制***结构图；

图6是上位机控制流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法，拓宽了四旋翼飞行器的应用领域，既可在室内也可在周边建筑密集的室外环境下进行空中作业，提高了空中作业精度。

如图1所示，本发明的***实施平台主要由四旋翼飞行器1，飞行控制器2，无线通信模块、舵机控制器3，摄像头4，三自由度机械臂包括机械手5、关节一6、关节三7、关节二8，重心平衡关节9，电池10，PC机11，被抓目标12和人工标志13组成。作业装置包括一个三自由度的机械臂和一个单自由度的重心平衡机构，它们对称安装于四旋翼飞行器下方；在***设计上将机械臂运动对四旋翼飞行器的影响视为对飞行控制的扰动，利用重心平衡机构减小机械臂运动对四旋翼飞行器的扰动影响。

具体步骤如下：

步骤一、分别对作业装置和四旋翼飞行器推导运动学和动力学方程

步骤1：在本专利中做了如下定义：

假设四旋翼飞行器、机械臂和重心平衡机构都是刚体，对相关坐标系做了如下定义：如图2所示，机体坐标系{O_b}-x_by_bz_b以四旋翼飞行器的形心为原点，坐标轴x_b与四旋翼飞行器前进方向一致，坐标轴z_b垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向，{O_c}-x_cy_cz_c和{O_i}-x_iy_iz_i分别表示重心平衡机构坐标系和机械臂坐标系，摄像头固定在四旋翼飞行器上，位于机械臂上方且镜头朝下；被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体，大小不超出机械手可抓取的尺寸范围且水平摆放于地面上，本专利以轻质长方体为例，如图3所示，世界坐标系{O_w}-x_wy_wz_w以被抓目标12上的人工标志13的中心为原点，z_w与重力方向相反。

步骤2：由D-H法，对机械臂和重心平衡机构进行运动学建模，建模参数如下：

表1机械臂和重心平衡机构D-H法建模参数；

其中，θ_i,d_i,α_i-1,a_i-1为D-H法建模参数，i＝1,2,3是机械臂的关节编号，c为重心平衡机构关节编号，0、0’为安装参考系编号，H＝[θ₁,θ₂,θ₃]T为机械臂关节角，θ_c为重心平衡机构关节角。可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为：

⁰T₃＝A₁A₂A₃(1)；

其中，A_i的表达式如下：

对正运动方程逆运动学求解后能确定每个关节的值，从而使机械臂到达期望的位姿。

步骤3：对四旋翼飞行器进行运动学建模：

世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下：

其中，P＝[x,y,z]^T表示坐标系{O_b}的原点在坐标系{O_w}中的位置，Φ＝[φ,θ,ψ]^T表示坐标系{O_b}在坐标系{O_w}中所测量的横滚角，俯仰角和偏航角，V＝[u,v,w]^T表示坐标系{O_b}相对于坐标系{O_w}的线速度，Ω＝[p,q,r]^T表示四旋翼飞行器的角速度，R为按照Z-Y-X顺序旋转的旋转矩阵，Q为机体角速度与欧拉角变化率之间的转换矩阵。

步骤4：根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模：

对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***，假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量。通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程，可得动力学方程组：

其中，m表示复合***的质量，G＝[0,0,-mg]^T表示复合***重力矢量，I_m表示复合***重心处惯性张量，重心偏移向量r_G＝[x_G,y_G,z_G]^T表示复合***的重心到机体坐标系{O_b}的偏移。F_p＝[0,0,f_p]^T表示桨叶作用于复合***上的推动力矢量，τ_p＝[τ_x,τ_y,τ_z]^T为作用于复合***推动力产生的力矩的矢量，

为***与环境交互力产生的力矩，考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态，复合***与环境的交互力F_e＝0。

步骤二、人工标志的检测及坐标系之间转换过程；

人工标志选用ArUco库中如图3所示的标志，大小为5cm*5cm，编号为0。摄像头成像使用小孔成像模型：

P＝K[R|T]Q(5)；

其中，Q是三维空间中标志的坐标(相对于世界坐标系)。[R|T]是摄像头外参矩阵，用于将世界坐标中的标志变换到摄像头坐标内。K是摄像头内参，用于将摄像头坐标中的某点投影到像平面上。P即为标志投影后的像素坐标。如图4所示，使用基于OpenCV_ArUco的识别算法识别定位人工标志，人工标志的检测及坐标系之间的转换有如下步骤：

步骤1：采用棋盘对摄像头进行标定，确定摄像头的内参矩阵K；

步骤2：检测标志，包括：

步骤2.1：搜索图像中的所有候选标志，利用自适应性阈值来分割标志，然后从阈值化的图像中提取外形轮廓，并且舍弃那些非凸多边形的，以及那些不是方形的，还使用了一些额外的滤波(来剔除那些过小或者过大的轮廓，过于相近的凸多边形等)；

步骤2.2：分析候选标志的内部编码来确定它们是否确实是标志，包括：

步骤2.2.1：对图像进行透视变换，来得到它规范的形态(正视图)；

步骤2.2.2：对规范的图像用Ossu阈值化以分离白色和黑色位，提取每个标志的标志位并分析出标志的编号；

步骤3：角点处的亚像素级的细化后，根据小孔成像模型，利用ArUco标志的四个顶点的像素坐标，通过solvePnP方法即可求取摄像机的旋转矩阵R和平移矩阵T；

步骤4：根据摄像头坐标系与世界坐标系之间的欧式变换关系，获取摄像头位置姿态信息，求解公式如下：

P_c＝-R^-1T

yaw(ψ)＝atan(r₂₁/r₁₁)(6)；

pitch(θ)＝asin(-r₃₁)

roll(φ)＝atan(r₃₂/r₃₃)

其中，Pc表示摄像头在世界坐标系的坐标。

步骤三、根据图5，图6的控制方法，分级控制四旋翼飞行器和作业装置

假设四旋翼飞行器抓取坐标已知目标的作业范围是S，四旋翼飞行器位姿信息由图4所示的图像处理后解算摄像头位姿信息得到，如果四旋翼飞行器未进入作业范围S，四旋翼飞行器则靠近或远离人工标志，目标轨迹生成器把目标位置P^*＝[x^*,y^*,z^*]^T和偏航角ψ_d分别输出给位置控制器和姿态稳定控制器，位置控制器主体为PD控制器，其输出通过***动力学方程计算得到姿态控制所需的姿态角θ_d,φ_d，姿态稳定控制器采用反演法进行设计，其输出经过动力分配模型解耦出电机转速ω_i(i＝1,2,3,4)，输出到电机，直至四旋翼飞行器进入作业范围S；

如果四旋翼飞行器进入作业范围S，四旋翼飞行器悬停，四旋翼飞行器位姿信息输出到机械臂轨迹生成器，机械臂轨迹生成器首先将四旋翼飞行器位姿信息转换为目标位姿信息然后规划各个关节运动角度θ_i(i＝1,2,3)并输出到重心平衡控制器，重心平衡控制器根据重心平衡方程公式计算重心平衡机构关节运动角度θ_c，最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取。重心平衡方程公式：

∑m_ix_i+m_cx_c+m_tx_t＝0(7)；

其中，m_i、m_c、m_t分别机械臂各部分、重心平衡机构以及被抓取物体的质量，x_i、x_c、x_t分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{O_b}中的位置。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (1)

1.一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法，具体步骤如下，其特征在于：

步骤一：构建***实施平台，所述***实施平台由四旋翼飞行器和空中作业装置两部分组成，其中空中作业装置包括一个三自由度的机械臂和一个单自由度的重心平衡机构；分别对空中作业装置和四旋翼飞行器推导运动学和动力学方程；

步骤一中对空中作业装置和四旋翼飞行器推导运动学和动力学方程步骤如下：

步骤1：假设四旋翼飞行器、机械臂和重心平衡机构都是刚体，对相关坐标系做了如下定义：机体坐标系{O_b}-x_by_bz_b以四旋翼飞行器的形心为原点，坐标轴x_b与四旋翼飞行器前进方向一致，坐标轴z_b垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向，{O_c}-x_cy_cz_c和{O_i}-x_iy_iz_i分别表示重心平衡机构坐标系和机械臂坐标系，摄像头固定在四旋翼飞行器上，位于机械臂上方且镜头朝下；被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体，大小不超出机械手可抓取的尺寸范围且水平摆放于地面上；

步骤2：由D-H法，对机械臂和重心平衡机构进行运动学建模，建模参数如下：

其中，θ_i,d_i,α_i-1,a_i-1为D-H法建模参数，i＝1,2,3是机械臂的关节编号，c为重心平衡机构关节编号，0、0’为安装参考系编号，H＝[θ₁,θ₂,θ₃]^T为机械臂关节角，θ_c为重心平衡机构关节角，可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为：

⁰T₃＝A₁A₂A₃ (1)；

其中，A_i的表达式如下：

对正运动方程逆运动学求解后能确定每个关节的值，从而使机械臂到达期望的位姿；

步骤3：对四旋翼飞行器进行运动学建模：

世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下：

其中，P＝[x,y,z]^T表示机体坐标系{O_b}的原点在坐标系{O_w}中的位置，Φ＝[φ,θ,ψ]^T表示坐标系{O_b}在坐标系{O_w}中所测量的横滚角，俯仰角和偏航角，V＝[u,v,w]^T表示坐标系{O_b}相对于坐标系{O_w}的线速度，Ω＝[p,q,r]^T表示四旋翼飞行器的角速度，R为按照Z-Y-X顺序旋转的旋转矩阵，Q为机体角速度与欧拉角变化率之间的转换矩阵；

步骤4：根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模：

对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***，假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量，通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程，可得动力学方程组：

其中，m表示复合***的质量，G＝[0,0,-mg]T表示复合***重力矢量，I_m表示复合***重心处惯性张量，重心偏移向量r_G＝[x_G,y_G,z_G]^T表示复合***的重心到机体坐标系{O_b}的偏移，F_p＝[0,0,f_p]^T表示桨叶作用于复合***上的推动力矢量，τ_p＝[τ_x,τ_y,τ_z]^T为作用于复合***推动力产生的力矩的矢量，

为***与环境交互力产生的力矩，考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态，复合***与环境的交互力F_e＝0；

步骤二：标定摄像头后，选用ArUco库中的任一人工标志，使用基于OpenCV_ArUco的识别算法识别定位标志，将标志在摄像头坐标系下的位置和姿态信息转换为摄像头在世界坐标系的位姿信息；

步骤二中人工标志选用ArUco库中相应的标志，摄像头成像使用小孔成像模型：

P＝K[R|T]Q (5)；

其中，Q是三维空间中标志的坐标，[R|T]是摄像头外参矩阵，用于将世界坐标中的标志变换到摄像头坐标内，K是摄像头内参，用于将摄像头坐标中的某点投影到像平面上，P即为标志投影后的像素坐标，使用基于OpenCV_ArUco的识别算法识别定位人工标志，人工标志的检测及坐标系之间的转换有如下步骤：

步骤1：采用棋盘对摄像头进行标定，确定摄像头的内参矩阵K；

步骤2：检测标志，包括：

步骤2.1：搜索图像中的所有候选标志，利用自适应性阈值来分割标志，然后从阈值化的图像中提取外形轮廓，并且舍弃那些非凸多边形的，以及那些不是方形的，还使用了一些额外的滤波；

步骤2.2：分析候选标志的内部编码来确定它们是否确实是标志，包括：

步骤2.2.1：对图像进行透视变换，来得到它规范的形态；

步骤2.2.2：对规范的图像用Ossu阈值化以分离白色和黑色位，提取每个标志的标志位并分析出标志的编号；

步骤3：角点处的亚像素级的细化后，根据小孔成像模型，利用ArUco标志的四个顶点的像素坐标，通过solvePnP方法即可求取摄像机的旋转矩阵R和平移矩阵T；

步骤4：根据摄像头坐标系与世界坐标系之间的欧式变换关系，获取摄像头位置姿态信息，求解公式如下：

其中，P_c表示摄像头在世界坐标系的坐标；

步骤三：整个控制***分为三个级别，分别是飞行器控制、机械臂控制、重心平衡机构控制；设定一个空中作业范围值S，根据由图像处理得到四旋翼飞行器位姿信息判断四旋翼飞行器的分级控制，如果四旋翼飞行器未进入空中作业范围，控制四旋翼飞行器靠近或远离人工标志直至进入空中作业范围，否则，四旋翼飞行器悬停，规划机械臂各关节角度并计算出重心平衡机构关节运动角度以减小机械臂运动对四旋翼飞行器的扰动，控制机械臂自主抓取目标；

步骤三中分级控制四旋翼飞行器和作业装置步骤如下：

假设四旋翼飞行器抓取坐标已知目标的作业范围是S，四旋翼飞行器位姿信息由图像处理后解算摄像头位姿信息得到，如果四旋翼飞行器未进入作业范围S，四旋翼飞行器则靠近或远离人工标志，目标轨迹生成器把目标位置P^*＝[x^*,y^*,z^*]^T和偏航角ψ_d分别输出给位置控制器和姿态稳定控制器，位置控制器主体为PD控制器，其输出通过***动力学方程计算得到姿态控制所需的姿态角θ_d,φ_d，姿态稳定控制器采用反演法进行设计，其输出经过动力分配模型解耦出电机转速ω_i(i＝1,2,3,4)，输出到电机，直至四旋翼飞行器进入作业范围S；

如果四旋翼飞行器进入作业范围S，四旋翼飞行器悬停，四旋翼飞行器位姿信息输出到机械臂轨迹生成器，机械臂轨迹生成器首先将四旋翼飞行器位姿信息转换为目标位姿信息然后规划各个关节运动角度θ_i(i＝1,2,3)并输出到重心平衡控制器，重心平衡控制器根据重心平衡方程公式计算重心平衡机构关节运动角度θ_c，最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取，重心平衡方程公式：

∑m_ix_i+m_cx_c+m_tx_t＝0 (7)；

其中，m_i、m_c、m_t分别机械臂各部分、重心平衡机构以及被抓取物体的质量，x_i、x_c、x_t分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{O_b}中的位置。

Images