CN102967305B - 基于大小回字标志物的多旋翼无人机位姿获取方法 - Google Patents

Abstract

本发明公开了一种基于大小回字标志物的多旋翼无人机位姿获取方法，首先根据无人机与标志物的相对高度来确定目标标志物，所述标志物为外边长长度为10倍关系的正方形大小回字；其次根据确定的目标标志物，建立无人机CCD摄像头与目标标志物之间的关系模型；最后根据建立的模型，利用直接线性变换法获取无人机的方位角和相应时刻无人机的位置信息。该方法可以精确的获取无人机的位姿。

Description

基于大小回字标志物的多旋翼无人机位姿获取方法

技术领域

本发明属于无人机技术领域，特别是基于大小回字标志物的多旋翼无人机位姿获取方法。

背景技术

近年来，随着地震、洪水、核污染等大型自然灾害频发，小型多旋翼无人机的研究逐渐成为无人机研究的热点。特别在小型多旋翼无人机自主导航及着陆方面，怎样更精确的得到小型多旋翼无人机的位姿信息显得格外重要。这几年应用最多的就是通过GPS和IMU（惯性导航设备）的组合来测无人机的位姿信息。但是GPS的应用有很大的局限性：只能在户外而且在有干扰信号的情况下检测的结果误差很大，导致无人机在自主飞行和着陆的时候不能精确定位，这就会造成无人机的损毁，并且IMU价格昂贵，影响经济效益。

机器视觉凭借可以获得更多的2维信息、拓展性强、可以避免GPS的的一些局限性，并且具有体积小、功耗低、价格便宜等优点，使其在无人机自主导航及着陆方便显得格外重要。此技术通过CCD摄像头获取图像，通过图像处理技术提取需要的信息，经过一定的转换及运算获得无人机的位姿信息。本发明提出了基于大小回字标志物的小型多旋翼无人机位姿估计方法，实现了小型多旋翼无人机精确自主着陆。

目前针对小型多旋翼无人机的位姿检测方面有：

公开号为CN101833761A，发明名称为基于合作目标特征线的无人机位姿估计方法的发明专利，该专利公开了一种通过无人机上的摄像头识别已知图片，提取特征线，通过灭影点和灭影线在像平面上的位置唯一的由该直线或平面的方向决定，它们与摄像机的空间姿态有着确定的关系来计算无人机的姿态信息。此方法虽然可测，但是当无人机在着陆到接近地平面的时候精确度大大降低。

公开号为CN101201248，名称为基于无人机的航空近景摄影位移测量系统及其方法的发明专利，该专利公开了一种通过在目标区域进行拍照，利用部分已知点坐标，对拍摄的照片按照近景摄影测量的的理论进行处理计算，得到目标区域未知点的3维坐标，将不同时期的照片所得到的3维坐标进行比较，就可以得到该点的位移，通过差分的方法进而得到目标点得表面位移。此方法要事先进行大量图片的获取，并且要进行处理分析，然后与实时获取的图片进行比对，工作量大、应用价值也不高。

在SpringLink中有一篇《Indoor UAV Control Using Multi-Camera VisualFeedback》中运用的是两个摄像头进行无人机位姿的估计的文章，虽然能测出位姿，但是这就会增加了无人机的负荷量，相对小一点的无人机就负载不了，有局限性,本发明专利采用单目摄像头小巧轻便大大减轻无人机的负载量。

发明内容

本发明所解决的技术问题在于提供一种位姿确定精度高的基于大小回字标志物的多旋翼无人机位姿获取方法。

实现本发明目的的技术解决方案为：一种基于大小回字标志物的多旋翼无人机位姿获取方法，包括以下步骤：

步骤一、根据无人机与标志物的相对高度来确定目标标志物，所述标志物为外边长长度为10倍关系的正方形大小回字；

步骤二、根据步骤一中确定的目标标志物，建立无人机CCD摄像头与目标标志物之间的关系模型；

步骤三、根据步骤二中建立的模型，利用直接线性变换法获取无人机的方位角和相应时刻无人机的位置信息。

进一步地优化方案，本发明基于大小回字标志物的多旋翼无人机位姿获取方法中，步骤一中确定目标标志物的方法具体为：

（1-1）、利用OPENCV图像处理工具寻找标志物大小回字的轮廓，再计算大小回字的轮廓面积；

（1-2）、判断无人机与标志物的相对高度，当该相对高度大于1米时，保留大回字轮廓面积，提取相应的角点坐标；否则，保留小回字轮廓面积，提取相应的角点坐标。

