本發(fa)明(ming)涉及(ji)無人(ren)機(ji)，特別涉及(ji)一種應用于(yu)旋翼(yi)無人(ren)機(ji)定高的tdcp-gnss/ins方法。

背景技術：

1、目前市場上無(wu)(wu)人(ren)(ren)(ren)機(ji)開始廣泛應(ying)用(yong)(yong)，無(wu)(wu)人(ren)(ren)(ren)機(ji)具(ju)有體積(ji)小、重量輕、費用(yong)(yong)低、操作靈活、安全(quan)性(xing)高的(de)(de)(de)(de)特點，廣泛應(ying)用(yong)(yong)于(yu)(yu)航拍、檢測、資源勘查等領域。現有的(de)(de)(de)(de)旋(xuan)翼無(wu)(wu)人(ren)(ren)(ren)機(ji)定高技術在(zai)無(wu)(wu)差分(fen)信號(hao)或短(duan)時間內(nei)差分(fen)信號(hao)中斷的(de)(de)(de)(de)情況下gnss信號(hao)不可用(yong)(yong)，純ins推算(suan)無(wu)(wu)法(fa)(fa)保證無(wu)(wu)人(ren)(ren)(ren)機(ji)高度(du)的(de)(de)(de)(de)精(jing)度(du)和穩(wen)(wen)定性(xing)，很容易(yi)造(zao)成無(wu)(wu)人(ren)(ren)(ren)機(ji)墜地。利用(yong)(yong)氣壓計獲取氣壓數據，通(tong)過解算(suan)可以(yi)獲得(de)當前位(wei)置(zhi)無(wu)(wu)人(ren)(ren)(ren)機(ji)的(de)(de)(de)(de)海拔高度(du)，但(dan)在(zai)無(wu)(wu)人(ren)(ren)(ren)機(ji)高速飛行時，很容易(yi)受(shou)氣流影響，因此(ci)(ci)利用(yong)(yong)氣壓計計算(suan)出的(de)(de)(de)(de)無(wu)(wu)人(ren)(ren)(ren)機(ji)高度(du)同樣(yang)也無(wu)(wu)法(fa)(fa)保證飛行高度(du)的(de)(de)(de)(de)穩(wen)(wen)定性(xing)，為(wei)此(ci)(ci)，我們提出了(le)一種應(ying)用(yong)(yong)于(yu)(yu)旋(xuan)翼無(wu)(wu)人(ren)(ren)(ren)機(ji)定高的(de)(de)(de)(de)tdcp-gnss/ins方法(fa)(fa)。

技術實現思路

1、為了解決上述問(wen)題，本發明提(ti)供一種應用于旋翼無人機定高的(de)tdcp-gnss/ins方法。

2、本(ben)發明中的(de)一種應用于(yu)旋翼無人機定高的(de)tdcp-gnss/ins方法，包括以下步驟：

3、步(bu)驟s1、ins計算模塊(kuai)初始(shi)化：包括確定kalman濾波器的初始(shi)狀(zhuang)態(tai)向(xiang)量、先驗方差和狀(zhuang)態(tai)噪聲矩陣。

4、步驟s2、ins機械編(bian)排算法：首(shou)先(xian)對imu原始數據進(jin)行(xing)零偏誤(wu)差和刻度(du)系(xi)數誤(wu)差補償，速度(du)更(geng)(geng)新時考慮(lv)(lv)劃(hua)槳誤(wu)差補償，姿態更(geng)(geng)新時考慮(lv)(lv)圓錐誤(wu)差補償；

5、在北東地導航(hang)坐標系(xi)(n系(xi))下(xia)構建gnss/ins組合導航(hang)系(xi)統。系(xi)統狀態參數x為：

6、

7、采用φ角(jiao)誤差模型構建狀態方程：

8、

9、步驟s3、rtk解算：利用ins計算的(de)先(xian)驗位置輔助rtk粗(cu)差探測，然后進(jin)行(xing)rtk解算；

10、步驟s3.1、若收到基站差(cha)分信號(hao)且rtk模糊度固(gu)定成功(gong)，則使用雙差(cha)載(zai)波(bo)相位(wei)觀測值進行擴展(zhan)卡(ka)爾曼(man)濾(lv)波(bo)ekf的量測更新；

11、在gnss信號良好(hao)時使用rtk/ins緊組(zu)合定(ding)位，rtk/ins緊組(zu)合觀測方程為：

12、

13、步驟s3.2、若基站差分信號中斷或無基站差分信號，則使(shi)用tdcp觀測值進行(xing)ekf量測更新；

