CN105890596B - 输出速率可调的视觉导航最优信息估计方法 - Google Patents

输出速率可调的视觉导航最优信息估计方法 Download PDF

Info

Publication number
CN105890596B
CN105890596B CN201610192247.7A CN201610192247A CN105890596B CN 105890596 B CN105890596 B CN 105890596B CN 201610192247 A CN201610192247 A CN 201610192247A CN 105890596 B CN105890596 B CN 105890596B
Authority
CN
China
Prior art keywords
location information
value
estimation
output
optimal
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201610192247.7A
Other languages
English (en)
Other versions
CN105890596A (zh
Inventor
李旺灵
孙永荣
黄斌
王勇
刘建业
吴玲
熊智
朱云峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201610192247.7A priority Critical patent/CN105890596B/zh
Publication of CN105890596A publication Critical patent/CN105890596A/zh
Application granted granted Critical
Publication of CN105890596B publication Critical patent/CN105890596B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G06T5/70—
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20024—Filtering details

Abstract

本发明公开了输出速率可调的视觉导航最优信息估计方法，属于视觉导航的技术领域。现有的方法由于图像处理耗时的不确定性造成图像数据的非等速率输出，不利于系统间的集成和信息交流；并且采集到的图像数据会受到噪声的干扰，干扰过大会造成数据丢失。针对以上两点，本发明采用多线程的方式，使图像处理和数据输出模块化，从而实现数据的等速率输出；并且对于图像解算后的位置信息进行卡尔曼滤波处理，实现视觉导航最优信息的估计。本发明可根据实际应用情况调节导航数据输出速率。

Description

输出速率可调的视觉导航最优信息估计方法
技术领域
本发明公开了输出速率可调的视觉导航最优信息估计方法，属于视觉导航的技术领域。
背景技术
随着导航技术的不断发展，越来越多新的导航方式被发现和研究。视觉导航便是其中尤为突出的一种导航方式。视觉导航是通过摄像机对周围环境进行图像采集，并对图像进行滤波和计算，完成自身位姿确定和路径识别，并做出导航决策的一种新的导航技术。由于视觉导航采用被动工作方式，设备简单，成本低廉，其应用范围很广，广泛应用于航空航天、机器人以及智能车辆交通领域。
视觉导航技术对于图像处理有着很高的要求，一方面，要求图像处理解算后的信息的准确性尽可能高；另一方面，对于图像信息的解算速率及输出更新速率要求比较严格。从图像中提取定位特征具有较大的不确定性，导致在视觉导航中不同图像的处理时间不同，而往往视觉导航系统需要跟其他子系统(如与惯性导航进行融合、输出数据到飞行控制系统等)进行交互，等速率的导航参数输出对于视觉导航系统的模块化及与其它子系统的集成具有重要意义。基于视觉导航技术对图像处理的要求，提出一套适合视觉导航的等速率数据输出方法，以解决图像解算精度和图像输出更新速率的问题。
发明内容
本发明所要解决的技术问题是针对上述背景技术的不足，考虑到视觉导航技术对图像解算精度的要求以及图像输出更新速率的要求，提供了输出速率可调的视觉导航最优信息估计方法，在保证导航参数等速率输出的同时，也保证了视觉导航系统输出信息的最优性，解决了图像解算精度和图像输出更新速率相互协调的技术问题。
本发明为实现上述发明目的采用如下技术方案：
输出速率可调的视觉导航最优信息估计方法，采用包括主线程、图像处理子线程、数据输出子线程的多线程机制实现视觉导航最优信息的估计和输出，各线程通过调用全局变量交换数据，具体为：
图像处理子线程：根据摄像头采集的图像解算位置信息的量测值以及飞机运动状态估计位置信息的预测值，对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估计值，更新全局变量并反馈当前位置信息的最优估计值给主线程；
主线程：在接收到图像处理子线程反馈的位置信息最优估计值后，更新全局变量并开启多线程的并发执行状态；
数据输出子线程：在输出时刻到来时请求主线程查询当前位置信息最优估计值对应的时刻，仅在当前输出时刻与当前位置信息最优估计值对应时刻的差值符合阈值要求时，根据当前位置信息最优估计值预估当前输出时刻的位置信息，输出当前输出时刻的位置信息预测值。
进一步的，所述输出速率可调的视觉导航最优信息估计方法中，图像处理子线程估计位置信息预测值的方法为：
以上一时刻的位置信息最优估计X(t0)为参考值，采用卡尔曼滤波器对当前时刻的位置信息预测值X(t1+Δt1|t0)进行估计：X(t1+Δt1|t0)＝AX(t0)+BU(t1+Δt1)，A、B为系统参数，U(t1+Δt1)为当前时刻对系统的控制量，U(t1+Δt1)取值为零，
更新当前时刻的位置信息预测值X(t1+Δt1|t0)的协方差P(t1+Δt1|t0)：P(t1+Δt1|t0)＝AP(t0)A-T+Q，P(t0)为上一时刻的位置信息最优预测值X(t0)的协方差，Q为系统过程噪声的协方差，dT为卡尔曼滤波器相邻两个估计时刻的时间差，dT的初始值取0.5。
再进一步的，所述输出速率可调的视觉导航最优信息估计方法中，图像处理子线程对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估计值，具体方法为：
先由表达式：X(t1+Δt1)＝X(t1+Δt1|t0)+Kg(t1+Δt1)(Z(t1+Δt1)-HX(t1+Δt1|t0))得到当前时刻位置信息最优估计值X(t1+Δt1)，再由表达式：P(t1+Δt1)＝(I-Kg(t1+Δt1)H)P(t1+Δt1|t0)更新当前时刻位置信息最优估计值X(t1+Δt1)的协方差P(t1+Δt1)，将当前时刻位置信息最优估计值X(t1+Δt1)作为估计下一时刻位置信息预测值的参考值，Z(t1+Δt1)为当前时刻位置信息的测量值，I为单位矩阵，Kg(t1+Δt1)为卡尔曼增益，Kg(t1+Δt1)＝P(t1+Δt1|t0)H-T/(HP(t1+Δt1|t0)H-T+R)，H为测量系统的参数矩阵，R为测量噪声的协方差，
更进一步的，所述输出速率可调的视觉导航最优信息估计方法中，数据输出线程输出时刻间隔接近图像处理时间的平均值或众数。
实现所述视觉导航最优信息估计方法的系统，包括：
根据摄像头采集的图像预测位置信息的图像处理模块，以及，
在输出时刻到来时根据当前位置信息进行最优估计并输出当前输出时刻位置信息的滤波预测输出模块。
本发明采用上述技术方案，具有以下有益效果：
(1)对图像采集的数据进行预处理和位姿解算，将所得到的数据作为量测值与通过系统模型得到的预测值进行卡尔曼滤波融合得到最优估计值，进而更新当前状态，大大提升可靠性和准确性，避免了因为某时刻噪声干扰过大使得图像数据丢失而导致导航失败的现象；
(2)数据输出线程实现了等速率导航数据的输出，并且可根据实际应用情况调节导航数据输出速率；
(3)通过多线程机制将图像处理部分与数据输出部分分离开，协调了图像处理耗时不确定与数据等速率输出之间的矛盾，并使得图像处理部分模块化，便于与其他模块集成。
附图说明
图1为图像数据处理以及图像数据输出的过程；
图2(a)、图2(b)分别为图像处理子线程、数据输出子线程的流程图。
具体实施方式
下面结合附图对发明的技术方案进行详细说明。
现有的方法由于图像处理耗时的不确定性造成图像数据的非等速率输出，不利于系统间的集成和信息交流；并且采集到的图像数据会受到噪声的干扰，干扰过大会造成数据丢失。针对以上两点，本发明采用多线程的方式，使图像处理和数据输出模块化，从而实现数据的等速率输出；并且对于图像解算后的位置信息进行卡尔曼滤波处理，实现视觉导航最优信息的估计。本发明可根据实际应用情况调节导航数据输出速率。
步骤一：建立多线程机制。
在Linux操作系统环境下创建线程对象，然后对线程对象进行管理：
(1)定义一个DataShare类，将其实例化后的对象变量作为全局变量，供各个线程之间相互调用，实现线程之间的数据交换；
(2)通过threading模块的方法创建图像处理线程和数据输出线程，继承Thread类，对类中的run()方法进行重写，来实现图像处理线程以及数据输出线程所要完成的任务；
(3)通过类中的setDaemon(True)方法将主线程设置为为守护线程，即主线程执行结束后，子线程(图像处理线程、数据输出线程)一并和主线程结束退出，然后通过类的start()方法开启子线程；
(4)在主线程和子线程并发执行时，全局变量作为连接多个线程的桥梁，子线程处理图像数据后对全局变量进行更新，同时将更新后的结果反馈给主线程进行处理，多线程并发执行。
启动图像处理子线程，具体线程执行步骤见步骤二，启动数据输出子线程，具体线程执行步骤见步骤三。
步骤二：在图像处理线程中加入卡尔曼滤波环节以实现对视觉导航信息的最优估计。
(1)卡尔曼滤波器根据上一时刻的位置信息最优估计值X(t0)进一步预测得到当前时刻的位置信息预测值X(t1+Δt1|t0)(预测更新)：
X(t1+Δt1|t0)＝AX(t0)+BU(t1+Δt1) (1)
式(1)中，A、B为系统参数，U(t1+Δt1)是当前时刻(t1)对系统的控制量，U(t1+Δt1)取值为0。
(2)对X(t1+Δt1|t0)的协方差P(t1+Δt1|t0)进行更新：
P(t1+Δt1|t0)＝AP(t0)A-T+Q (2)
式(2)中P(t0)为上一时刻的位置信息最优预测值X(t0)的协方差，
Q是系统过程噪声的协方差，dT为卡尔曼滤波器相邻两个估计时刻的时间差，dT的初始值取0.5。
(3)图像处理线程通过摄像头采集图像信息，然后对其进行图像处理和导航解算得到当前时刻位置信息的量测量Z(t1+Δt1)，然后对Z(t1+Δt1)和X(t1+Δt1|t0)进行卡尔曼滤波融合得到当前时刻位置信息的最优估计值X(t1+Δt1)(量测更新)：
X(t1+Δt1)＝X(t1+Δt1|t0)+Kg(t1+Δt1)(Z(t1+Δt1)-HX(t1+Δt1|t0)) (3)
式(3)中，Kg(t1+Δt1)为卡尔曼增益，
Kg(t1+Δt1)＝P(t1+Δt1|t0)H-T/(HP(t1+Δt1|t0)H-T+R)，H为测量系统的参数矩阵，R为测量噪声的协方差，
(4)对X(t1+Δt1)的协方差进行更新：
P(t1+Δt1)＝(I-Kg(t1+Δt1)H)P(t1+Δt1|t0) (4)
式(4)中，I为单位矩阵。
(5)X(t1+Δt1)作为下一时刻位置信息预测值的参考值，以此类推。
(6)此外当出现当前时刻的测量值为空时，即没有得到测量值时，卡尔曼滤波器会根据上一时刻的最优估计值进行预测估计，来作为当前的最优估计值。
步骤三：数据输出线程在输出时刻tp+nT到来时，查询当前卡尔曼最优估计值的时刻t，然后用其时间差tp+nT-t预测tp+nT时刻的输出值：
X(tp+nT)＝AX(t)+BU(tp+nT) (5)
最后将输出的图像位置数据供其它处理线程所用。
通过调节数据输出线程的时间常数T即可改变图像数据的输出更新速率，实现了数据速率可调节的要求。T的选取并不是任意的，需要根据图像的采集处理时间来调节，假设图像的采集、处理时间为20-40ms之间(每帧图像不同，处理的时间会有所差异)，那么我们T的选取也要遵照这个时间，最好选择在这个时间范围附近，太大和太小都会使得整个系统输出的数据精度下降。如果T太大，会使图像数据的更新速率过慢，延时严重，使得控制得不到实时的矫正；反之如果T过小(T＝2ms)，每十几次输出才会有一次图像数据的更新，速率不匹配造成大量无效的数据访问，影响了资源利用率。因此T的选择尽量接近图像的采集处理时间的平均值或者其众数。
鉴于本申请的多线程机制，构建根据摄像头采集的图像预测位置信息的图像处理模块，根据数据输出线程和卡尔曼最优估计值建立滤波预测输出模块，在滤波预测输出模块外部建立数据输出线程作为图像处理模块的外部接口。
图1为图像数据处理以及图像数据输出的过程，可以看出多线程的建立使得图像处理线程模块化，图像处理可以独立的循环执行，不受其他进程的影响而中断。其中数据输出线程为图像处理模块与外界其他模块间相互交流提供了访问接口，并可以改变数据输出线程的数据更新速率。从图中可以看出，在输出时刻tp+nT到来时，查询当前位置信息最优估计值对应的时刻tk+Δtk，根据当前位置信息最优估计值预估当前输出时刻的位置信息，输出当前输出时刻的位置信息预测值。
图2(a)、图2(b)分别为图像处理线程、数据输出线程的流程图，多线程的建立使得整个系统模块化，便于系统内部的集成。
综上所述，本发明具有以下有益效果：
(1)对图像采集的数据进行预处理和位姿解算，将所得到的数据作为量测值与通过系统模型得到的预测值进行卡尔曼滤波融合得到最优估计值，进而更新当前状态，大大提升可靠性和准确性，避免了因为某时刻噪声干扰过大使得图像数据丢失而导致导航失败的现象；
(2)数据输出线程实现了等速率导航数据的输出，并且可根据实际应用情况调节导航数据输出速率；
(3)通过多线程机制将图像处理部分与数据输出部分分离开，协调了图像处理耗时不确定与数据等速率输出之间的矛盾，并使得图像处理部分模块化，便于与其他模块集成。

Claims (5)

1.输出速率可调的视觉导航最优信息估计方法，其特征在于，采用包括主线程、图像处理子线程、数据输出子线程的多线程机制实现视觉导航最优信息的估计和输出，各线程通过调用全局变量交换数据，具体为：
图像处理子线程：根据摄像头采集的图像解算位置信息的量测值以及飞机运动状态估计位置信息的预测值，对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估计值，更新全局变量并反馈当前位置信息的最优估计值给主线程；
主线程：在接收到图像处理子线程反馈的位置信息最优估计值后，更新全局变量并开启多线程的并发执行状态；
数据输出子线程：在输出时刻到来时请求主线程查询当前位置信息最优估计值对应的时刻，仅在当前输出时刻与当前位置信息最优估计值对应时刻的差值符合阈值要求时，根据当前位置信息最优估计值预估当前输出时刻的位置信息，输出当前输出时刻的位置信息预测值。
2.根据权利要求1所述输出速率可调的视觉导航最优信息估计方法，其特征在于，图像处理子线程估计位置信息预测值的方法为：
以上一时刻的位置信息最优估计X(t0)为参考值，采用卡尔曼滤波器对当前时刻的位置信息预测值X(t1+Δt1|t0)进行估计：X(t1+Δt1|t0)＝AX(t0)+BU(t1+Δt1)，A、B为系统参数，U(t1+Δt1)为当前时刻对系统的控制量，U(t1+Δt1)取值为零，
更新当前时刻的位置信息预测值X(t1+Δt1|t0)的协方差P(t1+Δt1|t0)：P(t1+Δt1|t0)＝AP(t0)A-T+Q，P(t0)为上一时刻的位置信息最优预测值X(t0)的协方差，Q为系统过程噪声的协方差，dT为卡尔曼滤波器相邻两个估计时刻的时间差，dT的初始值取0.5。
3.根据权利要求2所述输出速率可调的视觉导航最优信息估计方法，其特征在于，图像处理子线程对位置信息的量测值以及预测值进行卡尔曼滤波融合得到位置信息的最优估计值，具体方法为：
先由表达式：X(t1+Δt1)＝X(t1+Δt1|t0)+Kg(t1+Δt1)(Z(t1+Δt1)-HX(t1+Δt1|t0))得到当前时刻位置信息最优估计值X(t1+Δt1)，再由表达式：P(t1+Δt1)＝(I-Kg(t1+Δt1)H)P(t1+Δt1|t0)更新当前时刻位置信息最优估计值X(t1+Δt1)的协方差P(t1+Δt1)，将当前时刻位置信息最优估计值X(t1+Δt1)作为估计下一时刻位置信息预测值的参考值，Z(t1+Δt1)为当前时刻位置信息的测量值，I为单位矩阵，Kg(t1+Δt1)为卡尔曼增益，Kg(t1+Δt1)＝P(t1+Δt1|t0)H-T/(HP(t1+Δt1|t0)H-T+R)，H为测量系统的参数矩阵，R为测量噪声的协方差，
4.根据权利要求3所述输出速率可调的视觉导航最优信息估计方法，其特征在于，数据输出线程输出时刻间隔接近图像处理时间的平均值或众数。
5.实现权利要求1所述视觉导航最优信息估计方法的系统，其特征在于，包括：
根据摄像头采集的图像预测位置信息的图像处理模块，以及，
在输出时刻到来时根据当前位置信息进行最优估计并输出当前输出时刻位置信息的滤波预测输出模块。
CN201610192247.7A 2016-03-30 2016-03-30 输出速率可调的视觉导航最优信息估计方法 Active CN105890596B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610192247.7A CN105890596B (zh) 2016-03-30 2016-03-30 输出速率可调的视觉导航最优信息估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610192247.7A CN105890596B (zh) 2016-03-30 2016-03-30 输出速率可调的视觉导航最优信息估计方法

Publications (2)

Publication Number Publication Date
CN105890596A CN105890596A (zh) 2016-08-24
CN105890596B true CN105890596B (zh) 2018-08-28

Family

ID=57014399

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610192247.7A Active CN105890596B (zh) 2016-03-30 2016-03-30 输出速率可调的视觉导航最优信息估计方法

Country Status (1)

Country Link
CN (1) CN105890596B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107870816B (zh) * 2016-09-27 2021-11-26 苏宁易购集团股份有限公司 一种用于图像处理和存储的方法及装置
CN109085840B (zh) * 2018-09-21 2022-05-27 大连维德集成电路有限公司 一种基于双目视觉的车辆导航控制系统及控制方法
CN110007601B (zh) * 2019-05-06 2022-03-25 广东工业大学 一种双边遥操作系统的控制装置和设备
CN110160524B (zh) * 2019-05-23 2020-12-01 深圳市道通智能航空技术有限公司 一种惯性导航系统的传感器数据获取方法及装置
CN113435386B (zh) * 2021-07-08 2023-06-02 浙江吉利控股集团有限公司 一种视觉车位无损滤波方法、装置及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103226357A (zh) * 2013-03-22 2013-07-31 海南大学 一种基于目标跟踪的多无人机通信决策方法
JP2014102559A (ja) * 2012-11-16 2014-06-05 Mitsubishi Electric Corp 画像処理装置、画像処理方法、画像処理を実行させるプログラム、及びプログラムを記録した情報記録媒体
CN104809917A (zh) * 2015-03-23 2015-07-29 南通大学 船舶实时跟踪监控方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014102559A (ja) * 2012-11-16 2014-06-05 Mitsubishi Electric Corp 画像処理装置、画像処理方法、画像処理を実行させるプログラム、及びプログラムを記録した情報記録媒体
CN103226357A (zh) * 2013-03-22 2013-07-31 海南大学 一种基于目标跟踪的多无人机通信决策方法
CN104809917A (zh) * 2015-03-23 2015-07-29 南通大学 船舶实时跟踪监控方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
基于多传感器的卫星自主导航信息融合算法;李丹，刘建业，熊智，张丽敏;《中国空间科学技术》;20080831(第4期);第13-18页 *
天文角度辅助的高超声速飞行器多信息融合导航算法;王融，熊智，刘建业，钟丽娜;《中国惯性技术学报》;20130831;第21卷(第4期);第484-488页 *
视觉导航技术的发展及其研究分析;王先敏，曾庆化，熊智，刘建业;《信息与控制》;20101031;第39卷(第5期);第607-613页 *
量测信息引导的组合导航融合滤波方法;王丽娜，黄斌，高晓颖，康国华，孙永荣;《上海交通大学学报》;20150930;第49卷(第9期);第1394-1399页 *

Also Published As

Publication number Publication date
CN105890596A (zh) 2016-08-24

Similar Documents

Publication Publication Date Title
CN105890596B (zh) 输出速率可调的视觉导航最优信息估计方法
CN107255476B (zh) 一种基于惯性数据和视觉特征的室内定位方法和装置
CN109034376B (zh) 一种基于lstm的无人机飞行状态预测方法及系统
CN106378780A (zh) 一种机器人系统、控制机器人的方法和服务器
CN110427047A (zh) 风速测算方法、风速估算器及无人机
CN105913051B (zh) 一种识别人脸图像的模版库的更新装置及方法
CN110039542A (zh) 具有速度方向控制的视觉伺服跟踪控制方法及机器人系统
CN109633664A (zh) 基于rgb-d与激光里程计的联合定位方法
CN109059930A (zh) 一种基于视觉里程计的移动机器人定位方法
CN110989682B (zh) 基于单基站的无人机精准降落方法
CN106780631A (zh) 一种基于深度学习的机器人闭环检测方法
CN108287550A (zh) 基于数据关联及错误检测的slam系统和构造数据关联的方法
CN108216249A (zh) 用于车辆的周围环境检测的系统和方法
CN112965507B (zh) 一种基于智能优化的集群无人机协同工作系统及方法
CN110211151A (zh) 一种运动物体的追踪方法和装置
CN109099920A (zh) 基于多传感器关联的传感器目标精确定位方法
CN108955676A (zh) 深海潜器导航系统和用于深海潜器状态切换的数据融合方法
CN110726413A (zh) 面向大规模slam的多传感器融合与数据管理机制
CN107203271B (zh) 基于多传感器融合技术的双手识别方法
CN116578106A (zh) 一种面向沿海声层析观测的水下直升机编队控制方法
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及系统
CN108051001A (zh) 一种机器人移动控制方法、系统及惯性传感控制装置
CN109062246A (zh) 具有多任务自调度的模块化飞控系统及其设计方法
CN109903309B (zh) 一种基于角度光流法的机器人运动信息估计方法
CN106373410B (zh) 一种城市交通信号优化控制方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant