基于Chan-Taylor聯(lián)合算法的低空無人機(jī)時(shí)差定位研究

羅正華， 雷 林， 周方均， 李 霞

(1.成都大學(xué) 信息科學(xué)與工程學(xué)院， 四川 成都 610106；2.電信科學(xué)技術(shù)第五研究所， 四川 成都 610062)

0 引 言

隨著無人機(jī)的普及，低空空域的安全問題受到人們的極大關(guān)注.針對(duì)該問題，本研究對(duì)“非合作型”無人機(jī)采用一種基于時(shí)差法的無源定位算法對(duì)其進(jìn)行實(shí)時(shí)定位.基于時(shí)差法的無源定位方法是根據(jù)求解無人機(jī)信號(hào)到達(dá)主站和各輔站的距離差，并聯(lián)合各基站坐標(biāo)所構(gòu)成的雙曲線方程組來實(shí)現(xiàn).該方法定位精度高，且不對(duì)外發(fā)射信號(hào)，可在機(jī)場(chǎng)等區(qū)域安全使用.

目前，Chan算法和Taylor算法是2種經(jīng)典的時(shí)差定位算法[1-4].其中，Chan算法在時(shí)差值精確的情況下，可以實(shí)現(xiàn)較高精度的定位，但如果時(shí)差值精度不夠，其定位精度會(huì)大幅降低.Taylor算法則是在已有的定位坐標(biāo)基礎(chǔ)上，進(jìn)行迭代遞歸，使定位出的坐標(biāo)接近于目標(biāo)的真實(shí)坐標(biāo).雖然Taylor算法定位精度較高，但需要提供初始估計(jì)坐標(biāo)，否則就無法實(shí)現(xiàn)準(zhǔn)確定位.基于2種算法的特點(diǎn)，本研究提出Chan-Taylor聯(lián)合算法，其思路是，將Chan算法解算出的目標(biāo)坐標(biāo)作為初始估計(jì)坐標(biāo)值賦給Taylor算法進(jìn)行迭代運(yùn)算，即使獲取的時(shí)差值存在一定誤差，使初始估計(jì)坐標(biāo)的精度不高，但可以通過迭代來提高定位坐標(biāo)的精度.通過算法對(duì)比和仿真分析表明，Chan-Taylor聯(lián)合算法較Chan算法具有更高的定位精度和穩(wěn)定性，較Taylor算法更具有實(shí)用性.

1 算法描述

1.1 Chan算法

基于Chan算法的無源定位是通過求解目標(biāo)源信號(hào)到達(dá)輔站與主站之間的時(shí)差并聯(lián)立各基站坐標(biāo)所得的雙曲線方程組來實(shí)現(xiàn)的.該算法是一種非迭代算法，不需要初始值，在時(shí)差精確、視距傳輸?shù)那闆r下，其定位效果良好，但在工程上，很難獲得滿足要求的時(shí)差初值.因此，Chan算法可作為其他算法的前置條件.

本研究以4站三維定位系統(tǒng)為例建立3組方程，該方程組為超定方程組.通常情況下，由于該方程組導(dǎo)出的矩陣不存在逆矩陣，方程組無法正常求解.所以，本研究利用偽逆法聯(lián)合最小二乘法對(duì)方程組進(jìn)行解算，即Chan算法.4站定位系統(tǒng)的定位原理如圖1所示.

圖1 4站定位系統(tǒng)示意圖

圖1中，主站坐標(biāo)聯(lián)立3個(gè)輔站坐標(biāo)，通過分別計(jì)算出的時(shí)差可構(gòu)建3條雙曲線，其交點(diǎn)就是無人機(jī)的位置.

(1)

本研究若不特別指明，均默認(rèn)i∈[1，3].對(duì)式(1)整理可得，

(2)

式中，Ri表示基站i到坐標(biāo)原點(diǎn)的距離；R0為主站到坐標(biāo)原點(diǎn)的距離；ri0為無人機(jī)到輔站與主站間的距離差.

4站三維定位系統(tǒng)存在一個(gè)由3組式(2)的關(guān)系式結(jié)合而成的方程組，如式(3)所示.當(dāng)A≠0時(shí)，線性方程組(3)有解.

AP=b

(3)

式中，A是方程組的系數(shù)矩陣，b是方程組的輸出向量.

利用偽逆法[3-4]可求得無人機(jī)坐標(biāo)為，

(4)

1.2 Chan-Taylor聯(lián)合算法

