一种航向信息采集系统.pdf

本发明公开了一种航向信息采集系统，本发明采用CMOS图像传感器，结合常用的六自由度微机电陀螺，采用图像识别、平面变换、模式匹配及融合运算的技术，实时计算无人机的航向参数，针对于低空以及室内飞行的小型无人机，实现一种导航精度高、集成度高、成本低廉的自主航向控制解决方案，能克服卫星导航时信号可能受到物体屏蔽、及惯性导航时信号易受电磁干扰及信号漂移等影响。

1.一种航向信息采集系统，包括主控制器，其特征在于：所述主控制器连接有CMOS图像
传感器、六自由度陀螺仪和测距传感器：
CMOS图像传感器直接连接主控制器，CMOS图像传感器通过帧捕获器获取图像数据，经
过实时处理，存入主控制器中，通过主控制计算出光流值；
六自由度陀螺仪通过SPI总线与主控芯片连接，六自由度陀螺仪采集三轴角速率及三
轴加速度信息，通过主控制器解算出姿态信息；
测距传感器通过串口与主控芯片连接，测距传感器测出镜头与被测图像之间的距离；
主控制器通过I2C总线连接有飞控系统；主控制器计算并确定无人机的航向信息，通过
I2C总线，传送到飞控系统中，实现航向控制。
2.如权利要求1所述的一种航向信息采集系统，其特征在于，CMOS图像传感器型号为
MT9V034、六自由度陀螺仪的型号为MPU6000、测距传感器MB1240，主控制器包括DMA直接存
储器、存储器和CORTEX-M4微控制器；CORTEX-M4微控制器的主控芯片为STM32M427；
所述CMOS图像传感器、六自由度陀螺仪和测距传感器采用DMA读取模式，将采集到的信
息存储于主控制器中。
3.如权利要求1所述的一种航向信息采集系统，其特征在于，所述主控制器解算出姿态
信息的方法为：
对六自由度陀螺仪及其SPI通信协议进行初始化，设置陀螺仪数据读写函数初值，确定
传输地址，进行数据传输，数据传输完毕再进行数据转换处理，实现三轴角速率及三轴加速
度信息的一次采集；
将陀螺仪测量出的重力加速度fg及重力加速度在无人机本体的X轴上 的分量xg、Y轴上
的分量yg及Z轴上的分量zg代入式(3)，解出无人机本体的俯仰角θ和横滚角φ；
。
4.如权利要求1所述的一种航向信息采集系统，其特征在于，所述主控制器计算光流值
的方法为：
CMOS图像传感器朝向地面固定在无人机的载体上，通过无人机载体的俯仰角θ和横滚
角φ确定姿态；CMOS图像传感器输出的图像数据包括：帧同步信号、行同步信号、像素输出
时钟和图像数据；
步骤1)确定连续2帧图片的最佳匹配块；
步骤2)计算最佳匹配块中的亚像素级像素偏移量；
步骤3)计算在X和Y轴上的光流值分量。
5.如权利要求4所述的一种航向信息采集系统，其特征在于，所述主控制器确定连续2
帧图片的最佳匹配块的方法为：
对图像数据进行灰度变换，再对灰度图像进行快速均值滤波，再进行锐化处理，得到滤
波锐化后的像素图片；使用式(4)计算SAD值，确定出连续2帧滤波锐化后的像素图片的最佳
匹配块；

