計(jì)算機(jī)視覺反饋輔助的導(dǎo)航定位系統(tǒng)研究

朱清坤

摘? 要：在無人機(jī)（uav）等現(xiàn)代飛行器導(dǎo)航定位中，通常使用捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）、GPS定位系統(tǒng)相結(jié)合的導(dǎo)航方式，對大型飛行器飛行軌跡、自主著陸地點(diǎn)實(shí)施導(dǎo)航。其中捷聯(lián)慣導(dǎo)平臺由機(jī)載計(jì)算機(jī)、陀螺儀和加速度計(jì)等部分組成，陀螺儀、線性加速度計(jì)組合GPS定位系統(tǒng)，負(fù)責(zé)測量無人機(jī)飛行角度、線性軌跡等信息，之后將測得的運(yùn)動姿態(tài)、飛行速度或目標(biāo)位置的數(shù)據(jù)，傳輸至微型計(jì)算機(jī)平臺，并利用離散卡爾曼濾波算法，展開飛行目標(biāo)導(dǎo)航定位的誤差仿真分析，來輸出最終的慣導(dǎo)系統(tǒng)定位結(jié)果。

關(guān)鍵詞：計(jì)算機(jī)視覺? SINS導(dǎo)航? 卡爾曼濾波? 導(dǎo)航定位

中圖分類號：TP391.41 ? ? ? ? ? 文獻(xiàn)標(biāo)識碼：A? ? ? ? ? ? 文章編號：1674-098X（2021）04（c）-0135-03

Research on Computer Vision Feedback Aided Navigation and Positioning System

ZHU? Qingkun

（Zhengzhou Preschool Education College， Zhengzhou， Henan Province， 450000? China）

Abstract： In the navigation and positioning of modern aircraft such as unmanned aerial vehicle （UAV）， the combination of strapdown inertial navigation system （SINS） and GPS positioning system is usually used to navigate the flight path and autonomous landing site of large aircraft. The strapdown inertial navigation platform is composed of airborne computer， gyroscope and accelerometer. The GPS positioning system of gyroscope and linear accelerometer is responsible for measuring the flight angle， linear trajectory and other information of UAV. Then， the measured data of motion attitude， flight speed or target position are transmitted to the microcomputer platform， and the discrete Kalman filter algorithm is used， The error simulation analysis of flight target navigation and positioning is carried out to output the final positioning result of INS.

Key Words： Computer vision; SINS navigation; Kalman filter; Navigation and positioning

當(dāng)前對于大型飛行器的視覺導(dǎo)航定位，主要利用三維圖像模型、GPS/SINS組合定位相結(jié)合方法，進(jìn)行目標(biāo)對象準(zhǔn)三維立體模型的構(gòu)建。本文借助于離散卡爾曼濾波算法，將計(jì)算機(jī)視覺系統(tǒng)獲得的圖像信息，包含SINS導(dǎo)航的運(yùn)動位置、飛行角度和軌跡信息，輸入到攝像機(jī)模型中展開卡爾曼濾波，輸出慣導(dǎo)系統(tǒng)導(dǎo)航的誤差數(shù)值，并對無人機(jī)目標(biāo)定位仿真結(jié)果進(jìn)行修正，提高其在飛行過程中的定位精度。

1? 計(jì)算機(jī)視覺導(dǎo)航定位技術(shù)的主要內(nèi)容概述

計(jì)算機(jī)視覺系統(tǒng)主要借助于捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）、GPS衛(wèi)星定位，進(jìn)行GPS衛(wèi)星圖像的獲取。之后根據(jù)圖像信息劃分為不同的結(jié)構(gòu)類別，對圖像細(xì)節(jié)展開預(yù)處理、特征抽取，輸出具有高信噪比的圖像數(shù)據(jù)，來確定目標(biāo)對象在三維空間結(jié)構(gòu)中的位置、運(yùn)動路徑等。

首先，GPS目標(biāo)定位是通過GPS衛(wèi)星信號、接收機(jī)本地參考信號的結(jié)合，對目標(biāo)物體在運(yùn)動過程中的傳播時(shí)延進(jìn)行測算，來得出衛(wèi)星監(jiān)測到目標(biāo)瞬時(shí)位置，與實(shí)際位置之間存在的距離。