因?yàn)镃han算法是非遞歸算法，對(duì)時(shí)差精度要求高，因此，本研究對(duì)該算法的定位結(jié)果進(jìn)行二次處理.Taylor算法是利用局部最小二乘解進(jìn)行迭代[5]的遞歸算法，其定位精度高，但需要初始估計(jì)坐標(biāo)，否則無法進(jìn)行定位.

基于低空無人機(jī)時(shí)差定位的實(shí)際需求，本研究結(jié)合Chan算法和Taylor算法提出了一種改進(jìn)的算法，即Chan-Taylor聯(lián)合算法.Chan-Taylor聯(lián)合算法是將Chan算法的解算結(jié)果作為初始估計(jì)坐標(biāo)送入Taylor算法，以達(dá)到對(duì)無人機(jī)坐標(biāo)進(jìn)行誤差計(jì)算和定位修正的作用.算法在迭代時(shí)，將誤差與設(shè)定的閾值進(jìn)行比較，若誤差值大于閾值，則繼續(xù)迭代；若誤差值小于設(shè)定閾值，則終止迭代并輸出結(jié)果.

1.3 Chan-Taylor聯(lián)合算法流程及計(jì)算原理

Chan-Taylor聯(lián)合算法流程如圖2所示，具體為：首先，算法獲得無人機(jī)信號(hào)到主站與各輔站之間的時(shí)差；然后，將時(shí)差用于Chan算法部分進(jìn)行初始估計(jì)坐標(biāo)值的計(jì)算，并利用該坐標(biāo)值在Taylor算法部分做誤差向量的計(jì)算，用以定位修正；同時(shí)，對(duì)誤差進(jìn)行閾值比較.如不滿足條件，則繼續(xù)迭代，如滿足條件，則結(jié)束迭代，并輸出最終結(jié)果.

圖2 Chan-Taylor算法流程

(5)

Aα=b+e

(6)

式中，α為目標(biāo)差值向量，b為時(shí)差的差值向量，e為時(shí)差估計(jì)誤差向量，H為時(shí)差估計(jì)的梯度矩陣.它們可分別表示為，

(7)

(8)

e=[e10，e20，e30]T

(9)

(10)

由式ri0=ri-r0=cτi0與站址坐標(biāo)，可得，

(11)

將式(10)與式(11)聯(lián)立，化簡(jiǎn)可得，

(12)

α=(HTH)-1HTQ-1b

(13)

式中，Q=E[eeT]，是時(shí)差估計(jì)誤差的協(xié)方差矩陣.算法將解算出來的α與前次無人機(jī)定位坐標(biāo)相加，并用于下次迭代，直到‖α‖小于閾值或迭代次數(shù)達(dá)到限制時(shí)截止.

2 算法仿真與分析

2.1 算法仿真計(jì)算

本研究以Pi0=(0,0,1)、Pi1=(750，1 229，2)、Pi2=(-750,1 229,1)與Pi3=(0,-1 500,0)為主站、輔站1、輔站2及輔站3的坐標(biāo)且4站呈Y字型的布站方式為例，對(duì)Chan算法、Chan-Taylor聯(lián)合算法進(jìn)行MATLAB仿真分析.圖3～圖8是無人機(jī)與基站的相對(duì)位置分布示意圖，數(shù)據(jù)A1～A6是輸入精確時(shí)差時(shí)Chan算法所得的坐標(biāo)，數(shù)據(jù)B1～B6是保留4位有效數(shù)據(jù)的非精確時(shí)差輸入Chan-Taylor聯(lián)合算法所得的坐標(biāo)，數(shù)據(jù)C1～C6是輸入精確時(shí)差時(shí)Chan-Taylor聯(lián)合算法所得的坐標(biāo).

圖3的無人機(jī)實(shí)際坐標(biāo)為(0，0，500)，算法的解算結(jié)果如下：A1，(-0.0020，-0.002，503.973)；B1，(-0.0274，0.138，500.248)；C1，(-0.0000，-0.000，500.000).

我們乘坐電梯并習(xí)以為常，殊不知，它也是一個(gè)默默付出不求回報(bào)的勞動(dòng)者，小作者善于挖掘電梯不為人知的另一面，讓詩歌變得新奇有深意。

圖3無人機(jī)坐標(biāo)為(0，0，500)時(shí)與基站的位置分布

圖4的無人機(jī)實(shí)際坐標(biāo)為(200，200，500)，算法的解算結(jié)果如下：A2，(199.997，199.997，504.523)；B2，(199.980，200.102，500.230)；C2，(199.999，199.999，500.000).

圖5的無人機(jī)實(shí)際坐標(biāo)為(500，500，500)，算法的解算結(jié)果如下：A3，(499.996，499.997，505.296)；B3，(500.111，500.091，500.196)；C3，(500.000，499.999，500.000).