式(4)式中：
x、y分别为参考帧中的预测块与当前帧中的当前块在水平、垂直方向的偏移值，
i、j分别为当前块内某像素的水平、垂直坐标，
S为当前块内某像素的亮度值，
R为预测块内对应像素的亮度值，
d为单方向最大搜索距离，
SAD值即为当前块与预测块之间每个像素点亮度差值的绝对值之和。
6.如权利要求4所述的一种航向信息采集系统，其特征在于，所述计算最佳匹配块中的
亚像素级像素偏移量的方法为：
非整像素点(x，y)在邻近的4个像素点分别为(i，j)、(i+1，j)、(i，j+1)、(i+1，j+1)，将
各像素点的灰度值的双线性插值代入式(5)：
g(x，y)＝A00+A10α+A01β+A11αβ 式(5)
其中：
A00＝g(i，j)，
A10＝g(i+1，j)-g(i，j)，
A01＝g(i，j+1)-g(i，j)，
A11＝g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1，j)，
α、β分别为点(x，y)到点(i，j)的距离的水平、垂直分量；
解出的α、β值即为最佳匹配块中亚像素级的像素偏移量。
7.如权利要求4所述的一种航向信息采集系统，其特征在于，所述计算计算X和Y轴上的
光流值分量的方法为：
用六自由度陀螺仪测得的三轴角速度，按式(7)计算得到图像平面在 X轴方向的光流
分量vx、Y轴方向的光流分量vy：

式(7)中，x、y分别为X、Y轴方向的像素偏移量；
Tx、Ty为图像平面在X、Y轴方向的平移变换系数；
z为镜头与被测图像之间的距离，f为镜头的焦距；
ωx为X轴方向上的旋转角速率、ωy及为Y轴方向上的旋转角速率、ωz为Z轴方向上的旋
转角速率。
8.如权利要求1所述的一种航向信息采集系统，其特征在于，所述主控制器确定航向的
方法为：
1)航向角计算：先将无人机平放在地面上，确定正北朝向，进行初始化设置；从初始化
开始，采用式(8)和(9)不断运算相邻两幅图像之间的航向增量角度Δψi，

Δψi＝Δψb cosφcosθ式(9)
其中Δψb为图像平面的航向增量角、vx为X轴方向的光流分量、vy为Y轴方向的光流分
量；θ为姿态俯仰角、φ为横滚角，Δψb图像平面的航向增量角；
通过递推方法，不断累加航向增量角度，采用式(10)算得瞬时航向角度值ψ：

2)航向控制：将瞬时航向角ψ与航向的指令角ψn进行比较，得出误差值Δψ，输入到PID控
制器的误差输入端，即可通过比例、积分、微分三个调节环节，运算输出无人机的舵机控制
信号，促使无人机飞行的瞬时航向角能够跟随指令航向角，从而实现航向控制。

一种航向信息采集系统

技术领域

本发明属于无人机技术领域，尤其涉及一种航向信息采集系统。

背景技术

无人机现有的航向检测与控制，主要有如下几种方法：

(1)卫星定位导航，采用卫星接收机，接收超过6颗以上的卫星信号，可运算出无人
机的三维坐标位置及航向等飞行参数。这种方法对通信卫星高度依赖，当无人机在崇山峻
岭、密集建筑群等低空飞行以及在室内飞行时，会出现信号被遮挡或被中断；而且开放给民
用的卫星测定数据，误差较大。

(2)惯性导航，采用高精度的MEMS微机电陀螺仪采集三轴角速率、三轴加速度及三
轴地球磁力信号，共计九自由度参数，递推解算出俯仰、横滚及航向角度。这种方法要用到
地球磁力的采集传感器，该磁力信号微弱、易受电磁信息干扰或物体屏蔽，造成所计算得出
的航向信息精度及可靠性不高；而且计算误差会随着时间积累，而引起精度下降。

(3)天文导航，根据天体来测定无人机位置和航向，但造价昂贵，检测与计算系统
十分复杂，很难应用在小型无人机系统中。

(4)多普勒导航，利用多普勒效应进行自主式位置及航向推算，能运算出高精度的
位置、速度和偏流角等参数，不过，定位误差随着时间推移会增大，且当飞机姿态超过限度，
多普勒雷达因收不到回波而不能工作，会产生导航失效。

(5)视觉导航，根据图像中每个像素点的灰度值，挖掘出航向参数值，这种技术涉
及图像识别及模式匹配等技术，要求传感器精度高，图像处理速度快；而且造价很高，目前
还没有见到成熟和推广的应用。