14、當rtk模糊度固定(ding)失敗且(qie)tdcp解算成功時，使用tdcp/ins緊(jin)組合(he)定(ding)位(wei)。考慮相(xiang)鄰歷元(yuan)電離層(ceng)和對流層(ceng)延遲等且(qie)未發生周(zhou)跳(tiao)，由tk-1和tk觀(guan)測歷元(yuan)構(gou)造(zao)tdcp觀(guan)測值為：

15、

16、首先(xian)將(jiang)前一時刻的組合結果作為(wei)已知值(zhi)計算(suan)然(ran)后將(jiang)ins推(tui)算(suan)的tdcp值(zhi)與(yu)式(4)作差，則(ze)tdcp/ins緊組合觀測方程為(wei)：

17、

18、s4、ekf量測(ce)更新：構(gou)建準確觀測(ce)模(mo)型及其噪聲方差(cha)陣，然后量測(ce)更新。

19、s5、反(fan)饋修(xiu)正(zheng)：將濾波(bo)估計(ji)得到(dao)的導(dao)航參(can)數誤差(cha)對慣導(dao)機械編排結果進行修(xiu)正(zheng)，將估計(ji)得到(dao)的加速(su)度計(ji)和陀(tuo)螺儀的零偏誤差(cha)反(fan)饋到(dao)imu原(yuan)始數據中(zhong)，構成(cheng)閉環反(fan)饋。

20、s6、導(dao)航結果(guo)輸出：可(ke)選(xuan)擇(ze)輸出所需(xu)位置、速度和姿(zi)態導(dao)航結果(guo)，ins誤差參數以及(ji)對應估計方(fang)差。

21、所述步驟s2中，φ、δvn、δrn分(fen)(fen)別(bie)為ins機械編排的三(san)(san)維姿態誤差、三(san)(san)維速(su)度誤差、三(san)(san)維位置(zhi)誤差；bg、sg分(fen)(fen)別(bie)為三(san)(san)軸陀螺儀零偏和比(bi)例(li)因子；ba、sa分(fen)(fen)別(bie)為三(san)(san)軸加速(su)度計(ji)零偏和比(bi)例(li)因子。

22、所述步驟(zou)s2中，為n系(xi)(xi)相對于地(di)(di)心地(di)(di)固(gu)坐標系(xi)(xi)(e系(xi)(xi))的旋轉(zhuan)(zhuan)角(jiao)(jiao)速(su)度(du)(du)矢(shi)量(liang)(liang)在n系(xi)(xi)下(xia)的投(tou)(tou)影；δθ為ins計算坐標系(xi)(xi)(c系(xi)(xi))與n系(xi)(xi)的姿(zi)態差異；為從載體坐標系(xi)(xi)(b系(xi)(xi)，前右下(xia))轉(zhuan)(zhuan)換到(dao)n系(xi)(xi)的方向余弦矩陣；fb和(he)分別(bie)為加速(su)度(du)(du)計比(bi)力(li)和(he)陀螺(luo)儀(yi)角(jiao)(jiao)速(su)度(du)(du)輸出；為地(di)(di)球子啊混角(jiao)(jiao)速(su)度(du)(du)矢(shi)量(liang)(liang)在n系(xi)(xi)下(xia)的投(tou)(tou)影；為當地(di)(di)重力(li)加速(su)度(du)(du)矢(shi)量(liang)(liang)在n系(xi)(xi)下(xia)的投(tou)(tou)影；為n系(xi)(xi)相對于慣性坐標系(xi)(xi)(i系(xi)(xi))的旋轉(zhuan)(zhuan)角(jiao)(jiao)速(su)度(du)(du)矢(shi)量(liang)(liang)在n系(xi)(xi)下(xia)的投(tou)(tou)影；δ(-)表示(shi)該變量(liang)(liang)的誤差；(-x)表示(shi)向量(liang)(liang)的反對稱矩陣，陀螺(luo)儀(yi)、加速(su)度(du)(du)計零偏和(he)比(bi)例因子均建模為一階高(gao)斯馬(ma)爾可夫模型(xing)。

23、所述在步(bu)驟(zou)s3.1中，z為殘差(cha)向量；為雙差(cha)載(zai)波(bo)(bo)(bo)相(xiang)位觀(guan)測(ce)(ce)值(zhi)，含(han)雙差(cha)固定解模糊度(du)；為ins推算的雙差(cha)距離(li)；λ為載(zai)波(bo)(bo)(bo)波(bo)(bo)(bo)長(chang)；h和v分別為觀(guan)測(ce)(ce)矩(ju)陣和觀(guan)測(ce)(ce)噪聲陣。

