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

权利要求书

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}中的位置。