上述5中方法中，天文导航和多普勒导航，体积大、重量重，很难用在小型无人飞机
上；视觉导航要求运算速度快、技术难度大、造价高，也难以用于小型无人机上。低空及室内
飞行的无人机航向控制，常用卫星定位导航结合惯性导航的综合导航模式。这几种导航方
式需要用到各种各样的陀螺仪检测装置，如：液浮陀螺仪，挠性陀螺仪，静电陀螺仪，激光陀
螺仪，光纤陀螺仪，微机电陀螺仪，等等。

低空及室内飞行的无人机的航向信息，常用微机电MEMS陀螺仪，该陀螺仪主要由
微惯性测量单元和数字磁罗盘组成，微惯性测量单元可以检测出三轴角速率和三轴加速度
信息，数字磁罗盘可以检测出三轴地球磁力信息。三轴角速率和三轴加速度参数经过诸如
求解四元数微分方程的四阶龙格-库塔方法，或采用当地地理的导航坐标系n与无人机载体
坐标系b之间的方向余弦变换等算法，即可递推解算出无人机的俯仰、横滚角度，结合三轴
磁力参数即可解算出航向角度信息，其结构框图如图1所示。然而，地磁信息相对微弱，且易
受电磁信息干扰或大型物体屏蔽，造成检测出来的航向信息参数精度不高，且会随着时间
的推移，引起累积误差，所以信号的可靠性也受到制约。

光流，是对产生视觉感知的光信号的一种运动规律的发掘，主要研究相邻两幅图
片之间具有相同或相似灰度值的像素点之间的对应关系，它不受磁场信息的干扰，用之替
代数字磁罗盘，能够提高航向测量的精度及可靠性。

发明内容：

为解决当前无人机导航中存在的问题，本发明提供了一种航向信息采集系统。

本发明是采用如下技术方案实现的：

一种航向信息采集系统，包括主控制器，所述主控制器连接有CMOS图像传感器、六
自由度陀螺仪和测距传感器：

CMOS图像传感器直接连接主控制器，CMOS图像传感器通过帧捕获器获取图像数
据，经过实时处理，存入主控制器中，通过主控制计算出光流值；

六自由度陀螺仪通过SPI总线与主控芯片连接，六自由度陀螺仪采集三轴角速率
及三轴加速度信息，通过主控制器解算出姿态信息；

测距传感器通过串口与主控芯片连接，测距传感器测出镜头与被测图像之间的距
离；

主控制器通过I2C总线连接有飞控系统；主控制器计算并确定无人机的航向信息，
通过I2C总线，传送到飞控系统中，实现航向控制。

进一步的改进，CMOS图像传感器型号为MT9V034、六自由度陀螺仪的型号为
MPU6000、测距传感器MB1240，主控制器包括DMA直接存储器、存储器和CORTEX-M4微控制器；
CORTEX-M4微控制器的主控芯片为STM32M427；

所述CMOS图像传感器、六自由度陀螺仪和测距传感器采用DMA读取模式，将采集到
的信息存储于主控制器中。

进一步的改进，所述主控制器解算出姿态信息的方法为：

对六自由度陀螺仪及其SPI通信协议进行初始化，设置陀螺仪数据读写函数初值，
确定传输地址，进行数据传输，数据传输完毕再进行数据转换处理，实现三轴角速率及三轴
加速度信息的一次采集；

将陀螺仪测量出的重力加速度fg及重力加速度在无人机本体的X轴上的分量xg、Y
轴上的分量yg及Z轴上的分量zg代入以下公式，解出无人机本体的俯仰角θ和横滚角φ；

进一步的改进，所述主控制器计算光流值的方法为：

CMOS图像传感器朝向地面固定在无人机的载体上，通过无人机载体的俯仰角θ和
横滚角φ确定姿态；CMOS图像传感器输出的图像数据包括：帧同步信号、行同步信号、像素
输出时钟和图像数据；

步骤1)确定连续2帧图片的最佳匹配块

步骤2)计算最佳匹配块中的亚像素级像素偏移量

步骤3)计算在X和Y轴上的光流值分量。

进一步的改进，所述主控制器确定连续2帧图片的最佳匹配块的方法为

