CN114035598B - 一种多旋翼吊挂系统的视觉摆角检测与减摆方法 - Google Patents

Abstract

本发明公开了一种多旋翼吊挂系统的视觉摆角检测与减摆方法，包括如下步骤：搭建多旋翼吊挂系统的视觉摆角检测装置，在负载上固定标识物，在多旋翼底板下方固定双目相机；建立多旋翼吊挂系统的动力学模型与负载摆角计算公式；利用双目相机获取标识物在相机坐标系下的三维坐标，并将其变换到负载坐标系中，利用建立的动力学模型与负载摆角计算公式，计算得到负载的摆角；将得到的负载摆角输入前馈控制器，对位置控制器输出的控制信号进行补偿，利用姿态控制器调整多旋翼的姿态，减少负载的摆动。本发明所公开的方法能够有效在室外环境进行负载的摆角检测，进一步实现多旋翼飞行或定点投放时负载的摆动抑制，具有更好的效率和实时性。

Description

一种多旋翼吊挂系统的视觉摆角检测与减摆方法

技术领域

本发明涉及一种双目视觉摆角检测和多旋翼控制方法，特别涉及一种多旋翼吊挂系统的视觉摆角检测与减摆方法。

背景技术

多旋翼吊挂系统是在多旋翼上安装一个缆绳收放装置，利用多旋翼吊挂负载以实现货物的运输。由于多旋翼无人机具有结构简单、体型较小、灵活性高且能实现自主飞行等优点，因此，在紧急救援和快递运送等场合中，多旋翼吊挂系统能够更加快捷高效的完成任务。

多旋翼吊挂系统实现货物的平稳运输和投放面临的一个重要的问题是对负载摆动的抑制，研究者们提出了几种不同的解决方案，总体上可以分为两类：主动控制和被动控制。主动控制是提前分析目标出现偏差的可能性，采取相应的防护性措施，使目标得以实现，常见的有最优控制和输入整形技术等，最优控制对模型建立的精准度较为依赖，通过精准的模型计算出轨迹，而输入整形技术对模型建立的不确定具有一定的鲁棒性。被动控制是通过对系统进行实时监控，将系统输出得到的速度、加速度信息进行处理，反馈传送给控制器的输入端，控制与输入端的偏差，以实现目标，最常见的有PID控制和自适应控制等。其他减摆控制有动态规划算法，强化学习算法和迭代LGQ控制等。

以上实现减摆的方法中，均需对负载的摆动进行实时检测，将摆角测量值反馈到控制器中形成闭环，较多的实现方式是在室内利用光学运动捕捉系统，如果在室外的真实运输场景中，这种摆角测量方式会有较大的局限性。

发明内容

为解决上述技术问题，本发明提供了一种多旋翼吊挂系统的视觉摆角检测与减摆方法，利用双目相机检测负载的摆角，并利用前馈控制器实现多旋翼的姿态控制补偿，从而实现多旋翼飞行或定点投放时减少吊挂系统负载摆动的目的。

为达到上述目的，本发明的技术方案如下：

一种多旋翼吊挂系统的视觉摆角检测与减摆方法，包括如下步骤：

步骤一，搭建多旋翼吊挂系统的视觉摆角检测装置，多旋翼与负载之间通过缆绳连接形成多旋翼吊挂系统，在负载上固定标识物，该标识物位于负载和缆绳之间，在多旋翼底板下方固定双目相机，双目相机的拍摄方向朝向所述标识物；

步骤二，建立多旋翼吊挂系统的动力学模型与负载摆角计算公式，得到负载的摆角、负载在负载坐标系下的三维坐标、负载与多旋翼之间缆绳长三者之间的关系；

步骤三，在多旋翼飞行过程中，利用安装在多旋翼底板上的双目相机，获取标识物在相机坐标系下的三维坐标，并将其变换到负载坐标系中，该标识物的坐标视为负载的坐标，然后结合绳长，利用建立的动力学模型与负载摆角计算公式，计算得到负载的摆角；

步骤四，将得到的负载摆角输入前馈控制器，对位置控制器输出的控制信号进行补偿，利用姿态控制器调整多旋翼的姿态，减少多旋翼飞行或定点投放时负载的摆动。