24、所述(shu)在步驟s3.2中(zhong)，δ表(biao)示(shi)差(cha)分(fen)算子；li為(wei)載波相位觀測值，其中(zhong)i表(biao)示(shi)不同頻率(lv)的信號；ρ為(wei)衛星和接收機之間的幾何距(ju)離；δtu為(wei)接收機鐘差(cha)；c為(wei)光速；εδl為(wei)tdcp測量的噪(zao)聲和殘余(yu)誤差(cha)。

25、所述在步驟s3.2中(zhong)，z為(wei)(wei)tdcp值殘差；為(wei)(wei)tk時刻(ke)e系(xi)下接(jie)收(shou)機位置，可由ins推算位置求得；為(wei)(wei)tk時刻(ke)e系(xi)下衛星位置；為(wei)(wei)從e系(xi)轉換到n系(xi)的方向余弦矩陣；lb為(wei)(wei)gnss天線(xian)的桿臂向量(liang)；接(jie)收(shou)機鐘差變化量(liang)可通過最小二乘方法解算tdcp觀測值求得。

26、本發明(ming)的(de)(de)優點和有益效果在于(yu)：本發明(ming)提供一種(zhong)應用(yong)于(yu)旋(xuan)翼(yi)(yi)無(wu)人(ren)機(ji)定高(gao)的(de)(de)tdcp-gnss/ins方法(fa)，傳統(tong)gnss/ins緊組合導航方法(fa)的(de)(de)精度(du)(du)(du)高(gao)度(du)(du)(du)依賴整周模糊度(du)(du)(du)的(de)(de)固(gu)(gu)定，當(dang)rtk模糊度(du)(du)(du)無(wu)法(fa)固(gu)(gu)定時(shi)，緊組合系(xi)統(tong)常用(yong)偽距(ju)觀(guan)測值進行(xing)測量(liang)更(geng)新，導致(zhi)定位(wei)精度(du)(du)(du)下降至(zhi)米級(ji)。無(wu)人(ren)機(ji)飛行(xing)過程(cheng)中(zhong)出(chu)現無(wu)差(cha)分信號(hao)或短時(shi)間內差(cha)分信號(hao)中(zhong)斷時(shi)，利用(yong)tdcp觀(guan)測值與ins緊組合，可(ke)以維持組合系(xi)統(tong)短期內較(jiao)高(gao)的(de)(de)定位(wei)精度(du)(du)(du)，滿足旋(xuan)翼(yi)(yi)無(wu)人(ren)機(ji)定高(gao)的(de)(de)需求。該種(zhong)tdcp-gnss/ins方法(fa)結構簡(jian)單(dan)，通過多種(zhong)高(gao)度(du)(du)(du)定位(wei)方法(fa)，有效的(de)(de)保(bao)障了旋(xuan)翼(yi)(yi)無(wu)人(ren)機(ji)飛行(xing)的(de)(de)穩定性。

技術特征：

1.一種應用于(yu)旋翼無(wu)人(ren)機定高的tdcp-gnss/ins方法(fa)，其(qi)特征在于(yu)，包(bao)括(kuo)以下步驟：

2.根據權利要求1所述的(de)一(yi)種應用于(yu)旋翼(yi)無人機(ji)定高的(de)tdcp-gnss/ins方法(fa)，其(qi)特(te)征在于(yu)，所述步驟s2中，φ、δvn、δrn分(fen)(fen)別(bie)為ins機(ji)械編排(pai)的(de)三(san)維姿態誤差(cha)、三(san)維速度誤差(cha)、三(san)維位置誤差(cha)；bg、sg分(fen)(fen)別(bie)為三(san)軸陀(tuo)螺儀零偏(pian)和比例因子；ba、sa分(fen)(fen)別(bie)為三(san)軸加速度計零偏(pian)和比例因子。