对图像数据进行灰度变换，再对灰度图像进行快速均值滤波，再进行锐化处理，得
到滤波锐化后的像素图片；使用式(4)计算SAD值，确定出连续2帧滤波锐化后的像素图片的
最佳匹配块；

$SAD(x,y)=\underset{i=1}{\overset{n}{\Σ}}\underset{j=1}{\overset{n}{\Σ}}|S(i,j)-R(i+x,j+y)|,-d\≤x,y\≤d---\left(4\right)$

式(4)式中：

x、y分别为参考帧中的预测块与当前帧中的当前块在水平、垂直方向的偏移值，

i、j分别为当前块内某像素的水平、垂直坐标，

S为当前块内某像素的亮度值，

R为预测块内对应像素的亮度值，

d为单方向最大搜索距离，

SAD值即为当前块与预测块之间每个像素点亮度差值的绝对值之和。

进一步的改进，所述计算最佳匹配块中的亚像素级像素偏移量的方法为：

非整像素点(x，y)在邻近的4个像素点分别为(i，j)、(i+1，j)、(i，j+1)、(i+1，j+
1)，将各像素点的灰度值的双线性插值代入式(5)：

g(x，y)＝A00+A10α+A01β+A11αβ (5)

其中：

A00＝g(i，j)，

A10＝g(i+1，j)-g(i，j)，

A01＝g(i，j+1)-g(i，j)，

A11＝g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1，j)，

α、β分别为点(x，y)到点(i，j)的距离的水平、垂直分量。

解出的α、β值即为最佳匹配块中亚像素级的像素偏移量。

进一步的改进，所述计算计算X和Y轴上的光流值分量的方法为：

用六自由度陀螺仪测得的三轴角速度，按式(7)计算得到图像平面在X轴方向的光
流分量vx、Y轴方向的光流分量vy：

$\left\{\begin{array}{c}{v}_x=-T_x\frac{f}{z}-{\mathrm{\ω}}_{y}f+{\mathrm{\ω}}_{z}y\\ v_y=-T_y\frac{f}{z}-{\mathrm{\ω}}_{x}f+{\mathrm{\ω}}_{z}x\end{array}\right.---\left(7\right)$

式(7)中，x、y分别为X、Y轴方向的像素偏移量；

Tx、Ty为图像平面在X、Y轴方向的平移变换系数；

z为镜头与被测图像之间的距离，f为镜头的焦距；

ωx为X轴方向上的旋转角速率、ωy及为Y轴方向上的旋转角速率、ωz为Z轴方向上
的旋转角速率。

进一步的改进，所述主控制器确定航向的方法为：

1)航向角计算：先将无人机平放在地面上，确定正北朝向，进行初始化设置；从初
始化开始，采用式(8)和(9)不断运算相邻两幅图像之间的航向增量角度Δψi，

${\mathrm{tan\Δ\ψ}}_b=\frac{v_y}{v_x}---\left(8\right)$

Δψi＝Δψb cosφcosθ (9)

其中Δψb为图像平面的航向增量角、vx为X轴方向的光流分量、vy为Y轴方向的光流
分量；θ为姿态俯仰角、φ为横滚角，Δψb图像平面的航向增量角；

通过递推方法，不断累加航向增量角度，采用式(10)算得瞬时航向角度值ψ：

$\mathrm{\ψ}=\underset{i=0}{\overset{n}{\Σ}}{\mathrm{\Δ\ψ}}_i---\left(10\right)$

2)航向控制：将瞬时航向角ψ与航向的指令角ψn进行比较，得出误差值Δψ，输入到
PID控制器的误差输入端，即可通过比例、积分、微分三个调节环节，运算输出无人机的舵机
控制信号，促使无人机飞行的瞬时航向角能够跟随指令航向角，从而实现航向控制。

与现有技术相比，本发明的优点在于：

(1)图像检测信息强且不易产生漂移，而地球磁场信息相对非常微弱且容易产生
漂移，因此实时检测航向信息，不会形成累积误差，精度较高；

(2)图像检测信息不受磁场干扰，不受大型物体屏蔽，而数字磁罗盘信息则相反，
因此，本发明检测出的航向角度值，抗干扰性能强；