上述方案中，所述标识物为单一色彩的轻质球体。

上述方案中，步骤二得到的三者之间的关系如下：

其中，摆角α是负载围绕负载坐标系中的Y_H轴旋转，在负载坐标系O_HX_HZ_H平面内的旋转角；摆角β是负载围绕负载坐标系中的X_H轴旋转，在负载坐标系O_HY_HZ_H平面内的旋转角，L为负载与多旋翼之间缆绳长。

上述方案中，步骤三中，利用双目相机获取标识物在相机坐标系下的三维坐标的方法如下：

(1)首先，标定相机，获取相机的内外参数；

(2)利用双目相机获取图像帧，将图像帧进行HSV色彩空间变换；

(3)利用变换后的图像，通过设置HSV阈值检测标识物，并通过霍夫圆检测获取标识物圆心的像素坐标；

(4)根据相机的内外参数以及标识物圆心的像素坐标，计算标识物在相机坐标系下的三维坐标。

上述方案中，步骤三中，将负载在相机坐标系下的三维坐标变换到负载坐标系下的变换公式如下：

式中，为相机坐标系到负载坐标系的旋转矩阵，T为相机坐标系原点到负载坐标系原点的平移向量，P_C为相机坐标系中的一点，P_P ^H为点P_C变换到负载坐标系下的点。

上述方案中，步骤四中，前馈控制器对摆角的补偿信号计算如下：

其中，u_α为前馈控制器对摆角α的补偿信号，u_β为前馈控制器对摆角β的补偿信号，k_α为前馈控制器对摆角α的增益，k_β为前馈控制器对摆角β的增益，α_d为摆角α的期望值，β_d为摆角β的期望值，α_r为摆角α的视觉检测值，β_r为摆角β的视觉检测值。

通过上述技术方案，本发明提供的一种多旋翼吊挂系统的视觉摆角检测与减摆方法，具有如下有益效果：

1、本发明通过对多旋翼吊挂系统的数学模型进行分析，得到了负载摆角与其在负载坐标系下三维位置的理论关系，从理论上证明了在室外环境下用双目视觉实现摆角检测具有可行性。

2、本发明利用安装在多旋翼底板上的双目相机检测标识物圆心的位置，进一步计算出摆角的大小，避免对负载位置的直接检测，从而避免因负载形状，颜色等差异在定位时带来的弊端；将检测出的标识物的圆心作为特征点，避免由特征点匹配和深度图的计算造成的计算资源浪费，具有更高的效率和更好的实时性。

3、针对负载的摆动问题，本发明结合多旋翼自身特点与负载摆角的动力学模型，设计了一种带有前馈控制器的抑制负载摆动的复合控制方案，前馈控制器根据实时检测到的摆角值，及时对位置控制器输出的控制信号进行补偿，通过姿态控制器快速调整多旋翼姿态，实现多旋翼飞行或定点投放时减少吊挂负载摆动的目的。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为本发明实施例所公开的减摆控制方法流程示意图；

图2为本发明实施例所公开的四旋翼吊挂系统示意图；

图3为针孔模型示意图；

图4为像素坐标系与物理成像平面示意图；

图5为双目相机成像模型示意图；

图6为双目相机几何模型示意图；

图7为双目相机安装示意图；

图8为摆角检测流程图；

图9为减摆控制方案原理图；

图10为标识物目标检测结果图像；

图11为标识物围绕x轴摆动的测量结果；

图12为标识物围绕y轴摆动的测量结果；

图13为α角的测量误差；

图14为β角的测量误差；

图15四旋翼位置和负载摆角仿真结果(无前馈控制器)；

(a)四旋翼在惯性坐标系中x轴方向上的运动轨迹(无前馈控制器)；

(b)四旋翼在惯性坐标系中y轴方向上的运动轨迹(无前馈控制器)；

(c)四旋翼在惯性坐标系中z轴方向上的运动轨迹(无前馈控制器)；

(d)负载摆角α的摆动情况(无前馈控制器)；

(e)负载摆角β的摆动情况(无前馈控制器)；

图16为四旋翼位置和负载摆角仿真结果(有前馈控制器)；