圖6的無人機(jī)實(shí)際坐標(biāo)為(700，700，500)，算法的解算結(jié)果如下：

A4，(112.269，460.848，676.871)；

圖4無人機(jī)坐標(biāo)為(200，200，500)時(shí)與基站的位置分布

圖5無人機(jī)坐標(biāo)為(500，500，500)時(shí)與基站的位置分布

B4，(112.303，460.943，673.527)；C4，(112.272，460.850，673.055).

圖6無人機(jī)坐標(biāo)為(700，700，500)時(shí)與基站的位置分布

圖7的無人機(jī)實(shí)際坐標(biāo)為(200，200，900)，算法的解算結(jié)果如下：A5，(199.997，199.998，903.231)；B5，(200.112，199.883，900.355)；C5，(199.999，199.999，900.000).

圖7無人機(jī)坐標(biāo)為(200，200，900)時(shí)與基站的位置分布

圖8的無人機(jī)實(shí)際坐標(biāo)為(200，200，2 500)，算法的解算結(jié)果如下：A6，(199.999，199.999，2 501.516)；B6，(200.230，200.263，2 501.305)；C6，(199.999，199.999，2 500.000).

圖8無人機(jī)坐標(biāo)為(200，200，2500)時(shí)與基站的位置分布

2.2 結(jié)果分析

1)將數(shù)據(jù)A1與B1、C1或A2與B2、C2或A3與B3、C3進(jìn)行對(duì)比可得出：雖然Chan算法可以較為精準(zhǔn)地定位無人機(jī)，但較Chan-Taylor聯(lián)合算法而言，定位誤差依舊很大.

2)將數(shù)據(jù)B1與C1或B2與C2或B3與C3進(jìn)行對(duì)比可得出：當(dāng)輸入時(shí)差精確度較低時(shí)，Chan-Taylor聯(lián)合算法定位精度也會(huì)隨之降低，但不影響其在工程上的使用.

3)將數(shù)據(jù)A1與B1或A2與B2或A3與B3對(duì)比可得出：雖然Chan算法輸入的時(shí)差值精確度較Chan-Taylor聯(lián)合算法高，但Chan算法所得的坐標(biāo)精度不如Chan-Taylor聯(lián)合算法所得的坐標(biāo)精度.該結(jié)果不僅證明了Chan-Taylor聯(lián)合算法的解算性能優(yōu)于Chan算法，也證明了Chan-Taylor聯(lián)合算法具有更好的容錯(cuò)性，允許由于各種因素引起的更大時(shí)差誤差.

4)將數(shù)據(jù)A2、A5、A6或B2、B5、B6或C2、C5、C6進(jìn)行對(duì)比可得出：隨著Z軸的值逐漸增大，Chan算法定位精度有一定提高，非精確時(shí)差情況下聯(lián)合算法的定位精度略微降低，但Chan-Taylor聯(lián)合算法定位精度仍然較Chan算法更高.輸入精確時(shí)差的Chan-Taylor聯(lián)合算法的定位精度基本上沒有變化，定位效果最佳.

5)將數(shù)據(jù)A3與A4或B3與B4或C3與C4進(jìn)行對(duì)比可得出：當(dāng)無人機(jī)超出穩(wěn)定定位范圍時(shí)，定位結(jié)果將不再準(zhǔn)確.該結(jié)果說明聯(lián)合算法中的Chan算法部分所提供的初始估計(jì)位置會(huì)對(duì)最終定位結(jié)果產(chǎn)生較大影響.

3 結(jié) 語

本研究以2種經(jīng)典的無源時(shí)差定位Chan算法與Taylor算法為基礎(chǔ)，提出了融合2種算法各自優(yōu)點(diǎn)所得到的Chan-Taylor聯(lián)合算法.Chan算法計(jì)算方便快捷，但定位精度較低；Taylor算法初始坐標(biāo)獲取困難，但在與Chan算法相同的條件下，若有初始估計(jì)坐標(biāo)，同樣可以對(duì)目標(biāo)定位，且精度較Chan算法更高；Chan-Taylor聯(lián)合算法則很好地融合了Chan算法的計(jì)算方便和Taylor算法定位精度高的優(yōu)點(diǎn)，彌補(bǔ)了Chan算法對(duì)于時(shí)差精度要求高、定位精度較低及Taylor算法對(duì)于初值敏感和初值獲取困難等缺點(diǎn)，可使定位結(jié)果更大程度靠近真實(shí)值.