(3)图像检测信息比微机电MEMS检测的信息较稳定可靠，航向测控的可靠性高。

(4)与采用数字磁罗盘检测方法相比，本方法可以实现在磁力传感器失效的情况
下，不间断地对小型无人机进行航向角的测控。

(5)本发明采用图像传感与距离传感相结合的模型，实现光流的获取，与Lucas-
Kanade算法相比，计算量小且可以快速的获取光流信息，可以在嵌入式系统上快速运行。

附图说明

图1为基于MEMS陀螺的航向姿态测量系统结构框图；

图2为本发明的航向信息采集总体结构框图

图3为六自由度信息采集电路图

图4为六自由度陀螺仪信息采集流程图

图5为光流图像传感器电路图

图6为图像传感器数据采集流程图

图7为无人机航向角递推运算流程图

具体实施方式

如图2所示，航向信息采集系统中的传感器采用了CMOS图像传感器MT9V034、六自
由度陀螺仪MPU6000及测距传感器MB1240，这3种传感器分别与嵌入式微控制器CORTEX-M4
主控芯片STM32M427连接，各自实现如下功能：

1)CMOS图像传感器直接连接主控微处理器，通过帧捕获器获取图像数据，经过实
时处理，存储于主控芯片的存储器中，主控芯片启动流模块，利用算法选择出连续两帧的最
佳匹配块，计算最佳匹配块的亚像素级像素偏移量，计算出两帧之间的光流值，再结合其他
的传感信息，即可解算出航向信息；

2)六自由度陀螺仪MPU6000采集三轴角速率及三轴加速度信息通过SPI总线与主
控芯片连接，经过主控芯片的递推运算，可解算出系统的姿态信息，用于航向的求解；

3)测距传感器MB1240检测镜头与被测图像之间的距离，通过串口与主控芯片连
接，供主控芯片求解航向信息。

这3种传感器信息采用DMA读取模式，能提高存取速度，将采集到的信息直接存储
于主控芯片的存储器中。主控芯片处理运算所采集的信息，求得光流值及航向等信息，通过
I2C总线，传送到飞控系统中，用于实现航向控制。

(2)六自由度陀螺信息采集。

采用六自由度MEMS陀螺MPU6000，检测无人机的三轴角速率及三轴加速度信息，该
六自由度信息检测电路如图3所示。

六自由度陀螺仪MPU6000与主控芯片之间采取SPI通信进行数据采集，主控芯片采
用CORTEX-M4芯片STM32F427，其信息采集的总体结构如图2所示。

首先对该陀螺仪及其SPI通信协议进行初始化，设置陀螺仪数据读写函数初值，接
着确定传输地址，进行数据传输，待数据传输完毕再进行数据转换处理，实现三轴角速率及
三轴加速度信息的一次采集，程序流程图如图4所示。

(3)陀螺的姿态角度的递推运算。

把如图4所示采集到的六自由度信息，通过当地地理的导航坐标系与无人机载体
坐标系之间的变换关系，采用方向余弦矩阵，进行相互转换，并递推运算出无人机姿态角
度，即：无人机的俯仰角度及横滚角度。

导航坐标系相对于载体坐标系之间存在如下式(1)所示的变换关系。

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\θ}\sin \mathrm{\φ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(1\right)$

式(1)中：

为导航坐标系n相对于载体坐标系b的方向余弦，

θ、φ及ψ分别是无人机本体的俯仰角、横滚角及航向角。

由陀螺仪MPU6000测量出重力加速度fg在无人机本体的X、Y及Z轴上的分量为分别
为xg、yg、zg，则根据公式(1)所示的导航坐标系n相对于载体坐标系b的方向余弦的变换关
系，有

$\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ 0\\ f_g\end{array}\right]---\left(2\right)$

把公式(1)中的代入公式(2)中去，可得出如下等式(3)。