进一步地优化方案，本发明基于大小回字标志物的多旋翼无人机位姿获取方法中，

步骤二中，建立模型的方法如下：

（2-1）、构建无人机CCD摄像头坐标系o_cx_cy_cz_c、图像坐标系o_ux_uy_u和目标标志物的三维世界坐标系是o_wx_wy_wz_w，其中，o_cx_c//o_ux_u，o_cy_c//o_uy_u，o_cz_c⊥π_c；π_c是摄像头拍摄的图像平面，o_o是摄像机光轴与图像平面的交点坐标；

（2-2）、确定空间任意一点在目标标志物的三维世界坐标系与无人机CCD摄像头坐标系的关系模型如下：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=p_{\mathrm{cw}}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

其中，p_cw为旋转平移矩阵，R_3x3为无人机的方位角，T_3x1为相应时刻无人机的位置；

进一步地优化方案，本发明基于大小回字标志物的多旋翼无人机位姿获取方法中，

所述步骤三中，获取无人机的方位角和相应时刻无人机的位置信息，具体如下：

（3-1）、首先建立数字图像坐标系，再根据数字图像坐标系与无人机CCD摄像头坐标系之间存在 关系，建立从图像像素坐标系到世界坐标系的关系如下：

$h\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_o\\ 0& \frac{1}{d_y}& v_o\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

（3-2）、选择至少7组对应的角点坐标值结合建立的图像像素坐标系到世界坐标系的关系，利用直接线性变换算法获得多旋翼无人机相对于目标标志物的位姿信息即R_3x3和T_3x1。

本发明与现有技术相比具有以下显著的有点：1）采用摄像机避免了采用陀螺仪受噪声的影响，在零漂和温漂的作用下，误差随着时间和距离的增加会越积越大的问题。2）采用摄像机避免了GPS在一些地域信号很弱甚至不存在的问题，3）采用摄像机比惯性导航设备大大节约成本。4）采用单目摄像机比多目摄像机处理的信息量大大减少，速度更快。5）本发明采用大小两个回字的标志物，实现从识别大回字的初步位姿确定到最终识别小回字的精确位姿确定，使无人机实现精确着陆。

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

附图说明

图1小型多旋翼无人机及大小回字标志物的总体结构图。

图2大回字的轮廓提取图。

图3小回字的轮廓提取图。

图4无人机CCD摄像头与目标标志物之间透视投影模型。

具体实施方式

本发明一种基于大小回字标志物的多旋翼无人机位姿获取方法，包括以下步骤：

步骤一、根据无人机与标志物的相对高度来确定目标标志物，所述标志物为外边长长度为10倍关系的正方形大小回字，具体为：

（1-1）、利用OPENCV图像处理工具寻找标志物大小回字的轮廓，再计算大小回字的轮廓面积；

（1-2）、判断无人机与标志物的相对高度，当该相对高度大于1米时，保留大回字轮廓面积，提取相应的角点坐标；否则，保留小回字轮廓面积，提取相应的角点坐标。

步骤二、根据步骤一中确定的目标标志物，建立无人机CCD摄像头与目标标志物之间的关系模型，具体为：

（2-1）、构建无人机CCD摄像头坐标系o_cx_cy_cz_c、图像坐标系o_ux_uy_u和目标标志物的三维世界坐标系是o_wx_wy_wz_w，其中，o_cx_c//o_ux_u，o_cy_c//o_uy_u，o_cz_c⊥π_c；π_c是摄像头拍摄的图像平面，o_o是摄像机光轴与图像平面的交点坐标；

（2-2）、确定空间任意一点在目标标志物的三维世界坐标系与无人机CCD摄像头坐标系的关系模型如下：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=p_{\mathrm{cw}}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

其中，p_cw为旋转平移矩阵，R_3x3为无人机的方位角，T_3x1为相应时刻无人机的位置；

步骤三、根据步骤二中建立的模型，利用直接线性变换法获取无人机的方位

角和相应时刻无人机的位置信息，具体为：

（3-1）、首先建立数字图像坐标系，再根据数字图像坐标系与无人机CCD摄像头坐标系之间存在 关系，建立从图像像素坐标系到世界坐标系的关系如下：

$h\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_o\\ 0& \frac{1}{d_y}& v_o\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

（3-2）、选择至少7组对应的角点坐标值结合建立的图像像素坐标系到世界坐标系的关系，利用直接线性变换算法获得多旋翼无人机相对于目标标志物的位姿信息即R_3x3和T_3x1。

