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

有效

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

1.一種單面結(jié)構(gòu)環(huán)境下的動力學(xué)模型/激光雷達(dá)組合導(dǎo)航方法，其特征在于，包括如下步驟：

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

(2)利用S(k)提取k時刻單面結(jié)構(gòu)環(huán)境的線特征；具體包括如下步驟：

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

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

(22)檢測S(k)中的撕裂點(diǎn)：

計算S(k)相鄰序號激光點(diǎn)之間的距離若大于閾值E_t，則p_i和p_i+1為撕裂點(diǎn)，記撕裂點(diǎn)集合為N_t為撕裂點(diǎn)的數(shù)量；

(23)檢測S(k)中的角點(diǎn)，將點(diǎn)云數(shù)據(jù)分群：

若dc大于閾值E_c則p_i為角點(diǎn)，且撕裂點(diǎn)不是角點(diǎn)，即dc的計算公式如下：

記角點(diǎn)集合為N_c為角點(diǎn)的數(shù)量，將角點(diǎn)和撕裂點(diǎn)作為分群的標(biāo)志，根據(jù)激光點(diǎn)在S(k)中的序號依次從p₁遍歷到將點(diǎn)云數(shù)據(jù)分群，記為s_g，1≤g≤N_g，N_g為點(diǎn)群的數(shù)量；

(24)篩選計算單面結(jié)構(gòu)環(huán)境的線特征的點(diǎn)云數(shù)據(jù)：

根據(jù)激光點(diǎn)在S(k)的序號，計算每個點(diǎn)群中序號最小的點(diǎn)與序號最大的點(diǎn)之間的距離篩選出最大的點(diǎn)群用于計算單面結(jié)構(gòu)環(huán)境的線特征，該點(diǎn)群記為

(25)利用點(diǎn)群擬合單面結(jié)構(gòu)環(huán)境的直線方程；

記單面結(jié)構(gòu)環(huán)境的直線方程為x＝a₁y+a₂，記q_i為的第i個激光點(diǎn)(i＝1,2,…,N₁)，N₁為中激光點(diǎn)的數(shù)量，和為q_i投影至飛行器所在水平面的二維坐標(biāo)，根據(jù)下式計算直線方程的參數(shù)a₁和a₂：

(3)計算k時刻四旋翼飛行器相對于單面結(jié)構(gòu)環(huán)境的相對距離、相對偏航角；具體包括如下步驟：

(31)若a₁＝0，則四旋翼飛行器相對于單面結(jié)構(gòu)環(huán)境的距離d(k)為a₂，偏航角ψ_l(k)為：

(32)若a₁≠0，則四旋翼飛行器相對于單面結(jié)構(gòu)環(huán)境的距離偏航角ψ_l(k)為：

其中，四旋翼飛行器機(jī)頭方向垂直指向于單面結(jié)構(gòu)時的偏航角為0，順時針旋轉(zhuǎn)為正；

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

(5)通過卡爾曼濾波器，結(jié)合四旋翼飛行器的阻力特性，對k時刻的姿態(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)為初始時刻機(jī)體系原點(diǎn)的位置。