CN103453875A - 一种用于无人机俯仰角与滚转角实时计算方法 - Google Patents

Abstract

本发明公开了一种用于无人机俯仰角与滚转角实时计算方法，首先建立大地坐标系、机体坐标系和图像坐标系，利用机载计算机计算摄像机采集到图像的光流场，根据天空光流场速度较小和地面光流场速度较大这一特点，提取出天地分割线在图像坐标系中的位置信息，之后计算天地分割线在图像坐标系中的斜率和截距，根据摄像机投影模型建立大地坐标系和图像坐标系之间的对应关系，并由此推导出机体坐标系与图像坐标系之间的对应关系，即无人机的滚转角及俯仰角与天地分割线斜率与截距之间的对应关系，进而计算出无人机实时的滚转角及俯仰角信息。本发明采用的设备少且简单，计算结果易于实现。

Description

一种用于无人机俯仰角与滚转角实时计算方法

技术领域

发明涉及一种用于无人机俯仰角与滚转角的实时计算方法，属于无人机控制技术领域。

背景技术

目前，公知的无人机俯仰角和滚转角的实时计算方法都是使用惯性测量组件、GPS等***来完成的，有部分计算方法使用两个摄像机来探测特征物体或特征点，从而计算无人机的俯仰角和滚转角。但是，上述方法使用的仪器设备重量较大且***结构复杂，不便于无人机使用。

发明内容

有鉴于此，本发明提供了一种用于无人机俯仰角与滚转角实时计算方法，仅需要一个摄像机，就可以实时计算无人机俯仰角和滚转角信息。

一种用于无人机俯仰角与滚转角实时计算方法，坐标预定义：建立机体坐标系OXYZ，其中原点O位于摄像机光心处，OZ与光轴平行且指向机头，OY垂直指向地心；建立图像坐标系O₀uv，定义无人机上安装的摄像机焦平面左上顶点为O_o，u和v分别是焦平面水平和垂直的两个方向；

第一步：建立搜索天地分割线的指标J，

J＝D(u_sky)+D(v_sky)+D(u_grd)+D(v_grd)

其中D(u_sky)表示天空区域光流场u方向上的方差，D(v_sky)表示天空区域光流场v方向上的方差，D(u_grd)表示大地区域光流场u方向上的方差，D(v_grd)表示大地区域光流场v方向上的方差；

第二步：利用安装在无人机上的摄像机采集图像，并获得图像的光流场，基于指标J提取出天地分割线在图像坐标系O₀uv中的直线方程；

该步骤的具体过程为：

首先，在图像坐标系O₀uv中，使用一根直线切割摄像机采集的图像，并将直线两侧区域分别定义为天空区域和大地区域；其次，分别计算直线两侧区域光流场在u和v方向上的方差，将这些方差求和得到指标J；再次，将这条直线在图像上移动直至遍历整个图像区域，将指标J取值最小时所对应的直线定义为天地分割线，提取出天地分割线在图像坐标系O₀uv中的直线方程v＝ku+b，其中k是直线斜率和b是直线截距；

第三步：将天地分割线的直线方程v＝ku+b中的k和b代入机体坐标系与图像坐标系之间的对应关系表达式，计算出横滚角φ和俯仰角θ，

$\left\{\begin{array}{c}\mathrm{\φ}=\arctan \left(k\frac{{\mathrm{\α}}_x}{{\mathrm{\α}}_y}\right)\\ \mathrm{\θ}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\φ}}{{\mathrm{\α}}_y}\right]\end{array}\right.$

其中α_x和α_y分别是从成像平面到图像平面在x方向和y方向的放大系数，u₀和v₀分别为图像的长和宽。

有益效果：

1、本发明仅需要使用一个摄像机，不需要其他冗余设备，使计算设备结构简单，容易实现。

2、本发明对天地分割线的提取采用计算光流的方法，即计算摄像机采集的包含天地分割线的图像信息的光流场，并通过天空背景光流场稀疏和大地光流场稠密的这一特点，划分天地分割线，与传统滚转角与俯仰角计算方法，特别是使用天地面积比获取俯仰角的方法相比，本方法获得的结果精度较高。

3、本发明在获得摄像机内参数等先验知识的情况下，使用摄像机内参数模型来计算飞行器的俯仰角和滚转角，充分利用了现有条件，使算法简化，节约了设备成本。

附图说明