$\left\{\begin{array}{c}-sin\mathrm{\θ}\·f_g=x_g\\ \cos \mathrm{\θ}\sin \mathrm{\φ}\·f_g=y_g\\ \cos \mathrm{\θ}\cos \mathrm{\φ}\·f_g=z_g\end{array}\right.---\left(3\right)$

求解等式(3)，可解出无人机本体的俯仰角θ和横滚角φ。

(4)计算CMOS图像传感器的光流值。

本发明采用CMOS图像传感器PX4FLOW，配有CMOS图像传感器MT9V034，拥有原生752
×480像素分辨率，计算光流的过程中采用4倍分级和剪裁算法。在白天的室外工作环境中，
计算速度能达到250Hz，具备很高的感光度；而在室内或者暗光的工作环境中，计算速度也
能达到120Hz，而无需额外照明。

把CMOS图像传感器，固定在无人机的载体上，正面朝下，对着地面，通过无人机载
体的俯仰角θ和横滚角φ确定其姿态。

CMOS图像传感器MT9V034的信息采集如图5所示，图像信息通过I2C通信接口传送
到主控芯片STM32F427中，之间的连接如图2的信息采集总体结构所示。该图像传感器具有
36万像素，采用整帧曝光，同一时间将整个传感器暴露出来，在对运行中的物体进行采集。
CMOS图像传感器输出的图像数据包括：帧同步信号、行同步信号、像素输出时钟、图像数据。
输出的图像数据转化为二维的坐标偏移量并以像素的形式存储在主控芯片STM32F427的存
储器中。

图像传感器数据采集与处理流程如图6所示，首先对图像传感器及I2C总线进行配
置及初始化，设置图像数据读写函数初值，接着确定传输地址，进行数据传输，发送读写地
址接收1帧图像信息，待1帧的图像数据接收完毕，再进行数据转换处理，并将获得的图像信
息传输到主控芯片的存储器中，接着可以进行下一幅图像信息的采集工作。

由于相邻的两幅图像存在相同的特征，这些特征点的位置变化信息采用光流算
法，可判断出物体水平的平均运动，这个运动结果可被转化为二维的坐标偏移量，并以像素
形式存储在特定的寄存器中，实现对物体运动的检测。

为了保证光流计算的运算快速性和准确性，在进行光流计算之前，需要对所采集
得到的图像进行数据转换，采用的方法是：先对图像数据进行灰度变换，再对灰度图像进行
快速均值滤波，得到比较少的噪声灰度图，然后对滤波后的图像进行锐化处理，使图像的边
缘信息更加明显，如此可得到经过滤波锐化后的像素图片，以便对之进行后续的匹配算法
及光流计算。

主控芯片STM32F427中集成有硬件FPU浮点运算单元，拥有M4扩展指令集和浮点运
算，可用于光流计算。光流计算采用块匹配算法，其原理是将视频图像的每一帧分成多个不
重叠的块，认为块内所有像素位移矢量相同，选择当前帧的某一块，依据一定的匹配原则，
在参考帧的给定范围搜索最为相似的块，根据相邻两块的相对位置计算出运动位移，即每
个像素点的运动矢量。

光流计算采用最小绝对误差之和(Sum of Absolute Differences,SAD)的块匹配
算法，其光流检测步骤如下。

1)确定最佳匹配块。

采用SAD块匹配算法，其计算公式如式(4)所示，用于确定出连续2帧图片的最佳匹
配块。

$SAD(x,y)=\underset{i=1}{\overset{n}{\Σ}}\underset{j=1}{\overset{n}{\Σ}}|S(i,j)-R(i+x,j+y)|,-d\≤x,y\≤d---\left(4\right)$

式(4)式中：

x、y分别为参考帧中的预测块与当前帧中的当前块在水平、垂直方向的偏移值，

i、j分别为当前块内某像素的水平、垂直坐标，

S为当前块内某像素的亮度值，

R为预测块内对应像素的亮度值，

d为单方向最大搜索距离，

SAD值即为当前块与预测块之间每个像素点亮度差值的绝对值之和。

两块之间的SAD值越小，说明两块之间的相似度越大，反之，两块之间的SAD值越
大，则相似度越小。选择搜索区域内相似度最大的匹配块作为最佳匹配块。

