專利名稱：：一種月面巡視探測器的三維定姿與局部定位方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種月面巡視探測器三維定姿與局部定位方法，適用于月面、火星表面等復(fù)雜未知地形中探測器的自主定姿定位，或地面野外車輛的自主導(dǎo)4元。
背景技術(shù)：
：月面巡視探測器是一類在月面實施巡視勘查的探測器，也稱月球探測車、月球車等，也可將月面巡視探測器、行星車統(tǒng)稱為探測車。廣義上講，月面巡視4果測器是一種能夠在月球表面移動，完成探測、采樣、運載等任務(wù)的航天器。狹義上講，月面巡視探測器是能適應(yīng)月球環(huán)境，攜帶科學探測儀器在月面進行巡視探測的航天器。因此，月面巡視探測器是一類特殊的航天器，不同于傳統(tǒng)的衛(wèi)星、飛船，在著陸月面之前，月面巡視探測器是著陸器的有效載荷，著陸后是獨立的、完整的移動探測器。月面巡視探測器的定位包括全局定位和局部定位。全局定位即確定月球探測車在月表坐標系下的絕對位置。局部定位即確定月球探測車相對于導(dǎo)航坐標系(坐標系原點通常取著陸器著陸所在點)的位置描述。由于月面環(huán)境的特殊性，地面移動機器人和自主導(dǎo)航車輛采用的導(dǎo)航方法不適用于月面巡視探測器。如月面上無法檢測到GPS信號，因此GPS無法用于月面巡視探測器的定^f立；利用星圖識別進行天文導(dǎo)航的精度很低(幾百米幾公里)；涉及已知環(huán)境地圖和外部信息(例如雷達信標和超聲波導(dǎo)航)的地圖匹配定位和導(dǎo)航信標等方法也不可用，因為這些方法需要充分的環(huán)境先驗知識，不適用于在未知環(huán)境中移動的月面巡視探測器。迄今為止，國外成功發(fā)射的月球車有美國的LRV和前蘇聯(lián)的Lunokhod1、Lunokhod2。美國的LRV是有人駕駛的月球車，前蘇聯(lián)的Lunokhod1和Lunokhod2采用遙操作方式。國外成功的火星車是美國1997年的Sojourner和2004年的MER(包括Spirit和Opportunity)。jt匕夕卜國內(nèi)外也進行了地面原理樣機的研制，比較典型的有FIDO、Rocky7、Marsokhod、Nomad等，其中除Nomad采用測距儀、傾斜儀、陀螺、慣量計和GPS等比較傳統(tǒng)的儀器結(jié)合起來進行定位外，其他都采用了IMU、太陽敏感器和輪系編碼器三類敏感器作為主要的定姿定位信息獲得手段，但系統(tǒng)配置、定姿定位算法方面有所不同。除以上已得到應(yīng)用的技術(shù)外，近年來公開發(fā)表的文獻中對月球及火星表面巡視探測器定姿定位方法的研究也比較多。但上述已有方法存在的缺點主要體現(xiàn)在(1)利用兩軸加速度計輸出求解傾斜角度，這在傾斜角度較大，或當?shù)匾铀俣容^常規(guī)值發(fā)生較大變化時，產(chǎn)生的誤差較大；(2)利用偏航軸陀螺測量偏航角，沒有考慮由地形起伏變化導(dǎo)致的滾動與俯仰角度；(3)利用水平兩維位置信息，或利用左右兩輪運動幾何關(guān)系建立與偏航姿態(tài)的關(guān)系，利用太陽敏感器輸出對位置進行修正；這種方法雖然簡單，適合于平坦地形，但模型誤差大，不適合于復(fù)雜起伏地形；(4)為了提高定姿定位精度，選取比較多的狀態(tài)量作為待估計值，如位置誤差、姿態(tài)誤差和慣性敏感器偏差等，建立的狀態(tài)方程和測量方程比較復(fù)雜，雖然一定程度上提高了定姿和局部定位精度，但由于計算的復(fù)雜性，工程應(yīng)用實現(xiàn)難度較大；(5)常用的航位推算方法只利用左右輪的轉(zhuǎn)速計算行駛里程，沒有考慮地形高度變化帶來的影響，該方法在復(fù)雜地形下會導(dǎo)致較大誤差。
發(fā)明內(nèi)容本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足之處，提供一種月面巡視探測器三維定姿與局部定位方法，該方法能夠獲得高的定姿和定位精度，同時計算簡單，工程實現(xiàn)容易。本發(fā)明的技術(shù)解決方案是一種月面巡視探測器的三維定姿與局部定位方法，其特點在于步驟如下(1)當月面巡視探測器處于靜態(tài)時，利用三軸加速度計敏感確定滾動和俯仰角；(2)利用太陽敏感器的輸出及上述步驟(1)獲得的滾動和俯仰角確定偏航角姿態(tài)；(3)以三軸姿態(tài)和陀螺偏差作為狀態(tài)量，三軸加速度計確定的滾動、俯仰角和由太陽敏感器確定的偏航角，及三個陀螺輸出作為測量信息，建立狀態(tài)方程和測量方程，利用擴展卡爾曼濾波估計三軸姿態(tài)和陀螺偏差；(4)當月面巡視探測器運動時，利用由步驟(3)估計得到的陀螺偏差對陀螺輸出進行補償后，計算月面巡視探測器的姿態(tài)變化，完成陀螺姿態(tài)預(yù)估，實現(xiàn)姿態(tài)更新，獲得由導(dǎo)航坐標系到本體坐標系的姿態(tài)矩陣；(5)采集月面巡視探測器各驅(qū)動輪轉(zhuǎn)速、轉(zhuǎn)向輪轉(zhuǎn)角、左右搖臂轉(zhuǎn)角信息，利用移動子系統(tǒng)的正運動學關(guān)系獲得探測器在本體坐標系中的位置增量；(6)將步驟(5)獲得探測器在本體坐標系中的位置增量轉(zhuǎn)變導(dǎo)航坐標系中的位置增量，求解巡視探測器在導(dǎo)航坐標系三個方向上的速度和位置信自本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點在于(1)本發(fā)明利用靜態(tài)時建立簡單的狀態(tài)方程，并利用慣性敏感器測量輸出與當時姿態(tài)之間的關(guān)系建立陀螺和加速度計靜態(tài)測量方程，再通過擴展Kalman濾波技術(shù)，克服了太陽敏感器和慣性敏感器的隨機噪聲，從而得到較高精度的姿態(tài)估計值，提高了定姿精度；在姿態(tài)確定的同時還完成了對陀螺偏差的估計。動態(tài)時利用陀螺輸出進行姿態(tài)預(yù)估，此時上一次的陀螺偏差估計值用于對陀螺輸出進行補償，提高了動態(tài)時陀螺姿態(tài)預(yù)估的精度。(2)利用探測器的全運動學關(guān)系和姿態(tài)信息進行局部位置確定。與僅采用單輪或左右兩輪轉(zhuǎn)速的定位方法相比，本發(fā)明利用基于全部驅(qū)動輪轉(zhuǎn)速、轉(zhuǎn)向輪轉(zhuǎn)角和搖臂轉(zhuǎn)角的運動學關(guān)系求解探測器在本體坐標系中的速度，降低了輪子滑移所造成的誤差；此外，姿態(tài)信息的引入實現(xiàn)了起伏地形下的三維定位，能夠獲得高度信息，與一般的忽略高度變化的平面簡化處理相比，定位精度提高。(3)本發(fā)明得到的位置信息可作為進一步基于視覺信息處理的地面平差處理的基礎(chǔ)。用于地面車輛的自主導(dǎo)航時，可采用電解質(zhì)型或力平衡伺服型等高精度傾角傳感器得到滾動和俯仰兩個水平傾角，聯(lián)合高精度絕對航向敏感器(如太陽敏感器或電子羅盤等)獲得偏航角，這樣可增加測量信息，且降低了測量函數(shù)的復(fù)雜程度，便于在線應(yīng)用。(4)位置確定時采用了基于六輪轉(zhuǎn)速、四輪轉(zhuǎn)角以及主副搖臂轉(zhuǎn)角的全運動學關(guān)系，并引入了姿態(tài)信息，這種方法有以下三個優(yōu)點(1)減小了單獨采用某一個或幾個輪子轉(zhuǎn)速進行定位時的滑移誤差；(2)減小了滑移的影響；(3)克服了地形高度變化帶來的影響?？傊景l(fā)明已在月面巡視探測器原理樣機中通過內(nèi)場和國內(nèi)首次沙漠外場試驗驗證，方法可行，工程技術(shù)易實現(xiàn)，因此具有實用性。圖1為本發(fā)明月面巡視探測器三維定姿與局部定位流程圖2為采用本發(fā)明方法獲得的滾動角的濾波估計值；圖3為未采用本發(fā)明直接測量的滾動角值；圖4為采用本發(fā)明方法獲得的俯仰角的濾波估計值；圖5為未采用本發(fā)明直接測量的俯仰角值；圖6為采用本發(fā)明方法獲得的偏航角的濾波估計值；圖7為未采用本發(fā)明直接測量的偏航角值；圖8為采用本發(fā)明方法獲得的滾動軸陀螺偏差估計值；圖9為采用本發(fā)明方法獲得的俯仰軸陀螺偏差估計值；圖10為采用本發(fā)明方法獲得的偏航軸陀螺偏差估計值。具體實施例方式實施例1:定義"東-北-天"月理坐標系為導(dǎo)航坐標系，以3-2-1(即先繞Z軸，再繞Y軸，然后X軸)轉(zhuǎn)序為例，定義巡視探測器本體坐標系相對導(dǎo)航坐標系的歐拉角p、AW為三軸姿態(tài)角，相對導(dǎo)航坐標系原點的位置為局部位置。月面巡視探測器以六輪搖臂式為例，其中六輪驅(qū)動，四輪轉(zhuǎn)向。如圖1所示，本發(fā)明的具體步驟如下(1)當月面巡視探測器處于靜態(tài)時，利用三軸加速度計敏感當?shù)刂亓铀俣龋?jīng)轉(zhuǎn)換后獲得滾動和俯仰角度。設(shè)沿本體坐標系正交安裝的三個加速度計的輸出分別為[4，,，U(不考慮測量噪聲和安裝誤差)，則滾動角A和俯仰角《'可按下式求解<formula>formulaseeoriginaldocumentpage10</formula>(2)利用太陽敏感器的輸出計算偏航角。首先利用太陽敏感器的測量輸出"、、獲得太陽矢量在測量坐標系中的坐標如下所示義一_tanA_<formula>formulaseeoriginaldocumentpage10</formula>定義為太陽敏感器的測量坐標系，其中z、、沿碼盤平面的法線方向，A、軸沿太陽光入射的狹縫方向，4軸按右手坐標系定義，則"、表示太陽敏感器的測量角，為太陽矢量在y"z"平面上的投影與z"軸的夾角，a表示太陽敏感器的進光角，為太陽矢量在x、.、z平面上的投影與z、、軸的夾角，兩者可以通過碼盤讀數(shù)、折射系數(shù)以及敏感軸與碼盤的垂直距離得出。將太陽矢量轉(zhuǎn)換到探測器本體坐標系中描述<formula>formulaseeoriginaldocumentpage11</formula>其中《是太陽敏感器的分配矩陣，是安裝矩陣G、的逆矩陣。G'的形式如下<formula>formulaseeoriginaldocumentpage11</formula>C中各元素的數(shù)值由太陽敏感器的安裝位置確定。同時，利用星歷數(shù)據(jù)推算獲得太陽矢量在巡視探測器當?shù)氐乩碜鴺讼抵械淖鴺?amp;'&'&,該坐標&'&，&作為已知的輸入值；利用滾動角^和俯仰角A,求解偏航角<formula>formulaseeoriginaldocumentpage11</formula>其中<formula>formulaseeoriginaldocumentpage11</formula>(3)靜態(tài)時，取三軸姿態(tài)和陀螺偏差作為狀態(tài)量，取由三軸加速度計確定的滾動、俯仰角和由太陽敏感器確定的偏航角、三個陀螺輸出作為測量信息，建立狀態(tài)方程和測量方程，利用擴展卡爾曼濾波估計三軸姿態(tài)和陀螺偏差。首先，取待估計的三個姿態(tài)信息W6、V/和三軸陀螺的偏差&、^v、^定義成為一個六維狀態(tài)列向量，如下所示碌)《病維),&WW，J"從而建立狀態(tài)方程為単)^(A:，A:-1)尊-l)+w04)其中，w("為動態(tài)噪聲矢量，A表示第"欠采樣。上述狀態(tài)方程中，D可根據(jù)所采用的敏感器特性以及建模精度要求而定。結(jié)合對月面巡視探測器處于靜態(tài)時的模型進行分析，可以認為待估計姿態(tài)和敏感器偏差都是不變的，即在任何一個采樣時刻其值都是相同的。那么可取維")=/,。然后，取由加速度計靜態(tài)輸出和太陽敏感器獲得的三軸姿態(tài)測量值&、&、W"三個陀螺的靜態(tài)輸出g'^、g礦定義成為一個六維狀態(tài)列向量，^口下戶斤示W(wǎng)^'("g肌(A)從而建立量測方程早)4(碌))+v(A0其中為與敏感器的測量精度有關(guān)的測量噪聲，*同樣表示第*次采樣。/KXW)為測量函數(shù)，與當前的狀態(tài)量和從導(dǎo)航坐標系到本體坐標系的姿態(tài)變換矩陣有關(guān)。設(shè)當?shù)亟?jīng)綿度分別為人"，這種情況下，推導(dǎo)獲得函數(shù)^X(")如下所示—A,_《，&0(cos6sin^cos;7—sin^sin7)+、6>0((cos9cos+sinpsin^sincos/7+sinpcosSsin77)+6",w0((-sinpcosw+cospsin6"sincos7+cospcosPsin7)+、其中，叫是月球自轉(zhuǎn)角速率。建立了狀態(tài)方程和觀測方程后，利用擴展卡爾曼濾波估計三軸姿態(tài)和陀螺偏差。(4)當月面巡視探測器運動時，利用由步驟(.3)估計得到的陀螺偏差對陀螺輸出進行補償后，計算月面巡視探測器的姿態(tài)變化，完成陀螺姿態(tài)預(yù)估，完成姿態(tài)更新，獲得由導(dǎo)航坐標系到本體坐標系的姿態(tài)矩陣G"。上述的求解過程如下由補償后的陀螺輸出和上一次的姿態(tài)矩陣求取當前姿態(tài)角速率^*,即月面巡視探測器本體坐標系相對導(dǎo)航坐標系的角速度在本體坐標系中的分量<table>tableseeoriginaldocumentpage13</column></row><table>其中《是導(dǎo)航坐標系相對月固坐標系的角速度在本體坐標系中的分量；"'t是月固坐標系相對慣性坐標系的角速度(即月球自轉(zhuǎn)角速度)在本體標系中的分量。利用(《求解四元數(shù)微分方程，進行四元數(shù)更新<table>tableseeoriginaldocumentpage13</column></row><table>根據(jù)四元數(shù)求取姿態(tài)矩陣:<table>tableseeoriginaldocumentpage13</column></row><table>《11一《3+《42(暢-頓4)2(她+狄4)2(《2《3—験)根據(jù)G"與姿態(tài)角的關(guān)系求出姿態(tài)參數(shù)^、隊「以3-2-1轉(zhuǎn)序為例，G"與姿態(tài)角的關(guān)系為cos9coscpcos9sincp—sin6cosij/sinq)+simj/sin6cos(pcosv|/coscp+sim(/sin6sirKpsini|;cos6sinx|;sin(p+cos\|;sin6cos(p—sinv);cos(p+cosi^sin6sin(pcosi)/cos6其它4爭序作對應(yīng)修改。(5)采集各驅(qū)動輪轉(zhuǎn)速、轉(zhuǎn)向輪轉(zhuǎn)角、左右搖臂轉(zhuǎn)角信息，利用移動子系統(tǒng)的正運動學關(guān)系獲得探測器的位置增量在本體坐標系中的分量AxA，△%，=i(cos^cos《辨+cos伊2cos^盧2+cos—《+cosp2c/(94+cosAcos(55,+cos/2cos(5g(i《)=i(sin《d6^+sin<52t/02+sin《(ie5十sin&c616)AzA=i(sin^cos《辨+sinp2cos《2辨+sin+sin^2辨+sin",cos<55,+sin/2cos《辨)其中W、^為中間變量，^=A+A，％=A+A，A，A，A，A分別為左右主副搖臂轉(zhuǎn)角；,^6為六個驅(qū)動輪轉(zhuǎn)速，由每個輪子上安裝的編碼器獲得；S^2,S5，S6為左前、右前、左后和右后四個轉(zhuǎn)向輪轉(zhuǎn)角。(6)將步驟(5)獲得探測器在本體坐標系中的位置增量轉(zhuǎn)變導(dǎo)航坐標系中的位置增量，求解巡視探測器在導(dǎo)航坐標系三個方向上的速度和位置信息。位置增量與探測器當前姿態(tài)有關(guān)，且隨姿態(tài)變化而變化，不能直接用于導(dǎo)航計算，需利用當前姿態(tài)信息轉(zhuǎn)換到在導(dǎo)航坐標系中的分量&"，"，。<formula>formulaseeoriginaldocumentpage14</formula>其中c6=c6"。對該位置增量轉(zhuǎn)換到在導(dǎo)航坐標系描述后，求解巡視探測器在導(dǎo)航坐標系三個方向上的速度、、V、和位置信息、"^、Z"。、=Ax',/A/1^=,+(7)因任務(wù)的需求月球探測車將在靜止狀態(tài)和運動狀態(tài)之間切換，在月球探測車整個工作過程中，月球探測車重復(fù)進行圖1所示的三維定姿與局部定位流程，從而實現(xiàn)工作全程獲得月球探測車比較準確的姿態(tài)和位置信息，保證月球探測車安全有效地完成科學探測的任務(wù)。圖2為采用本發(fā)明方法獲得的滾動角的濾波估計值，圖3為未采用本發(fā)明直接測量的滾動角值，比較圖2、圖3發(fā)現(xiàn)，使用本發(fā)明所介紹的方法獲取的滾動角，相比較未采用本發(fā)明方法而使用敏感器直接測量計算得到的滾動角，精度能夠獲得較大提高。圖4為采用本發(fā)明方法獲得的俯仰角的濾波估計值，圖5未采用本發(fā)明直接測量的俯仰角值，比較圖4、圖5發(fā)現(xiàn)，使用本發(fā)明所介紹的方法獲取的俯仰角，相比較未采用本發(fā)明方法而使用敏感器直接測量計算得到的俯仰角，精度能夠獲得較大提高。圖6為采用本發(fā)明方法獲得的偏航角的濾波估計值，圖7為未采用本發(fā)明直接測量的偏航角值，比較圖6、圖7發(fā)現(xiàn)，使用本發(fā)明所介紹的方法獲取的偏航角，相比較未采用本發(fā)明方法而使用敏感器直接測量計算得到的偏航角，精度能夠獲得較大提高。圖8為采用本發(fā)明方法獲得的滾動軸陀螺偏差估計值，由圖8可以發(fā)現(xiàn)，使用本發(fā)明所介紹的方法能夠較準確地估計出滾動軸方向上的陀螺偏差。圖9為采用本發(fā)明方法獲得的俯仰軸陀螺偏差估計值，由圖9可以發(fā)現(xiàn)，使用本發(fā)明所介紹的方法能夠較準確地估計出俯仰軸方向上的陀螺偏差。圖10采用本發(fā)明方法獲得的偏航軸陀螺偏差估計值，由圖10可以發(fā)現(xiàn)，使用本發(fā)明所介紹的方法能夠較準確地估計出偏航軸方向上的陀螺偏差。實施例2按其他轉(zhuǎn)序定義姿態(tài)角時，分析過程同上，可得出相似結(jié)果。此外對不同構(gòu)形的月面巡視探測器，需要根據(jù)運動學模型建立步驟(5)中與各車輪的運動關(guān)系，用于速度和位置確定，其方法與實施例1相似。權(quán)利要求1、一種月面巡視探測器的三維定姿與局部定位方法，其特征在于步驟如下(1)當月面巡視探測器處于靜態(tài)時，利用三軸加速度計敏感確定滾動和俯仰角；(2)利用太陽敏感器的輸出及上述步驟(1)獲得的滾動和俯仰角確定偏航角姿態(tài)；(3)以三軸姿態(tài)和陀螺偏差作為狀態(tài)量，三軸加速度計確定的滾動、俯仰角和由太陽敏感器確定的偏航角，及三個陀螺輸出作為測量信息，建立狀態(tài)方程和測量方程，利用擴展卡爾曼濾波估計三軸姿態(tài)和陀螺偏差(4)當月面巡視探測器運動時，利用由步驟(3)估計得到的陀螺偏差對陀螺輸出進行補償后，計算月面巡視探測器的姿態(tài)變化，完成陀螺姿態(tài)預(yù)估，實現(xiàn)姿態(tài)更新，獲得由導(dǎo)航坐標系到本體坐標系的姿態(tài)矩陣；(5)采集月面巡視探測器各驅(qū)動輪轉(zhuǎn)速、轉(zhuǎn)向輪轉(zhuǎn)角、左右搖臂轉(zhuǎn)角信息，利用移動子系統(tǒng)的正運動學關(guān)系獲得探測器在本體坐標系中的位置增量；(6)將步驟(5)獲得月面探測器在本體坐標系中的位置增量轉(zhuǎn)換成導(dǎo)航坐標系中的位置增量，求解月面巡視探測器在導(dǎo)航坐標系三個方向上的速度和位置信息。2、根據(jù)權(quán)利要求1所述的月面巡視探測器的三維定姿與局部定位方法，其特征在于所述的步驟(1)利用三軸加速度計敏感確定滾動和俯仰角如下利用三軸加速度計敏感當?shù)刂亓铀俣?，?jīng)轉(zhuǎn)換后獲得滾動和俯仰角度，即設(shè)沿本體坐標系正交安裝的三個加速度計的輸出分別為[,肌，,"rU,則滾動角&和俯仰角《'可按下式求解<formula>seeoriginaldocumentpage3</formula>3、根據(jù)權(quán)利要求1所述的月面巡視探測器的三維定姿與局部定位方法，其特征在于所述步驟(2)的確定偏航角姿態(tài)的方法為(1)首先利用太陽敏感器的測量輸出αs,βs,獲得太陽矢量在測量坐標系中的坐標如下所示<formula>seeoriginaldocumentpage3</formula>αs表示太陽敏感器的測量角，βs表示太陽敏感器的進光角；(2)將太陽矢量轉(zhuǎn)換到探測器本體坐標系中描述<formula>seeoriginaldocumentpage3</formula>其中Cssb是太陽敏感器的分配矩陣，是安裝矩陣Cbss的逆矩陣，Cbss的形式如下:<formula>seeoriginaldocumentpage3</formula>C中各元素的數(shù)值由太陽敏感器的安裝位置確定；(3)利用星歷數(shù)據(jù)推算獲得太陽矢量在巡視探測器當?shù)氐乩碜鴺讼抵械淖鴺薙gs,Sgy,Sgz(4)利用步驟(2)和(3)的結(jié)果和滾動角&、俯仰角θat解偏航角<formula>seeoriginaldocumentpage3</formula>其中<formula>seeoriginaldocumentpage4</formula>4、根據(jù)權(quán)利要求1所述的月面巡視探測器的三維定姿與局部定位方法，其特征在于所述步驟(3)中的狀態(tài)方程為X("^^，A-1),其中wW為動態(tài)噪聲矢量，A表示第"欠采樣，0(^-i)可根據(jù)所采用的敏感器特性以及建模精度要求確定；測量方程y("^(xw)+vw，其中v("為與敏感器的測量精度有關(guān)的測量噪聲，&同樣表示第*次采樣，WX(")為測量函數(shù)，與當前的狀態(tài)量和從導(dǎo)航坐標系到本體坐標系的姿態(tài)變換矩陣有關(guān)。5、根據(jù)權(quán)利要求1所述的月面巡視探測器的三維定姿與局部定位方法，其特征在于所述步驟(4)的求解過程如下(1)由補償后的陀螺輸出和上一次的姿態(tài)矩陣求取當前姿態(tài)角速率<A,即月面巡浮見探測器本體坐標系相對導(dǎo)航坐標系的角速度在本體坐標系中的分量<formula>seeoriginaldocumentpage4</formula>其中Whcg是導(dǎo)航坐標系相對月固坐標系的角速度在本體坐標系中的分量；Whic是月固坐標系相對慣性坐標系的角速度，即月球自轉(zhuǎn)角速度在本體標系中的分量；(2)利用wa求解四元數(shù)微分方程，進行四元數(shù)更新<formula>seeoriginaldocumentpage5</formula>(3)根據(jù)四元數(shù)求取姿態(tài)矩陣<formula>seeoriginaldocumentpage5</formula>全文摘要一種月面巡視探測器的三維定姿與局部定位方法如下(1)探測器處于靜態(tài)時，利用三軸加速度計敏感確定滾動和俯仰角；(2)利用太陽敏感器確定偏航角姿態(tài)；(3)三軸姿態(tài)和陀螺偏差作為狀態(tài)量，三軸加速度計確定的滾動、俯仰角和由太陽敏感器確定的偏航角及三個陀螺輸出作為測量信息，建立狀態(tài)方程和測量方程，利用擴展卡爾曼濾波估計三軸姿態(tài)和陀螺偏差；(4)探測器運動時，利用估計得到的陀螺偏差對陀螺輸出進行補償后，計算探測器的姿態(tài)變化，完成陀螺姿態(tài)預(yù)估，實現(xiàn)姿態(tài)更新；(5)采集探測器各驅(qū)動輪轉(zhuǎn)速、轉(zhuǎn)向輪轉(zhuǎn)角、左右搖臂轉(zhuǎn)角信息，利用正運動學關(guān)系獲得探測器在本體坐標系中的位置增量。本發(fā)明的定姿和定位精度高，計算簡單，工程實現(xiàn)容易。文檔編號G01C21/10GK101173858SQ200710123200公開日2008年5月7日申請日期2007年7月3日優(yōu)先權(quán)日2007年7月3日發(fā)明者琰刑,云劉,志張申請人:北京控制工程研究所