图1为本发明摄像机与无人机的安装位置示意图；

图2为本发明建立的三个坐标系的示意图。

图3为本发明中提取天地分割线的算法流程图。

其中，1.无人机，2.摄像机，3.大地，4.天地分割线，5.大地坐标系，6.机体坐标系，7.图像坐标系。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种用于无人机俯仰角与滚转角实时计算方法，如附图1所示，摄像机2与无人机1固连，安装在其头部；建立大地坐标系5、机体坐标系6和图像坐标系7，三个坐标系均为右手坐标系；如附图2所示，O_WX_WY_WZ_W是大地坐标系5，O_W位于无穷远处，O_WY_W竖直向上，O_WZ_W与机体纵轴平行；OXYZ是机体坐标系6，其中O位于摄像机光心处，OZ与光轴平行且指向机头，OY垂直指向地心；O₀uv是成像平面的图像坐标系7，定义焦平面左上顶点为O_o，u和v分别是焦平面水平和垂直的两个方向。

利用摄像机2采集到图像并获得图像的光流场，提取出天地分割线4在图像坐标系7中的位置信息，之后计算天地分割线4在图像坐标系7中的斜率和截距，其提取和计算过程如下：

首先获取图像的光流场，考虑到光流场中对应天空的部分速度变化较为平缓，对应地面的部分变化较为剧烈；因此，如果把天空部分对应的像素作为一个集合，其对应光流速度值方差应该较小；同理，地面部分也具有相同属性，当天地被理想分割时，天空部分和地面部分应当具有方差都最小的特征，因此采用下式作为搜索天地分割线4的指标：

J＝D(u_sky)+D(v_sky)+D(u_grd)+D(v_grd)

其中D(u_sky)表示天空区域光流场u方向上的方差，D(v_sky)表示天空区域光流场v方向上的方差，D(u_grd)表示大地区域光流场u方向上的方差，D(v_grd)表示大地区域光流场v方向上的方差；

在图像坐标系7中，使用一根直线切割获取的图像，假设图像长宽为(u₀,v₀)，参数γ∈[0,180°)，其中γ为直线与水平方向的夹角，然后分别计算计算直线两侧区域在光流场在u和v方向上的方差，将这些方差求和并记为J；将这条直线在图像上移动直至遍历整个图像区域，使得J的取值最小的直线位置即是天地分割线的位置；由此获得天地分割线4的直线方程v＝ku+b，其中k是直线斜率和b是直线截距；具体流程如下：

1:输入初值J＝∞,γ＝0°，输入图像长宽u₀，v₀，输入中间变量B＝0，K＝0；

2：判断γ是否等于90°，若否，则继续下一步，若是，则直接跳转至8；

3：令变量k＝tanγ,b＝0；

4：判断b是否小于v₀-ku₀，若是，继续下一步，若否，则直接跳转至7；

5：以直线v＝ku+b分割图像，计算D(u_sky)，D(v_sky)，D(u_grd)以及D(v_grd)，计算J₁＝D(u_sky)+D(v_sky)+D(u_grd)+D(v_grd)，若J₁＜J，则令J＝J₁，B＝b，K＝k，继续下一步，否则，不对J、B和K重新赋值，直接跳转至6；

6：令b自加1，跳转至4；

7：令γ自加1，跳转至2；

8：令b＝B，k＝K，输出天地分割线方程v＝ku+b，结束；如附图3所示；

即可获得天地分割线4的直线方程v＝ku+b，其中k是直线斜率，b是直线截距；

根据摄像机投影模型建立大地坐标系5和图像坐标系7之间的对应关系，并由此推导出机体坐标系6与图像坐标系7之间的对应关系，其推导步骤如下：

设天地分割线4上任意一点p在O_WX_WY_WZ_W大地坐标系5坐标为(x_h,0,0)，如果将摄像机坐标系的原点O作为中心，由于地平面是圆周堆成的，可将世界坐标系的原点O_W设在摄像机坐标系yOz平面上，这样，摄像机的世界坐标(X_W,Y_W,Z_W)可以设定为(0,t_y,t_z)，其中t_y是摄像机距离地面的高度，t_z是摄像机坐标系原点距离天地分割线4的水平距离；

根据摄像机模型，建立大地坐标系p(x_h,0,0)5与图像坐标系7上点p′(u,v)的关系如下：

