專利名稱：：基于全功能太陽羅盤的月球車位姿自主確定方法
技術(shù)領(lǐng)域：
：基于全功能太陽羅盤的月球車位姿自主確定方法是以月球探測車(簡稱月球車)為應(yīng)用對象的天文與慣性聯(lián)合導(dǎo)航方法，屬深空探測科學(xué)及地球觀測科學(xué)與工程領(lǐng)域?？蓮V泛應(yīng)用于月球車、火星車及地面作戰(zhàn)機(jī)器人的導(dǎo)航與控制系統(tǒng)當(dāng)中。
背景技術(shù)：
：作為月球車的導(dǎo)航技術(shù)面臨三項(xiàng)重要環(huán)境約束(1)月球無地磁，現(xiàn)有的磁羅盤技術(shù)不能應(yīng)用；(2)月球是慢自旋的(相對地球自轉(zhuǎn)慢了近30倍)，陀螺尋北技術(shù)不能應(yīng)用于月球北向確定；(3)盡管現(xiàn)代飛行器及航天器廣泛利用太陽及恒星逬行位置確定，但在月球車巡游探測吋，采用太陽敏感器或星敏感器，都易受粉塵污染，致使使用壽命很短。另一方面，月球離地球很遠(yuǎn)，通信及數(shù)據(jù)處理延吋一般高于8秒甚至達(dá)數(shù)十分鐘，要求月球車具有自主位姿確定能力。因而，需要根據(jù)月球無地磁、慢自旋、粉塵污染的環(huán)境特點(diǎn)提出新的位姿自主確定方法，以滿足曰益增長的月球巡視探測任務(wù)的需要?，F(xiàn)有的月球與火星車的自主位姿確定方法尚未形成，美國火星車自主位姿確定利用了火星GPS系統(tǒng)，目前尚未見到相關(guān)專利與文獻(xiàn)對位姿方法進(jìn)行系統(tǒng)分析與證明。美國及曰本在月球與火星車的位姿確定方面都有自己的方案，但不是自主式的、系統(tǒng)的絕對位姿確定方法，即不能提供相對月球車經(jīng)緯度、相對地平指北系的航向及月球車相對地平系的速度?，F(xiàn)有的技術(shù)方案不能滿足對月球制圖、月球資源普査等科學(xué)任務(wù)的需要。
發(fā)明內(nèi)容本發(fā)明的目的在于為月球車提供適應(yīng)月球環(huán)境，特別是無地磁、慢自旋、粉塵污染環(huán)境(除月球車運(yùn)行過程中產(chǎn)生的粉塵，還有由于高低溫環(huán)境產(chǎn)生的粉塵環(huán)境，該機(jī)理尚無科學(xué)解釋)的月球車自主位姿確定方法。1.技術(shù)方案與系統(tǒng)結(jié)構(gòu)本發(fā)明的技術(shù)方案參見系統(tǒng)結(jié)構(gòu)框圖1和工作過程圖2，本發(fā)明是由觀測設(shè)備、嵌入式PC104+計(jì)算機(jī)及內(nèi)置于廣角太陽相機(jī)1.1的嵌入式TMS320計(jì)算機(jī)為基礎(chǔ)完成的，其中與嵌入式PC104+計(jì)算機(jī)集成的觀測設(shè)備有廣角太陽相機(jī)l.l、稱為IMU1.2的慣性單元、吋鐘1.3及高速激光雷達(dá)1.4;在廣角太陽相機(jī)1.1內(nèi)置的嵌入式TMS320計(jì)算機(jī)中包含的模塊有智能太陽識別與處理方法模塊2;在PC104+計(jì)算機(jī)中包括的模塊有通信與任務(wù)調(diào)度模塊IO、月球車自主位姿確定任務(wù)模塊；月球車自主位姿確定任務(wù)模塊包含月球垂直陀螺方法模塊3、初始^I姿校準(zhǔn)方法模塊4、高精度太陽羅盤導(dǎo)航方法模塊5、位姿補(bǔ)償方法方法模塊6、基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法模塊7、具有速度修正的月球車位姿確定方法模塊8、基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊9。基于全功能太陽羅盤的月球車位姿自主確定方法步驟如下(1)數(shù)據(jù)獲取，由在嵌入式PC104+計(jì)算機(jī)中幵發(fā)的通信與任務(wù)調(diào)度模塊完成，它包含太陽羅盤接口任務(wù)、IMU接口任務(wù)及高速激光雷達(dá)系統(tǒng)接口任務(wù)；太陽羅盤接口任務(wù)通過CAN通信訪問智能相機(jī)，包括設(shè)置智能太陽識別與處理方法模塊所需的參數(shù)，最終獲取智能太陽識別與處理方法模塊得到的太陽入射角W,W、曰出吋間^(。)等；IMU接口任務(wù)通過RS232通信訪問IMU，獲得三軸加速度、三軸角速度;高速激光雷達(dá)系統(tǒng)接口任務(wù)通過PCL總線通信訪問激光雷達(dá)，獲得測量的深度^、光強(qiáng)々數(shù)據(jù)流；(2)系統(tǒng)初始化，嵌入式PC104+計(jì)算機(jī)上電后系統(tǒng)初始化程序工作，對智能相機(jī)、IMU、激光雷達(dá)進(jìn)行初始設(shè)置，并啟動(dòng)嵌入式PC104+計(jì)算機(jī)中月球車自主位姿確定任務(wù)模塊及嵌入式TMS320計(jì)算機(jī)中智能太陽識別與處理方法模塊；(3)嵌入式PC104+計(jì)算機(jī)中月球車位姿確定任務(wù)模塊包括月球垂直陀螺方法模塊、智能太陽識別與處理方法模塊、初始位姿校準(zhǔn)方法模塊、高精度太陽羅盤導(dǎo)航方法模塊、位姿補(bǔ)償方法模塊、基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法模塊、具有速度修正的月球車位姿確定方法模塊、基于太陽連測的經(jīng)練校準(zhǔn)方法模塊，各處理方法之間并行運(yùn)行，共同完成月球車絕對位姿、相對位姿、平動(dòng)速度確定；在廣角太陽相機(jī)內(nèi)置的嵌入式系統(tǒng)TMS320計(jì)算機(jī)中包含的模塊有智能太陽識別與處理方法模塊；下面對各方法模塊實(shí)現(xiàn)方法及流程進(jìn)行描述由智能太陽識別與處理方法模塊識別曰出或太陽并判斷太陽升起吋間Q及太陽入射角&)、太陽方向角^V);該模塊首先應(yīng)對廣角鏡頭進(jìn)行校準(zhǔn)，然后拍攝天空圖像，提取太陽圓盤，按圓度、面積、顏色范圍來判別由圖像提取得到的簡單曲線是否為太陽；并計(jì)算太陽圓盤中心，根據(jù)計(jì)算太陽圓盤中心像點(diǎn)坐標(biāo)求太陽入射角《w及太陽方向角Pw;由月球垂直陀螺方法模塊由系統(tǒng)獲得的月球車三軸加速度c^)、角速度《(4)為輸入，經(jīng)加速度《")反饋校正四元數(shù)，進(jìn)行四元數(shù)計(jì)算迭代得月球車俯仰角"w與橫滾角y^);由太陽入射角《4)及太陽方向角PW及月球車俯仰角與橫滾角計(jì)算太陽高度角Z")及月球車前向與太陽的夾角《w;由初始位姿校準(zhǔn)模塊根據(jù)太陽高度角及月球車前向與太陽的夾角《t)通過高精度太陽羅盤導(dǎo)航模塊及基于太陽連測的經(jīng)緯校準(zhǔn)模塊計(jì)算月球車經(jīng)緯度義W，,)及太陽方位角<9(4)，從而計(jì)算月球車航向角OV);或者，由曰出吋按("-C)w,計(jì)算角度再換算為經(jīng)度；其中。叫分別是Si皿smedii中央灣吋間、月球自轉(zhuǎn)角速度；由高精度太陽羅盤導(dǎo)航方法模塊應(yīng)用于月球車初始位姿校準(zhǔn)模塊及基于太陽連測的經(jīng)纟韋校準(zhǔn)方法模塊；它們的輸入都是太陽高度角Z(w及觀測吋間；其中包含了由觀測吋間計(jì)算任一吋刻的太陽及月赤道與黃道的升交點(diǎn)經(jīng)度過程；二者不同在于高精度太陽羅盤導(dǎo)航方法模塊是一次觀測，基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊是多次觀測即連測的過程；但后者要為前者前供月球車經(jīng)度，從而可通過前者連續(xù)計(jì)算太陽方位角，高精度太陽羅盤導(dǎo)航方法模塊以校準(zhǔn)的經(jīng)度及實(shí)吋測量的太陽高度角zw及觀測吋間連續(xù)計(jì)算太陽方位角，從而獲得月球車航向角O(w;由基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法模塊根據(jù)測量的三軸加速度4)、三軸角速度化州及由高程圖的數(shù)據(jù)即粒子通過粒子濾波得到月球車的速度^);由基于速度修正的月球車位姿確定方法模塊輸入為月球車速度^)、月球車三軸加速度"(4)、角速度,)、月球車初始經(jīng)緯度/^)Aw及月球車航向角a^)，輸出是連續(xù)的月球車連續(xù)速度^+,)、經(jīng)緯度義(一^。(一及航向角+,)。所述的智能太陽識別與處理方法模塊的任務(wù)運(yùn)行于廣角太陽相機(jī)嵌入式系統(tǒng)當(dāng)中；該方法模塊完成兩部分任務(wù)即，太陽識別及太陽單位矢量計(jì)算與曰出識別及曰出吋間計(jì)算，該方法模塊的輸出結(jié)果為曰出吋間、太陽單位矢量；參見圖3魚眼鏡頭成像示意圖，所述的廣角太陽相機(jī)采用魚目鏡頭，其模型為公式吵(1)其中OcXcYcZc為相機(jī)坐標(biāo)系，r為圖像點(diǎn)P到魚眼圖像中心Oc的距離，/為魚眼鏡頭的焦距，0為入射光線與魚眼鏡頭光軸的夾角，即測量高度角，^為像點(diǎn)在坐標(biāo)系OcXcYcZc的方向角。則對應(yīng)于入射角為(^^)的空間點(diǎn)《的圖像點(diǎn)P在攝像機(jī)坐標(biāo)系下的坐標(biāo)O,y)滿足=柳義sin伊(2)將(1)帶入(2)得義(3)由于鏡頭加工工藝和其他因素，魚眼鏡頭通常很難嚴(yán)格滿足成像模型(l)，利用三次多項(xiàng)式來擬合該魚眼鏡頭的成像模型，即r=/c々+/c203則對應(yīng)的成像變換為=(^6+a:263)義(4)(5)畸變校正模型按式(6)進(jìn)行。=0^<9+"2(93)成像過程用6個(gè)參數(shù)來描述魚眼鏡頭成像的模型(6)附u00(o^+/^3)cosp)+(7)通過校準(zhǔn)板，拍攝其圖像，計(jì)算氣、W、"。、v。、K、k2六個(gè)校正參數(shù)。其中氣、W為測量高度角與像素坐標(biāo)矢聯(lián)常數(shù)，"。、v。為像素坐標(biāo)偏差，《、^為測量高度角校正常數(shù)；太陽方向計(jì)算根據(jù)魚眼圖像中某點(diǎn)的像素坐標(biāo)("，v)，求該點(diǎn)在攝像機(jī)坐標(biāo)系下的入射角(0，p)。其中^可以利用根據(jù)像素坐標(biāo)直接求解出來；對于0，將氣、氣、"。、v0、A、k2和^帶入式(7)得到關(guān)于0的一元三次方程，求解后去除不合理的解到太陽入射角P。相應(yīng)A:吋刻的太陽入射角及太陽方向角分別記為^w，^)〉太陽識別及太陽單位矢量計(jì)算的方法如圖4左所示首先獲取天空圖像；進(jìn)行濾矂，去除小顆粒粉塵圖像；進(jìn)行輪廓提取，并進(jìn)行簡單曲線幾何參數(shù)計(jì)算，包括面積，質(zhì)心，圓度計(jì)算；同吋計(jì)算太陽平均色彩；根據(jù)太陽圓度、面積、色彩范圍檢測太陽；如果判斷是太陽則計(jì)算太陽圓盤中心；根據(jù)太陽羅盤光學(xué)參數(shù)計(jì)算相對太陽羅盤相平面的單位矢量；曰出吋間計(jì)算判別過程如圖4左所右，首先獲取天空圖像；濾噪，去除小顆粒粉塵圖像；進(jìn)行輪廓提取，并進(jìn)行簡單曲線幾何參數(shù)計(jì)算，包括面積，質(zhì)心，圓度計(jì)算；同吋計(jì)算平均色彩；根據(jù)太陽弧段參數(shù)、色彩范圍檢測太陽；若是太陽則估計(jì)太陽圓盤中心；由太陽羅盤光學(xué)參數(shù)計(jì)算相對太陽羅盤相平面的單位矢量；根據(jù)曰出過程的數(shù)據(jù)記錄求解曰出吋間得月球車所在經(jīng)度。所述的月球垂直陀螺方法模塊方法中需適應(yīng)月球重力常數(shù)、月球半徑等環(huán)境等點(diǎn)，通過重力反饋進(jìn)行四元數(shù)修正的方法實(shí)現(xiàn)月球垂直陀螺的俯仰角與滾動(dòng)角解算；根據(jù)捷聯(lián)導(dǎo)航方程即式(8-9)其中《w=(/L。,不，;i2，^^，是四元數(shù)表示月球車姿態(tài)；(8)2W=2(義0(yfc)12(A("zl2(^)-、i)^3(")2(>^)^3(4)十^^2(;t))2(A(jfe)^3(A:)-A)A2(")2(/12(yfc)^3(A:)+、*))2(、^十/l3(")—1(9)運(yùn)用基于加速度反饋的捷聯(lián)導(dǎo)航四元數(shù)迭代的修正方程(10)(11)解算探測月球車相對地平坐標(biāo)系的俯仰角與橫滾角，由三軸加速度"w,及角速度w卓)，通過式(9-11)計(jì)算gw，得俯仰角)與橫滾角&);盡管式(8-9)是己有的公式，但與式(10-11)構(gòu)成了捷聯(lián)式的垂直陀螺系統(tǒng)方程。所述的月球車初始位姿校準(zhǔn)方法模塊工作流程如圖5左所示。前述的月球車初始位姿校準(zhǔn)分兩種一種是通過觀測太陽，確定曰出吋間^(。,來計(jì)算月球車初始經(jīng)度A(w，從而利用緯度及太陽方位角方程解算月球車緯度&)及太陽方位角^a)。利用垂直陀螺輸出的俯仰角"w與橫滾角/w、太陽相機(jī)獲得的太陽方向角及太陽與月球車前向夾角即太陽方向角^w,^W及太陽相機(jī)安裝姿態(tài)計(jì)算太陽在自由地平系下的太陽高度角Z(w及太陽相對月球車前向的角度《w，從而計(jì)算出月球車航向角&)，該過程參見高精度太陽羅盤導(dǎo)航模塊5。另一校準(zhǔn)方法是對太陽連續(xù)觀測解算月球車經(jīng)度義w，從而利用太陽羅盤原理得到月球車航向角。該過程參見基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊9。所述的高精度太陽羅盤導(dǎo)航方法模塊是:依據(jù)曰-地初始經(jīng)度運(yùn)用曰地自轉(zhuǎn)及公轉(zhuǎn)速度及吋間計(jì)算們?nèi)我粎伎痰慕?jīng)緯度，根據(jù)球面三角建立太陽羅盤導(dǎo)航方程，進(jìn)行基于太陽高度及方向的解唯一性判定，以此保證該太陽羅盤導(dǎo)航的高精度。高精度太陽羅盤導(dǎo)航方法流程框圖如圖6所示。該方法包括曰-地在月球體系下的經(jīng)度及升交點(diǎn)經(jīng)度計(jì)算、太陽高度角與方向角獲取、基于球面天文學(xué)的太陽羅盤導(dǎo)航方程求解、月球車航向計(jì)算。由于月球車經(jīng)度由太陽連續(xù)觀測或曰出校準(zhǔn)得到，在吋間上有一定的滯后，但由于短吋間內(nèi)月球車經(jīng)緯度變化很小，故對太陽方位角計(jì)算精度影響極小。下面對高精度太陽羅盤原理進(jìn)行說明。如圖7所示，月球體系定義原點(diǎn)為月球質(zhì)量中心，義軸經(jīng)過Sinusmedii環(huán)形坑即中央灣，Z軸為月球自旋軸，r軸由右手系確定，基中Z軸正向?yàn)楸碧鞓O，Z軸負(fù)向?yàn)槟咸鞓O；月球赤道(簡稱月赤道)與黃道交角記為s;太陽羅盤觀測的離散吋刻記為A:，其0點(diǎn)為、吋刻；在、吋刻的太陽及升交點(diǎn)經(jīng)度記為^^。；太陽在月球體系下的經(jīng)度與緯度分別記為^),乓w;月球探測月球車(觀測)位置記為^w,《cw;太陽入射角&,，太陽方向角^w，太陽相對月球車前向的角度為^t)，亦稱太陽方向角；太陽在當(dāng)?shù)氐仄较迪屡c自轉(zhuǎn)北向的夾角即太陽方向角記為W);由太陽入射角&)，太陽方向角PW及月球車俯仰角aw與橫滾角計(jì)算太陽高度角Zw及月球車前向與太陽的夾角^t)。高精度太陽羅盤導(dǎo)航方法包括太陽在月球體系下的經(jīng)度及升交點(diǎn)經(jīng)度計(jì)算方法、太陽羅盤導(dǎo)航方程。所述的曰-地在月球體系下的經(jīng)度及升交點(diǎn)經(jīng)度計(jì)算是由在、吋刻的太陽及升交點(diǎn)經(jīng)度Q,"，計(jì)算相對、觀測離散吋刻A:的在月球體系下的經(jīng)度/及升交點(diǎn)經(jīng)度^，它們隨吋間作周期性變化；通常是根據(jù)儒略曰及J2000的曰-地-月的軌道根數(shù)計(jì)算太陽經(jīng)緯度、黃白升交點(diǎn)經(jīng)度、等，但過程繁雜，不適合月球車嵌入式系統(tǒng)的計(jì)算。故將各、吋刻的太陽及升交點(diǎn)經(jīng)度^。,。序列事先根據(jù)天體動(dòng)力學(xué)計(jì)算并保存于嵌入式計(jì)算機(jī)中，該計(jì)算過程具有非常高的精度且計(jì)算量較小。記地球自轉(zhuǎn)角速度化、地球繞曰公轉(zhuǎn)角速度《￡、月球自轉(zhuǎn)角速度叫、月球繞地公轉(zhuǎn)角速度A、月赤道與黃道交角S、地赤道與黃道交角《、地赤道與月赤道交角&;在短吋間內(nèi)將月-地視為勻速轉(zhuǎn)動(dòng)來計(jì)算短期內(nèi)任一觀測吋刻A:的太陽經(jīng)緯度^w,及黃白升交點(diǎn)經(jīng)度(W。所述的高精度太陽羅盤導(dǎo)航方法是根據(jù)事先計(jì)算的定期的曰-地-月星歷數(shù)據(jù)保存于月球車嵌入式系統(tǒng)中，根據(jù)式下述公式(12-15)完成適合月球車嵌入式系統(tǒng)進(jìn)行任一吋刻的太陽及月赤道與黃道的升交點(diǎn)經(jīng)度；,、fartan(cosCtana)+;r/2//"Ce|"/;r—;r/2,/;r+;r/2")，/=1,2...f(C,a)Hr、(12)、'卜artan(cosCtana)—;z72Ce[/;r+;r/2,(/+l);r+;r/2),/=1,2...Cot)^,o+f(化A:,&)+f(巧A:,《)+f(f(ft^A:,fe),A)+f(6^A:，A(13)=Artan(sin(^)-Q))tans)(15)其中Q-太陽經(jīng)度、Q-白黃升交點(diǎn)經(jīng)度、化-地球自轉(zhuǎn)角速度、《￡-地球繞曰公轉(zhuǎn)角速度、q-月球自轉(zhuǎn)角速度、化-月球繞地公轉(zhuǎn)角速度、《-白道與黃道交角、《-地赤道與黃道交角、^-地赤道與月赤道交角；^w,《(太陽經(jīng)緯度、w-黃白升交點(diǎn)經(jīng)度，A:月球零經(jīng)度離散的標(biāo)準(zhǔn)吋；然后由計(jì)算的任一吋刻的太陽經(jīng)度、w及月赤道與黃道的升交點(diǎn)經(jīng)度f^)運(yùn)用公式(16-19郷(C/(4+i)-C"A+2))(Cz(/t+2)_Cz(/t+3))_(C/(>t+2)-C,(A+3))(Cz師)-Cz(t+2))4+2)=artan(16)^>(i)=2artanc早)r1—9廣(*)、00fc))(17)=artan月球車航向角計(jì)算公式廣99、(18)計(jì)算月球車的經(jīng)緯度《w及月球車航向角。式(16-18)是根據(jù)球面三角導(dǎo)航方程即(19)Cz=S(5&。+c《c《。c(w。)推理得到的。前提是以太陽連測的太陽緯度變化極小即5&+1)+2)+3)為前提得到的。所述的太陽羅盤位姿補(bǔ)償方法是依據(jù)現(xiàn)有天文知識，對太陽羅盤經(jīng)緯度進(jìn)行大氣折射補(bǔ)償、繞曰運(yùn)動(dòng)補(bǔ)償、光行差補(bǔ)償、周曰視差補(bǔ)償、觀測視差補(bǔ)償、月球及其被繞行星的歲差及章動(dòng)補(bǔ)償，從而保證高精度太陽羅盤導(dǎo)航方法的高精度。所述的基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法是利用高程圖及其對應(yīng)的障礙圖數(shù)據(jù)特征通過粒子濾波技術(shù)實(shí)現(xiàn)月球車速度估計(jì)，以保證月球車速度估計(jì)的收斂性與可靠性；由垂直陀螺系統(tǒng)提供俯仰角與滾動(dòng)角，由激光雷達(dá)測量環(huán)境深度圖，經(jīng)坐標(biāo)系變換得到高程圖，按式(20)求z)'網(wǎng)格點(diǎn)求四向平均梯度從而星到障礙圖，用于地圖重建與即吋定位(SLAM)使用，即為SLAM提供粒子；=)^(l力4》.,l+l+W-一+I,"-~I+1-~.I)/(")>G。(20)其中^A:J〉A(chǔ)，z',y,A:為整數(shù)，~為高度，G".為z)'網(wǎng)格點(diǎn)的梯度，G。為設(shè)置的閾值；經(jīng)四向梯度障礙提取得到障礙圖。建立地圖重建與即吋定位(SLAM)的導(dǎo)航方程，利用粒子濾波實(shí)現(xiàn)環(huán)境數(shù)據(jù)與航位推算過程的融合。激光雷達(dá)精度很高，達(dá)l-4mm。由于粒子數(shù)的限制，只能利用月球車當(dāng)前環(huán)境樣本進(jìn)行濾波，故利用障礙距離修正航位推算過程能夠使系統(tǒng)穩(wěn)定，但僅能保證月球車平動(dòng)速度具有較高的精度，而相對位置在長吋間運(yùn)行之后會出現(xiàn)較大的誤差累積，故仍需要外絕對位置校準(zhǔn)?；诘貓D重建與即吋定位(SLAM)月球車速度估計(jì)方法如圖9所示。過程算法流程如圖10所示，該過程由式(21-24)分7步完成(1)由慣性單元(加速度計(jì))在地平面下的加速度(fl^)，(4))及月球車航向角&,，經(jīng)二次積分得到該單步過程的位置增量AXt,其中AATA=(a,a)即是月球車的相對位置；(2)結(jié)合上一步最優(yōu)位姿估計(jì)值X乙給出當(dāng)前的先驗(yàn)位姿估計(jì)<formula>complexformulaseeoriginaldocumentpage18</formula>(21)(3)假定過程b的誤差模型服從某一分布(如高斯分布)，由此確定粒子采樣所依據(jù)的建議分布，并進(jìn)行粒子釆樣'《~iVp^")(a為事先估計(jì)的噪聲分布方差)；(4)讀取當(dāng)前激光雷達(dá)的環(huán)境測量數(shù)據(jù)^，并與之前所創(chuàng)建的地圖特征^w配準(zhǔn)。配準(zhǔn)過程采用ICP(IteratedClosedPoints)算法。這樣對于每個(gè)粒子得到一個(gè)最佳的位姿估計(jì)<formula>complexformulaseeoriginaldocumentpage18</formula>(22)在上述配準(zhǔn)過程中，同吋對環(huán)境觀測量和地圖之間的似然程度逬行評價(jià)，據(jù)此計(jì)算每個(gè)粒子的重要性權(quán)值系數(shù)w，并逬行規(guī)一化處理<formula>complexformulaseeoriginaldocumentpage18</formula>。(5)由此計(jì)算等效粒子數(shù)<formula>complexformulaseeoriginaldocumentpage18</formula>。若該值小于設(shè)定閾值A(chǔ)^〈r，重新對粒子群采樣。(6)月球車位姿狀態(tài)的最優(yōu)值估計(jì)應(yīng)為所有粒子的加權(quán)和<formula>complexformulaseeoriginaldocumentpage18</formula>(23)(7)加入當(dāng)前的環(huán)境觀測數(shù)據(jù)，對地圖進(jìn)行更新<formula>complexformulaseeoriginaldocumentpage18</formula>(24)所述的基于太陽連測的經(jīng)緯校準(zhǔn)方法是以高精度太陽羅盤導(dǎo)航方法為基礎(chǔ)，通過基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)提供的月球車速度進(jìn)行航位計(jì)算，并利用高精度太陽羅盤導(dǎo)航方法實(shí)吋地計(jì)算出任一吋刻的月球車經(jīng)緯度及航向，再根據(jù)太陽羅盤的太陽方位角方程計(jì)算太陽方位，該方法參見圖12所示。它是通過多次測量(至少3次)按式下面公式(25-27)迭代計(jì)算，最后按式(27)計(jì)算月球車的經(jīng)緯度及太陽方位角，其前提是連測吋月球車保持靜止；<formula>complexformulaseeoriginaldocumentpage18</formula>(25)<formula>complexformulaseeoriginaldocumentpage19</formula>記連續(xù)觀測太陽W次，0SA^iV-2，得到^-l次測量結(jié)果。，對它們分別求平均得最終的測量結(jié)果<formula>complexformulaseeoriginaldocumentpage19</formula>所述的基于太陽連測的經(jīng)緯校準(zhǔn)方法是建立太陽連測導(dǎo)航方程，通過太陽連測，并進(jìn)行月球車經(jīng)緯度的更新計(jì)算，提高了經(jīng)緯度計(jì)算精度，同吋也保證了嵌入式計(jì)算的實(shí)吋性，確保在線位姿的準(zhǔn)確性?；谒俣刃拚脑虑蜍囄蛔舜_定方法是在高精度太陽羅盤導(dǎo)航方法的基礎(chǔ)上，通過基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)提供的月球車速度進(jìn)行航位計(jì)算，并利用高精度太陽羅盤導(dǎo)航方法實(shí)吋地計(jì)算出任一吋刻的月球車經(jīng)緯度及航向，并根據(jù)太陽羅盤的太陽方位角方程計(jì)算太陽方位。由于在航位推算過程中月球車航向是根據(jù)更新的經(jīng)緯確定的，保證了月球車航向的高精度，從而大大提高了經(jīng)緯度的計(jì)算精度。該方法參見圖11所示。該方法利用了成熟的卡爾曼濾波技術(shù)實(shí)現(xiàn)月球車速度估計(jì)、高精度太陽羅盤位姿確定及月球車捷聯(lián)系統(tǒng)的融合。本發(fā)明與現(xiàn)有太陽羅盤及相關(guān)技術(shù)相比具有突出的本質(zhì)區(qū)別現(xiàn)有的月球車位姿主要通過星敏感器利用位置圓原理進(jìn)行逬行確定，其缺點(diǎn)是(1)星敏感器對工作環(huán)境清潔度要求極高，在月球表面，極易受粉塵污染，致使壽命很短；(2)星敏感器造價(jià)高，重量相對太陽敏感器要高得多，從而大大增加了制造及發(fā)射成本；(3)星敏感器工作吋要求背對太陽，要進(jìn)行位置確定還需轉(zhuǎn)動(dòng)以觀測不同的恒星，因此在月球車使用星敏感器逬行位姿確定就要安裝兩個(gè)自由度的高精密云臺，從而保證星敏感器背著太陽并保證星敏感器的測量精度。這樣星敏感器在月球車上應(yīng)用就遇到了很大困難，—方面增加了制造與發(fā)射成本，同吋也大大降低了可靠性。美國火星車使用了太陽敏感器，但是火星有較強(qiáng)的地磁、自轉(zhuǎn)周期較小，故通過磁羅盤及捷聯(lián)平臺就能完成位姿確定功能?；鹦擒嚿系奶柮舾衅饕蠹褐鹦擒嚨拇蟾沤?jīng)緯度，才能確定探測車的方向。本發(fā)明的主要特點(diǎn)是(1)適應(yīng)月球重力環(huán)境；(2)適應(yīng)月球慢自旋環(huán)境；(3)適應(yīng)月球粉塵環(huán)境；(4)適應(yīng)月球無地磁環(huán)境；(5)能夠同吋完成較高精度的經(jīng)緯度及航向確定功能；(6)系統(tǒng)質(zhì)量較低，有利于降低月球車整體質(zhì)量及研制與運(yùn)營成本；(7)高可靠性，可以滿足長吋間的位姿自主確定需求。圖l基于全功能太陽羅盤的月球車位姿自主確定系統(tǒng)結(jié)構(gòu)框圖；標(biāo)號如下觀測設(shè)備1、廣角太陽相機(jī)1.1、慣性單元IMU1.2、吋鐘1.3、及高速激光雷達(dá)1.4,智能太陽識別與處理方法模塊2、月球垂直陀螺方法模塊3、初始位姿校準(zhǔn)方法模塊4、高精度太陽羅盤導(dǎo)航方法模塊5、位姿補(bǔ)償方法方法模塊6、基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法模塊7、具有速度修正的月球車位姿確定方法模塊8、基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊9、通信與任務(wù)調(diào)度模塊10;圖2基于全功能太陽羅盤的月球車位姿自主確定方法工作過程圖；圖3魚眼鏡頭成像示意圖4基于全功能太陽羅盤的月球車位姿自主確定方法流程圖；圖5月球車位姿校準(zhǔn)模式及垂直陀螺工作方法流程圖；圖6高精度太陽羅盤導(dǎo)航方法流程圖；圖2-6符號如下D-表示任務(wù)；z^7-表示數(shù)據(jù)輸入；O-表示數(shù)據(jù)保存至內(nèi)存；I^i-表示函數(shù)；O-表示條件判斷；O-表示任務(wù)或程度終結(jié)；圖7基于球面天文學(xué)的太陽羅盤原理符號表示如下月球體系-I定義原點(diǎn)為月球質(zhì)量中心，義軸經(jīng)過Sinusmedii環(huán)形坑，Z軸為月球自旋軸，F(xiàn)軸由右手系確定，基中Z軸正向?yàn)楸碧鞓O，Z軸負(fù)向?yàn)槟咸鞓O；Sinusmedii-是月球正面的環(huán)形坑，即中央灣，被國際天文聯(lián)合會定義為月球體系的0經(jīng)度經(jīng)的地方；A^-月球自轉(zhuǎn)軸北向，即月球北天極；、-月球自轉(zhuǎn)軸南向，即月球南天極；S"-太陽中心；^-太陽所在緯度線與月赤道在無窮遠(yuǎn)處的交點(diǎn)；E-月球車所有位置的東向；W-月球車所有位置的西向；y。-升交點(diǎn)，即月赤道與黃道在無窮遠(yuǎn)處的交點(diǎn)；^-是月球車系統(tǒng)計(jì)算吋刻，是系統(tǒng)計(jì)算太陽經(jīng)度、升交點(diǎn)經(jīng)度的參考吋刻，它與系統(tǒng)采用北京地方吋一致；W^是月球車系統(tǒng)計(jì)算吋刻對應(yīng)的格林威治吋間，即國際標(biāo)準(zhǔn)吋；A-月球車所在位置天的方向，即天頂，與重力方向基本一致，但方向指向天空；3-(^&與月赤道的夾角，它隨吋間按一定規(guī)律變化，可用球面三角公式計(jì)算；G。-在、吋刻的太陽經(jīng)度，是太陽在月赤道投影與月球體系x軸的夾角；^-在C吋刻的升交點(diǎn)經(jīng)度，是&y。與x軸的夾角；e-白道(月球赤道)與黃道交角記為s，是白道面與黃道面法向構(gòu)成的交角；/-C^/"與x軸的交角；^-太陽在月球體系下的緯度，月球體系原點(diǎn)(^與太陽構(gòu)成的矢量與月赤道的夾角；/。-月球車(觀測)經(jīng)度；《-月球車(觀測)緯度；Z,-太陽羅盤觀測的高度角，即月球體系原點(diǎn)C^與太陽構(gòu)成的矢量與太陽太陽羅盤所在位置的天頂所張的角度；Q太陽羅盤觀測太陽的觀測方向角，即6^S"在太陽羅盤所在位置的自由地平系投影與該系x軸所張的角；y-太陽方向角，即0^5在太陽羅盤所在位置的地平指系投影與北向所張的角；圖8基于球面天文學(xué)的太陽羅盤導(dǎo)航方程顯式求解可視化結(jié)果示意(a)觀測緯度解算結(jié)果(b)太陽方位算結(jié)果；圖9基于地圖重建與即吋定位(SLAM)的月球車速度估計(jì)模塊流程圖;符號如下D-表示任務(wù)；D-表示數(shù)據(jù)輸入；O-表示數(shù)據(jù)保存至內(nèi)存；I^l-表示函數(shù)；O-表示條件判斷；O-表示任務(wù)或程度終結(jié)；圖IO具有速度修正的月球車位姿確定算法流程圖I表示月球車位姿(包括平面上的二維位置坐標(biāo)和航向角)，下標(biāo)1"表示第A個(gè)單步，上標(biāo)"-"表示先驗(yàn)估計(jì)，"+"表示后驗(yàn)估計(jì)；、表示當(dāng)前粒子群中某個(gè)粒子/所攜帶的位姿，w:表示每個(gè)粒子/的重要性權(quán)值，^表示規(guī)一化后的權(quán)值；^表示當(dāng)前的環(huán)境觀測量；A^表示當(dāng)前地圖的狀態(tài)估計(jì)；w為粒子群中包含的粒子個(gè)數(shù)；圖ll具有速度修正的月球車位姿確定方法.流程圖12基于太陽連測的經(jīng)緯校準(zhǔn)方法流程圖11-12符號如下D-表示任務(wù)；D-表示數(shù)據(jù)輸入；O-表示數(shù)據(jù)保存至內(nèi)存；I_l-表示函數(shù)；O-表示條件判斷；O-表示任務(wù)或程度終結(jié)；其它符號說明如下。/deg-角度單位，度；'-角度單位-分；"-角度單位-秒；Hr-吋間單位，小吋；min-吋間單位，分；s-吋間單位，秒；ms-吋間單位，毫秒;g,-月球重力加速度，1.62m"2;m-長度單位，米；mm-長度單位，毫米；圖13蝴蝶、蜣螂利用偏振光進(jìn)行導(dǎo)航及它們的眼睛結(jié)構(gòu)示意圖14基于全功能太陽羅盤的位姿自主確定與SLAM的聯(lián)合定位仿真1;圖15基于全功能太陽羅盤的位姿自主確定與SLAM的聯(lián)合定位仿真2。本發(fā)明技術(shù)方案的技術(shù)思路來源(1)來源于生物位姿確定的仿生原理如圖12所示，蝴蝶、蜣螂利用偏振光逬行導(dǎo)航。發(fā)明人發(fā)現(xiàn)借助曰光或月光導(dǎo)航的生物的習(xí)性如下a.在春分及秋分吋期十分活躍，猜測它們位姿確定在該吋期應(yīng)更準(zhǔn)確，否則它們應(yīng)該感至緊張或行為變得不活躍。b.它們在清晨、傍晚吋十分活躍，猜測此吋它們的位姿確定也很準(zhǔn)確的。發(fā)明人提出的高精度太陽羅盤導(dǎo)航方法的分析結(jié)果也證明了現(xiàn)有的猜測。圖8仿真結(jié)果表明，在夏至及冬至吋期的清晨與傍晚吋段航向確定不準(zhǔn)確或不能確定的，正午航向確定是不精確的或不可能的，此吋必需借助其它位姿確定方法。該季節(jié)許多依據(jù)太陽導(dǎo)航的生物在清晨、傍晩及正午也是不活躍的。在春分與秋分至附近吋在清晨、傍晩吋經(jīng)緯度確定及航向確定都是精確的。而在此季節(jié)依據(jù)太陽導(dǎo)航的生物在清晨、傍晩吋是非?；钴S的，而僅在正午經(jīng)緯度確定是不精確的，此吋依據(jù)太陽導(dǎo)航的生物行為也是不活躍的。本發(fā)明為解釋依據(jù)太陽導(dǎo)航的生物行為提供了理論基礎(chǔ)。同吋，借助曰光或月光導(dǎo)航的生物一般以曰光或月光逬行粗略的定位與精確的定向，而精確的定位是以視景圖像為基礎(chǔ)的。(2)本發(fā)明思路也來源于手機(jī)及古地磁探測采用的太陽羅盤定向原理。其過程是使用者將手機(jī)或古地磁太陽羅盤的指示太陽或0度線與真實(shí)太陽對準(zhǔn)，并將手機(jī)或古地磁太陽羅盤保持水平，從而用太陽羅盤指出北向角度。它是利用球面三角建立的太陽方位方程逬行計(jì)算的。但是使用者必須為它提供經(jīng)緯度，或以告知所地城市名稱的形式來提供，本質(zhì)上是提供了近似的經(jīng)緯度。它的缺點(diǎn)是使用者要提供經(jīng)緯度及水平位置，同吋北向精度確定很差，只能用于粗略的北向確定，而遠(yuǎn)遠(yuǎn)不能滿足月球車航向確定需求。具體實(shí)施例方式本發(fā)明中所選用的部分方法模塊的技術(shù)構(gòu)成及技術(shù)指標(biāo)說明如下1、觀測設(shè)備由廣角太陽相機(jī)、IMU組成、高速激光雷達(dá)及吋鐘組成。廣角太陽相機(jī)視場角為185°;分辯率1024x1098;采用CAN通信方式。在該相機(jī)中專門開發(fā)了智能太陽識別與處理系統(tǒng)，用于紛塵過濾、太陽識別并計(jì)算單位矢量、識別曰出并輸出曰出吋間；IMU:加速度計(jì)范圍i5g;加速度計(jì)線性度1.5%;光纖陀螺游走1.8。/V^，光纖陀螺速度范圍100。"，光纖陀螺線性度1.5%;通信方式采用RS232。輸出三軸加速度、三軸角速度、系統(tǒng)溫度；高速激光雷達(dá)深度檢測范圍16m;速率5萬點(diǎn)/秒；視場120°;具有兩個(gè)自由度，通過高速激光梭鏡方向伺控制，實(shí)現(xiàn)對三維空間進(jìn)行高速掃描測量。通信方式采用PCI總線。輸出被測點(diǎn)的深度值、激光強(qiáng)度、雷達(dá)系統(tǒng)溫度、行場位置編碼；輸出數(shù)據(jù)經(jīng)光強(qiáng)及溫度補(bǔ)償后得到高精度深度值。并利用月球車俯仰角、橫滾角、雷達(dá)行場編碼建立高精度環(huán)境高程圖。吋鐘由嵌入式計(jì)算機(jī)根據(jù)星歷預(yù)報(bào)曰出吋間，在曰出前一曰進(jìn)行吋鐘設(shè)置。在曰出前30分鐘產(chǎn)生吋鐘中斷，從而啟動(dòng)太陽相機(jī)中的曰出識別及處理任務(wù)。2、垂直陀螺是在IMU(三軸微機(jī)械加速度計(jì)、三軸光纖陀螺)基礎(chǔ)上進(jìn)行二次開發(fā)而成，即完成垂直陀螺的俯仰角、滾動(dòng)角解算。采用光纖陀螺主要是因?yàn)橘|(zhì)量輕，抗沖擊性能好，同吋具有較高的檢測精度，適應(yīng)月球車工作環(huán)境的需要。應(yīng)用CrossBow垂直陀螺的三軸加速度與角速度解算的俯仰角與滾動(dòng)角與CrossBow垂直陀螺輸出結(jié)果基本一致。加速度小于0.2§吋，俯仰角與滾動(dòng)角誤差在5'以下。月球垂直陀螺參數(shù)與地球上的垂直陀螺不同，主要表現(xiàn)于重力常數(shù)，同吋應(yīng)考慮月球重力異常(重力瘤)、月球半徑、重力反饋四元數(shù)系數(shù)等。因而現(xiàn)有的地球垂直陀螺不能直接應(yīng)用于月球車，必需了解垂直陀螺原理才能開發(fā)月球垂直陀螺。本發(fā)明的主要實(shí)施步驟如下步驟l系統(tǒng)構(gòu)建根據(jù)月球車自主位姿確定指標(biāo)選擇適合的IMU,其中IMU的光纖陀螺線性度好于2%、游走1.8。/i。加速度線性優(yōu)于1%，檢測精度優(yōu)于10"g。激光雷達(dá)測距精度優(yōu)于3mm，速度優(yōu)于4萬點(diǎn)/秒。相機(jī)分辨率優(yōu)于1024x1024，相機(jī)嵌入式系統(tǒng)計(jì)算速度高于3200MIPS;廣角鏡頭形變率低于0.2%，鏡頭模型為〃=/0，視場可根據(jù)需要選擇；開發(fā)相應(yīng)接口程度。從而可以獲得月球車三軸加速度"w、角速度"a,。步驟2智能太陽識別與處理模塊開發(fā)。首先對廣角鏡頭進(jìn)行校準(zhǔn)，通過拍攝校正板圖案并按校正板圖案像點(diǎn)位置參照式(7)逬行，求解氣，附v，^^,"。,v。。開發(fā)太陽檢測程序，提取太陽圓盤，按圓度、面積、顏色范圍來判別由圖像提取得到的簡單曲線是否為太陽。并計(jì)算太陽圓盤中心，根據(jù)計(jì)算太陽圓盤中心像點(diǎn)坐標(biāo)參照式(7)求太陽入射角^，太陽方向角P由太陽圓盤中心像點(diǎn)坐標(biāo)直接計(jì)算。相應(yīng)^吋刻的太陽入射角及太陽方向角分別記為{)，％)}。步驟3月球垂直陀螺方法模塊開發(fā)。建立捷聯(lián)系統(tǒng)方程(8-9)，以獲取月球車三軸加速度"w、角速度w(w為輸入，并按(10)計(jì)算加速度反饋用的角速度，加入式(11)中逬行四元數(shù)計(jì)算迭代得月球車俯仰角《a)與橫滾角~。步驟4由步驟2得到的太陽入射角&,，太陽方向角^4)及步驟3得到的月球車俯仰角aw與橫滾角/w計(jì)算太陽高度角Zw及月球車前向與太陽的夾角《w。步驟5月球車初始位姿校準(zhǔn)模塊開發(fā)。由智能太陽識別與處理模塊識別太陽并判斷太陽升起吋間(.。，由"。-U叫計(jì)算角度再換算為經(jīng)度。其中。",分別是Si皿smedii中央灣吋間、月球自轉(zhuǎn)角速度?；蛴商栠B測的經(jīng)緯校準(zhǔn)方法進(jìn)行經(jīng)度測量。該方法參考式(24-27)及太陽連測的經(jīng)緯校準(zhǔn)方法一節(jié)，即將步驟4得到的太陽高度角Z(,)代入公式(24-27)求得^),^w,^V)。由式(19)計(jì)算月球車航向角^"。步驟6基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)模塊開發(fā)。獲取環(huán)境深度圖并投影至水平面。根據(jù)式(20)求場景中每像點(diǎn)的四向平均梯度，并判斷是否大于閾值，若成立則視為障礙點(diǎn)，于是得環(huán)境障礙圖。由步驟5得到的CV)及步驟1得到的月球車三軸加速度fl(,)、角速度^v)進(jìn)行航位推算，得月球車單步位移增量A^，將之及激光測得深度值、—的代入式(21-24)逬行粒子濾波，獲得月球車速度^,。步驟7基于速度修正的月球車位姿確定模塊開發(fā)首先建立捷聯(lián)平臺及具有車體速度觀測的狀態(tài)方程捷聯(lián)導(dǎo)航方程進(jìn)行迭代計(jì)算，同吋加入式(10)即垂直陀螺產(chǎn)生的校正角速度。由輸入即月球車速度^)、月球車三軸加速度"w、角速度"w、月球車初始經(jīng)緯度義w,&w及月球車方向。w，對捷聯(lián)導(dǎo)航方程進(jìn)行四元數(shù)迭代求解。從而得連續(xù)的月球車連續(xù)速度^+,,、經(jīng)緯度^+,)^。(一及航向角^+,)。步驟8月球車連續(xù)運(yùn)行一段吋間(根據(jù)實(shí)驗(yàn)測的誤差范圍來確定)后，重復(fù)步驟2-6，即完成再次太陽連測校準(zhǔn)后進(jìn)行月球車位姿自主確定。本發(fā)明的實(shí)施過程及結(jié)果已由工程實(shí)驗(yàn)得到驗(yàn)證。具體如下應(yīng)用例1應(yīng)用發(fā)明人研制的基于全功能太陽羅盤的位姿確定方法，對太陽進(jìn)行了觀測。測試環(huán)境與條件吋間2008.6.6全天；地點(diǎn)北京工業(yè)大學(xué)綜合樓前廣場；天氣晴；天空有少量云；風(fēng)4級。測試設(shè)備發(fā)明人開發(fā)的太陽羅盤+垂直陀螺VG700、校準(zhǔn)設(shè)備GPS，定位精度4米，方向精度0.15。。測試人員王亮、居鶴華(北京工業(yè)大學(xué)電控學(xué)院教師)測試結(jié)果參見表l表1測試結(jié)果<table>complextableseeoriginaldocumentpage26</column></row><table>測試結(jié)論由上面的測試結(jié)果可知，證明了本發(fā)明正確性。盡管與GPS測量結(jié)果對比有一定誤差，但自主位姿確定功能是可靠的，并且不依賴地磁、不需檢測地球轉(zhuǎn)動(dòng)并且對粉塵污染影響很小。主要誤差原因是太陽相機(jī)測量精度較低，同吋測量設(shè)備沒有很好校準(zhǔn)，通過提高設(shè)備校準(zhǔn)精度后，預(yù)計(jì)經(jīng)緯及太陽方位角測量精度優(yōu)于l-5'。應(yīng)用例2將本發(fā)明應(yīng)用于月球車導(dǎo)航中，將基于全功能太陽羅盤的位姿自主確定方法與地圖重建及即吋定位技術(shù)相結(jié)合用于月球車相對定位。測試環(huán)境與條件吋間2008.6.9-11;地點(diǎn)北京工業(yè)大學(xué)綜合樓821;在個(gè)人計(jì)算機(jī)上進(jìn)行仿真分析；仿真中IMU基本參數(shù)為加速度計(jì)線性度1.5%，漂移0.1m"l;陀螺游走1.8°/|，陀螺線性度1.5%;通過測量地形高程精度10mm;測試人員王亮、居鶴華(北京工業(yè)大學(xué)電控學(xué)院教師)測試結(jié)果根據(jù)上述測試條件逬行基于全功能太陽羅盤的位姿自主確定與SLAM的聯(lián)合定位仿真，得圖14-15所示的仿真結(jié)果，其中上圖為使用慣性單元的航位推算結(jié)果、基于太陽羅盤與垂直陀螺的位姿自主確定與SLAM的聯(lián)合定位誤差曲線，下圖為月球車運(yùn)行軌跡及用于位置校準(zhǔn)的樣本點(diǎn)集。由圖14-15可知，在僅使用IMU逬行航位推算吋很快就變得發(fā)散，而使用基于太陽羅盤與垂直陀螺的位姿自主確定與SLAM的聯(lián)合定位后，相對位置是收斂的，并具有較高的精度。該實(shí)驗(yàn)中，使用了太陽羅盤與垂直陀螺一起確定的月球車航向，即航向可以精確檢測，并且不依賴于磁羅盤及慣導(dǎo)平臺。測試結(jié)論仿真結(jié)果表明本發(fā)明對月球車相對定位精度的提高具有非常重要的意義。表現(xiàn)于本發(fā)明能夠適應(yīng)無地磁、慢自旋、粉塵污染的環(huán)境要求，同吋航向精度很高，在靜態(tài)吋精度能夠提高至太陽觀測相機(jī)的次像素級，即可達(dá)到l-5'左右，而動(dòng)態(tài)精度可達(dá)到0.2°。而絕對位置定位精度同樣可確定到相同的數(shù)量級。最后應(yīng)說明的是以上實(shí)施例僅用以說明本發(fā)明而并非限制本發(fā)明所描述的技術(shù)方案；因此，盡管本說明書參照上述的各個(gè)實(shí)施例對本發(fā)明已進(jìn)行了詳細(xì)的說明，但是，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，仍然可以對本發(fā)明進(jìn)行修改或等同替換；而一切不脫離發(fā)明的精神和范圍的技術(shù)方案及其改進(jìn)，其均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍當(dāng)中。權(quán)利要求1、基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，它是以觀測設(shè)備、嵌入式PC104+計(jì)算機(jī)及內(nèi)置于廣角太陽相機(jī)(1.1)的嵌入式TMS320計(jì)算機(jī)為基礎(chǔ)完成的，其中與嵌入式PC104+計(jì)算機(jī)集成的觀測設(shè)備有廣角太陽相機(jī)(1.1)、慣性單元——以下稱為IMU(1.2)、時(shí)鐘(1.3)及高速激光雷達(dá)(1.4)；在廣角太陽相機(jī)(1.1)內(nèi)置的嵌入式TMS320計(jì)算機(jī)中包含的模塊有智能太陽識別與處理方法模塊(2)；在PC104+計(jì)算機(jī)中包括的模塊有通信與任務(wù)調(diào)度模塊(10)、月球車自主位姿確定任務(wù)模塊；月球車自主位姿確定任務(wù)模塊包含月球垂直陀螺方法模塊(3)、初始位姿校準(zhǔn)方法模塊(4)、高精度太陽羅盤導(dǎo)航方法模塊(5)、位姿補(bǔ)償方法模塊(6)、基于地圖重建與即時(shí)定位(SLAM)月球車速度估計(jì)方法模塊(7)、具有速度修正的月球車位姿確定方法模塊(8)、基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊(9)；基于全功能太陽羅盤的月球車位姿自主確定方法步驟如下(1)數(shù)據(jù)獲取，由在嵌入式PC104+計(jì)算機(jī)中開發(fā)的通信與任務(wù)調(diào)度模塊完成，它包含太陽羅盤接口任務(wù)、IMU接口任務(wù)及高速激光雷達(dá)系統(tǒng)接口任務(wù)；太陽羅盤接口任務(wù)通過CAN通信訪問智能相機(jī)，包括設(shè)置智能太陽識別與處理方法模塊所需的參數(shù)，最終獲取智能太陽識別與處理方法模塊得到的太陽入射角id="icf0001"file="A2008101168070002C1.tif"wi="14"he="5"top="143"left="120"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>日出時(shí)間ts(0)等；IMU接口任務(wù)通過RS232通信訪問IMU，獲得三軸加速度as(k)、三軸角速度ωs(k)；高速激光雷達(dá)系統(tǒng)接口任務(wù)通過PCL總線通信訪問激光雷達(dá)，獲得測量的深度hij、光強(qiáng)Iij數(shù)據(jù)流；(2)系統(tǒng)初始化，嵌入式PC104+計(jì)算機(jī)上電后系統(tǒng)初始化程序工作，對智能相機(jī)、IMU、激光雷達(dá)進(jìn)行初始設(shè)置，并啟動(dòng)嵌入式PC104+計(jì)算機(jī)中月球車自主位姿確定任務(wù)模塊及嵌入式TMS320計(jì)算機(jī)中智能太陽識別與處理方法模塊；(3)嵌入式PC104+計(jì)算機(jī)中月球車位姿確定任務(wù)模塊包括月球垂直陀螺方法模塊、智能太陽識別與處理方法模塊、初始位姿校準(zhǔn)方法模塊、高精度太陽羅盤導(dǎo)航方法模塊、位姿補(bǔ)償方法模塊、基于地圖重建與即時(shí)定位(SLAM)月球車速度估計(jì)方法模塊、具有速度修正的月球車位姿確定方法模塊、基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊，各處理方法之間并行運(yùn)行，共同完成月球車絕對位姿、相對位姿、平動(dòng)速度確定；在廣角太陽相機(jī)(1.1)內(nèi)置的嵌入式系統(tǒng)TMS320計(jì)算機(jī)中包含的模塊有智能太陽識別與處理方法模塊(2)；下面對各方法模塊實(shí)現(xiàn)方法及流程進(jìn)行描述由智能太陽識別與處理方法模塊識別日出或太陽并判斷太陽升起時(shí)間ts0及太陽入射角θ(k)、太陽方向角id="icf0002"file="A2008101168070002C2.tif"wi="8"he="4"top="266"left="59"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>該模塊首先應(yīng)對廣角鏡頭進(jìn)行校準(zhǔn)，然后拍攝天空圖像，提取太陽圓盤，按圓度、面積、顔色范圍來判別由圖像提取得到的簡單曲線是否為太陽；并計(jì)算太陽圓盤中心，根據(jù)計(jì)算太陽圓盤中心像點(diǎn)坐標(biāo)求太陽入射角θ(k)及太陽方向角id="icf0003"file="A2008101168070003C1.tif"wi="8"he="4"top="37"left="168"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由月球垂直陀螺方法模塊由系統(tǒng)獲得的月球車三軸加速度a(k)、角速度ω(k)為輸入，經(jīng)加速度a(k)反饋校正四元數(shù)，進(jìn)行四元數(shù)計(jì)算迭代得月球車俯仰角α(k)與橫滾角β(k)；由太陽入射角θ(k)及太陽方向角id="icf0004"file="A2008101168070003C2.tif"wi="5"he="3"top="69"left="88"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>及月球車俯仰角α(k)與橫滾角β(k)計(jì)算太陽高度角Z(k)及月球車前向與太陽的夾角ξ(k)；由初始位姿校準(zhǔn)模塊根據(jù)太陽高度角Z(k)及月球車前向與太陽的夾角ξ(k)通過高精度太陽羅盤導(dǎo)航模塊及基于太陽連測的經(jīng)緯校準(zhǔn)模塊計(jì)算月球車經(jīng)緯度λ(k)，δ0(k)及太陽方位角θ(k)，從而計(jì)算月球車航向角Φ(k)；或者，由日出時(shí)按(ts0-tm)ωl計(jì)算角度再換算為經(jīng)度；其中tm，ωl分別是Sinusmedii中央灣時(shí)間、月球自轉(zhuǎn)角速度；由高精度太陽羅盤導(dǎo)航方法模塊應(yīng)用于月球車初始位姿校準(zhǔn)模塊及基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊；它們的輸入都是太陽高度角Z(k)及觀測時(shí)間；其中包含了由觀測時(shí)間計(jì)算任一時(shí)刻的太陽及月赤道與黃道的升交點(diǎn)經(jīng)度過程；二者不同在于高精度太陽羅盤導(dǎo)航方法模塊是一次觀測，基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊是多次觀測即連測的過程；但后者要為前者前供月球車經(jīng)度，從而可通過前者連續(xù)計(jì)算太陽方位角，高精度太陽羅盤導(dǎo)航方法模塊以校準(zhǔn)的經(jīng)度及實(shí)時(shí)測量的太陽高度角Z(k)及觀測時(shí)間連續(xù)計(jì)算太陽方位角，從而獲得月球車航向角Φ(k)；由基于地圖重建與即時(shí)定位(SLAM)月球車速度估計(jì)方法模塊根據(jù)測量的三軸加速度as(k)、三軸角速度ωs(k)及由高程圖的數(shù)據(jù)即粒子通過粒子濾波得到月球車的速度V(k)；由基于速度修正的月球車位姿確定方法模塊輸入為月球車速度V(k)、月球車三軸加速度a(k)、角速度ω(k)、月球車初始經(jīng)緯度λ(k)，δ0(k)及月球車航向角Φ(k)，輸出是連續(xù)的月球車連續(xù)速度V(k+i)、經(jīng)緯度λ(k+i)，δ0(k+i)及航向角Φ(k+i)。2、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的智能太陽識別與處理方法模塊的任務(wù)運(yùn)行于廣角太陽相機(jī)嵌入式系統(tǒng)當(dāng)中；該方法模塊完成兩部分任務(wù)即，太陽識別及太陽單位矢量計(jì)算與曰出識別及曰出吋間計(jì)算，該方法模塊的輸出結(jié)果為曰出吋間、太陽單位矢量；廣角太陽相機(jī)采用魚目鏡頭，其模型為公式(l)其中OcXcYcZc為相機(jī)坐標(biāo)系，r為圖像點(diǎn)P到魚眼圖像中心Oc的距離，/為魚眼鏡頭的焦距，e為入射光線與魚眼鏡頭光軸的夾角，即測量高度角，伊為像點(diǎn)在坐標(biāo)系OcXcYcZc的方向角，則對應(yīng)于入射角為(^p)的空間點(diǎn)《的圖像點(diǎn)P在攝像機(jī)坐標(biāo)系下的坐標(biāo)(x,y)滿足<formula>complexformulaseeoriginaldocumentpage4</formula>(2)將(1)帶入(2)得<formula>complexformulaseeoriginaldocumentpage4</formula>(3)由于鏡頭加工工藝和其他因素，魚眼鏡頭通常很難嚴(yán)格滿足成像模型(l),利用三次多項(xiàng)式來擬合該魚眼鏡頭的成像模型，即A*=AT々+則對應(yīng)的成像變換為cosp(4)(5)畸變校正模型按式(6)進(jìn)行cosp成像過程用6個(gè)參數(shù)來描述魚眼鏡頭成像的模型(0^(9+、03)w00w..cosp)+(6)(7)通過校準(zhǔn)板，拍攝其圖像，計(jì)算氣、mv、"。、v。、k,、^六個(gè)校正參數(shù)，其中m、W為測量高度角與像素坐標(biāo)矢聯(lián)常數(shù)，"。、v。為像素坐標(biāo)偏差，^、K為測量高度角校正常數(shù)；太陽方向計(jì)算根據(jù)魚眼圖像中某點(diǎn)的像素坐標(biāo)(",v)，求該點(diǎn)在攝像機(jī)坐標(biāo)系下的入射角(e,p),其中伊可以利用根據(jù)像素坐標(biāo)直接求解出來；對于e,將氣、W、"。、v。、K、A和P帶入式(7)得到關(guān)于6的一元三次方程,求解后去除不合理的解到太陽入射角^,相應(yīng)A:吋刻的太陽入射角及太陽方向角分別記為。3、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的月球垂直陀螺方法模塊方法中需適應(yīng)月球重力常數(shù)、月球半徑等環(huán)境等點(diǎn)，通過重力反饋進(jìn)行四元數(shù)修正的方法實(shí)現(xiàn)月球垂直陀螺的俯仰角與滾動(dòng)角解算；根據(jù)捷聯(lián)導(dǎo)航方程即式(8-9)<formula>complexformulaseeoriginaldocumentpage5</formula>(8)其中g(shù)w-Wo,A，義2，Ar,是四元數(shù)表示月球車姿態(tài)；<formula>complexformulaseeoriginaldocumentpage5</formula>(9)運(yùn)用基于加速度反饋的捷聯(lián)導(dǎo)航四元數(shù)迭代的修正方程<formula>complexformulaseeoriginaldocumentpage5</formula>(10)<formula>complexformulaseeoriginaldocumentpage5</formula>(11)解算探測月球車相對地平坐標(biāo)系的俯仰角與橫滾角.由三軸加速度",w及角速度通過式(9-11)計(jì)算。w,得俯仰角"("與橫滾角/^;前式(8-9)與式(10-11)構(gòu)成了捷聯(lián)式的垂直陀螺系統(tǒng)方程。4、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的月球車初5^Z姿校準(zhǔn)方法是，通過曰出吋間計(jì)算太陽月球車經(jīng)度，利用高精度太陽羅盤導(dǎo)航方法模塊(5)確定緯度及太陽方位角；同吋可以使用基于太陽連測的經(jīng)緯校準(zhǔn)方法模塊(9)直接對太陽連續(xù)觀測確定月球車經(jīng)銪度。5、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于.所述的高精度太陽羅盤導(dǎo)航方法是根據(jù)事先計(jì)算的定期的曰-地-月星歷數(shù)據(jù)保存于月球車嵌入式系統(tǒng)中，根據(jù)式下述公式(12-15)完成適合月球車嵌入式系統(tǒng)逬行任一吋刻的太陽及月赤道與黃道的升交點(diǎn)經(jīng)度；<formula>complexformulaseeoriginaldocumentpage5</formula>(12)<formula>complexformulaseeoriginaldocumentpage5</formula>(13)《"=Artan(sin(仏)-"))tans)(15)其中r,。-太陽經(jīng)度、Q-白黃升交點(diǎn)經(jīng)度、％-地球自轉(zhuǎn)角速度、6^-地球繞曰公轉(zhuǎn)角速度、^-月球自轉(zhuǎn)角速度、A-月球繞地公轉(zhuǎn)角速度、s,-白道與黃道交角、^-地赤道與黃道交角、Sfe-地赤道與月赤道交角；^),^)-太陽經(jīng)緯度、^-黃白升交點(diǎn)經(jīng)度，^月球零經(jīng)度標(biāo)準(zhǔn)吋；然后由計(jì)算的任一吋刻的太陽經(jīng)度^w及月赤道與黃道的升交點(diǎn)經(jīng)度q"運(yùn)用公式(16-19)即4+2)二artan+1)—Sot+2))(Cz(*+2)—Cz(lt+3))-(S,(jt+2)-S(i十3))(Cz(/t+l)—Cz(lt+2))(16)<50(A)=2artan，)(17)^=artan月球車航向角計(jì)算公式,廣99、、w(一緣)-,ow)VCz(^。("-C早)乂(18)①(19)計(jì)算月球車的經(jīng)緯度A"，Aw及月球車航向角)；式(16-18)是以太陽連測的太陽練度變化極小即《("1)5("2)5("3)為前提,根據(jù)球面三角導(dǎo)航方程即<formula>complexformulaseeoriginaldocumentpage6</formula>推理得到的。6、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于,所述的太陽羅盤位姿補(bǔ)償方法是依據(jù)現(xiàn)有天文知識，對太陽羅盤經(jīng)纟韋度逬行大氣折射補(bǔ)償、繞曰運(yùn)動(dòng)補(bǔ)償、光行差補(bǔ)償、周曰視差補(bǔ)償、觀測視差補(bǔ)償、月球及地球的歲差及章動(dòng)補(bǔ)償，從而保證高精度太陽羅盤導(dǎo)航方法的高精度。7、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)方法是利用高程圖及其對應(yīng)的P章礙圖數(shù)據(jù)特征通過粒子濾波技術(shù)實(shí)現(xiàn)月球車速度估計(jì)，以保證月球車速度估計(jì)的收斂性與可靠性；由垂直陀螺系統(tǒng)提供俯仰角與滾動(dòng)角，由激光雷達(dá)測量環(huán)境深度圖，經(jīng)坐標(biāo)系變換得到高程圖，按式(20)求々'網(wǎng)格點(diǎn)求四向平均梯度從而得到障礙圖，用于地圖重建與即吋定位(SLAM)使用，即為SLAM提供粒子；=}主1"一"—~1+1CM)—|)/(")>G0(20)其中/4,7'",/,M為整數(shù)，~為高度，(^為i/網(wǎng)格點(diǎn)的梯度，G。為設(shè)置的閾值；經(jīng)四向梯度障礙提取得到障礙圖。建立地圖重建與即吋定位(SLAM)的導(dǎo)航方程，利用粒子濾波實(shí)現(xiàn)環(huán)境數(shù)據(jù)與航位推算過程的融合。該過程由式(21-24)分7步完成(1)由慣性單元(加速度計(jì))在地平面下的加速度("^),(4))及月球車航向角&),經(jīng)二次積分得到該單步過程的位置增量AA^，其中AXt=0ct,h)即是月球車的相對位置；(2)結(jié)合上一步最優(yōu)位姿估計(jì)值Z"給出當(dāng)前的先驗(yàn)位姿估計(jì)《《《1+AZt(21)(3)假定過程b的誤差模型服從某一分布(如高斯分布)，由此確定粒子采樣所依椐的建議分布，并進(jìn)行粒子采樣、-iV(《,cr)(c7為事先估計(jì)的噪聲分布方差)；(4)讀取當(dāng)前激光雷達(dá)的環(huán)境測量數(shù)據(jù)^，并與之前所創(chuàng)建的地圖特征^-,配準(zhǔn)。配準(zhǔn)過程采用ICP(IteratedClosedPoints)算法。這樣對于每個(gè)粒子得到一個(gè)最佳的位姿估計(jì)=argmax一(^、-,恥—,)./('《,)J(22)在上述配準(zhǔn)過程中，同吋對環(huán)境觀測量和地圖之間的似然程度逬行評價(jià)，據(jù)此計(jì)箅每個(gè)粒子的重要性權(quán)值系數(shù)^，并進(jìn)行規(guī)一化處理SI=</力(5)由此計(jì)算等效粒子數(shù)A^-l/^^:。若該值小于設(shè)定閾值A(chǔ)V〈7^重新對粒子群采樣；(6)月球車位姿狀態(tài)的最優(yōu)值估計(jì)應(yīng)為所有粒子的加權(quán)和:<formula>complexformulaseeoriginaldocumentpage8</formula>(23)(7)加入當(dāng)前的環(huán)境觀測數(shù)據(jù)，對地圖逬行更新<formula>complexformulaseeoriginaldocumentpage8</formula>(24)8、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的基于太陽連測的經(jīng)緯校準(zhǔn)方法是以高精度太陽羅盤導(dǎo)航方法為基礎(chǔ)，通過基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)提供的月球車速度進(jìn)行航位計(jì)算，并利用高精度太陽羅盤導(dǎo)航方法實(shí)吋地計(jì)算出任一吋刻的月球車經(jīng)緯度及航向，翔艮據(jù)太陽羅盤的太陽方位角方程計(jì)算太陽方位；它是通過多次測量(至少3次)按式(25-27)迭代計(jì)算，最后按式(27)計(jì)算月球車的經(jīng)緯度及太陽方位角，其前提是連測吋月球車保持靜止；<formula>complexformulaseeoriginaldocumentpage8</formula>(27)記連續(xù)觀測太陽W次，0《A:S7V-2，得到W-1次測量結(jié)果。，對它們分別求平均得最終的測量結(jié)果<formula>complexformulaseeoriginaldocumentpage8</formula>(27)所述的基于太陽連測的經(jīng)緯校準(zhǔn)方法是建立太陽連測導(dǎo)航方程，保證在線位姿確定。9、根據(jù)權(quán)利要求1所述的基于全功能太陽羅盤的月球車位姿自主確定方法，其特征在于，所述的基于速度修正的月球車位姿確定方法是在高精度太陽羅盤導(dǎo)航方法的基礎(chǔ)上，通過基于地圖重建與即吋定位(SLAM)月球車速度估計(jì)提供的月球車速度進(jìn)行航位計(jì)算，并利用高精度太陽羅盤導(dǎo)航方法實(shí)吋地計(jì)算出任一吋刻的月球車經(jīng)緯度及航向，并根據(jù)太陽羅盤的太陽方位角方程計(jì)算太陽方位。全文摘要本發(fā)明是一種用于深空探測科學(xué)及地球觀測領(lǐng)域的基于全功能太陽羅盤的月球車位姿自主確定方法。該方法是以觀測設(shè)備、嵌入式PC104+計(jì)算機(jī)及內(nèi)置于廣角太陽相機(jī)的嵌入式TMS320計(jì)算機(jī)為基礎(chǔ)完成的。本發(fā)明利用在嵌入式系統(tǒng)中的廣角太陽相機(jī)、IMU及高速激光雷達(dá)接口程序獲取數(shù)據(jù)；并將月球車位姿確定任務(wù)交與系統(tǒng)中的智能太陽識別與處理、月球垂直陀螺、初始位姿校準(zhǔn)、高精度太陽羅盤導(dǎo)航、位姿補(bǔ)償方法、基于地圖重建與即時(shí)定位(SLAM)月球車速度估計(jì)、具有速度修正的月球車位姿確定、基于太陽連測的經(jīng)緯校準(zhǔn)的各方法模塊并行運(yùn)行，整個(gè)系統(tǒng)中所執(zhí)行的處理方法共同完成了月球車位姿的確定。該方法為月球車提供適應(yīng)月球環(huán)境的自主位姿確定提供了高可靠性的方案。文檔編號G01C21/00GK101344391SQ20081011680公開日2009年1月14日申請日期2008年7月18日優(yōu)先權(quán)日2008年7月18日發(fā)明者居鶴華,李秀智,亮王,裴?？∩暾埲?北京工業(yè)大學(xué)