(d)四旋翼在惯性坐标系中x轴方向上的运动轨迹(有前馈控制器)；

(e)四旋翼在惯性坐标系中y轴方向上的运动轨迹(有前馈控制器)；

(f)四旋翼在惯性坐标系中z轴方向上的运动轨迹(有前馈控制器)；

(d)负载摆角α的摆动情况(有前馈控制器)；

(e)负载摆角β的摆动情况(有前馈控制器)；

图17为四旋翼和负载在三维空间的运动轨迹(无前馈控制器)；

图18四旋翼和负载在三维空间的运动轨迹(有前馈控制器)。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本发明提供了一种多旋翼吊挂系统的视觉摆角检测与减摆方法，如图1所示，具体实施例如下：

步骤一，搭建多旋翼吊挂系统的视觉摆角检测装置，如图2所示，多旋翼与负载之间通过缆绳连接形成多旋翼吊挂系统，在负载上固定标识物，该标识物位于负载和缆绳之间，在多旋翼底板下方固定双目相机，双目相机的拍摄方向朝向标识物。该标识物为色彩单一的轻质球体，便于识别。

步骤二，建立多旋翼吊挂系统的动力学模型与负载摆角计算公式，得到负载的摆角、负载在负载坐标系下的三维坐标、负载与多旋翼之间缆绳长三者之间的关系；

为能够清晰地描述多旋翼和负载的运动特性，定义如下的三个参考坐标系：惯性坐标系{I}、机体坐标系{B}和负载坐标系{H}。其中，机体坐标系和负载坐标系的原点位于多旋翼的几何中心，机体坐标系的XY平面与多旋翼机身平面重合，负载坐标系的坐标轴方向始终平行于惯性坐标系的坐标轴。{φ,θ,ψ}分别为多旋翼的滚转角、俯仰角和偏航角。摆角α是负载围绕负载坐标系中的Y_H轴旋转，在负载坐标系O_HX_HZ_H平面内的旋转角，摆角β是负载围绕负载坐标系中的X_H轴旋转，在负载坐标系O_HY_HZ_H平面内的旋转角。

多旋翼的刚体动力学模型包括位置动力学模型和姿态动力学模型，其欧拉角模型可以表示为：

式中，^Ip为多旋翼重心向量，^Iv表示多旋翼的速度，g表示重力加速度，e₃是惯性坐标系下沿O_IZ_I轴的单位向量，R为从机体坐标系到惯性坐标系的旋转矩阵，Θ表示机体姿态角，^Bω表示机体旋转角速度，J为多旋翼刚体转动惯量，G_a表示陀螺力矩，τ表示螺旋桨在机体轴上产生的力矩，W是表征姿态变化率与机体角速度关系的矩阵，F为多旋翼的升力，M为多旋翼的质量。

当吊挂物处于自然下垂状态时，标识物在负载坐标系{H}中的初始位置表示为：

式中，L表示负载与多旋翼之间的缆绳长，缆绳长度可通过收放装置均匀变化。

当吊挂物摆动时，标识物在负载坐标系中的位置为：

其中，R_P为旋转矩阵，具体形式为：

式中，c_i＝cos(i)，s_i＝sin(i)。

结合式(3)和(4)，可得负载摆动后的位置为：

假设多旋翼在惯性坐标系中的位置表示为则负载在惯性坐标系中的坐标为

上式的二阶导数为：

若吊挂负载的缆绳上的拉力为f_c，则拉力在惯性坐标系沿坐标轴方向上的分力矢量为R_P[0 0 -f_c]^T，根据牛顿第二定律，负载在各轴上的加速度和缆绳的拉力存在如下关系：

其中，m为负载质量。

令(8)×c_α-(10)×s_α，并整理得

同理，令(9)×c_αc_β+(10)×s_β，整理可得：

式(1),(11)和(12)构成了多旋翼吊挂系统的动力学模型。

根据式(5)可解得摆角和负载在负载坐标系下存在如下关系：

由式(13)可知，只要求得负载坐标系中，标识物的三维坐标和L的长度，即可求得空间中负载的摆角α与β值。