其次，捷聯(lián)式慣性導(dǎo)航系統(tǒng)（SINS），是通過陀螺儀、加速度計(jì)等精密測量工具，進(jìn)行無人機(jī)飛行器速度、加速度、角速率，以及參考坐標(biāo)、運(yùn)動軌跡、航行方向的測量與計(jì)算。捷聯(lián)慣導(dǎo)系統(tǒng)在目標(biāo)對象線性加速度、相對空間位置的測算中，通常將以上敏感元器件安裝到運(yùn)動載體上，監(jiān)測目標(biāo)飛行器做無規(guī)律運(yùn)動時(shí)，其在地球坐標(biāo)系O（xe，ye，ze）、機(jī)體坐標(biāo)系O（xb，yb，zb）、地理坐標(biāo)系O（xg，yg，zg）、導(dǎo)航坐標(biāo)系O（xn，yn，zn）中所處的位置。之后對慣性器件量測的目標(biāo)瞬時(shí)位置數(shù)值，經(jīng)過復(fù)雜坐標(biāo)變換，解算為計(jì)算機(jī)能夠識別的加速度、角速度、參考坐標(biāo)等矩陣信息，提取出目標(biāo)物體運(yùn)動的航向角、姿態(tài)信息。之后采用離散卡爾曼濾波算法，對多種大型飛行器瞬時(shí)位置、運(yùn)動軌跡、加速度、角速率等動力學(xué)數(shù)據(jù)，展開量測微分方程的離散卡爾曼濾波仿真分析，并得出GPS定位系統(tǒng)監(jiān)測數(shù)據(jù)、慣性導(dǎo)航器件監(jiān)測數(shù)據(jù)之間，存在的測量角度誤差、定位誤差。

2? 計(jì)算機(jī)視覺反饋輔助系統(tǒng)開展目標(biāo)導(dǎo)航定位的實(shí)現(xiàn)原理

2.1 計(jì)算機(jī)視覺系統(tǒng)對無人機(jī)飛行器的導(dǎo)航定位流程

計(jì)算機(jī)視覺的目標(biāo)導(dǎo)航定位系統(tǒng)，主要由GPS/SINS組合定位、攝像機(jī)圖像定位兩方面內(nèi)容組成。攝像機(jī)成像定位通過攝像機(jī)的二維、三維圖像信息，對無人機(jī)空間物體的三維位置坐標(biāo)進(jìn)行測算，一般會用到攝像機(jī)坐標(biāo)系、像素坐標(biāo)系、世界坐標(biāo)系等。其中攝像機(jī)坐標(biāo)系也被稱為針孔攝像坐標(biāo)系，攝像機(jī)光軸、無人機(jī)之間存在著一定夾角θ，在應(yīng)用到距離攝像機(jī)較遠(yuǎn)目標(biāo)對象的運(yùn)動時(shí)，需要測算目標(biāo)點(diǎn)在攝像機(jī)成像的投影位置，并將其順時(shí)針旋轉(zhuǎn)角度θ，得到目標(biāo)對象與之對應(yīng)的世界坐標(biāo)系，具體如圖1所示。因此運(yùn)動目標(biāo)攝像機(jī)坐標(biāo)系（xc，yc，zc）、世界坐標(biāo)系（xw，yw，zw）之間的轉(zhuǎn)換，通常會采取正交旋轉(zhuǎn)變化、坐標(biāo)平移等方式來實(shí)現(xiàn)，轉(zhuǎn)換關(guān)系式為：

（x0，y0，z0）：攝像機(jī)光心O在世界坐標(biāo)系的位置坐標(biāo);：無人機(jī)飛行器運(yùn)動角度矩陣信息。

在無人機(jī)飛行器由自主航行到著陸的整個(gè)流程，以無人機(jī)跑道軸線l3作為世界坐標(biāo)系Xw軸，標(biāo)志線l1為世界坐標(biāo)系Yw軸，沿l1、l3交點(diǎn)的垂線為Zw軸。當(dāng)無人機(jī)目標(biāo)對象進(jìn)入到攝像機(jī)視野后，就開始對運(yùn)動物體在攝像平面投影點(diǎn)進(jìn)行位置求解，求出目標(biāo)的攝像機(jī)坐標(biāo)系（xc，yc，zc），具體如圖1所示。之后根據(jù)陀螺儀、加速度計(jì)等測量工具，監(jiān)測到的無人機(jī)水平夾角θ、位置高度等，求得在世界坐標(biāo)系（xw，yw，zw）中目標(biāo)特征點(diǎn)的定位信息。