$\mathrm{sm}={\mathrm{MR}}_z^{\mathrm{\φ}}{R}_x^{\mathrm{\θ}}{R}_y^{\mathrm{\ψ}}({\mathrm{Rp}}_w+t)---\left(1\right)$

其中s是比例因子，m＝[u,v,1]是点p′在图像坐标系O₀uv7上的齐次坐标，M是相机的内参数矩阵，

$M=\left[\begin{array}{ccc}{\mathrm{\α}}_x& 0& u_0\\ 0& {\mathrm{\α}}_y& v_0\\ 0& 0& 1\end{array}\right]---\left(2\right)$

内参数矩阵中的α_x和α_y分别是从成像平面到图像平面在x轴方向和y轴方向的放大系数，α_x、α_y、u₀和v₀通过事先对摄像机2的标定获取；

是摄像机2分别绕x轴旋转θ，绕y轴旋转ψ，绕z轴旋转φ的旋转矩阵，它们分别为：

$R_x^{\mathrm{\θ}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(3\right)$

$R_y^{\mathrm{\ψ}}=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& 0& \sin \mathrm{\ψ}\\ 0& 1& 0\\ -\sin \mathrm{\ψ}& \sin \mathrm{\θ}& \cos \mathrm{\ψ}\end{array}\right]---\left(4\right)$

$R_z^{\mathrm{\φ}}=\left[\begin{array}{ccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& 0\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 1& 1& 1\end{array}\right]---\left(5\right)$

R为机体坐标系6与大地坐标系5的转换矩阵，

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]---\left(6\right)$

p_W为天地分割线上任意一点的世界坐标，

$p_W=\left[\begin{array}{c}{x}_h\\ 0\\ 0\end{array}\right]---\left(7\right)$

t为世界坐标系原点O_W在摄像机坐标系中的坐标，是一个平移向量，假设O_W在xOy平面上有

$t=\left[\begin{array}{c}0\\ t_y\\ t_z\end{array}\right]---\left(8\right)$

将公式（2-8）代入（1），则摄像机模型可变形为：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathrm{\α}}_x[x_h\cos \mathrm{\φ}-\sin \mathrm{\φ}(t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ})]+u_0(t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ})\\ {\mathrm{\α}}_y[x_h\sin \mathrm{\φ}+\cos \mathrm{\φ}(t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ})]+u_0(t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ})\\ t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}\end{array}\right]---\left(9\right)$

令s=1，（9）式可简化为：

$\left\{\begin{array}{c}u-u_0=\frac{{\mathrm{\α}}_x[x_h\cos \mathrm{\φ}-\sin \mathrm{\φ}(t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ})]}{t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}}\\ v-v_0=\frac{{\mathrm{\α}}_y[x_h\sin \mathrm{\φ}+\cos \mathrm{\φ}(t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ})]}{t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}}\end{array}\right.---\left(10\right)$

置换变量，令：

$\left\{\begin{array}{c}x=u-u_0\\ y=v-v_0\\ A=t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ}\\ B=t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}\end{array}\right.---\left(11\right)$

则可以得到：

$\left\{\begin{array}{c}x=\frac{{\mathrm{\α}}_x(x_h\cos \mathrm{\φ}-A\sin \mathrm{\φ})}{B}\\ y=\frac{{\mathrm{\α}}_y(x_h\sin \mathrm{\φ}-A\cos \mathrm{\φ})}{B}\end{array}\right.---\left(12\right)$

当时，消去x_h，得到：

$y=\left(\frac{{\mathrm{\α}}_y}{{\mathrm{\α}}_x}\tan \mathrm{\φ}\right)x+\frac{{\mathrm{\α}}_y}{\cos \mathrm{\φ}}\frac{A}{B}---\left(13\right)$

由公式（11），可得：

$\frac{A}{B}=\frac{t_y\cos \mathrm{\θ}-t_z\sin \mathrm{\θ}}{t_y\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}}=\frac{(t_y/t_z)\cos \mathrm{\θ}-\sin \mathrm{\θ}}{(t_y/t_z)\sin \mathrm{\θ}+t_z\cos \mathrm{\θ}}---\left(14\right)$

对于摄像机，天地分割线4处于无穷远处，因此存在t_z＞＞t_y，所以：

$\underset{t_z\→\∞}{\mathrm{lin}}(t_y/t_z)=0$

即：