步骤三，在多旋翼飞行过程中，利用安装在多旋翼底板上的双目相机，获取设置于负载与缆绳之间的标识物在相机坐标系下的三维坐标，并将其变换到负载坐标系中，该标识物的坐标视为负载的坐标，然后结合绳长，利用建立的动力学模型与负载摆角计算公式，计算得到负载的摆角。

双目立体视觉是利用两个摄像机从不同的位置拍摄同一物体，在两张图像中的成像位置不同，通过立体匹配算法将两张图像中的像素点对应起来，由此获取对应像素间的视差；再根据三角测量原理恢复出该像素点的深度信息。

三维世界中的坐标点通过相机映射到二维图像平面的过程能够用一个几何模型描述，最为常用和有效的是针孔模型，其成像原理图如图3所示。

图中，P_c为相机坐标系中的一点，p为其在图像平面上的投影，根据三角形相似关系，有

由于在数码相机中，物理成像平面是一个具有一定尺寸的光敏单元网格，它直接与图像的像素相对应，如图4所示，像素平面中，像素坐标是一个非负整数的二维向量，按照惯例，像素坐标系原点在图像平面的左上角。

像素坐标与物理成像平面坐标的关系为

u＝γx+u₀,v＝ρy+v₀ (15)

γ和ρ是像素坐标系在u和v轴上的缩放倍数。

将式(14)带入(15)中，得

其中，f_x＝γf,f_y＝ρf。

因此，图像中的某一个像素可通过下式转化到相机坐标系下，即

由于三维空间点通过单个相机成像时，其深度信息丢失，因此仅根据一个像素无法确定该空间点的具体位置。解决方案是，通过双目立体视觉测量出某空间点的深度，再利用式(17)即可获取某像素点对应的空间位置。

双目相机通常是由左眼相机和右眼相机两个水平放置的相机组成，其成像模型如图5所示。在双目相机中，左右两个相机都可以看作水平放置的针孔相机，意味着它们的光圈中心都位于x轴上，两者之间的距离称为双目相机的基线，记作b。

其几何模型如图6所示，O_L,O_R是左右相机的光圈中心，f为焦距。u_L和u_R为成像平面的坐标。P是一个空间点，它在左右相机中各成一像，分别记为P_L和P_R。z是空间点到光圈中心的深度。根据三角形相似关系，有如下关系：

定义视差d为左右像素横坐标之差，即d＝u_L-u_R，重新整理上式可得

利用双目相机获取标识物在相机坐标系下的三维坐标的方法如下：

(1)首先，标定相机，获取相机的内外参数；

(2)利用双目相机获取图像帧，将图像帧进行HSV色彩空间变换；

(3)利用变换后的图像，通过设置HSV阈值检测标识物，并通过霍夫圆检测获取标识物圆心的像素坐标；

(4)根据相机的内外参数以及标识物圆心的像素坐标，计算标识物在相机坐标系下的三维坐标。

如图7所示，将双目相机安装在多旋翼的底板上，相机坐标系位于左相机光心处。由于相机的位姿随着多旋翼机体的变化而变化，可以认为相机坐标系与多旋翼机体坐标系姿态一致。因此，在相机坐标系中测出的标识物的三维坐标需要经过平移和旋转，变换到负载坐标系中，变换关系如式(20)所示

式中，为相机坐标系到负载坐标系的旋转矩阵，T为相机坐标系原点到负载坐标系原点的平移向量，P_C为相机坐标系中的一点，P_P ^H为点P_C变换到负载坐标系下的点。

由于立体匹配算法需要较大的计算量获取图像中每个像素点的在另一幅图像中的对应像素位置，从而获取每个像素点的深度。但是，在本发明的测量方案中，仅有标识物的深度信息才作为有效信息，图像场景中其他物体的深度信息不作为研究对象，因此，无需浪费计算资源计算全局图像的深度信息。为了减少计算量，提高实时性，本发明将标识物的图像中投影的圆心作为特征点，获取该特征点的三维位置坐标并计量绳长L，利用公式(13)，即可实现摆角测量，具体实现流程如图8所示。

步骤四，将得到的负载摆角输入前馈控制器，对位置控制器输出的控制信号进行补偿，利用姿态控制器调整多旋翼的姿态，实现减少多旋翼飞行或定点投放时负载的摆动的目的。

根据式(11)和(12)可知，在多旋翼吊挂系统中负载的摆动受到多旋翼加速度的直接影响。由于多旋翼和负载之间采用的是软连接方式，因此无法对负载的摆动进行直接有效的控制，而只能通过调整多旋翼的姿态间接实现负载的减摆。

由于多旋翼系统具有非线性，强耦合的特点,目前串级控制是单一多旋翼系统的常见控制方案，即位置控制器和姿态控制器内外两环来完成期望位置的飞行(在开源自驾仪中，内外两环控制通常选用PID控制来完成)。

结合多旋翼自身特点与吊挂负载摆角的动力学模型，设计如图9所示的复合控制抑制负载摆角(减摆)的控制方案。在该控制方案中，摆角的检测通过双目相机检测装置完成，前馈控制器利用检测到的摆角值输出相应的补偿控制信号，该信号对外环位置控制的输出进行补偿，共同调节多旋翼的姿态。从系统的数学模型可知，摆角的摆动是受到多旋翼运动加速度的直接激励，因此，前馈控制器的思想就是根据负载的摆动状态，不断调整多旋翼姿态从而调整其加速度的过程。当摆角较大时，前馈控制器对多旋翼姿态控制进行补偿，适当减小多旋翼的加速度，在不断的动态调整过程中，既能实现多旋翼运送负载到达目标位置，又能实现在运动过程中对摆角的抑制作用。这种控制方案在适当牺牲多旋翼姿态调整效率的情况下，实现即能到达期望位置的同时，也能减少吊挂系统负载的摆动。

前馈控制器对摆角的补偿信号计算如下：

其中，u_α为前馈控制器对摆角α的补偿信号，u_β为前馈控制器对摆角β的补偿信号，k_α为前馈控制器对摆角α的增益，k_β为前馈控制器对摆角β的增益，α_d为摆角α的期望值，β_d为摆角β的期望值，α_r为摆角α的视觉检测值，β_r为摆角β的视觉检测值。

仿真与实验

1、摆角检测仿真

Gazebo是ROS机器人操作系统中的一个功能强大的三维物理仿真平台，支持多种传感器数据的仿真。本发明利用ROS/Gazebo平台搭建了四旋翼吊挂系统，在四旋翼底板上加入双目相机模块用于模拟仿真本发明所提摆角测量方案。机架臂的对称轴为四旋翼的机头方向，小球代表标识物(负载已省略)。

模拟的双目相机的参数如表1所示。

表1模拟双目相机参数表

将左右相机中的图像用Opencv分别进行HSV颜色空间变换，根据标识物的颜色特征，将标识物提取出来，再利用霍夫圆检测算法，检测与计算出标识物的圆心位置。标识物的检测结果如图10所示，(a)和(b)分别是左右相机中拍摄的RGB源图像，(c)和(d)是经过HSV变换后的目标检测图像，(e)和(f)是霍夫圆检测后的图像。进一步可利用双目视觉的深度检测获得标识物在相机坐标系中的坐标，并利用公式(20)获得标识物球心在负载坐标系中的坐标。

为了验证所提四旋翼吊挂系统摆角测量方案的有效性，在Gazebo仿真系统中分别让吊挂物围绕x轴和y轴按照给定的信号摆动，给定的摆动信号均为v(t)＝0.3sin(t)。仿真结果如图11和图12所示。

根据图11和图12中的仿真结果可看出，所提的摆角测量方案能够有效的检测出摆角值的大小，测量值与真实值具有较高得吻合性。从图13和图14的测量误差分析结果可知，α角的最大测量误差为-0.024rad，平均误差为-0.0059rad。β角的最大测量误差为0.068rad，平均误差为0.0037rad。根据算法原理分析可知，误差来源主要有图像处理误差、相机坐标系与负载坐标系的测量误差、以及相机标定时的误差等。根据实际飞行过程中的摆动情况，该误差值可以满足视觉测量的常规精度要求。

2、减摆控制方案仿真

在MATLAB/Simulink中建立了四旋翼吊挂系统的数学模型，验证了所提出的减摆复合控制方案。假设四旋翼悬挂系统的飞行轨迹为