2.2 離散卡爾曼濾波算法在目標(biāo)對象導(dǎo)航定位中的應(yīng)用方程

離散卡爾曼濾波算法屬于卡爾曼濾波的種類之一，其可以被應(yīng)用到GPS/SINS組合導(dǎo)航定位數(shù)據(jù)的仿真計(jì)算中。在無人機(jī)由飛行狀態(tài)到自主著陸的過程中，GPS目標(biāo)定位系統(tǒng)、捷聯(lián)慣導(dǎo)系統(tǒng)等，會輸出導(dǎo)航坐標(biāo)系O（xn，yn，zn）中目標(biāo)對象的定位參數(shù)信息，之后將其代入到計(jì)算機(jī)視覺定位系統(tǒng)，解算出針對世界坐標(biāo)系（xw，yw，zw）的目標(biāo)物體位置點(diǎn)，這一位置信息即為飛行器目標(biāo)的真實(shí)位置預(yù)測值。GPS/SINS組合導(dǎo)航系統(tǒng)對無人機(jī)飛行器的定位數(shù)值求解如圖2所示。

之后利用測得的圖像識別數(shù)據(jù)、導(dǎo)航參數(shù)值，借助離散卡爾曼濾波算法，進(jìn)行某一時(shí)間段內(nèi)，目標(biāo)對象動力學(xué)方程、自適應(yīng)噪聲方差矩陣的求解，以及目標(biāo)物體非線性濾波方程的加權(quán)求和，得到目標(biāo)物體運(yùn)動中的運(yùn)動軌跡、瞬時(shí)位置測量值。通過對目標(biāo)前一狀態(tài)、后一狀態(tài)自適應(yīng)噪聲量測，完成飛行狀態(tài)量值、濾波誤差矩陣數(shù)據(jù)的分析，并修復(fù)無人機(jī)自主著陸過程中導(dǎo)航參數(shù)的測量誤差。假設(shè)m時(shí)刻計(jì)算機(jī)視覺系統(tǒng)的目標(biāo)狀態(tài)值為Xm，在受到系統(tǒng)白噪聲干擾情況下，目標(biāo)對象瞬時(shí)位置的非線性隨機(jī)函數(shù)方程為：

（Φm，m-1：m到m-1時(shí)刻內(nèi)離散后的狀態(tài)轉(zhuǎn)移矩陣;Γm-1：系統(tǒng)噪聲驅(qū)動陣;Wm-1：狀態(tài)的噪聲向量）。

3? 計(jì)算機(jī)視覺反饋輔助系統(tǒng)中無人機(jī)目標(biāo)導(dǎo)航定位的仿真分析

3.1 計(jì)算機(jī)視覺系統(tǒng)中無人機(jī)導(dǎo)航定位的仿真環(huán)境

無人機(jī)航行器在著陸飛行過程中，其上存在著陀螺儀、加速度計(jì)等慣導(dǎo)系統(tǒng)設(shè)備，以及周圍區(qū)域環(huán)境成像的攝像機(jī)設(shè)備，攝像機(jī)與水平方向呈現(xiàn)一定夾角θ=-15°，機(jī)體參考坐標(biāo)系為O（xb，yb，zb）、攝像機(jī)坐標(biāo)系為O（xm，ym，zm）。利用GPS導(dǎo)航定位、慣性導(dǎo)航系統(tǒng)定位（SINS），開展無人機(jī)導(dǎo)航定位時(shí)，通常運(yùn)用離散卡爾曼濾波法算法，對得到的輸出目標(biāo)定位信息進(jìn)行校正。

3.2 計(jì)算機(jī)視覺系統(tǒng)中GPS/SINS組合導(dǎo)航定位的仿真實(shí)驗(yàn)與結(jié)果

假設(shè)無人機(jī)航行器的飛行高度H0=100m、飛行速度為V0=40m/s，飛行過程中無人機(jī)下滑角度為5°。攝像機(jī)監(jiān)測到飛行器當(dāng)前航行方向位置，所對應(yīng)的地面投影點(diǎn)為O點(diǎn)，航行器沿著PP'軌跡路線飛行。兩個(gè)地面攝像機(jī)之間的方向距離為d=100m，測得無人機(jī)飛行器導(dǎo)航位置矩陣為、無人機(jī)運(yùn)動角度矩陣為、機(jī)體坐標(biāo)系到測量坐標(biāo)系的變換矩陣為。那么m時(shí)刻飛行器的攝像坐標(biāo)系矢量為、，具體公式表示為：