$\underset{t_z\→\∞}{\mathrm{lin}}\frac{A}{B}=\frac{-\sin \mathrm{\θ}}{\cos \mathrm{\θ}}=-\tan \mathrm{\θ}---\left(15\right)$

则公式（13）可以写成：

$y=\left(\frac{{\mathrm{\α}}_y}{{\mathrm{\α}}_x}\tan \mathrm{\φ}\right)x-\frac{{\mathrm{\α}}_y}{\cos \mathrm{\φ}}\tan \mathrm{\θ}---\left(16\right)$

将公式（11）代入（16）：

$v-v_0=\left(\frac{{\mathrm{\α}}_y}{{\mathrm{\α}}_x}\tan \mathrm{\φ}\right)(u-u_0)-\frac{{\mathrm{\α}}_y}{\cos \mathrm{\φ}}\tan \mathrm{\θ}---\left(17\right)$

上式可变形为直线方程：

$v=\left(\frac{{\mathrm{\α}}_y}{{\mathrm{\α}}_x}\tan \mathrm{\φ}\right)u+v_0-\left(\frac{{\mathrm{\α}}_y}{{\mathrm{\α}}_x}\tan \mathrm{\φ}\right)u_0-\frac{{\mathrm{\α}}_y}{\cos \mathrm{\φ}}\tan \mathrm{\θ}---\left(18\right)$

上式即是图像平面中天地分割线4的直线方程，其斜率k和截距b分别为：

$\left\{\begin{array}{c}k=\frac{a_y\tan \mathrm{\φ}}{a_x}\\ b=v_0-{\mathrm{ku}}_0-\frac{a_y\tan \mathrm{\θ}}{\cos \mathrm{\φ}}\end{array}\right.---\left(19\right)$

和则横滚角φ和俯仰角θ可由上式中的斜率k和截距b分别求得：

$\left\{\begin{array}{c}\mathrm{\φ}=\arctan \left(k\frac{{\mathrm{\α}}_x}{{\mathrm{\α}}_y}\right)\\ \mathrm{\θ}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\φ}}{{\mathrm{\α}}_y}\right]\end{array}\right.---\left(20\right)$

其中α_x和α_y分别是从成像平面到图像平面在x方向和y方向的放大系数，u₀和v₀分别为图像的长和宽。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种用于无人机俯仰角与滚转角实时计算方法，其特征在于，计算步骤如下：

坐标预定义：建立机体坐标系OXYZ，其中原点O位于摄像机光心处，OZ与光轴平行且指向机头，OY垂直指向地心；建立图像坐标系O₀uv，定义无人机上安装的摄像机焦平面左上顶点为O_o，u和v分别是焦平面水平和垂直的两个方向；

第一步：建立搜索天地分割线的指标J，

J＝D(u_sky)+D(v_sky)+D(u_grd)+D(v_grd)

其中D(u_sky)表示天空区域光流场u方向上的方差，D(v_sky)表示天空区域光流场v方向上的方差，D(u_grd)表示大地区域光流场u方向上的方差，D(v_grd)表示大地区域光流场v方向上的方差；

第二步：利用安装在无人机上的摄像机采集图像，并获得图像的光流场，基于指标J提取出天地分割线在图像坐标系O₀uv中的直线方程；

该步骤的具体过程为：

首先，在图像坐标系O₀uv中，使用一根直线切割摄像机采集的图像，并将直线两侧区域分别定义为天空区域和大地区域；其次，分别计算直线两侧区域光流场在u和v方向上的方差，将这些方差求和得到指标J；再次，将这条直线在图像上移动直至遍历整个图像区域，将指标J取值最小时所对应的直线定义为天地分割线，提取出天地分割线在图像坐标系O₀uv中的直线方程v＝ku+b，其中k是直线斜率和b是直线截距；

第三步：将天地分割线的直线方程v＝ku+b中的k和b代入机体坐标系与图像坐标系之间的对应关系表达式，计算出横滚角φ和俯仰角θ，

$\left\{\begin{array}{c}\mathrm{\φ}=\arctan \left(k\frac{{\mathrm{\α}}_x}{{\mathrm{\α}}_y}\right)\\ \mathrm{\θ}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\φ}}{{\mathrm{\α}}_y}\right]\end{array}\right.$

其中α_x和α_y分别是从成像平面到图像平面在x方向和y方向的放大系数，u₀和v₀分别为图像的长和宽。

Images