一种基于定位传感装置的飞行器定位方法

一种基于定位传感装置的飞行器定位方法
【专利摘要】本发明涉及一种基于定位传感装置的飞行器定位方法，通过惯性传感单元获取飞行器姿态信息，利用超声波传感器收集相对高度信息，利用摄像头获取地面的实时图像信息；利用改进的光流法获取飞行器相对位移信息，利用网格信息获取飞行器全局位置与姿态信息；利用滤波算法实现多传感器信息融合获取飞行器准确鲁棒的定位方法。在无GPS信号等外界辅助导航情况下，能实现飞行器的长期准确定位，且鲁棒性好，无累积误差。
【专利说明】
一种基于定位传感装置的飞行器定位方法
技术领域
[0001] 本发明涉及飞行器定位领域，具体涉及一种基于定位传感装置的飞行器定位方 法。
【背景技术】
[0002] 由于缺乏外界辅助导航，无人飞行器难以在未知环境下估计无人飞行器的位姿与 运动，飞行器自主导航的过程中需要解决这个关键问题。而这个问题的解决方法与飞行器 机载传感器的类型紧密联系，因此无人飞行器的硬件构架对相关算法的选择有直接影响。 目前针对不同的飞行器硬件构建，为上述问题提出了不同解决方案。
[0003] 对于地面移动机器人，姿态信息的估计值通常利用惯性测量单元IMU(Inertial Measurement Unit)解算获得。而位置信息的获取，则有赖于一类被称为里程计(Odometry) 的导航方法。就其本质而言，里程计是一类采用了匹配与积分技术的算法。根据里程计匹配 数据的类型，可将里程计划分为视觉里程计、激光里程计以及编码盘里程计等。里程计的解 算精度不仅与匹配算法有关，对传感器的测量误差也十分敏感。无人飞行器由于负载有限， 无法沿用许多在地面移动机器人上得到广泛应用的高精度IMU与里程计。为此，需要从重 量、大小、实时性与精度等方面对机载传感器的选择进行权衡考虑。在近十年的相关研究 中，比较有代表性的组合方案包括如下几种：
[0004] 1)单/双目视觉系统与IMU的组合。视觉系统在机器人领域的应用较为成熟。单目 视觉系统的运动恢复结构算法通过计算序列图像对应的基础矩阵，估算摄像机投影矩阵， 可恢复场景特征点的投影结构，获得载体的姿态变化;在载体运动已知的情况下，能够恢复 特征点在世界坐标系的真实位置，从而实现基于单目视觉的相对局部环境的位置估计。双 目立体视觉系统通过匹配两个摄像头视野的图像特征，利用空间特征的投影关系，获得载 体的相对位置信息。但该方法鲁棒性差，可能存在错匹配等情况，将直接影响位姿估计的准 确性。
[0005] 2)光流法与IMU的组合。光流法同样依赖于机载摄像头提供的图像灰度信息。但与 1)所不同的是，光流法无需对图像进行特征提取与特征匹配，转而考虑图像灰度在时间上 的变化率与摄像头和图像场景之间相对变化的关系。在灰度信息之外，光流法还要求提供 图像像素对应的深度信息，从而完成对位姿的解算。但该方法存在累积误差，在飞行器长期 飞行的情况下，其定位误差将逐渐累积，因此无法保证飞行器的长期稳定准确飞行定位。
[0006] 3)激光测距仪与頂U的组合。激光测距仪可以感知环境的结构信息，得到环境的二 维点云数据。利用迭代最近邻点ICP(Iterative Closest Point)算法，对相邻时间的两帧 点云数据进行匹配，可求取载体在该时间段内的平移运动分量及旋转运动分量。对平移运 动分量和旋转运动分量进行积分，进而可获得飞行器的位置和姿态估计值。可利用ICP算法 对激光测距仪的二维点云数据进行匹配，得到飞行器在运动平面上的位置及飞行器朝向的 估计值，并采用扩展卡尔曼滤波器EKF(Extended Kalman Filter)将ICP算法解算得到的飞 行器位姿信息与IMU解算获得的飞行器姿态信息进行融合，实现了无人飞行器在局部场景 下的位姿估计。但利用该方法需要在飞行器上安装激光测距仪，该装置体积较大，价格昂 贵，并不适用于微飞行器平台的安装及推广使用。
[0007] 4)深度摄像机(RGB-D摄像机)与頂U的组合。RGB-D摄像机是结合了摄像头与测距 仪优点的一种新型传感器，其在获得视野图像灰度信息的同时，也可获得相应的深度信息。 同双目立体视觉系统相比，RGB-D摄像机采样深度信息的效率更高，获得的深度信息更为精 确。在无人飞行器上配备Microsoft Kinect，利用和双目立体视觉系统类似的原理，从场景 图像中提取特征点及其深度信息并进行特征匹配，得到空间特征的投影关系，获得载体的 相对运动信息。使用该传感器同样由于体积较大，并不适用于安装在微小飞行器平台中。

【发明内容】

[0008] 本发明的目的在于针对现有技术的不足，提供一种基于定位传感装置的飞行器定 位方法，本方法在无GPS信号等外界辅助导航情况下，能实现飞行器的长期准确定位，且鲁 棒性好，无累积误差。
[0009] 本发明所提供的技术方案为:一种基于定位传感装置的飞行器定位方法，包括如 下步骤：
[0010] 1)通过惯性传感单元中的加速度计和陀螺仪分别测得飞行器实时的加速度和角 速度信息，通过磁力计确定方位，获得飞行器的实时姿态信息;所述的实时姿态信息包括偏 航角、横滚角和俯仰角；
[0011]利用超声波传感器收集飞行器相对于地面的高度信息，并利用惯性传感单元获取 的实时姿态信息进行姿态补偿，实时校正垂直高度信息；
[0012] 利用摄像头，获取地面的实时图像信息；
[0013] 2)对获取的实时图像信息作灰度化处理，获取灰度图，对每帧灰度图选择固定间 隔固定数量的像素块，通过匹配相邻两帧灰度图，计算光流向量，并通过差分，获取光流速 度；
[0014] 3)同步惯性传感单元、超声波传感器和摄像头的时间戳，利用惯性传感单元获取 的实时姿态信息，对光流速度进行姿态补偿，得到光流速度的水平面分量;利用超声波传感 器获取的垂直高度信息对光流速度的水平面分量进行尺度还原，得到飞行器的水平速度；
[0015] 4)对飞行器的水平速度积分得到相对位移信息，累加相对位移信息，即得水平的 位置信息，结合超声波传感器得到的垂直高度信息，即可得到飞行器的位置信息。
[0016] 所述的步骤2)中光流速度的获取方法为：
[0017] 对每帧灰度图选择固定间隔固定数量的像素块，将前一帧灰度图中的像素块位置 作为后一帧灰度图的像素块的初始位置，并建立一个搜索区域，阈值函数的公式如下： N-1.N-1
[0018] EZIGi^-^_G〇(/+w-/ +v')| ⑴ /=〇 j=Q
[0019] 式（1)中，U表示X轴方向的搜索像素个数，v表示y轴方向的搜索像素个数;Go和 别表示前一帧和后一帧像素块中某一个像素点的灰度值；i，j分别表示图像平面像素块中 某一个像素点的X，y轴方向的坐标，N表示像素块的X轴或者y轴方向上的像素个数，GKi，j) 表示前一帧像素块中每一像素点的灰度值，Go(i+u，j+v)表示将前一帧图像中的像素块投 射到后一帧图像中相同位置后并沿X轴、y轴方向分别移动u、v像素的像素块中每一像素点 的灰度值；
[0020] 通过令前一帧的像素块在后一阵的灰度图的搜索区域内搜索灰度差和最小，求得 后一帧的像素块位置，两帧像素块的距离差，即为光流向量，通过差分，获取光流速度。
[0021] 进一步，所述的步骤3)中获得飞行器的水平速度的方法为：
[0022] 建立图像平面中的点与摄像机坐标系中的点的转换关系：
(2)
[0024]式(2)中，u为图像平面中的点的坐标，pc为摄像机坐标系的点的坐标，f为焦距，z 为垂直高度；
[0025] 对式(2)两边求导，即得光流速度：
(3 )
[0027]式(3)中，为摄像头坐标系下点的运动速度，vz为垂直方向上的运动速度，v为图 像平面的运动速度，即光流速度；
[0028]利用刚体运动学的公式：
[0029] Vc = -Vw-co Xpc (4)
[0030] 式(4)中，vw表示世界坐标系下点的运动速度，ω表示角速度；
[0031] 将公式(4)带入公式(3)，展开可得到X轴和y轴方向的光流速度与世界坐标系下的 飞行器速度的关系：
[0034] 式(5)中，vu表示图像平面X轴方向的光流速度，Vc；x表示相机坐标系下X轴方向的速 度，Vwx表示世界坐标系下X轴方向的速度，Vwz表示世界坐标系下Z轴方向的速度，W x，Wy，Wz分 别表示x，y，z轴方向上的角速度，X。表示相机坐标系下X轴方向的坐标，y。表示相机坐标系下 y轴方向的坐标;u，v分别表示图像平面X，y轴方向的坐标；
[0035] 式(6)中，Vv表示图像平面y轴方向的光流速度，vwy表示世界坐标系下y轴方向的速 度; Vwz表示世界坐标系下z轴方向的速度;对垂直高度信息差分可以求得世界坐标系下z轴 方向的速度，即
[0036]根据式(5)和(6)，以及已知的世界坐标系下z轴方向的速度，可以求得世界坐标系 下X轴和y轴方向的飞行器的速度，即为飞行器的水平速度。
[0037]进一步，当飞行器定高飞行时，式(5)和(6)简化为： (7) (8 )
[0040] 作为改进，对所述的飞行器定位方法进行优化，所述的摄像头获取地面的实时图 像信息为规则的网格信息，包括如下步骤：
[0041] i)利用摄像头，获取地面的实时图像信息，进一步处理实时图像信息中的规则的 网格信息，使用霍夫变换算法，调整筛选直线的阈值，对其中的直线信息进行准确提取，从 而获取实时图像信息中的直线交点坐标；
[0042] ii)同步惯性传感单元、超声波传感器和摄像头的时间戳，对飞行器的水平速度积 分，计算飞行器的初始相对位移;给定飞行器的初始位置估计;利用惯性传感单元给定实时 姿态信息，给定飞行器的初始姿态信息估计;使用摄像头的投影模型和实时图像信息中获 取的直线交点坐标，获取地面网格交点的坐标；
[0043] iii)根据地面网格交点相邻距离恒定的条件，对求得的地面网格交点的坐标进行 求精操作，获取准确的地面网格交点的坐标，再利用实时图像信息对应的坐标，建立匹配点 对，使用直接线性变换算法或者迭代算法，求得飞行器准确的位置信息及姿态信息。
[0044] 进一步，所述的步骤ii)中获取地面网格交点的实际坐标的方法为：
[0045] 根据实时图像信息中的粗略位置估计，公式如下：
[0046] p： =pl_} (9)
[0047] 式(9)中，vw为水平光流速度，片％为前一时刻水平位置，％为后一时刻水平位置，dt 为间隔时间，另外令超声波传感器在k_埘刻和k时刻给入的垂直高度信息分别为:， 给定飞行器的初始位置估计，其中/< =(.￥''，,.乂，和;= 均为三维列向量，表示X，y，Z轴方向的位置；
[0048] 由惯性传感单元给定飞行器实时的姿态信息，包括偏航角、横滚角和俯仰角，如 下：
[0049] roll= γ ,pitch = 9 ,yaw=it (10)
[0050] 式(10)中，roll、pitch、yaW分别为该时刻的横滚角、俯仰角和偏航角；
[0051] 对于步骤i)中获取的实时图像信息中的直线交点坐标(u，v)，都满足直线方程：
[0052] au+bv+c = 0 (11)；
[0053] 摄像头的投影模型由以下公式表达：
[0055]式（12)中，λ表示相机坐标系下特征点的z轴坐标，（u，v，1 )τ表示实时图像信息中 特征点的齐次坐标，Κ表示摄像头的内参矩阵，fx，fy，CX，Cy分别表示X，y轴方向的像素焦距 以及主点的像素坐标；；表示世界坐标系相对于相机坐标系的转换矩阵，即外参矩阵，前 三行三列矩阵表示旋转矩阵，最后一列矩阵表示平移向量；（^，~，2^1)7表示世界坐标系 下特征点的齐次坐标；
[0056]将直线方程公式(11)代入摄像头的投影模型公式(12)，得到公式如下：
[0058] 给定飞行器的初始姿态信息估计和初始位置估计，如下： ? ：Γη i\2 厂^ eos γ cos: ψ - sin γ sin Θ sin ^ - cos 0 sin sin;/ cos ^ + cos ^sin 0：sin
[0059] r.n r23 = co：s/sini// + a sin ^ cost// gos Θ cos (// iV in ^sin ψ - cos f sin Θ cosy/ (14) Kr,n ;;2 r；3 J k -sin ^ eos ^ sin Θ cos ^ cos γ 》 Μ ??
[0060] r,. = ,p：' (15) j
[0061] 将给定的参数代入公式（13)，求解公式（13)的直线方程，设求出的世界坐标系的 坐标点Pw为：
[0062] pw=(xw'，yw'， Zw'） (16)
[0063] 对世界坐标系中的直线交点(心，7|，&)，同样建立直线方程：
[0064] a，xw+b，yw+c，zw+d，= 0 (17)
[0065] 利用规则的网格信息的特征，直线只分为两类，两类直线互相垂直，且认为地面为 平面，则z w=0，将直线方程分为两类：
(18)
[0067] 由公式（18)对世界坐标系的坐标点进行求精操作，即水平和垂直的两类直线分布 间隔均匀，地面的直线实际交点坐标为常值，以此可以对1^=(^'，^'+'）求出的坐标中 的 Xw'，yw'取与之相差绝对值最小的常值x/，y/，zw'认为是0;求精后的世界坐标系下的直 线交点的坐标值为Pw= (xw〃，yw〃，0);
[0068] 利用公式（13)代入求精后pw的坐标，如下：
[0070]简化后得到：
[0071]
[?/>；! +^/>21 +(ac, +bcr +c)?3iX "+[^2 +￥/n +(ac^hcy +e)%]jw" ( 20 ) -\-[afj^ -\-bfjr + (ac\ + hcv + c)t ] = 0
[0072] 由公式(18)知直线方程有两种，故将方程(19)分为两类：
[0073]
["/:厂丨丨+,)/:.厂2丨+ (说·' 咖丨]=〇
[a0]2+hfyrn+(ac, +bcY+c)ri2]y,tn+[afj, +Η/γ[γ+(αα(+?κ\.+€)? ] = 〇 (21)
[qf .j], + Λ/,α, + ((-ic, + bcr + c)r.?] - 0 Wj'u +(^-, +-)ΛιΧ n^[afyx+bfJx +(acx +bct +r'K ] - 0
[0074] 提取未知数1'11，打1，打1，1'12，打2，打2,1^山，1:2，重写方程（13)，有 ：
[0076] 方程(22)是在取两类直线中的各一条建立的方程组，由于存在9个未知数，至少需 要五条直线才能解该线性方程组;但直线数超过5条后，该方程组为超定方程组，对其进行 奇异值分解即可解该方程组；
[0077] 至此，解出旋转矩阵R和平移向量t，如下： r\ [? ri2 rn t'.x
[0078] R- t? (23) νΓ?1 Γ32 r3.J Uv
[0079] 在已求出rn，r21，r31，r 12，r22，r32的情况下，可利用旋转矩阵是单位正交矩阵这一 特性，求出ri3，Γ23，?3 ;
[0080] 再将旋转矩阵转换为姿态角，得到姿态信息：
[0081] roll=a tan2(r32，r33)
[0082] pitch = β tan 2(-rit, ^/?2 + ) -( 24 X
[0083] yaw = a tan2(r2i,rn)
[0084] 进一步改进，对所述的飞行器定位方法进一步优化，采用光流速度信息，网格定位 的位置信息、惯性传感单元的加速度信息，建立状态方程和观测方程，对视觉算法和惯性传 感单元获取的结果使用扩展卡尔曼滤波算法，进行融合处理，获取飞行器准确的位置及姿 态信息。
[0085] 所述的获取飞行器准确的位置及姿态信息具体的算法推导如下： (25)
[0086]非线性连续状态方程如下：
[0089] 对其进行离散化操作，状态量包括有网格定位得到的位置信息P，光流法提供的光 流速度信息^和惯性传感单元提供的加速度a; △ t表示连续两次采样的间隔时间，A表示状 态转移矩阵；
[0090] 观测方程如下，分为视觉算法部分的观测方程，观测量位置p;惯性传感单元的观 测方程，观测量取加速度a: rn- 0 〇]Xi
[0091] r (26：) 气i = H，.,; x{ = [0 0 1] x4
[0092] 其中公式(26)中，Zv,k，Zl,k分别是视觉算法和惯性传感单元的观测量，分别是世界 坐标系下的水平方向的位置信息和加速度信息；
[0093]根据卡尔曼滤波算法的公式： P； = ^P?f^r+Q
[0094] (2:7) +κ,Γ-Η,αχΑ) P^d-K.HJP, P;=Ur+Q κα-+R,)-1 _] - ? Η -、 (2Μ - A + Κ,.λ- (.T/iV/i/ - Hu-χΑ-) Ρ, =(Ι--Κ;,Η",)Ρ：
[0096] 可分别计算视觉算法部分和惯性传感单元部分的卡尔曼增益Kv,k，K1>k，其中？!^表 不对应于状态xk-i的协方差，A表不对应于状态 χ丨的协方差，Q表不系统噪声的协方差矩 阵，Rv，Ri分别表示视觉算法和惯性传感单元噪声的协方差矩阵;再结合预测值和测量值，分 别求取视觉部分和惯性传感单元部分状态量的最优估值Xk，其中 Χν181〇η，χΙΜυ分别表示视觉 算法的测量值和惯性传感单元的测量值;最后再次更新相对于状态量 Xk的协方差矩阵。
[0097] 同现有技术相比，本发明的有益效果体现在：
[0098] (1)本发明利用改进的光流法获取飞行器相对位移信息；利用网格信息获取飞行 器全局位置与姿态信息;利用滤波算法实现多传感器信息融合获取飞行器准确鲁棒的定位 方法。
[0099] (2)采用光流与网格信息相结合的组合定位方法，各自巧妙地将两者的优势结合 起来，相比单纯使用光流信息，避免了累计误差，定位效果可保证长期较高的准确度。
[0100] (3)采用视觉算法与惯性传感单元信息融合的扩展卡尔曼滤波算法，不仅确保了 提供飞行器飞行轨迹的平滑性，更进一步保证了定位算法的鲁棒性。
【附图说明】
[0101 ]图1为本发明飞行器的定位传感装置的结构示意图；
[0102] 图2为本发明实现飞行器定位的场景图；
[0103] 图3为本发明实现飞行器定位的传感数据处理流程图；
[0104] 图4为本发明基于定位传感装置的飞行器定位方法的流程图。
【具体实施方式】
[0105] 如图1所示为飞行器的定位传感装置，由微处理器(NUC)l，惯性传感单元（MU)2， 超声波传感器3，摄像头4组成。
[0106] 微处理器采用Intel公司的NUC(NUC5i7RYH)系列，整重仅0.607Kg，该系列处理器 具备体积纤小、多接口、处理速度快、功能强大、低功耗、散热快等特点，本发明由于摄像头 的高帧率采集到的图像信息庞大，对数据处理的实时能力有很高的要求;且所有的数据处 理要求均在linux(ubuntu)系统下进行;要求整套装置能安装于微型飞行器上，故基于以上 要求，由于NUC5i7RYH的大容量数据处理能力、轻薄外观、跨平台的特性(全面支持Windows、 Linux)，选择它非常合适。
[0107] 摄像头与超声波模块，选用PX4FL0W，它是一款开源智能光学流动传感器，拥有752 X480像素分辨率以及250Hz刷新率，计算光学流的过程中采用了 4倍分级和裁剪算法，具备 非常高的感光度，能够在室内或者室外暗光环境下工作，且硬件和软件结构均开源，适合对 设计的算法进行重新编程，二次开发；
[0108] 惯性传感单元采用LPMS系列，LP-RESEARCH公司该系列产品使用了先进的数据融 合算法，为用户提供高精度高稳定性的姿态动作信息以及3轴加速度/角速度/地磁量/方位 角等数据，为算法实现提供准确的姿态信息。
[0109] 如图2所示：飞行器在大理石地面、地板等有规则和纹理地面条件下飞行示意图。 下标w的坐标系意为世界坐标系，即在整个场景下建立的全局坐标系。飞行器上下标为c的 坐标系意为机体坐标系，由于设计了多传感固连装置，惯性传感单元、摄像头、超声波传感 器、机体均为刚性连接，因此各自坐标系只存在一个固定的坐标转换关系，该关系在设计固 连装置时已给定，如此解决了各传感器之间的坐标转换问题。坐标轴为u，v的为图像坐标 系，任意地面一点投影到图像平面将在该平面建立一个像素单位的坐标。相机为下视视野， 能够向下观测到部分地面的网格和纹理信息。
[0110] 如图3所示:定位传感装置中的传感器套件进行初始化操作。在微处理器下运行各 节点，主要包括图像节点，用于接收地面图像信息;超声波节点，用于检测相对高度信息;惯 性传感单元节点，用于接收整个模块的角速度，加速度信息；时间同步节点，用于对多传感 器数据进行同步操作;处理运算节点，对同步后的数据进行定位算法运算处理。运算主节点 将订阅其他节点的信息，若无数据通信，则保持通讯状态;若接收到传感器信息，将首先对 各传感器的时间戳进行识别，以时间戳作为同步原则，对不同帧率的传感器数据首先进行 同步处理。接下来，运算主节点将根据本实施例设计的算法，对处理后的数据进行运算，实 时输出飞行器的位置与姿态，作为飞行器控制器的输入量。
[0111] 如图4所示，基于定位传感装置的飞行器定位方法:包括如下步骤：
[0112] 1)通过惯性传感单元中的加速度计和陀螺仪分别测得飞行器实时的加速度和角 速度信息，通过磁力计确定方位，获得飞行器的实时姿态信息;所述的实时姿态信息包括偏 航角、横滚角和俯仰角；
[0113]利用超声波传感器收集飞行器相对于地面的高度信息，并利用惯性传感单元获取 的实时姿态信息进行姿态补偿，实时校正垂直高度信息；
[0114]利用摄像头，获取地面的实时图像信息；
[0115] 2)对获取的实时图像信息作灰度化处理，获取灰度图，对每帧灰度图选择固定间 隔固定数量的像素块，通过匹配相邻两帧灰度图，计算光流向量，并通过差分，获取光流速 度；
[0116] 3)同步惯性传感单元、超声波传感器和摄像头的时间戳，利用惯性传感单元获取 的实时姿态信息，对光流速度进行姿态补偿，得到光流速度的水平面分量;利用超声波传感 器获取的垂直高度信息对光流速度的水平面分量进行尺度还原，得到飞行器的水平速度；
[0117] 4)对飞行器的水平速度积分得到相对位移信息，累加相对位移信息，即得水平的 位置信息，结合超声波传感器得到的垂直高度信息，即可得到飞行器的位置信息。
[0118] 所述的步骤2)中光流速度的获取方法为：
[0119] 对每帧灰度图选择固定间隔固定数量的像素块，将前一帧灰度图中的像素块位置 作为后一帧灰度图的像素块的初始位置，并建立一个搜索区域，阈值函数的公式如下：
⑴
[0121] 式（1)中，u表示X轴方向的搜索像素个数，v表示y轴方向的搜索像素个数;Go和6:分 别表示前一帧和后一帧像素块中某一个像素点的灰度值；i，j分别表示图像平面像素块中 某一个像素点的X，y轴方向的坐标，N表示像素块的X轴或者y轴方向上的像素个数，GKi，j) 表示前一帧像素块中每一像素点的灰度值，Go(i+u，j+v)表示将前一帧图像中的像素块投 射到后一帧图像中相同位置后并沿X轴、y轴方向分别移动u、v像素的像素块中每一像素点 的灰度值；
[0122] 通过令前一帧的像素块在后一阵的灰度图的搜索区域内搜索灰度差和最小，求得 后一帧的像素块位置，两帧像素块的距离差，即为光流向量，通过差分，获取光流速度。
[0123] 进一步，所述的步骤3)中获得飞行器的水平速度的方法为：
[0124]建立图像平面中的点与摄像机坐标系中的点的转换关系：
(2)
[0126] 式(2)中，u为图像平面中的点的坐标，p。为摄像机坐标系的点的坐标，f为焦距，z 为垂直高度；
[0127] 对式(2)两边求导，即得光流速度：
(3)
[0129]式(3)中，为摄像头坐标系下点的运动速度，vz为垂直方向上的运动速度，v为图 像平面的运动速度，即光流速度；
[0130] 利用刚体运动学的公式：
[0131] Vc = -Vw-co Xpc (4)
[0132] 式(4)中，vw表示世界坐标系下点的运动速度，ω表示角速度；
[0133] 将公式(4)带入公式(3)，展开可得到X轴和y轴方向的光流速度与世界坐标系下的 飞行器速度的关系：
[0136] 式(5)中，vu表示图像平面X轴方向的光流速度，Vc；x表示相机坐标系下X轴方向的速 度，Vwx表示世界坐标系下X轴方向的速度，Vwz表示世界坐标系下Z轴方向的速度，W x，Wy，Wz分 别表示x，y，z轴方向上的角速度，X。表示相机坐标系下X轴方向的坐标，y。表示相机坐标系下 y轴方向的坐标;u，v分别表示图像平面X，y轴方向的坐标；
[0137] 式(6)中，Vv表示图像平面y轴方向的光流速度，vwy表示世界坐标系下y轴方向的速 度; Vwz表示世界坐标系下z轴方向的速度;对垂直高度信息差分可以求得世界坐标系下z轴 方向的速度，即
[0138] 根据式(5)和(6)，以及已知的世界坐标系下z轴方向的速度，可以求得世界坐标系 下X轴和y轴方向的飞行器的速度，即为飞行器的水平速度。
[0139] 进一步，当飞行器定高飞行时，式(5)和(6)简化为：
(7) (8) 〇
[0142] 对所述的飞行器定位方法进行优化，所述的摄像头获取地面的实时图像信息为规 则的网格信息，包括如下步骤：
[0143] i)利用摄像头，获取地面的实时图像信息，进一步处理实时图像信息中的规则的 网格信息，使用霍夫变换算法，调整筛选直线的阈值，对其中的直线信息进行准确提取，从 而获取实时图像信息中的直线交点坐标；
[0144] ii)同步惯性传感单元、超声波传感器和摄像头的时间戳，对飞行器的水平速度积 分，计算飞行器的初始相对位移;给定飞行器的初始位置估计;利用惯性传感单元给定实时 姿态信息，给定飞行器的初始姿态信息估计;使用摄像头的投影模型和实时图像信息中获 取的直线交点坐标，获取地面网格交点的坐标；
[0145] iii)根据地面网格交点相邻距离恒定的条件，对求得的地面网格交点的坐标进行 求精操作，获取准确的地面网格交点的坐标，再利用实时图像信息对应的坐标，建立匹配点 对，使用直接线性变换算法或者迭代算法，求得飞行器准确的位置信息及姿态信息。
[0146] 进一步，所述的步骤ii)中获取地面网格交点的实际坐标的方法为：
[0147] 根据实时图像信息中的粗略位置估计，公式如下：
[0148] Pl'^Pk-i+vJ, (9)
[0149] 式(9)中，vw为水平光流速度，/心为前一时刻水平位置，/〇后一时刻水平位置， dt为间隔时间，另外令超声波传感器在k-i时刻和k时刻给入的垂直高度信息分别为和 ^?给定飞行器的初始位置估计，其中< 和= 均为三维列向量，表示x，y，z轴方向的位置；
[0150] 由惯性传感单元给定飞行器实时的姿态信息，包括偏航角、横滚角和俯仰角，如 下：
[0151] roll= γ ,pitch = 9 ,yaw=it (10)
[0152] 式(10)中，ro 11、p i tch、yaw分别为该时刻的横滚角、俯仰角和偏航角；
[0153] 对于步骤i)中获取的实时图像信息中的直线交点坐标(u，v)，都满足直线方程：
[0154] au+bv+c = 0 (11)；
[0155] 摄像头的投影模型由以下公式表达：
[0157] 式（12)中，λ表示相机坐标系下特征点的z轴坐标，（u，v，1 )τ表示实时图像信息中 特征点的齐次坐标，Κ表示摄像头的内参矩阵，fx，fy，C X，Cy分别表示X，y轴方向的像素焦距 以及主点的像素坐标;C：：表示世界坐标系相对于相机坐标系的转换矩阵，g卩外参矩阵，前三 行三列矩阵表示旋转矩阵，最后一列矩阵表示平移向量；（^，^，^，1)7表示世界坐标系下 特征点的齐次坐标；
[0158] 将直线方程公式(11)代入摄像头的投影模型公式(12)，得到公式如下：
[0160] 给定飞行器的初始姿态信息估计和初始位置估计，如下：
[0161] 'rn /-j.2 [ cos γ cos ψ - sin γ sin Θ sin ψ - cos sin ψ sin ycoai;/ +cos (vsin ^sin ψ ^ r2l rn ν?, = cos/sin cos Θ cos {// .v in sin ψ - cos 7 sir. Θ cos ψ (14) v：r31 }\? n -sin ;/cos6* sin Θ cos Θ cos γ ) /A \ / lv I vA
[0162] tY = Yp： (15) V ^ J ^ J Pk y
[0163] 将给定的参数代入公式（13)，求解公式（13)的直线方程，设求出的世界坐标系的 坐标点Pw为：
[0164] pw= (xw，，yw，，Zw，） (16)
[0165] 对世界坐标系中的直线交点(^，7|，~），同样建立直线方程：
[0166] a ' Xw+b ' yw+c ' Zw+d ' = 0 (17)
[0167] 利用规则的网格信息的特征，直线只分为两类，两类直线互相垂直，且认为地面为 平面，则zw=0，将直线方程分为两类：
(18)
[0169] 由公式（18)对世界坐标系的坐标点进行求精操作，即水平和垂直的两类直线分布 间隔均匀，地面的直线实际交点坐标为常值，以此可以对如=(^'，^'+'）求出的坐标中 的 Xw'，yw'取与之相差绝对值最小的常值x/，y/，zw'认为是0;求精后的世界坐标系下的直 线交点的坐标值为Pw= (xw〃，yw〃，0);
[0170] 利用公式（13)代入求精后pw的坐标，如下：
(19)
[0172] 简化后得到：
[0173]
[<rn+h02·, +Κ +^)^|]^ "ΗΦ\2 +h〇22 " ( 2〇 ) +[cifj, +M./r +bc.. +<：·)/*]-0
[0174] 由公式(18)知直线方程有两种，故将方程(19)分为两类：
[0175]
[afj], + bf\.r：A + (iic\ + bcr + c)r；i ] - 0
[Φ'η +￥/22+(αοχ +bcy+c)r22]yu +(^,+bey+c)iJ - 〇
[af j], + bf \.r:l + {acx -f bcY -f v)rr] ^ 0 WJu +(^, +^·,· +c)rM]x, +b/J, +(acx +bct +r)i ] = 0
[0176] ?^Φ.7^^Ρ|δ；Π?, Γ21 , Γ31, Γ12 , Γ22 , Γ32 ,tx,ty,tz, fM( 13),^:
[0177]
[0178] 方程(22)是在取两类直线中的各一条建立的方程组，由于存在9个未知数，至少需 要五条直线才能解该线性方程组;但直线数超过5条后，该方程组为超定方程组，对其进行 奇异值分解即可解该方程组；
[0179] 至此，解出旋转矩阵R和平移向量t，如下：
(23)
[0181] 在已求出如^^^^⑶的情况下河利用旋转矩阵是单位正交矩阵这一 特性，求出ri3，Γ23，?3 ;
[0182] 再将旋转矩阵转换为姿态角，得到姿态信息：
[0183] roll=a tan2(r32，r33)
[0184] pitch = a\m2(-riX, ^/ + ?2) (: 24 )。
[0185] yaw = a tan2(r2i,rn)
[0186] 对所述的飞行器定位方法进一步优化，采用光流速度信息，网格定位的位置信息、 惯性传感单元的加速度信息，建立状态方程和观测方程，对视觉算法和惯性传感单元获取 的结果使用扩展卡尔曼滤波算法，进行融合处理，获取飞行器准确的位置及姿态信息。
[0187] 所述的获取飞行器准确的位置及姿态信息具体的算法推导如下：
[0188] 非线性连续状态方程如下：
[0189] Xk+i = f (xk,Wk)
(25 )
[0191] 对其进行离散化操作，状态量包括有网格定位得到的位置信息p，光流法提供的光 流速度信息^和惯性传感单元提供的加速度a; △ t表示连续两次采样的间隔时间，A表示状 态转移矩阵；
[0192] 观测方程如下，分为视觉算法部分的观测方程，观测量位置p;惯性传感单元的观 测方程，观测量取加速度a: zv,k= ,~[ι 0 ^*1 xi： n..、
[0193] r , (26) z,/. =H,.，/，[0 0 φ?
[0194] 其中公式(26)中，Zv,k，Zl,k分别是视觉算法和惯性传感单元的观测量，分别是世界 坐标系下的水平方向的位置信息和加速度信息；
[0195] 根据卡尔曼滤波算法的公式： Pf4V+Q
[0196] ( 2:7 ) x, =xA：+Kia(.t,,s;on-H^Xi：) Ρ, ^(Ι-Κ,Η,)^ P；=/iP^r+Q Κ,,^ΡΧ^Η,,ΡΧ,+^Γ1
[0197] /2g\ Xi =(+Ku(%/f 广 Η"χα-) -Pi- = (?--乂)Pf
[0198] 可分别计算视觉算法部分和惯性传感单元部分的卡尔曼增益Kv,k，K1>k，其中？!^表 示对应于状态Xk-i的协方差，？^表示对应于状态Xi的协方差，Q表示系统噪声的协方差矩 阵;Rv，!^分别表示视觉算法和惯性传感单元噪声的协方差矩阵;再结合预测值和测量值，分 别求取视觉部分和惯性传感单元部分状态量的最优估值Xk，其中 Χν181〇η，χΙΜυ分别表示视觉 算法的测量值和惯性传感单元的测量值;最后再次更新相对于状态量 Xk的协方差矩阵。
【主权项】
1. 一种基于定位传感装置的飞行器定位方法，其特征在于，包括如下步骤： 1) 通过惯性传感单元中的加速度计和陀螺仪分别测得飞行器实时的加速度和角速度 信息，通过磁力计确定方位，获得飞行器的实时姿态信息；所述的实时姿态信息包括偏航 角、横滚角和俯仰角； 利用超声波传感器收集飞行器相对于地面的高度信息，并利用惯性传感单元获取的实 时姿态信息进行姿态补偿，实时校正垂直高度信息； 利用摄像头，获取地面的实时图像信息； 2) 对获取的实时图像信息作灰度化处理，获取灰度图，对每帧灰度图选择固定间隔固 定数量的像素块，通过匹配相邻两帧灰度图，计算光流向量，并通过差分，获取光流速度； 3) 同步惯性传感单元、超声波传感器和摄像头的时间戳，利用惯性传感单元获取的实 时姿态信息，对光流速度进行姿态补偿，得到光流速度的水平面分量;利用超声波传感器获 取的垂直高度信息对光流速度的水平面分量进行尺度还原，得到飞行器的水平速度； 4) 对飞行器的水平速度积分得到相对位移信息，累加相对位移信息，即得水平的位置 信息，结合超声波传感器得到的垂直高度信息，即可得到飞行器的位置信息。2. 根据权利要求1所述的基于定位传感装置的飞行器定位方法，其特征在于，所述的步 骤2)中光流速度的获取方法为： 对每帧灰度图选择固定间隔固定数量的像素块，将前一帧灰度图中的像素块位置作为 后一帧灰度图的像素块的初始位置，并建立一个搜索区域，阈值函数的公式如下：(1 ) 式（1)中，U表示X轴方向的搜索像素个数，V表示y轴方向的搜索像素个数;GdPG1分别表 示前一帧和后一帧像素块中某一个像素点的灰度值;i，j分别表示图像平面像素块中某一 个像素点的X，y轴方向的坐标，N表示像素块的X轴或者y轴方向上的像素个数，G 1 (i，j)表示 前一帧像素块中每一像素点的灰度值，Go(i+u，j+v)表示将前一帧图像中的像素块投射到 后一帧图像中相同位置后并沿X轴、y轴方向分别移动u、v像素的像素块中每一像素点的灰 度值； 通过令前一帧的像素块在后一阵的灰度图的搜索区域内搜索灰度差和最小，求得后一 帧的像素块位置，两帧像素块的距离差，即为光流向量，通过差分，获取光流速度。3. 根据权利要求2所述的基于定位传感装置的飞行器定位方法，其特征在于，所述的步 骤3)中获得飞行器的水平速度的方法为： 建立图像平面中的点与摄像机坐标系中的点的转换关系：(2) τm I產平面中的点的坐标，Pc为摄像机坐标系的点的坐标，f为焦距，z为垂直 高度； 对式(2)两边求导，即得光流速度： (3) 式(3)中，Vc为摄像头坐标系下点的运动速度，vz为垂直方向上的运动速度，v为图像平 面的运动速度，即光流速度； 菊丨田_丨优彳云効登的/A忒.式(4)中，Vw表示世界坐标系下点的运动速度，ω表示角速度； 将公式(4)带入公式(3)，展开可得到X轴和y轴方向的光流速度与世界坐标系下的飞行 器速度的关系：式(5)中，Vu表示图像平面X轴方向的光流速度，表示相机坐标系下X轴方向的速度， Vwx表示世界坐标系下X轴方向的速度，Vwz表示世界坐标系下Z轴方向的速度，Wx，Wy，W z分别 表示X，y, ζ轴方向上的角速度，X。表示相机坐标系下X轴方向的坐标，y。表示相机坐标系下y 轴方向的坐标;u，V分别表示图像平面X，y轴方向的坐标； 式(6)中，Vv表示图像平面y轴方向的光流速度，vwy表示世界坐标系下y轴方向的速度； Vwz表示世界坐标系下ζ轴方向的速度;对垂直高度信息差分可以求得世界坐标系下ζ轴方向 的速度根据式(5)和(6)以及已知的世界坐标系下ζ轴方向的速度，可以求得世界坐标系下X轴 和y轴方向的飞行器的速度，即为飞行器的水平速度。4. 根据权利要求3所述的基于定位传感装置的飞行器定位方法，其特征在于，当飞行器 定高飞行时，式(5)和(6)简化为：(7) (8X5. 根据权利要求1所述的基于定位传感装置的飞行器定位方法，其特征在于，对所述的 飞行器定位方法进行优化，所述的摄像头获取地面的实时图像信息为规则的网格信息，包 括如下步骤： i) 利用摄像头，获取地面的实时图像信息，进一步处理实时图像信息中的规则的网格 信息，使用霍夫变换算法，调整筛选直线的阈值，对其中的直线信息进行准确提取，从而获 取实时图像信息中的直线交点坐标； ii) 同步惯性传感单元、超声波传感器和摄像头的时间戳，对飞行器的水平速度积分， 计算飞行器的初始相对位移;给定飞行器的初始位置估计;利用惯性传感单元给定实时姿 态信息，给定飞行器的初始姿态信息估计;使用摄像头的投影模型和实时图像信息中获取 的直线交点坐标，获取地面网格交点的坐标； iii)根据地面网格交点相邻距离恒定的条件，对求得的地面网格交点的坐标进行求精 操作，获取准确的地面网格交点的坐标，再利用实时图像信息对应的坐标，建立匹配点对， 使用直接线性变换算法或者迭代算法，求得飞行器准确的位置信息及姿态信息。6.根据权利要求5所述的基于定位传感装置的飞行器定位方法，其特征在于，所述的步 骤ii)中获取地面网格交点的实际坐标的方法为：相据走时_梅信自.由的粗略位置估计，公式如下： (9) 式(9)中，vw为水平光流速度，?为前一时刻水平位置，g为后一时刻水平位置，dt为 间隔时间，另外令超声波传感器在k-l时刻和k时刻给入的垂直高度信息分别为 ，给定飞行器的初始位置估计，其中均为三维列向量，表示X，y，ζ轴方向的位置； 由惯性传感单元给定飞行器实时的姿态信息，包括偏航角、横滚角和俯仰角，如下： roll= γ ,pitch = Θ ,yaw = it (10) 式(10)中，rο 11、ρ i t ch、yaw分别为该时刻的横滚角、俯仰角和偏航角； 对于步骤i)中获取的实时图像信息中的直线交点坐标(u，v)，都满足直线方程： au+bv+c = 0 (11); 摄像头的投影模型由以下公式表达：式(12)中，λ表示相机坐标系下特征点的ζ轴坐标，（nl)1表示实时图像信息中特征点 的齐次坐标，K表示摄像头的内参矩阵，匕，心，(^，(^分别表示1，7轴方向的像素焦距以及主 点的像素坐标;C=表示世界坐标系相对于相机坐标系的转换矩阵，即外参矩阵，前三行三列 矩阵表示旋转矩阵，最后一列矩阵表示平移向量；（^，^，^，1)7表示世界坐标系下特征点 的齐次坐标； 将直线方程公式(11)代入摄像头的投影模型公式(12)，得到公式如下：给定飞行器的初始姿态信息估计和初始位置估计，如下：将给定的参数代入公式（13)，求解公式（13)的直线方程，设求出的世界坐标系的坐标 点Pw为：对世界坐标系中的直线交点Uw，yw，ZW)，同样建立直线方程：利用规则的网格信息的特征，直线只分为两类，两类直线互相垂直，且认为地面为平 面._ L=η _彡忽吉姥卞'fg公先两类： (18) 由公式（18)对世界坐标系的坐标点进行求精操作，即水平和垂直的两类直线分布间隔 均匀，地面的直线实际交点坐标为常值，以此可以对1^=(^/，^'，2/)求出的坐标中的^/， yw'取与之相差绝对值最小的常值xw"，yw"，zw'认为是0;求精后的世界坐标系下的直线交点 的坐标值为Pw= (xw"，yw"，0); 利用公式(13)代入求精后pw的坐标，如下： 简化后得到：提取未知数ril，Γ21，?1，ri2，Γ22，?2，tx，ty，tz，重写方程（13)，有：I ZZ )\ 方程(22)是在取两类直线中的各一条建立的方程组，由于存在9个未知数，至少需要五 条直线才能解该线性方程组;但直线数超过5条后，该方程组为超定方程组，对其进行奇异 值分解即可解该方程组； 至此，解出旋转矩阵R和平移向量t，如下：(23 ) 在已求出^^^^^以的情况下河利用旋转矩阵是单位正交矩阵这一特性， 求 ttSri3，Γ23，?3 ; 再将旋转矩阵转换为姿态角，得到姿态信息：(24 )。7. 根据权利要求6所述的基于定位传感装置的飞行器定位方法，其特征在于，对所述的 飞行器定位方法进一步优化，采用光流速度信息，网格定位的位置信息、惯性传感单元的加 速度信息，建立状态方程和观测方程，对视觉算法和惯性传感单元获取的结果使用扩展卡 尔曼滤波算法，进行融合处理，获取飞行器准确的位置及姿态信息。8. 跟据权利要求7所述的基于定位传感装置的飞行器定位方法，其特征在于，所述的获 取飞行器准确的位置及姿态信息具体的算法推导如下： 非线性连续状杰方稈如下：(25 ) 对其进行离散化操作，状态量包括有网格定位得到的位置信息Ρ，光流法提供的光流速 度信息^和惯性传感单元提供的加速度a; At表示连续两次采样的间隔时间，A表示状态转 移矩阵； 观测方程如下，分为视觉算法部分的观测方程，观测量位置P ;惯性传感单元的观测方 程，观测量取加速度a:(26) 其中公式(26)中，Zv,k，Zl,k分别是视觉算法和惯性传感单元的观测量，分别是世界坐标 系下的水平方向的位置信息和加速度信息； 根据卡尔曼滤波算法的公式：可分别计算视觉算法部分和惯性传感单元部分的卡尔曼增益Kv,k，Ki,k，其中Pk-i表示对 应于状态协方差，:P；表示对应于状态X &的协方差，Q表示系统噪声的协方差矩阵，Rv， R1分别表示视觉算法和惯性传感单元噪声的协方差矩阵;再结合预测值和测量值，分别求 取视觉部分和惯性传感单元部分状态量的最优估值Xk，其中Xv lslcin, X1MU分别表示视觉算法 的测量值和惯性传感单元的测量值;最后再次更新相对于状态量Xk的协方差矩阵。
【文档编号】G01C21/20GK106017463SQ201610362320
【公开日】2016年10月12日
【申请日】2016年5月26日
【发明人】许超, 叶波
【申请人】浙江大学