運(yùn)用陀螺儀、加速度計(jì)測得的飛行目標(biāo)對象旋轉(zhuǎn)度數(shù)、加速度，會產(chǎn)生30°/h、50mg左右的數(shù)值漂移。本文通過對陀螺儀、加速度計(jì)、GPS目標(biāo)定位系統(tǒng)，監(jiān)測到的飛行器位置點(diǎn)、速度、姿態(tài)角，進(jìn)行離散卡爾曼濾波算法分析，得到位置白噪聲均方差、速度誤差、姿態(tài)角白噪聲均方差。然后根據(jù)陀螺儀漂移、加速度計(jì)漂移程度，計(jì)算無人機(jī)飛行過程中監(jiān)測的導(dǎo)航位置信息，與無人機(jī)真實(shí)位置點(diǎn)存在的定位誤差，具體如表1所示。

目標(biāo)對象在m到m-1時(shí)間節(jié)點(diǎn)內(nèi)，離散卡爾曼濾波的狀態(tài)預(yù)測方程、狀態(tài)估值方程，以及預(yù)測均方誤差方程、估計(jì)均方誤差方程分別為：

由表1數(shù)據(jù)可以得出，采取離散卡爾曼濾波算法，進(jìn)行飛行器目標(biāo)位置仿真分析，可以得到m到m-1時(shí)間節(jié)點(diǎn)內(nèi)，地球坐標(biāo)系中無人機(jī)飛行位置、速度、角度，所存在的誤差值、白噪聲均方差，且其仿真誤差都控制在有效范圍內(nèi)，能夠?qū)INS/GPS組合導(dǎo)航監(jiān)測的目標(biāo)位置、速度、姿態(tài)數(shù)據(jù)得到修正，提升計(jì)算機(jī)視覺系統(tǒng)導(dǎo)航定位的精度。

4? 結(jié)語

在計(jì)算機(jī)視覺系統(tǒng)的目標(biāo)對象定位過程中，需要使用SINS/GPS組合導(dǎo)航定位，進(jìn)行無人機(jī)飛行器偽距、實(shí)測數(shù)據(jù)的定位分析，包括建立偽距多普勒頻移公式、電波載頻方程，精確檢測信號白噪聲、傳播時(shí)延而導(dǎo)致的定位位置誤差等。由于無人機(jī)飛行器運(yùn)動位置，會隨著時(shí)間變化而發(fā)生無規(guī)律變化。因此通過離散卡爾曼濾波算法，開展目標(biāo)無人機(jī)加速度、角速率、運(yùn)動軌跡，以及世界坐標(biāo)系位置信息的預(yù)測與估算，可以得到飛行物體在某一時(shí)間段內(nèi)的預(yù)測均方誤差、估計(jì)均方誤差，從而最大程度保障目標(biāo)對象導(dǎo)航定位結(jié)果的準(zhǔn)確性。

參考文獻(xiàn)

[1] 茅正沖，陳強(qiáng).基于機(jī)器視覺的AGV導(dǎo)航與定位算法研究[J].計(jì)算機(jī)與數(shù)字工程，2020，48（8）：1914-1918.

[2] 趙博.基于視覺導(dǎo)航的自動導(dǎo)引車設(shè)計(jì)與實(shí)現(xiàn)[D].濟(jì)南：山東大學(xué)，2017.

[3] Tongqing Feng，Bin Jiao. The vision guidance and image processing of AGV[J].Journal of Physics： Conference Series，2020（1）.

[4] 羅高，袁亮.基于二維碼的單目視覺測距移動機(jī)器人定位研究[J].組合機(jī)床與自動化加工技術(shù)，2017（10）：114-117，121.

[5] 肖大偉，翟軍勇.輪式移動機(jī)器人單目視覺的目標(biāo)測距方法[J].計(jì)算機(jī)工程，2017，43（4）：287-291.

[6] 張英.GPS拒止條件下無人機(jī)自主全局定位技術(shù)[D].西安：西安電子科技大學(xué)，2019.