[發(fā)明專利]基于函數(shù)迭代積分的剛體姿態(tài)解算方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201810236436.9
• 申請(qǐng)日: 2018-03-21
• 公開（公告）號(hào): CN108534774B
• 公開（公告）日: 2020-02-21
• 發(fā)明（設(shè)計(jì)）人: 武元新
• 申請(qǐng)（專利權(quán)）人: 上海交通大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16
• 代理公司: 上海漢聲知識(shí)產(chǎn)權(quán)代理有限公司 31236
• 代理人: 莊文莉

本發(fā)明提供了一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法及系統(tǒng)，包括：根據(jù)時(shí)間區(qū)間上的陀螺測(cè)量值，擬合出角速度的切比雪夫多項(xiàng)式函數(shù)；利用得到的角速度的切比雪夫多項(xiàng)式系數(shù)以及羅德里格向量積分方程，迭代計(jì)算羅德里格向量的切比雪夫多項(xiàng)式系數(shù)，并對(duì)每次迭代的結(jié)果按照預(yù)設(shè)的階數(shù)進(jìn)行多項(xiàng)式截?cái)啵桓鶕?jù)得到的羅德里格向量的切比雪夫多項(xiàng)式系數(shù)以及對(duì)應(yīng)的切比雪夫多項(xiàng)式計(jì)算得到羅德里格向量，以四元數(shù)的形式給出時(shí)間區(qū)間上的姿態(tài)變化。本發(fā)明基于函數(shù)迭代積分的技術(shù)，利用羅德里格向量，實(shí)現(xiàn)從陀螺測(cè)量快速重建姿態(tài)，在不顯著降低計(jì)算精度的情況下提高計(jì)算速度。

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

本發(fā)明涉及測(cè)試測(cè)量技術(shù)領(lǐng)域，具體而言，涉及一種基于函數(shù)迭代積分的剛體姿態(tài)解算快速計(jì)算方法及系統(tǒng)。

背景技術(shù)

三維空間剛體運(yùn)動(dòng)的計(jì)算或估計(jì)是物理、機(jī)器人、導(dǎo)航制導(dǎo)、機(jī)械、計(jì)算機(jī)視覺等眾多領(lǐng)域中的核心問題。與速度、位置等平移運(yùn)動(dòng)不同，姿態(tài)不能被直接測(cè)量，只能通過角速度積分或向量匹配等間接方式獲得。角速度積分方式的姿態(tài)解算是完全自主的，不需要外部信息輔助，因此在很多(如衛(wèi)星導(dǎo)航系統(tǒng)不能發(fā)揮作用的)應(yīng)用場(chǎng)合備受青睞。

近年來，本領(lǐng)域研究人員提出了若干高精度姿態(tài)解算方法。申請(qǐng)人在申請(qǐng)?zhí)枮镃N201710273489.3的發(fā)明專利中提出一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法，即：根據(jù)時(shí)間區(qū)間上的陀螺測(cè)量值，擬合出角速度的多項(xiàng)式函數(shù)；利用角速度的多項(xiàng)式擬合函數(shù)以及羅德里格向量(Rodrigues)積分方程，迭代計(jì)算羅德里格向量，進(jìn)而根據(jù)迭代結(jié)果，以四元數(shù)的形式給出時(shí)間區(qū)間上的姿態(tài)變化。該方法具有計(jì)算精度高的優(yōu)勢(shì)，但在迭代過程中沒有充分利用切比雪夫多項(xiàng)式的良好性質(zhì)，且羅德里格向量的多項(xiàng)式階數(shù)隨著迭代過程急劇增長(zhǎng)，計(jì)算量大，難以滿足實(shí)時(shí)應(yīng)用。例如，對(duì)于利用八個(gè)陀螺測(cè)量值進(jìn)行角速度多項(xiàng)式擬合的情況，在第七次迭代時(shí)，羅德里格向量多項(xiàng)式的階數(shù)超過一千！實(shí)際上，由于角速度測(cè)量存在誤差，對(duì)羅德里格向量不需要用到如此高階多項(xiàng)式。

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

針對(duì)現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法及系統(tǒng)。

根據(jù)本發(fā)明提供的一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法，包括：

擬合步驟：根據(jù)時(shí)間區(qū)間上的陀螺測(cè)量值，擬合出角速度的切比雪夫多項(xiàng)式函數(shù)；

迭代步驟：利用得到的角速度的切比雪夫多項(xiàng)式系數(shù)以及羅德里格向量積分方程，迭代計(jì)算羅德里格向量的切比雪夫多項(xiàng)式系數(shù)，并對(duì)每次迭代的結(jié)果按照預(yù)設(shè)的階數(shù)進(jìn)行多項(xiàng)式截?cái)啵?/p>

姿態(tài)解算步驟：根據(jù)得到的羅德里格向量的切比雪夫多項(xiàng)式系數(shù)以及對(duì)應(yīng)的切比雪夫多項(xiàng)式計(jì)算得到羅德里格向量，以四元數(shù)的形式給出時(shí)間區(qū)間上的姿態(tài)變化。

較佳的，所述陀螺測(cè)量值包括角速度測(cè)量值或者角增量測(cè)量值。

較佳的，擬合步驟具體包括：

對(duì)于t_k時(shí)刻的N個(gè)角速度測(cè)量值或角增量測(cè)量值k＝1,2,...N；令將原時(shí)間區(qū)間映射到[-1 1]上；角速度采用不超過N-1階的切比雪夫多項(xiàng)式進(jìn)行擬合近似

其中n為角速度切比雪夫多項(xiàng)式的階數(shù)，c_i為第i階切比雪夫多項(xiàng)式的系數(shù)向量，F(xiàn)_i(τ)為第i階第一類切比雪夫多項(xiàng)式，τ為映射后的時(shí)間自變量。

較佳的，迭代步驟具體包括：

在l次迭代時(shí)，羅德里格向量的切比雪夫多項(xiàng)式記做：

其中n_T為預(yù)設(shè)的截?cái)嚯A數(shù)，b_l,i為l次迭代時(shí)第i階切比雪夫多項(xiàng)式的系數(shù)，當(dāng)l＝0時(shí)，g_l＝0。

較佳的，羅德里格向量的切比雪夫多項(xiàng)式系數(shù)按如下迭代計(jì)算：