实施例

将本发明基于大小回字标志物的多旋翼无人机位姿获取方法应用到无人机通过该方法获取位姿准确着陆完成自动跟换电池系统中。

图1所示，小型多旋翼无人机包括：电机驱动部分、CCD摄像头、无线发射/接受装置、电池仓、主控板、起落架。地面包括：地面控制站、无线发射/接收装置、标有大小回字的标志物。

小型多旋翼无人机中间的主控板下方是电池仓，电池仓下方是CCD摄像头。主控板周边是电机驱动及无线发射/接收装置，整体通过起落架与地面接触。通过主控板控制CCD摄像头进行对地面的大小回字标志物进行图片采集分析计算，最终通过得到的位姿信息控制电机旋转，从而实现自主着陆。在此过程中通过无线发射/接收装置进行飞机姿态数据的实时显示。

为实现小型多旋翼无人机从初定位到最终的精确定位所采用的标志物板子是大回字为外边长为70厘米的正方形，小回字为外边长为7厘米的正方形。

结合图2、图3小型多旋翼无人机在距离标志物1米以上及1米以内的图像处理结果图。

当无人机距离标志物在1米以上时，首先通过实时性和处理效果相对比较好的Sobel算子进行边界的提取,然后用OPENCVOPENCV自带的函数cvFindContours先寻找轮廓，再通过cvContourArea计算轮廓面积。保留轮廓大的轮廓面积，同时提取相应的角点信息坐标，用于后面的位姿计算。

当无人机距离标志物在1米以内时，首先通过实时性和处理效果相对比较好的Sobel算子进行边界的提取,然后用OPENCVOPENCV自带的函数cvFindContours先寻找轮廓，再通过cvContourArea计算轮廓面积。保留轮廓小的轮廓面积，同时提取相应的角点信息坐标，用于后面的位姿计算。

结合图4CCD摄像头与标志物之间透视投影模图。

定义π_w为地面着陆标志物板的平面，π_c是摄像头拍摄的图像平面。在这里本研究定义标志物的三维世界坐标系是o_wx_wy_wz_w，对应的摄像头坐标系是o_cx_cy_cz_c，图像坐标系是o_ux_uy_u，并且o_cx_c//o_ux_u，o_cy_c//o_uy_u，o_cz_c⊥π_c；o_o是摄像机光轴与图像平面的交点坐标，p_i，w为图像上任意一点在世界坐标系里面的坐标，p_i，u是图像坐标系里面的任意一点。在某一时刻摄像头相对于标志物的运动用3×3单位正交矩阵R代表小型多旋翼无人机的方位角，里面有相对于x轴的翻滚角，相对于y轴的俯仰角和相对于z轴的偏航角，通过实验发现只有偏航角比较准确，所以这边只采用相对z轴的偏航角。然后用3×1的矩阵T代表某个时刻小型多旋翼无人机的位置。由此可以建立空间一点在世界坐标系和摄像机坐标系中的关系如下：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=p_{\mathrm{cw}}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(1\right)$

图像坐标系没有用物理单位表示出像素在图像中的物理位置，因此要建立数字图像坐标系，下面是两个坐标系之间的关系：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\left[\begin{array}{ccc}1/d_x& 0& u_o\\ 0& 1/d_y& v_o\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]---\left(2\right)$

再根据数字图像坐标系与摄像头坐标系之间存在 最终可以建立从图像像素坐标系到世界坐标系的关系如下：

$h\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_o\\ 0& \frac{1}{d_y}& v_o\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(3\right)$

f表示摄像头的有效焦距；h和z_c是相等的，是摄像头到物体的距离,d_x和d_y分别表示每个像素在x轴方向和y轴方向上的物理尺寸，本研究把它们作为单位1毫米处理。（u_o，v_o）是标志物在数字图像下的0点坐标。由以上公式可以看出，为了让小型多旋翼无人机更好的实现自主导航，找到着陆点，就要求能够尽可能精确的求得R和T。

将（1）式展开可以得到:

$\left.\begin{array}{c}{x}_c=R_{11}{x}_w+R_{12}{y}_w+R_{13}{z}_w+T_x\\ y_c=R_{21}{x}_w+R_{22}{y}_w+R_{23}{z}_w+T_y\\ z_c=R_{31}{x}_w+R_{32}{y}_w+R_{33}{z}_w+T_z\end{array}\right\}---\left(4\right)$

根据径向约束可以得到：

