CN108453738B - 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 - Google Patents
一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 Download PDFInfo
- Publication number
- CN108453738B CN108453738B CN201810279649.XA CN201810279649A CN108453738B CN 108453738 B CN108453738 B CN 108453738B CN 201810279649 A CN201810279649 A CN 201810279649A CN 108453738 B CN108453738 B CN 108453738B
- Authority
- CN
- China
- Prior art keywords
- rotor aircraft
- mechanical arm
- coordinate system
- camera
- gravity center
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000012545 processing Methods 0.000 title claims abstract description 16
- 230000005484 gravity Effects 0.000 claims abstract description 75
- 230000007246 mechanism Effects 0.000 claims abstract description 41
- 239000002131 composite material Substances 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000008859 change Effects 0.000 claims description 9
- 238000003384 imaging method Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 238000009434 installation Methods 0.000 claims description 6
- 230000003993 interaction Effects 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000006641 stabilisation Effects 0.000 claims description 2
- 238000011105 stabilization Methods 0.000 claims description 2
- 238000009795 derivation Methods 0.000 claims 1
- 238000011160 research Methods 0.000 description 5
- 239000003550 marker Substances 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 208000028804 PERCHING syndrome Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64C—AEROPLANES; HELICOPTERS
- B64C27/00—Rotorcraft; Rotors peculiar thereto
- B64C27/04—Helicopters
- B64C27/08—Helicopters with two or more rotors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENT OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
- B64D47/00—Equipment not otherwise provided for
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
- B64U2101/00—UAVs specially adapted for particular uses or applications
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Robotics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明提出了一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法。四旋翼飞行器作业***由四旋翼飞行器和作业装置两部分组成,其中作业装置由对称安装于四旋翼飞行器下方的机械臂和重心平衡机构构成。本发明利用四旋翼飞行器上的摄像头获取被抓目标及人工标志的图像,利用Opencv处理图像并解算摄像头位姿信息,从而得到四旋翼飞行器的位姿信息,根据四旋翼飞行器的位姿信息分级控制四旋翼飞行器、机械臂及重心平衡机构。本发明拓宽了四旋翼飞行器的应用领域,既可在室内也可在周边建筑密集的室外环境下进行空中作业,提高了空中作业精度。
Description
技术领域
本发明涉及空中机器人控制技术领域,特别是涉及一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法。
背景技术
四旋翼飞行器凭借机动灵活、便于携带等特点一直是军事和商业领域的研究热点,在航拍勘测、农药喷洒植保、目标跟踪定位等领域应用广泛。相对于地面移动机器人的移动作业,具有空中作业能力的无人飞行器将作业空间扩展到了三维,这拓宽了无人机的应用领域,在复杂环境下危险物体的搬运、勘测设备的放置与回收、空中目标的抓取等方面有着广阔的应用前景。
在对四旋翼飞行器的研究中,位姿估计问题即定位问题一直是研究的核心问题。四旋翼飞行器自身位姿的精确估计是四旋翼飞行器控制、避障、路径规划、暂栖、抓取等研究方向的重要基础和前提。目前四旋翼飞行器的定位主要有两类方法:一类是基于四旋翼飞行器自身视觉获得位置信息。另一类是基于外部设备提供位置信息,如全球卫星定位***(GPS),运动捕捉***。但GPS易受周边环境与气候的影响,在室内或周边建筑密集的情况下,无人机难以依靠GPS获取准确位置信息。而运动捕捉***只能用于实验室等特定的研究场所,这限制了四旋翼飞行器的应用范围。
发明内容
为了解决上述存在的问题,针对四旋翼飞行器利用自身携带的机械臂抓取物体时要求抓取精确高,作业过程稳定性强以及操作简单、灵活等特点,本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法,拓宽了四旋翼飞行器的应用领域,既可在室内也可在周边建筑密集的室外环境下进行空中作业,提高了空中作业精度,为达此目的,本发明提供一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法,具体步骤如下:
步骤一:构建***实施平台,所述***实施平台由四旋翼飞行器和空中作业装置两部分组成,其中空中作业装置包括一个三自由度的机械臂和一个单自由度的重心平衡机构;分别对空中作业装置和四旋翼飞行器推导运动学和动力学方程;
步骤二:标定摄像头后,选用ArUco库中的任一标志,使用基于OpenCV_ArUco的识别算法识别定位标志,将标志在摄像头坐标系下的位置和姿态信息转换为摄像头在世界坐标系的位姿信息;
步骤三:整个控制***分为三个级别,分别是飞行器控制、机械臂控制、重心平衡机构控制;设定一个空中作业范围值S,根据由图像处理得到四旋翼飞行器位姿信息判断四旋翼飞行器的分级控制,如果四旋翼飞行器未进入空中作业范围,控制四旋翼飞行器靠近或远离人工标志直至进入空中作业范围,否则,四旋翼飞行器悬停,规划机械臂各关节角度并计算出重心平衡机构关节运动角度以减小机械臂运动对四旋翼飞行器的扰动,控制机械臂自主抓取目标。
本发明的进一步改进,步骤一中对作业装置和四旋翼飞行器推导运动学和动力学方程步骤如下:
步骤1:假设四旋翼飞行器、机械臂和重心平衡机构都是刚体,对相关坐标系做了如下定义:机体坐标系{Ob}-xbybzb以四旋翼飞行器的形心为原点,坐标轴xb与四旋翼飞行器前进方向一致,坐标轴zb垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向,{Oc}-xcyczc和{Oi}-xiyizi分别表示重心平衡机构坐标系和机械臂坐标系,摄像头固定在四旋翼飞行器上,位于机械臂上方且镜头朝下;被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体,大小不超出机械手可抓取的尺寸范围且水平摆放于地面上;
步骤2:由D-H法,对机械臂和重心平衡机构进行运动学建模,建模参数如下:
其中,θi,di,αi-1,ai-1为D-H法建模参数,i=1,2,3是机械臂的关节编号,c为重心平衡机构关节编号,0、0’为安装参考系编号,H=[θ1,θ2,θ3]T为机械臂关节角,θc为重心平衡机构关节角。可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为:
0T3=A1A2A3(1);
其中,Ai的表达式如下:
对正运动方程逆运动学求解后能确定每个关节的值,从而使机械臂到达期望的位姿;
步骤3:对四旋翼飞行器进行运动学建模:
世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下:
其中,P=[x,y,z]T表示坐标系{Ob}的原点在坐标系{Ow}中的位置,Φ=[φ,θ,ψ]T表示坐标系{Ob}在坐标系{Ow}中所测量的横滚角,俯仰角和偏航角,V=[u,v,w]T表示坐标系{Ob}相对于坐标系{Ow}的线速度,Ω=[p,q,r]T表示四旋翼飞行器的角速度,R为按照Z-Y-X顺序旋转的旋转矩阵,Q为机体角速度与欧拉角变化率之间的转换矩阵;
步骤4:根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模:
对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***,假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量。通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程,可得动力学方程组:
其中,m表示复合***的质量,G=[0,0,-mg]T表示复合***重力矢量,Im表示复合***重心处惯性张量,重心偏移向量rG=[xG,yG,zG]T表示复合***的重心到机体坐标系{Ob}的偏移。Fp=[0,0,fp]T表示桨叶作用于复合***上的推动力矢量,τp=[τx,τy,τz]T为作用于复合***推动力产生的力矩的矢量,为***与环境交互力产生的力矩,考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态,复合***与环境的交互力Fe=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:根据摄像头坐标系与世界坐标系之间的欧式变换关系,获取摄像头位置姿态信息,求解公式如下:
Pc=-R-1T
yaw(ψ)=atan(r21/r11)(6);
pitch(θ)=asin(-r31)
roll(φ)=atan(r32/r33)
其中,Pc表示摄像头在世界坐标系的坐标。
本发明的进一步改进,步骤三中分级控制四旋翼飞行器和作业装置步骤如下:
假设四旋翼飞行器抓取坐标已知目标的作业范围是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,最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取,重心平衡方程公式:
∑mixi+mcxc+mtxt=0(7);
其中,mi、mc、mt分别机械臂各部分、重心平衡机构以及被抓取物体的质量,xi、xc、xt分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{Ob}中的位置。
本发明提供一种基于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所示,机体坐标系{Ob}-xbybzb以四旋翼飞行器的形心为原点,坐标轴xb与四旋翼飞行器前进方向一致,坐标轴zb垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向,{Oc}-xcyczc和{Oi}-xiyizi分别表示重心平衡机构坐标系和机械臂坐标系,摄像头固定在四旋翼飞行器上,位于机械臂上方且镜头朝下;被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体,大小不超出机械手可抓取的尺寸范围且水平摆放于地面上,本专利以轻质长方体为例,如图3所示,世界坐标系{Ow}-xwywzw以被抓目标12上的人工标志13的中心为原点,zw与重力方向相反。
步骤2:由D-H法,对机械臂和重心平衡机构进行运动学建模,建模参数如下:
表1机械臂和重心平衡机构D-H法建模参数;
其中,θi,di,αi-1,ai-1为D-H法建模参数,i=1,2,3是机械臂的关节编号,c为重心平衡机构关节编号,0、0’为安装参考系编号,H=[θ1,θ2,θ3]T为机械臂关节角,θc为重心平衡机构关节角。可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为:
0T3=A1A2A3(1);
其中,Ai的表达式如下:
对正运动方程逆运动学求解后能确定每个关节的值,从而使机械臂到达期望的位姿。
步骤3:对四旋翼飞行器进行运动学建模:
世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下:
其中,P=[x,y,z]T表示坐标系{Ob}的原点在坐标系{Ow}中的位置,Φ=[φ,θ,ψ]T表示坐标系{Ob}在坐标系{Ow}中所测量的横滚角,俯仰角和偏航角,V=[u,v,w]T表示坐标系{Ob}相对于坐标系{Ow}的线速度,Ω=[p,q,r]T表示四旋翼飞行器的角速度,R为按照Z-Y-X顺序旋转的旋转矩阵,Q为机体角速度与欧拉角变化率之间的转换矩阵。
步骤4:根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模:
对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***,假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量。通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程,可得动力学方程组:
其中,m表示复合***的质量,G=[0,0,-mg]T表示复合***重力矢量,Im表示复合***重心处惯性张量,重心偏移向量rG=[xG,yG,zG]T表示复合***的重心到机体坐标系{Ob}的偏移。Fp=[0,0,fp]T表示桨叶作用于复合***上的推动力矢量,τp=[τx,τy,τz]T为作用于复合***推动力产生的力矩的矢量,为***与环境交互力产生的力矩,考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态,复合***与环境的交互力Fe=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:根据摄像头坐标系与世界坐标系之间的欧式变换关系,获取摄像头位置姿态信息,求解公式如下:
Pc=-R-1T
yaw(ψ)=atan(r21/r11)(6);
pitch(θ)=asin(-r31)
roll(φ)=atan(r32/r33)
其中,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,最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取。重心平衡方程公式:
∑mixi+mcxc+mtxt=0(7);
其中,mi、mc、mt分别机械臂各部分、重心平衡机构以及被抓取物体的质量,xi、xc、xt分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{Ob}中的位置。
以上所述,仅是本发明的较佳实施例而已,并非是对本发明作任何其他形式的限制,而依据本发明的技术实质所作的任何修改或等同变化,仍属于本发明所要求保护的范围。
Claims (1)
1.一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法,具体步骤如下,其特征在于:
步骤一:构建***实施平台,所述***实施平台由四旋翼飞行器和空中作业装置两部分组成,其中空中作业装置包括一个三自由度的机械臂和一个单自由度的重心平衡机构;分别对空中作业装置和四旋翼飞行器推导运动学和动力学方程;
步骤一中对空中作业装置和四旋翼飞行器推导运动学和动力学方程步骤如下:
步骤1:假设四旋翼飞行器、机械臂和重心平衡机构都是刚体,对相关坐标系做了如下定义:机体坐标系{Ob}-xbybzb以四旋翼飞行器的形心为原点,坐标轴xb与四旋翼飞行器前进方向一致,坐标轴zb垂直于螺旋桨旋转平面且沿着四旋翼飞行器上升方向,{Oc}-xcyczc和{Oi}-xiyizi分别表示重心平衡机构坐标系和机械臂坐标系,摄像头固定在四旋翼飞行器上,位于机械臂上方且镜头朝下;被抓目标为带有人工标志的外形规则的轻质长方体或立方体或圆柱体,大小不超出机械手可抓取的尺寸范围且水平摆放于地面上;
步骤2:由D-H法,对机械臂和重心平衡机构进行运动学建模,建模参数如下:
其中,θi,di,αi-1,ai-1为D-H法建模参数,i=1,2,3是机械臂的关节编号,c为重心平衡机构关节编号,0、0’为安装参考系编号,H=[θ1,θ2,θ3]T为机械臂关节角,θc为重心平衡机构关节角,可知从机械臂关节坐标系3到安装参考坐标系0的齐次变换矩阵即正运动学方程为:
0T3=A1A2A3 (1);
其中,Ai的表达式如下:
对正运动方程逆运动学求解后能确定每个关节的值,从而使机械臂到达期望的位姿;
步骤3:对四旋翼飞行器进行运动学建模:
世界坐标系和机体坐标系间机体角速度与欧拉角变化率的关系如下:
其中,P=[x,y,z]T表示机体坐标系{Ob}的原点在坐标系{Ow}中的位置,Φ=[φ,θ,ψ]T表示坐标系{Ob}在坐标系{Ow}中所测量的横滚角,俯仰角和偏航角,V=[u,v,w]T表示坐标系{Ob}相对于坐标系{Ow}的线速度,Ω=[p,q,r]T表示四旋翼飞行器的角速度,R为按照Z-Y-X顺序旋转的旋转矩阵,Q为机体角速度与欧拉角变化率之间的转换矩阵;
步骤4:根据牛顿-欧拉法可以对搭载空中作业装置的四旋翼飞行器的复合***进行动力学建模:
对于由四旋翼飞行器、机械臂以及重心平衡机构组成的复合***,假设机械臂和重心平衡机构的各关节的转动是平滑缓慢的,这种缓慢的变化仅影响***的重心和转动惯量,通过牛顿-欧拉动力学方程可以推导出复合***在载体坐标系中重心处的动力学方程,可得动力学方程组:
其中,m表示复合***的质量,G=[0,0,-mg]T表示复合***重力矢量,Im表示复合***重心处惯性张量,重心偏移向量rG=[xG,yG,zG]T表示复合***的重心到机体坐标系{Ob}的偏移,Fp=[0,0,fp]T表示桨叶作用于复合***上的推动力矢量,τp=[τx,τy,τz]T为作用于复合***推动力产生的力矩的矢量,为***与环境交互力产生的力矩,考虑到机械臂运动比较缓慢及机械手抓取前或抓取后的状态,复合***与环境的交互力Fe=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:根据摄像头坐标系与世界坐标系之间的欧式变换关系,获取摄像头位置姿态信息,求解公式如下:
其中,Pc表示摄像头在世界坐标系的坐标;
步骤三:整个控制***分为三个级别,分别是飞行器控制、机械臂控制、重心平衡机构控制;设定一个空中作业范围值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,最后将全部关节角度信号无线发送到舵机控制器控制作业装置完成抓取,重心平衡方程公式:
∑mixi+mcxc+mtxt=0 (7);
其中,mi、mc、mt分别机械臂各部分、重心平衡机构以及被抓取物体的质量,xi、xc、xt分别机械臂各部分、重心平衡机构以及被抓取物体的重心在机体系{Ob}中的位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810279649.XA CN108453738B (zh) | 2018-03-30 | 2018-03-30 | 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810279649.XA CN108453738B (zh) | 2018-03-30 | 2018-03-30 | 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108453738A CN108453738A (zh) | 2018-08-28 |
CN108453738B true CN108453738B (zh) | 2021-04-16 |
Family
ID=63238086
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810279649.XA Active CN108453738B (zh) | 2018-03-30 | 2018-03-30 | 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108453738B (zh) |
Families Citing this family (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108994874A (zh) * | 2018-09-17 | 2018-12-14 | 北京臻迪科技股份有限公司 | 用于无人机的机械臂和无人机 |
CN109573012A (zh) * | 2018-10-30 | 2019-04-05 | 华南农业大学 | 一种汽车辅助飞行装置 |
CN109374003A (zh) * | 2018-11-06 | 2019-02-22 | 山东科技大学 | 一种基于ArUco码的移动机器人视觉定位与导航方法 |
CN109895099B (zh) * | 2019-03-28 | 2020-10-02 | 哈尔滨工业大学(深圳) | 一种基于自然特征的飞行机械臂视觉伺服抓取方法 |
CN110203411A (zh) * | 2019-06-25 | 2019-09-06 | 上海电机学院 | 一种带有可拆卸取物机械手的无人机及无人机*** |
CN110525642A (zh) * | 2019-08-26 | 2019-12-03 | 核工业北京地质研究院 | 一种无人机载多传感器野外查证与定点测量*** |
CN110427043B (zh) * | 2019-09-04 | 2021-09-28 | 福州大学 | 基于作业飞行机器人重心偏移的位姿控制器设计方法 |
CN111070216B (zh) * | 2019-12-09 | 2020-12-01 | 北京大学 | 基于无人机的两移一转的三自由度派件机器人 |
CN111015673A (zh) * | 2020-01-02 | 2020-04-17 | 福州大学 | 作业型飞行机器人的四自由度机械臂遥操作***及方法 |
CN111185907B (zh) * | 2020-01-13 | 2021-07-13 | 福州大学 | 一种作业型飞行机器人抓取后的位姿稳定控制方法 |
CN111627075B (zh) * | 2020-05-22 | 2021-07-23 | 上海黑眸智能科技有限责任公司 | 基于aruco码的摄像头外参标定方法、***、终端及介质 |
CN111923049B (zh) * | 2020-08-21 | 2023-11-03 | 福州大学 | 基于球面模型的飞行机械臂视觉伺服与多任务控制方法 |
CN112308900B (zh) * | 2020-10-22 | 2022-10-21 | 大连理工大学 | 一种基于led圆环检测的四旋翼无人机相对位姿估计方法 |
CN112550708B (zh) * | 2020-12-10 | 2022-04-15 | 国网浙江省电力有限公司江山市供电公司 | 无人机验电设备 |
CN112550742B (zh) * | 2020-12-10 | 2022-04-15 | 国网浙江省电力有限公司江山市供电公司 | 转动式无人机验电装置 |
CN112572797A (zh) * | 2020-12-28 | 2021-03-30 | 广西电网有限责任公司梧州供电局 | 一种无人机的高压电力塔涉鸟异物自动清除方法及*** |
CN112965520A (zh) * | 2021-02-02 | 2021-06-15 | 重庆特斯联智慧科技股份有限公司 | 环境智能监测***及具有其的机器人 |
CN113021336B (zh) * | 2021-02-25 | 2022-07-05 | 上海交通大学 | 一种基于主从移动操作机器人的档案取放***及方法 |
CN113977575A (zh) * | 2021-09-28 | 2022-01-28 | 北京炎凌嘉业机电设备有限公司 | 机械臂作业与全身协调稳定控制方法 |
CN114248921B (zh) * | 2021-12-31 | 2023-12-22 | 福州大学 | 一种可以在空中抓取和停留的四旋翼飞行器 |
CN114663494A (zh) * | 2022-03-18 | 2022-06-24 | 欧姆龙(上海)有限公司 | 三维物体的空间姿态的标注方法、装置以及电子设备 |
CN115570564B (zh) * | 2022-09-26 | 2024-06-14 | 北京航空航天大学 | 一种家用服务机器人对目标位姿的识别与规范化方法 |
CN115922731B (zh) * | 2023-01-09 | 2023-05-30 | 深圳鹏行智能研究有限公司 | 一种机器人的控制方法以及机器人 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9836117B2 (en) * | 2015-05-28 | 2017-12-05 | Microsoft Technology Licensing, Llc | Autonomous drones for tactile feedback in immersive virtual reality |
CN105573343A (zh) * | 2016-02-22 | 2016-05-11 | 铱格斯曼航空科技集团有限公司 | 一种无人机抓捕*** |
CN105799935B (zh) * | 2016-04-18 | 2017-12-08 | 南京航空航天大学 | 一种无人机飞行运载*** |
JP6371959B2 (ja) * | 2016-09-02 | 2018-08-15 | 株式会社プロドローン | ロボットアームおよびこれを備える無人航空機 |
CN106897982B (zh) * | 2017-02-23 | 2019-06-14 | 淮阴工学院 | 基于图像无标记识别的现实增强方法 |
CN107450579B (zh) * | 2017-07-27 | 2020-02-18 | 东南大学 | 一种搭载机械臂的四旋翼飞行器平衡作业控制方法 |
-
2018
- 2018-03-30 CN CN201810279649.XA patent/CN108453738B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN108453738A (zh) | 2018-08-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108453738B (zh) | 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法 | |
EP3903164B1 (en) | Collision avoidance system, depth imaging system, vehicle, map generator, amd methods thereof | |
Shen et al. | Vision-based state estimation for autonomous rotorcraft MAVs in complex environments | |
US11726501B2 (en) | System and method for perceptive navigation of automated vehicles | |
Bao et al. | Vision-based horizon extraction for micro air vehicle flight control | |
Duan et al. | A binocular vision-based UAVs autonomous aerial refueling platform | |
Irfan et al. | Vision-based guidance and navigation for autonomous mav in indoor environment | |
CN115291536A (zh) | 基于视觉的无人机跟踪地面目标半物理仿真平台验证方法 | |
CN111862200A (zh) | 一种煤棚内无人机定位方法 | |
Li et al. | A survey of vision based autonomous aerial refueling for unmanned aerial vehicles | |
Martínez et al. | Trinocular ground system to control UAVs | |
CN110309883A (zh) | 一种基于视觉slam的无人机自主定位方法 | |
Zenkin et al. | Quadcopter simulation model for research of monitoring tasks | |
Darma et al. | Visual servoing quadrotor control in autonomous target search | |
Ammann et al. | Undelayed initialization of inverse depth parameterized landmarks in UKF-SLAM with error state formulation | |
Shi et al. | Vision-based real-time 3D mapping for UAV with laser sensor | |
Hernández et al. | Visual SLAM with oriented landmarks and partial odometry | |
Jeong et al. | Cartesian space control of a quadrotor system based on low cost localization under a vision system | |
Qi et al. | An Autonomous Pose Estimation Method of MAV Based on Monocular Camera and Visual Markers | |
Cheng et al. | Positioning method research for unmanned aerial vehicles based on meanshift tracking algorithm | |
Yang et al. | Inertial-aided vision-based localization and mapping in a riverine environment with reflection measurements | |
Alshbatat | Adaptive vision-based system for landing an autonomous hexacopter drone on a specific landing platform | |
Hao et al. | A method for indoor and outdoor collaborative localization and mapping based on multi-sensors unmanned platforms | |
Zamudio et al. | Stereo vision for the stabilization of a quadrotor | |
Effland et al. | Application of machine vision in unmanned aerial systems for autonomous target tracking |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |