输出速率可调的视觉导航最优信息估计方法
【专利摘要】本发明公开了输出速率可调的视觉导航最优信息估计方法，属于视觉导航的技术领域。现有的方法由于图像处理耗时的不确定性造成图像数据的非等速率输出，不利于系统间的集成和信息交流；并且采集到的图像数据会受到噪声的干扰，干扰过大会造成数据丢失。针对以上两点，本发明采用多线程的方式，使图像处理和数据输出模块化，从而实现数据的等速率输出；并且对于图像解算后的位置信息进行卡尔曼滤波处理，实现视觉导航最优信息的估计。本发明可根据实际应用情况调节导航数据输出速率。
【专利说明】
输出速率可调的视觉导航最优信息估计方法
技术领域
[0001] 本发明公开了输出速率可调的视觉导航最优信息估计方法，属于视觉导航的技术 领域。
【背景技术】
[0002] 随着导航技术的不断发展，越来越多新的导航方式被发现和研究。视觉导航便是 其中尤为突出的一种导航方式。视觉导航是通过摄像机对周围环境进行图像采集，并对图 像进行滤波和计算，完成自身位姿确定和路径识别，并做出导航决策的一种新的导航技术。 由于视觉导航采用被动工作方式，设备简单，成本低廉，其应用范围很广，广泛应用于航空 航天、机器人以及智能车辆交通领域。
[0003] 视觉导航技术对于图像处理有着很高的要求，一方面，要求图像处理解算后的信 息的准确性尽可能高;另一方面，对于图像信息的解算速率及输出更新速率要求比较严格。 从图像中提取定位特征具有较大的不确定性，导致在视觉导航中不同图像的处理时间不 同，而往往视觉导航系统需要跟其他子系统(如与惯性导航进行融合、输出数据到飞行控制 系统等)进行交互，等速率的导航参数输出对于视觉导航系统的模块化及与其它子系统的 集成具有重要意义。基于视觉导航技术对图像处理的要求，提出一套适合视觉导航的等速 率数据输出方法，以解决图像解算精度和图像输出更新速率的问题。
【发明内容】
[0004] 本发明所要解决的技术问题是针对上述【背景技术】的不足，考虑到视觉导航技术对 图像解算精度的要求以及图像输出更新速率的要求，提供了输出速率可调的视觉导航最优 信息估计方法，在保证导航参数等速率输出的同时，也保证了视觉导航系统输出信息的最 优性，解决了图像解算精度和图像输出更新速率相互协调的技术问题。
[0005] 本发明为实现上述发明目的采用如下技术方案：
[0006] 输出速率可调的视觉导航最优信息估计方法，采用包括主线程、图像处理子线程、 数据输出子线程的多线程机制实现视觉导航最优信息的估计和输出，各线程通过调用全局 变量交换数据，具体为：
[0007] 图像处理子线程:根据摄像头采集的图像解算位置信息的量测值以及飞机运动状 态估计位置信息的预测值，对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置 信息的最优估计值，更新全局变量并反馈当前位置信息的最优估计值给主线程；
[0008]主线程:在接收到图像处理子线程反馈的位置信息最优估计值后，更新全局变量 并开启多线程的并发执行状态；
[0009]数据输出子线程:在输出时刻到来时请求主线程查询当前位置信息最优估计值对 应的时刻，仅在当前输出时刻与当前位置信息最优估计值对应时刻的差值符合阈值要求 时，根据当前位置信息最优估计值预估当前输出时刻的位置信息，输出当前输出时刻的位 置信息预测值。
[0010] 进一步的，所述输出速率可调的视觉导航最优信息估计方法中，图像处理子线程 估计位置信息预测值的方法为：
[0011] 以上一时刻的位置信息最优估计x(t〇)为参考值，采用卡尔曼滤波器对当前时刻 的位置信息预测值X(ti+A ti I to)进行估计:X(ti+ Δ ti I to) =AX(to)+BU(ti+ Δ ti)，A、B为系 统参数，U ( t i + Δ t i )为当前时刻对系统的控制量，U ( t i + Δ t i )取值为零，
[0012] 更新当前时刻的位置信息预测值X(t# Δ t! I to)的协方差PU1+Δ t! I to) JU1+ Δ乜 七0)=4?^〇)4〃 + 〇，？^())为上一时刻的位置信息最优预测值父4())的协方差，
JT为卡尔曼滤波器相 邻两个估计时刻的时间差，dT的初始值取0.5。
[0013] 再进一步的，所述输出速率可调的视觉导航最优信息估计方法中，图像处理子线 程对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估计值，具体 方法为：
[0014] 先由表达式:X(ti+ Δ ti) = X(ti+ Δ ti I to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)_HX(ti+ Δ ti to))得到当前时刻位置信息最优估计值X(ti+A ti)，再由表达式:P(ti+A ti) = (I_Kg(ti+ Δ ti)H)P(ti+A ti I to)更新当前时刻位置信息最优估计值X(ti+A ti)的协方差P(ti+A ti)，将 当前时刻位置信息最优估计值XU1+At1)作为估计下一时刻位置信息预测值的参考值，Z U 1+At1)为当前时刻位置信息的测量值，I为单位矩阵，Kg(WAt1)为卡尔曼增益，KgU 1+ Δ ti) =PU1+ Δ ti I to)H-V(HP(ti+ Δ ti I to)H-T + R)，H为测量系统的参数矩阵，
[0015] 更进一步的，所述输出速率可调的视觉导航最优信息估计方法中，数据输出线程 输出时刻间隔接近图像处理时间的平均值或众数。
[0016] 实现所述视觉导航最优信息估计方法的系统，包括：
[0017] 根据摄像头采集的图像预测位置信息的图像处理模块，以及，
[0018] 在输出时刻到来时根据当前位置信息进行最优估计并输出当前输出时刻位置信 息的滤波预测输出模块。
[0019] 本发明采用上述技术方案，具有以下有益效果：
[0020] (1)对图像采集的数据进行预处理和位姿解算，将所得到的数据作为量测值与通 过系统模型得到的预测值进行卡尔曼滤波融合得到最优估计值，进而更新当前状态，大大 提升可靠性和准确性，避免了因为某时刻噪声干扰过大使得图像数据丢失而导致导航失败 的现象；
[0021] (2)数据输出线程实现了等速率导航数据的输出，并且可根据实际应用情况调节 导航数据输出速率；
[0022] (3)通过多线程机制将图像处理部分与数据输出部分分离开，协调了图像处理耗 时不确定与数据等速率输出之间的矛盾，并使得图像处理部分模块化，便于与其他模块集 成。
【附图说明】
[0023]图1为图像数据处理以及图像数据输出的过程；
[0024]图2(a)、图2(b)分别为图像处理子线程、数据输出子线程的流程图。
【具体实施方式】
[0025]下面结合附图对发明的技术方案进行详细说明。
[0026]现有的方法由于图像处理耗时的不确定性造成图像数据的非等速率输出，不利于 系统间的集成和信息交流;并且采集到的图像数据会受到噪声的干扰，干扰过大会造成数 据丢失。针对以上两点，本发明采用多线程的方式，使图像处理和数据输出模块化，从而实 现数据的等速率输出；并且对于图像解算后的位置信息进行卡尔曼滤波处理，实现视觉导 航最优信息的估计。本发明可根据实际应用情况调节导航数据输出速率。
[0027] 步骤一:建立多线程机制。
[0028]在Linux操作系统环境下创建线程对象，然后对线程对象进行管理：
[0029] (1)定义一个DataShare类，将其实例化后的对象变量作为全局变量，供各个线程 之间相互调用，实现线程之间的数据交换；
[0030] (2)通过threading模块的方法创建图像处理线程和数据输出线程，继承Thread 类，对类中的run()方法进行重写，来实现图像处理线程以及数据输出线程所要完成的任 务；
[0031] (3)通过类中的SetDaemon(True)方法将主线程设置为为守护线程，即主线程执行 结束后，子线程（图像处理线程、数据输出线程）一并和主线程结束退出，然后通过类的 StartO方法开启子线程；
[0032] (4)在主线程和子线程并发执行时，全局变量作为连接多个线程的桥梁，子线程处 理图像数据后对全局变量进行更新，同时将更新后的结果反馈给主线程进行处理，多线程 并发执行。
[0033] 启动图像处理子线程，具体线程执行步骤见步骤二，启动数据输出子线程，具体线 程执行步骤见步骤三。
[0034] 步骤二:在图像处理线程中加入卡尔曼滤波环节以实现对视觉导航信息的最优估 计。
[0035] (1)卡尔曼滤波器根据上一时刻的位置信息最优估计值X(to)进一步预测得到当 前时刻的位置信息预测值XU1+ Δ tl I to)(预测更新）：
[0036] X(ti+ Δ ti I to) =AX(to)+BU(/ ti+ A t, nI (I)
[0037] 式（I)中，A、B为系统参数， ilKti+Δ ti)是当前时刻 (t i)对系统的控制量，U (△ t i)取值为0。
[0038] (2)对父(1：1+&1：1|1:。)的协方差？(1：1+&1：1|1:。)进行更新：
[0039] P(ti+Ati|to)=AP(to)A-T+Q (2)
[0040] 式（2)中P (to)为上一时刻的位置信息最优预测值X (to)的协方差，
IT为卡尔曼滤波器相 邻两个估计时刻的时间差，dT的初始值取0.5。
[0041] (3)图像处理线程通过摄像头采集图像信息，然后对其进行图像处理和导航解算 得到当前时刻位置信息的量测量ZU1+ Δ tl)，然后对ZU1+ Δ tl)和XU1+ Δ tl I to)进行卡尔 曼滤波融合得到当前时刻位置信息的最优估计值XU1+At1K量测更新）：
[0042] X(ti+ Δ ti) = X(ti+ Δ ti | to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)-HX(ti+ Δ ti | to)) (3)
[0043] 式⑶中，Kg(^At1)为卡尔曼增益，
[0044] KgU1+ Δ ti) =PU1+ Δ ti I to)H-tAHPU1+ Δ ti I to)H-T+R)，H为测量系统的参数矩 阵
[0045] (4)对XUdAt1)的协方差进行更新：
[0046] P(ti+ Ati) = (I~Kg(ti+ Δ ti)H)P(ti+ Δ ti | to) (4)
[0047] 式(4)中，I为单位矩阵。
[0048] (S)X(WAt1)作为下一时刻位置信息预测值的参考值，以此类推。
[0049] (6)此外当出现当前时刻的测量值为空时，即没有得到测量值时，卡尔曼滤波器会 根据上一时刻的最优估计值进行预测估计，来作为当前的最优估计值。
[0050] 步骤三:数据输出线程在输出时刻tP+nT到来时，查询当前卡尔曼最优估计值的时 刻t，然后用其时间差t P+nT-t预测tP+nT时刻的输出值：
[0051] X(tP+nT)=AX(t)+BU(t) (5)
[0052] 最后将输出的图像位置数据供其它处理线程所用。
[0053] 通过调节数据输出线程的时间常数T即可改变图像数据的输出更新速率，实现了 数据速率可调节的要求。T的选取并不是任意的，需要根据图像的采集处理时间来调节，假 设图像的采集、处理时间为20-40ms之间（每帧图像不同，处理的时间会有所差异），那么我 们T的选取也要遵照这个时间，最好选择在这个时间范围附近，太大和太小都会使得整个系 统输出的数据精度下降。如果T太大，会使图像数据的更新速率过慢，延时严重，使得控制得 不到实时的矫正;反之如果T过小(T = 2ms)，每十几次输出才会有一次图像数据的更新，速 率不匹配造成大量无效的数据访问，影响了资源利用率。因此T的选择尽量接近图像的采集 处理时间的平均值或者其众数。
[0054]鉴于本申请的多线程机制，构建根据摄像头采集的图像预测位置信息的图像处理 模块，根据数据输出线程和卡尔曼最优估计值建立滤波预测输出模块，在滤波预测输出模 块外部建立数据输出线程作为图像处理模块的外部接口。
[0055]图1为图像数据处理以及图像数据输出的过程，可以看出多线程的建立使得图像 处理线程模块化，图像处理可以独立的循环执行，不受其他进程的影响而中断。其中数据输 出线程为图像处理模块与外界其他模块间相互交流提供了访问接口，并可以改变数据输出 线程的数据更新速率。从图中可以看出，在输出时刻t P+nT到来时，查询当前位置信息最优 估计值对应的时刻tk+Atk，根据当前位置信息最优估计值预估当前输出时刻的位置信息， 输出当前输出时刻的位置信息预测值。
[0056]图2(a)、图2(b)分别为图像处理线程、数据输出线程的流程图，多线程的建立使得 整个系统模块化，便于系统内部的集成。
[0057]综上所述，本发明具有以下有益效果：
[0058] (1)对图像采集的数据进行预处理和位姿解算，将所得到的数据作为量测值与通 过系统模型得到的预测值进行卡尔曼滤波融合得到最优估计值，进而更新当前状态，大大 提升可靠性和准确性，避免了因为某时刻噪声干扰过大使得图像数据丢失而导致导航失败 的现象；
[0059] (2)数据输出线程实现了等速率导航数据的输出，并且可根据实际应用情况调节 导航数据输出速率；
[0060] (3)通过多线程机制将图像处理部分与数据输出部分分离开，协调了图像处理耗 时不确定与数据等速率输出之间的矛盾，并使得图像处理部分模块化，便于与其他模块集 成。
【主权项】
1. 输出速率可调的视觉导航最优信息估计方法，其特征在于，采用包括主线程、图像处 理子线程、数据输出子线程的多线程机制实现视觉导航最优信息的估计和输出，各线程通 过调用全局变量交换数据，具体为： 图像处理子线程:根据摄像头采集的图像解算位置信息的量测值以及飞机运动状态估 计位置信息的预测值，对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息 的最优估计值，更新全局变量并反馈当前位置信息的最优估计值给主线程； 主线程:在接收到图像处理子线程反馈的位置信息最优估计值后，更新全局变量并开 启多线程的并发执行状态； 数据输出子线程:在输出时刻到来时请求主线程查询当前位置信息最优估计值对应的 时刻，仅在当前输出时刻与当前位置信息最优估计值对应时刻的差值符合阈值要求时，根 据当前位置信息最优估计值预估当前输出时刻的位置信息，输出当前输出时刻的位置信息 预测值。2. 根据权利要求1所述输出速率可调的视觉导航最优信息估计方法，其特征在于，图像 处理子线程估计位置信息预测值的方法为： 以上一时刻的位置信息最优估计X(t〇)为参考值，采用卡尔曼滤波器对当前时刻的位置 十目息预测值X(ti+ Δ ti I to)进彳丁估计:X(ti+ Δ ti I to) =AX(to)+BU(ti+ Δ ti)，A、B为系统参数， U( h)为当前时刻对系统的控制量，U(Δ )取值为零，更新当前时刻的位置?目息预测值X(ti+ Δ ti I to)的协方差P(ti+ Δ ti I to) :P(ti+ Δ ti I to)邻两个估计时刻的时间差，dT的初始值取0.5。3. 根据权利要求2所述输出速率可调的视觉导航最优信息估计方法，其特征在于，图像 处理子线程对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估 计值，具体方法为： 先由表达式:x(ti+ Δ ti) = X(ti+ Δ ti I to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)-HX(ti+ Δ ti I to))得 到当前时刻位置信息最优估计值XU1+Δ t〇，再由表达式:PU1+Δ t〇 = (I-KgU1+Δ ")Η)Ρ (ti+Δ ti| to)更新当前时刻位置信息最优估计值X(ti+A ti)的协方差P(ti+A ti)，将当前时 刻位置信息最优估计值X(ti+A ti)作为估计下一时刻位置信息预测值的参考值，Z(ti+A t〇为当前时刻位置信息的测量值，I为单位矩阵，Kg(WAt 1)为卡尔曼增益，KgU1+At1) = Pft.! + Λ t.! I t. η?Η - T"HPft. ! + Λt. ! I t. η?Η - T + m 测骨蒸絲的暴数矩阵， 4 .根据权利要求3所述输出速率可调的视觉导航最优信息估计方法，其特征在于，数据 输出线程输出时刻间隔接近图像处理时间的平均值或众数。5.实现权利要求1所述视觉导航最优信息估计方法的系统，其特征在于，包括： 根据摄像头采集的图像预测位置信息的图像处理模块，以及， 在输出时刻到来时根据当前位置信息进行最优估计并输出当前输出时刻位置信息的 滤波预测输出模块。
【文档编号】G01C21/20GK105890596SQ201610192247
【公开日】2016年8月24日
【申请日】2016年3月30日
【发明人】李旺灵, 孙永荣, 黄斌, 王勇, 刘建业, 吴玲, 熊智, 朱云峰
【申请人】南京航空航天大学