3.根(gen)據(ju)權利要求1所述的(de)(de)一種(zhong)應(ying)用于旋翼無(wu)人機定高(gao)的(de)(de)tdcp-gnss/ins方(fang)法，其特征(zheng)在(zai)于，所述步驟s2中，為(wei)n系(xi)相對(dui)于地心(xin)地固坐標系(xi)(e系(xi))的(de)(de)旋轉角(jiao)速(su)度(du)矢量在(zai)n系(xi)下的(de)(de)投影；δθ為(wei)ins計算坐標系(xi)(c系(xi))與n系(xi)的(de)(de)姿(zi)態差異(yi)；為(wei)從載體坐標系(xi)(b系(xi)，前右下)轉換到n系(xi)的(de)(de)方(fang)向余弦矩陣；fb和(he)分別(bie)為(wei)加(jia)速(su)度(du)計比力和(he)陀(tuo)螺儀角(jiao)速(su)度(du)輸出；為(wei)地球子(zi)啊混角(jiao)速(su)度(du)矢量在(zai)n系(xi)下的(de)(de)投影；為(wei)當地重力加(jia)速(su)度(du)矢量在(zai)n系(xi)下的(de)(de)投影；為(wei)n系(xi)相對(dui)于慣性(xing)坐標系(xi)(i系(xi))的(de)(de)旋轉角(jiao)速(su)度(du)矢量在(zai)n系(xi)下的(de)(de)投影；δ(-)表示該變量的(de)(de)誤(wu)差；(-x)表示向量的(de)(de)反(fan)對(dui)稱矩陣，陀(tuo)螺儀、加(jia)速(su)度(du)計零偏和(he)比例因子(zi)均建(jian)模為(wei)一階高(gao)斯(si)馬(ma)爾可夫模型(xing)。

4.根據(ju)權利(li)要求1所述(shu)的(de)一種應用于(yu)旋翼無(wu)人機定高(gao)的(de)tdcp-gnss/ins方法，其(qi)特(te)征在于(yu)，所述(shu)在步(bu)驟s3.1中，z為(wei)殘(can)差(cha)(cha)向(xiang)量；為(wei)雙差(cha)(cha)載波(bo)相位觀測值，含雙差(cha)(cha)固定解模糊度；為(wei)ins推算的(de)雙差(cha)(cha)距(ju)離；λ為(wei)載波(bo)波(bo)長；h和(he)v分別(bie)為(wei)觀測矩陣(zhen)和(he)觀測噪聲陣(zhen)。

5.根據權利要求1所述的(de)(de)(de)一種應用(yong)于旋翼(yi)無人(ren)機(ji)定高(gao)的(de)(de)(de)tdcp-gnss/ins方法(fa)，其特征在于，所述在步驟(zou)s3.2中，δ表示(shi)差分算子；li為(wei)(wei)(wei)載波相位觀測(ce)值，其中i表示(shi)不同(tong)頻率(lv)的(de)(de)(de)信號；ρ為(wei)(wei)(wei)衛星(xing)和(he)接收(shou)機(ji)之間(jian)的(de)(de)(de)幾何距離；δtu為(wei)(wei)(wei)接收(shou)機(ji)鐘差；c為(wei)(wei)(wei)光速(su)；εδl為(wei)(wei)(wei)tdcp測(ce)量的(de)(de)(de)噪聲(sheng)和(he)殘余誤差。

6.根據權利要求1所述(shu)的(de)一種(zhong)應用于旋(xuan)翼(yi)無(wu)人機定高的(de)tdcp-gnss/ins方法，其特征在于，所述(shu)在步(bu)驟s3.2中，z為tdcp值(zhi)殘差；為tk時刻e系下接收(shou)機位置，可(ke)由ins推算位置求得；為tk時刻e系下衛星位置；為從e系轉(zhuan)換(huan)到n系的(de)方向余弦矩陣；lb為gnss天線的(de)桿臂(bei)向量；接收(shou)機鐘差變化量可(ke)通過最小二乘(cheng)方法解算tdcp觀測值(zhi)求得。

技術總結
本發明公開了一種應用于旋翼無人機定高的TDCP?GNSS/INS方法。包括如下步驟：步驟S1、INS計算模塊初始化；步驟S2、INS機械編排算法；步驟S3、RTK解算；S4、EKF量測更新；S5、反饋修正；S6、導航結果輸出；當RTK模糊度無法固定時，緊組合系統常用偽距觀測值進行測量更新，導致定位精度下降至米級。無人機飛行過程中出現無差分信號或短時間內差分信號中斷時，利用TDCP觀測值與INS緊組合，可以維持組合系統短期內較高的定位精度，滿足旋翼無人機定高的需求。該種TDCP?GNSS/INS方法結構簡單，通過多種高度定位方法，有效的保障了旋翼無人機飛行的穩定性。

技術研發人員：杭義軍,張云睿,呂印新
受保護的技術使用者：阿慕優（昆山）導航科技有限公司
技術研發日：
技術公布日：2024/9/19