[發(fā)明專利]一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法

有效

• 申請?zhí)枺?/td>: 201810214097.4
• 申請日: 2018-03-15
• 公開（公告）號: CN108536163B
• 公開（公告）日: 2021-04-16
• 發(fā)明（設(shè)計(jì)）人: 呂品;石鵬;賴際舟;鄒冰倩;付相可
• 申請（專利權(quán)）人: 南京航空航天大學(xué)
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10;G01C21/20
• 代理公司: 南京蘇高專利商標(biāo)事務(wù)所(普通合伙) 32204
• 代理人: 王安琪

本發(fā)明公開了一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法，包括如下步驟：(1)周期讀取k時(shí)刻四旋翼飛行器機(jī)載傳感器信息，包括激光雷達(dá)點(diǎn)云信息S(k)，陀螺儀信息加速度計(jì)信息氣壓高度計(jì)信息h_b(k)；(2)利用S(k)提取k時(shí)刻單面結(jié)構(gòu)環(huán)境的線特征；(3)計(jì)算k時(shí)刻四旋翼飛行器相對于單面結(jié)構(gòu)環(huán)境的相對距離、相對偏航角；(4)預(yù)測k時(shí)刻四旋翼飛行器的姿態(tài)四元數(shù)、速度、位置；(5)通過卡爾曼濾波器，結(jié)合四旋翼飛行器的阻力特性，對k時(shí)刻的姿態(tài)四元數(shù)、速度、位置進(jìn)行校正。本發(fā)明能夠在單面結(jié)構(gòu)環(huán)境下，完成基于激光雷達(dá)的四旋翼飛行器姿態(tài)、速度、位置的解算。

技術(shù)領(lǐng)域

本發(fā)明涉及組合導(dǎo)航領(lǐng)域，尤其是一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法。

背景技術(shù)

四旋翼飛行器具有體積小、結(jié)構(gòu)簡單、可懸停和垂直起降等優(yōu)點(diǎn)，近年來在軍用、民用領(lǐng)域得到了廣泛的應(yīng)用。導(dǎo)航系統(tǒng)為四旋翼飛行器提供其飛行控制系統(tǒng)所必須的導(dǎo)航信息，是其完成各種復(fù)雜飛行任務(wù)的必要保障。目前，衛(wèi)星導(dǎo)航與視覺導(dǎo)航是四旋翼飛行器常用的導(dǎo)航方法。然而，衛(wèi)星導(dǎo)航信號易受干擾，視覺導(dǎo)航易受光線干擾。當(dāng)衛(wèi)星導(dǎo)航、視覺導(dǎo)航不可用時(shí)，激光雷達(dá)是四旋翼飛行器常用的導(dǎo)航方式。

激光雷達(dá)是以發(fā)射激光束探測目標(biāo)的位置、速度等特征量的雷達(dá)系統(tǒng)。其工作原理是向目標(biāo)發(fā)射探測信號，然后將接收到的從目標(biāo)反射回來的信號與發(fā)射信號進(jìn)行比較，作適當(dāng)處理后，就可獲得目標(biāo)的有關(guān)信息。當(dāng)激光雷達(dá)用于導(dǎo)航時(shí)，普遍采用同時(shí)定位與地圖構(gòu)建方法(Simultaneous Localization And Mapping，簡稱SLAM)，在載體上通過對各種傳感器數(shù)據(jù)進(jìn)行采集和計(jì)算，實(shí)現(xiàn)對其自身位置、姿態(tài)的定位以及對場景地圖的構(gòu)建。

在采用激光雷達(dá)SLAM方法時(shí)，要求周圍環(huán)境具有較為明顯的結(jié)構(gòu)特征，當(dāng)特征稀疏時(shí)會影響SLAM效果。本發(fā)明中涉及的單面結(jié)構(gòu)環(huán)境，指的是僅有光滑的一個(gè)墻面的環(huán)境，屬于特征稀疏環(huán)境。四旋翼飛行器在執(zhí)行貼墻飛行任務(wù)時(shí)，激光雷達(dá)所探測的環(huán)境有時(shí)會出現(xiàn)單面結(jié)構(gòu)環(huán)境，此時(shí)通過傳統(tǒng)的激光雷達(dá)SLAM方法得到的導(dǎo)航精度較差。

發(fā)明內(nèi)容

本發(fā)明所要解決的技術(shù)問題在于，提供一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法，能夠在單面結(jié)構(gòu)環(huán)境下，完成基于激光雷達(dá)的四旋翼飛行器姿態(tài)、速度、位置的解算。

為解決上述技術(shù)問題，本發(fā)明提供一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法，包括如下步驟：

(1)周期讀取k時(shí)刻四旋翼飛行器機(jī)載傳感器信息，包括激光雷達(dá)點(diǎn)云信息S(k)，陀螺儀信息加速度計(jì)信息氣壓高度計(jì)信息h_b(k)；

(2)利用S(k)提取k時(shí)刻單面結(jié)構(gòu)環(huán)境的線特征；

(3)計(jì)算k時(shí)刻四旋翼飛行器相對于單面結(jié)構(gòu)環(huán)境的相對距離、相對偏航角；

(4)預(yù)測k時(shí)刻四旋翼飛行器的姿態(tài)四元數(shù)、速度、位置；

(5)通過卡爾曼濾波器，結(jié)合四旋翼飛行器的阻力特性，對k時(shí)刻的姿態(tài)四元數(shù)、速度、位置進(jìn)行校正；其中，機(jī)體系的X、Y、Z軸分別為飛行器的機(jī)頭方向、右向、下向，激光雷達(dá)與機(jī)體系相固連，導(dǎo)航系的Z軸豎直向下，X軸垂直指向單面結(jié)構(gòu)，Y軸根據(jù)右手定則確定，導(dǎo)航系原點(diǎn)為初始時(shí)刻機(jī)體系原點(diǎn)的位置。

優(yōu)選的，步驟(2)中，提取k時(shí)刻單面結(jié)構(gòu)環(huán)境的線特征具體包括如下步驟：

(21)計(jì)算S(k)中激光點(diǎn)投影至飛行器所在水平面的二維坐標(biāo)：

記p_i為S(k)的第i個(gè)激光點(diǎn)(i＝1,2,…,N₀)，N₀為S(k)中激光點(diǎn)的數(shù)量，和為p_i在機(jī)體系中的坐標(biāo)，記θ(k-1)和φ(k-1)分別為飛行器在k-1時(shí)刻的俯仰角和橫滾角，p_i投影至飛行器所在水平面的二維坐標(biāo)和根據(jù)下式計(jì)算：