$\frac{x}{y}=\frac{x_c}{y_c}=\frac{R_{11}{x}_w+r_{12}{y}_w+R_{13}{z}_w+T_x}{R_{21}{x}_w+R_{22}{y}_w+R_{23}{z}_w+T_y}---\left(5\right)$

通过将上式与式（2）结合起来，由至少7组对应点可以求得一组解（R₁₁,R₁₂,R₁₃,R₂₁,R₂₂,R₂₃,T_x,T_y)再根据（R₃₁,R₃₂,R₃₃)=(R₁₁,R₁₂,R₁₃)×(R₂₁,R₂₂,R₂₃)求得（R₃₁，R₃₂，R₃₃）接下来就是求取T_z,和f，根据（3）式可以得到以下等式：

$\left.\begin{array}{c}(u-u_o)(R_{31}{x}_w+R_{32}{y}_w+R_{33}{z}_w+T_z)=f(R_{11}{x}_w+R_{12}{y}_w+R_{13}{z}_w+T_x)\\ (v-v_o)(R_{31}{x}_w+R_{32}{y}_w+R_{33}{z}_w+T_z)=f(R_{21}{x}_w+R_{22}{y}_w+R_{23}{z}_w+T_y)\end{array}\right\}---\left(6\right)$

将以上求取得值代入等式可以求得T_z和f。通过以上改进的直接线性变换（DLT）算法可以求得小型多旋翼无人机相对于着陆标志物的位姿信息即以上求得的R和T。最终能够实现小型多旋翼无人机的准确着陆。

Claims (4)

1.一种基于大小回字标志物的多旋翼无人机位姿获取方法，其特征在于，包括以下步骤：

步骤一、根据无人机与标志物的相对高度来确定目标标志物，所述标志物为外边长长度为10倍关系的正方形大小回字,确定目标标志物的方法如下：

A、利用OPENCV图像处理工具寻找标志物大小回字的轮廓，再计算大小回字的轮廓面积；

B、判断无人机与标志物的相对高度，当该相对高度大于1米时，保留大回字轮廓面积，提取相应的角点坐标；否则，保留小回字轮廓面积，提取相应的角点坐标；

步骤二、根据步骤一中确定的目标标志物，建立无人机CCD摄像头与目标标志物之间的关系模型；

步骤三、根据步骤二中建立的模型，利用直接线性变换法获取无人机的方位角和相应时刻无人机的位置信息。

2.根据权利要求1所述的基于大小回字标志物的多旋翼无人机位姿获取方法，其特征在于，所述步骤一中，大回字为外边长为70厘米的正方形，小回字为外边长为7厘米的正方形。

3.根据权利要求1所述的基于大小回字标志物的多旋翼无人机位姿获取方法，其特征在于，所述步骤二中，建立模型的方法如下：

(2-1)、构建无人机CCD摄像头坐标系o_cx_cy_cz_c、图像坐标系o_ux_uy_u和目标标志物的三维世界坐标系o_wx_wy_wz_w，其中，o_cx_c//o_ux_u，o_cy_c//o_uy_u，o_cz_c⊥π_c；π_c是摄像头拍摄的图像平面，o_o是摄像机光轴与图像平面的交点坐标；

(2-2)、确定空间任意一点在目标标志物的三维世界坐标系与无人机CCD摄像头坐标系的关系模型如下：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=p_{\mathrm{cw}}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

其中，p_cw为旋转平移矩阵，R_3×3为无人机的方位角，T_3×1为相应时刻无人机的位置。

4.根据权利要求1所述的基于大小回字标志物的多旋翼无人机位姿获取方法，其特征在于：所述步骤三中，获取无人机的方位角和相应时刻无人机的位置信息，具体如下：

A、首先建立数字图像坐标系，再根据数字图像坐标系与无人机CCD摄像头坐标系之间存在关系，f为摄像头的有效焦距，建立从图像像素坐标系到世界坐标系的关系如下：

$h\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_o\\ 0& \frac{1}{d_y}& v_o\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×3}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

h是摄像头到目标标志物的距离，(u、v)是目标标志物在数字图像下的非零点坐标，(u_o，v_o)是目标标志物在数字图像下的0点坐标，d_x和d_y分别表示每个像素在x轴方向和y轴方向上的物理尺寸；

B、选择至少7组对应的角点坐标值结合建立的图像像素坐标系到世界坐标系的关系，利用直接线性变换算法获得多旋翼无人机相对于目标标志物的位姿信息即为R_3×3和T_3×1。