2)最佳匹配块中的亚像素级像素偏移量计算。

采用双线性插值法将最佳匹配块中的像素进行亚像素细化，假设非整像素点(x，
y)在邻近的4个像素点分别为(i，j)、(i+1，j)、(i，j+1)、(i+1，j+1)，则，亚像素点(x，y)上灰
度值的双线性插值可用如下式(5)来计算得出。

g(x，y)＝A00+A10α+A01β+A11αβ (5)

式(5)中，

A00＝g(i，j)，

A10＝g(i+1，j)-g(i，j)，

A01＝g(i，j+1)-g(i，j)，

A11＝g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1，j)，

α、β分别为点(x，y)到点(i，j)的距离的水平、垂直分量。

根据式(5)，即可计算出最佳匹配块中亚像素级的像素偏移量。

3)光流计算。

由测距传感器测出的镜头与景物之间的距离z及镜头的焦距f，可求出图像平面在
X、Y轴方向的光流分量如式(6)所示。

$\left\{\begin{array}{c}{v}_x=-T_x\frac{f}{z}\\ v_y=-T_y\frac{f}{z}\end{array}\right.---\left(6\right)$

式(6)中，

Tx、Ty为图像平面在X、Y轴方向的平移变换系数，

z为镜头与景物之间的距离。

由于镜头存在X、Y及Z三轴方向上的旋转运动，这三个方向上的旋转角速率ωx、ωy
及ωz会产生附加光流量值，需要进行旋转补偿，补偿后可得图像平面在X、Y轴方向的光流
分量vx、vy如式(7)所示。

$\left\{\begin{array}{c}{v}_x=-T_x\frac{f}{z}-{\mathrm{\ω}}_{y}f+{\mathrm{\ω}}_{z}y\\ v_y=-T_y\frac{f}{z}-{\mathrm{\ω}}_{x}f+{\mathrm{\ω}}_{z}x\end{array}\right.---\left(7\right)$

式(7)中，x、y分别为X、Y轴方向的像素偏移量。

(5)航向计算。

1)航向增量角度计算。

根据式(7)计算得出的图像平面在X、Y轴方向的光流分量vx、vy值，可以计算出图像
平面的航向增量角Δψb如式(8)所示。

${\mathrm{tan\Δ\ψ}}_b=\frac{v_y}{v_x}---\left(8\right)$

即：

${\mathrm{\Δ\ψ}}_b=\arctan \frac{v_y}{v_x}$

考虑到拍照时刻，镜头随着无人机本体，具有姿态俯仰角θ及横滚角φ，因此需要
把ψb经过方向余弦变换，转换到当地地理的导航坐标系n的平面上，获得航向增量角值Δψ，
如下式(9)所示。

Δψi＝Δψbcosφcosθ(9)

2)航向角计算。

从航向角等于0开始进行初始化操作，把无人机平放在地面上，确定正北朝向，进
行初始化设置，此时复位光流的初始值是

$\left\{\begin{array}{c}{v}_{x0}=0\\ v_{y0}=0\end{array}\right.$

此时，初始航向角ψ0＝0。从初始化开始，采用式(9)不断运算相邻两幅图像之间的
航向增量角度Δψi，通过递推方法，不断累加航向增量角度，即可算得瞬时航向角度值ψ如
式(10)所示。

$\mathrm{\ψ}=\underset{i=0}{\overset{n}{\Σ}}{\mathrm{\Δ\ψ}}_i---\left(10\right)$

其迭代流程如图7所示。

(6)航向控制。

如图7所示迭代计算得出的无人机的瞬时航向角ψ之后，将之与航向的指令角ψn进
行比较，得出误差值Δψ，输入到PID控制器的误差输入端，即可通过比例、积分、微分三个调
节环节，运算输出无人机的舵机控制信号，促使无人机飞行的瞬时航向角能够跟随指令航
向角，从而实现航向控制。

以上实例的说明只是用于帮助理解本发明的核心思想；同时，对于本领域的一般
技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，
本说明书内容不应理解为对本发明的限制。