CN104965516A - 一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法 - Google Patents

Abstract

本发明提出了一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法，其中，四旋翼飞行器的下端连接有用于抓取的三自由度机械臂，包括如下步骤：获取四旋翼飞行器姿态传感器的三轴加速度和三轴角加速度；进行融合得重力加速度在载体坐标系下各轴的分量；将分量转换为力反馈手控器执行末端在其坐标系下应受反馈力；操作员根据反馈力的大小和方向并结合视觉信息控制四旋翼飞行器的运动以及机械手的抓取作业。操作作业时，根据视觉和力反馈信息控制四旋翼悬停于作业对象附近，然后微调机械臂准确抓住物体。该方法将机械手抓取过程中的受力作为四旋翼姿态的扰动力传递到反馈力中，使四旋翼的运动控制和机械手的抓取作业更加容易、更准确。

Description

一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法

技术领域

本发明涉及一种四旋翼飞行器空中抓取作业系统，引入力觉反馈并结合视觉反馈来提高物体抓取的准确性和稳健性。特别适用于要求一次性抓取且抓取过程不允许剧烈晃动的易碎、不稳定的物体。

背景技术

随着制造成本的降低和控制技术的成熟，旋翼型飞行器在航拍、侦查领域已经得到广泛应用，而在短程货物配送以及物体抓取作业方面业已成为研究的热点。国外网络零售巨头亚马逊和国内快递公司顺丰先后在旋翼型无人机短程货物配送方面投下大量研发资金并不断取得新的成果，但是这些配送物品的装入和卸下大多依靠人工而非机械手自动抓取，配送效率低下。众多的研究院所也在旋翼型飞行器的物体抓取方面做出探索，主要集中于基于视觉的单边遥控抓取和基于视觉伺服的自主抓取。前者将飞行器和机械手仅仅作为执行机构，通过人眼或摄像头获取飞行器与被抓取物体的状态信息，然后不断调整飞行状态以完成物体的抓取；这种方式虽然技术简单容易实现，但作业效果与操作员经验和状态密切相关，因而抓取精度低且效果不稳定。后者主要是将视觉定位和飞行器控制算法结合在一起，着重于物体识别、定位和飞行控制算法研究，适用于特定物体和特定环境下的快速抓取，但是该方法技术实现难度大、应用灵活性差。

随着双边遥操作概念、力反馈技术的日臻完善，遥控操作人员将不再是执行机构的命令发送方，而且可以通过力反馈设备与操作环境相交互，使操作人员具有良好的临场感和沉浸感，这样不仅可以降低了操作人员的工作难度而且可以大大提高执行机构的控制精度。

发明内容

针对飞行器在危险物品处理、特殊物品搬运等领域对抓取精度要求高、作业过程稳定性强以及操作简单、灵活等应用要求，本发明提供了一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法，能够克服背景技术中所提到的单边遥控方式抓取精度低、操作难度大和自主抓取难度大、应用局限等缺点。

为解决上述技术问题，本发明采用的技术方案是：

一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法，其中，四旋翼飞行器的下端连接有用于抓取的三自由度机械臂，其特征在于，包括如下步骤：

步骤一、获取四旋翼飞行器姿态传感器的三轴加速度a_x、a_y、a_z和三轴角加速度ω_x、ω_y、ω_z，对获取的姿态传感器数据进行融合得重力加速度在载体坐标系下各轴的分量A_x、A_y、A_z；

步骤二、将步骤一融合得到的分量A_x、A_y、A_z转换为力反馈手控器执行末端在其坐标系下应受反馈力F_x、F_y、F_z；

步骤三、操作员根据反馈力的大小和方向并结合视觉信息控制四旋翼飞行器的运动以及机械手的抓取作业。

步骤一得到分量A_x、A_y、A_z的方法为：周期性地采集四旋翼飞行器上的姿态传感器的原始姿态数据α_x、α_y、α_z、ω_x、ω_y、ω_z，使用数字低通滤波器消除高频干扰，通过卡尔曼滤波算法对四旋翼飞行器姿态传感器数据进行融合得重力加速度在载体坐标系下各轴的分量A_x、A_y、A_z。

所述步骤二中转换的方法为：

根据分量A_x、A_y、A_z，得到俯仰角φ和横滚角

定义反馈力产生矩阵 $T=\left[\begin{array}{ccc}a& 0& 0\\ 0& b& 0\\ 0& 0& c\end{array}\right],$ 力源矩阵反馈力矩阵 $F=\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right];$

其中，a,b,c为反馈力系数；