z_d(t)＝3,0≤t

仿真结果如图15～18所示。图15显示了没有减摆前馈控制器时的四旋翼飞行器的运动轨迹，图16所示带有减摆前馈控制器的四旋翼悬挂系统的运动轨迹。如图17、图18所示为它们在三维空间中的运动轨迹。从仿真结果中可得，没有前馈控制器时，四旋翼的运动轨迹更加平滑，但是负载的摆动较为严重，负载的运动轨迹波动较大，到达目标的后的摆动收敛速度较慢。而采用前馈控制器时，四旋翼的运动轨迹会有轻微的震荡，较之前相比，平滑度下降，这正是前馈控制器不断调节四旋翼姿态的结果，但是负载的摆动有了明显的改善，摆动收敛的速度也有了较大的提升。仿真结果符合预期，验证了所提方案的有效性和可行性。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (3)

1.一种多旋翼吊挂系统的视觉摆角检测与减摆方法，其特征在于，包括如下步骤：

步骤一，搭建多旋翼吊挂系统的视觉摆角检测装置，多旋翼与负载之间通过缆绳连接形成多旋翼吊挂系统，在负载上固定标识物，该标识物位于负载和缆绳之间，在多旋翼底板下方固定双目相机，双目相机的拍摄方向朝向所述标识物；

步骤二，建立多旋翼吊挂系统的动力学模型与负载摆角计算公式，得到负载的摆角、负载在负载坐标系下的三维坐标、负载与多旋翼之间缆绳长三者之间的关系；

三者之间的关系如下：

其中，摆角α是负载围绕负载坐标系中的Y_H轴旋转，在负载坐标系O_HX_HZ_H平面内的旋转角；摆角β是负载围绕负载坐标系中的X_H轴旋转，在负载坐标系O_HY_HZ_H平面内的旋转角，L为负载与多旋翼之间的缆绳长；

步骤三，在多旋翼飞行过程中，利用双目相机获取标识物在相机坐标系下的三维坐标，并将其变换到负载坐标系中，该标识物的坐标视为负载的坐标，然后结合绳长，利用建立的动力学模型与负载摆角计算公式，计算得到负载的摆角；

利用双目相机获取标识物在相机坐标系下的三维坐标的方法如下：

(1)首先，标定相机，获取相机的内外参数；

(2)利用双目相机获取图像帧，将图像帧进行HSV色彩空间变换；

(3)利用变换后的图像，通过设置HSV阈值检测标识物，并通过霍夫圆检测获取标识物圆心的像素坐标；

(4)根据相机的内外参数以及标识物圆心的像素坐标，计算标识物在相机坐标系下的三维坐标；

将负载在相机坐标系下的三维坐标变换到负载坐标系下的变换公式如下：

式中，为相机坐标系到负载坐标系的旋转矩阵，T为相机坐标系原点到负载坐标系原点的平移向量，P_C为相机坐标系中的一点，P_P ^H为点P_C变换到负载坐标系下的点；

将标识物的图像中投影的圆心作为特征点，获取该特征点的三维位置坐标并计量绳长L，利用步骤二得到的公式，即可实现摆角α与β的测量；

步骤四，将得到的负载摆角输入前馈控制器，对位置控制器输出的控制信号进行补偿，利用姿态控制器调整多旋翼的姿态，减少多旋翼飞行或定点投放时负载的摆动。

2.根据权利要求1所述的一种多旋翼吊挂系统的视觉摆角检测与减摆方法，其特征在于，所述标识物为单一色彩的轻质球体。

3.根据权利要求1所述的一种多旋翼吊挂系统的视觉摆角检测与减摆方法，其特征在于，步骤四中，前馈控制器对摆角的补偿信号计算如下：

其中，u_α为前馈控制器对摆角α的补偿信号，u_β为前馈控制器对摆角β的补偿信号，k_α为前馈控制器对摆角α的增益，k_β为前馈控制器对摆角β的增益，α_d为摆角α的期望值，β_d为摆角β的期望值，α_r为摆角α的视觉检测值，β_r为摆角β的视觉检测值。