计算反馈力F：F＝TY。

所述步骤三中控制四旋翼飞行器的方法为，通过改变操作末端坐标x,y,z的位置控制飞行器的飞行，具体是：

基于四旋翼的飞行控制原理得到四个旋翼产生的升力F₁、F₂、F₃、F₄：

$\left\{\begin{array}{c}K\·z-G=F_1+F_2+F_3+F_4\\ K\·x=F_4-F_2\\ K\·y=F_3-F_1\\ 0=(F_1+F_3-F_2-F_4)l\end{array}\right.$

其中K为力反馈手控器控制灵敏度系数，G为是飞行器重量，l为四旋翼重心到螺旋桨轴线的距离；

计算四旋翼各电机的电压量V₁、V₂、V₃、V₄：

$\begin{array}{cc}{V}_i=k\sqrt{F_i}& (i=\mathrm{1,2,34})\end{array}$

其中k为变换系数。

本发明基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法，其系统平台包括控制端、作业端和通讯环节。其特征是：控制端包括控制中心、六自由度力反馈手控器，力反馈手控器包括三个位置自由度、三个关节自由度以及一对控制键。力反馈手控器的三个位置自由度可以产生空间位置信息，控制中心根据力反馈手控器产生的空间位置信息生成四旋翼飞行器左转/右转、上升/下降、前进/后退方向的飞行控制命令；力反馈手控器的三个位置自由度对应于反馈力的执行机构，控制中心根据四旋翼飞行器的姿态信息产生反馈力执行机构所需的命令码；力反馈手控器的三个转动自由度与机械臂的三个关节相映射，用于机械臂的运动控制；力反馈手控器的一对控制键与机械手张合相对应，用于机械手的抓放控制。作业端包括四旋翼飞行器、摄像头和三个自由度机械臂(含机械手)等。四旋翼飞行器接收控制端发来的飞行控制命令进行相应的机动或悬停，摄像头获取飞行器所在空间和抓取作业空间的视觉信息，机械臂和机械手接收运动控制命令进行相应动作以完成物体的抓取和释放。通信环节在控制端和作业端各有一个无线收发模快，用于飞行器控制命令、机械臂控制命令、飞行器姿态信息以及视频信息在控制端和作业端之间的传输。

针对现有技术的不足和飞行器抓取作业在危险物品处理领域的应用要求，本发明具有如下优点：一、将力反馈引入到飞行器运动控制中，使得飞行器更易控制；二、将抓取位置的确定过程分解为飞行器运动控制和机械臂运动控制两部分，使得抓取更准确；三、将物体抓取过程中机械手的受力作为扰动引入到飞行器的飞行控制中并加以抵消，使得抓取过程更稳健。

附图说明：

图1是系统结构示意图；

图2是三个自由度机械臂结构图；

图3是六个自由度力反馈手控器结构图；

图4是力反馈手控器操作末端参数示意图；

图5是四旋翼飞行原理相关参数示意图；

图6是控制系统原理图。

具体实施方式

下面结合附图对本发明做进一步说明：

参见图1所示，本发明系统平台主要由摄像头1、四旋翼飞行器2、无线模块3、六自由度力反馈手控器4、PC机5、三自由度机械臂(含机械手)组成。

参见图2和图3所示，力反馈手控器的位置自由度4-1、4-2、4-3的转动可以改变手控器操作末端4-4的空间位置；转动自由度4-5用于控制机械臂关节6-1的转动，转动自由度4-8用于控制机械臂关节6-2的转动，转动自由度4-7用于控制机械臂关节6-3的转动，按键4-6用于控制机械手的松开和握紧；

参见图4所示，PC机5可以通过API实时获取手控器操作末端所在空间坐标系的位置信息x、y、z和转动自由度4-5、4-8、4-7所在关节的转动角α、β、γ；PC机5可以通过API实时设定手控器操作末端4-4所在空间坐标系O-XYZ中坐标(x、y、z)处沿X轴、Y轴、Y轴方向的反馈力大小F_x、F_y、F_z。

参见图5所示，姿态传感器坐标系与四旋翼载体坐标系O-X_bY_bZ_b重合,a_x、a_y、a_z为三轴加速度，ω_x、ω_y、ω_z为三轴角速度。F₁、F₂、F₃、F₄分别为四个旋翼产生的升力，正比于无刷电机的输入电压。定义操作末端4-4的坐标x,y,z分别控制四旋翼飞行器的左转/右转、上升/下降、前进/后退方向的飞行控制命令，由四旋翼的飞行控制原理可得：

$\left\{\begin{array}{c}K\·z-G=F_1+F_2+F_3+F_4\\ K\·x=F_4-F_2\\ K\·y=F_3-F_1\\ 0=(F_1+F_3-F_2-F_4)l\end{array}\right.---\left(1\right)$

其中K为力反馈手控器控制灵敏度系数，G为是飞行器重量，l为四旋翼重心到螺旋桨轴线的距离。

参见图6所示，周期性地采集四旋翼飞行器2上的姿态传感器的原始姿态数据α_x、α_y、α_z、ω_x、ω_y、ω_z，使用数字低通滤波器消除高频干扰，通过卡尔曼滤波算法对四旋翼飞行器姿态传感器数据进行融合得重力加速度在载体坐标系下各轴的分量A_x、A_y、A_z。对四旋翼飞行器进行姿态解算得到俯仰角φ和横滚角分别为：

定义反馈力产生矩阵 $T=\left[\begin{array}{ccc}a& 0& 0\\ 0& b& 0\\ 0& 0& c\end{array}\right],$ 力源矩阵反馈力矩阵 $F=\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right];$

其中，a,b,c为反馈力系数，由试验给出；

则力反馈产生算法为F＝TY。(3)

由于力反馈控制器的转动自由度与机械臂的三个自由度一一对应，机械臂控制命令的生成为反馈控制器的转动自由度关节角的线性映射，设α'、β'、γ'为机械臂关节控制量，则 $\left[\begin{array}{c}{\mathrm{\α}}^{\′}\\ {\mathrm{\β}}^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right]=\left[\begin{array}{ccc}m& 0& 0\\ 0& n& 0\\ 0& 0& v\end{array}\right]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\\ \mathrm{\γ}\end{array}\right]---\left(4\right)$ 其中m、n、v为机械臂转动灵敏度系数。

由公式(1)可解算出四旋翼飞行控制命令的相关量F₁、F₂、F₃、F₄，由于各相关量与螺旋桨的转速的平方成正比，而螺旋桨的转速与无刷电机的电压成正比，所以四旋翼飞行器各电机的电压量V₁、V₂、V₃、V₄可求得：

$\begin{array}{cc}{V}_i=k\sqrt{F_i}& (i=\mathrm{1,2,34})\end{array}---\left(5\right)$

其中k为变换系数。

Claims (4)

1.一种基于视觉和力反馈的四旋翼飞行器进行空中抓取作业的控制方法，其中，四旋翼飞行器的下端连接有用于抓取的三自由度机械臂，其特征在于，包括如下步骤：

步骤一、获取四旋翼飞行器姿态传感器的三轴加速度a_x、a_y、a_z和三轴角加速度ω_x、ω_y、ω_z，对获取的姿态传感器数据进行融合得重力加速度在载体坐标系下各轴的分量A_x、A_y、A_z；

步骤二、将步骤一融合得到的分量A_x、A_y、A_z转换为力反馈手控器执行末端在其坐标系下应受反馈力F_x、F_y、F_z；

步骤三、操作员根据反馈力的大小和方向并结合视觉信息控制四旋翼飞行器的运动以及机械手的抓取作业。

2.根据权利要求1所述的控制方法，其特征在于：步骤一得到分量A_x、A_y、A_z的方法为：周期性地采集四旋翼飞行器上的姿态传感器的原始姿态数据α_x、α_y、α_z、ω_x、ω_y、ω_z，使用数字低通滤波器消除高频干扰，通过卡尔曼滤波算法对四旋翼飞行器姿态传感器数据进行融合得重力加速度在载体坐标系下各轴的分量A_x、A_y、A_z。

3.根据权利要求1所述的控制方法，其特征在于：所述步骤二中转换的方法为：

根据分量A_x、A_y、A_z，得到俯仰角φ和横滚角

定义反馈力产生矩阵力源矩阵反馈力矩阵 

其中a,b,c为反馈力系数；

计算反馈力F：F＝TY。

4.根据权利要求1所述的控制方法，其特征在于：所述步骤三中控制四旋翼飞行器的方法为，通过改变操作末端坐标x,y,z的位置控制飞行器的飞行，具体是：

基于四旋翼的飞行控制原理得到四个旋翼产生的升力F₁、F₂、F₃、F₄：

其中K为力反馈手控器控制灵敏度系数，G为是飞行器重量，l为四旋翼重心到螺旋桨轴线的距离；

计算四旋翼各电机的电压量V₁、V₂、V₃、V₄：

其中k为变换系数。