專利名稱：深空探測(cè)器大角度機(jī)動(dòng)路徑自主生成方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種實(shí)現(xiàn)深空探測(cè)器自主生成大角度機(jī)動(dòng)路徑的方法。
背景技術(shù)：
深空探測(cè)是21世紀(jì)世界宇航大國(guó)關(guān)注的焦點(diǎn)之一，我國(guó)也在《中國(guó)航天》白皮書中提出，將“開(kāi)展以月球探測(cè)為主的深空探測(cè)的預(yù)先研究”。由于探測(cè)目標(biāo)距離遠(yuǎn)，飛行環(huán)境不確定等因素，深空探測(cè)器的控制和操作存在實(shí)時(shí)性和可靠性等方面的問(wèn)題。應(yīng)用自主技術(shù)是解決這些問(wèn)題的一條有效途徑，它通過(guò)在探測(cè)器上構(gòu)造軟件系統(tǒng)，完成探測(cè)器任務(wù)規(guī)劃、活動(dòng)分解執(zhí)行、故障診斷、故障恢復(fù)等功能，使探測(cè)器的操作、控制形成真正的星上閉環(huán)。規(guī)劃技術(shù)是實(shí)現(xiàn)自主探測(cè)器的關(guān)鍵技術(shù)之一。
探測(cè)器是深空探測(cè)任務(wù)中的主體，它由地球發(fā)射進(jìn)入太空，實(shí)施月球探測(cè)、行星及其衛(wèi)星探測(cè)和小行星與彗星探測(cè)。根據(jù)探測(cè)任務(wù)的不同，探測(cè)器可攜帶著陸器或探測(cè)車，并將其釋放到星體表面進(jìn)行近距離探測(cè)。深空探測(cè)中的探測(cè)目標(biāo)與地球的距離十分遙遠(yuǎn)(現(xiàn)已達(dá)7.2×109Km)，而且我們對(duì)深空環(huán)境還不十分了解，僅依靠以前的地面直接控制或遙控的方式很難完成對(duì)太陽(yáng)系星體的探測(cè)活動(dòng)。這就為深空探測(cè)任務(wù)提出了十分苛刻的要求，如操作費(fèi)用、任務(wù)的可靠性、通訊網(wǎng)絡(luò)和實(shí)時(shí)性的要求等。解決的辦法就是采用人工智能中的規(guī)劃調(diào)度技術(shù)、模式識(shí)別技術(shù)、故障診斷技術(shù)等，建立探測(cè)器上的自主控制系統(tǒng)。實(shí)現(xiàn)探測(cè)器自主性的核心是探測(cè)器自主任務(wù)規(guī)劃技術(shù)，它根據(jù)地面的任務(wù)要求選擇所要執(zhí)行的活動(dòng)，并給它們分配資源和時(shí)間，這些活動(dòng)一旦執(zhí)行便可以達(dá)到預(yù)期的目標(biāo)。
探測(cè)器是一個(gè)復(fù)雜的個(gè)體，要完成自主的任務(wù)規(guī)劃需要考慮到探測(cè)器的各種活動(dòng)、約束，以及探測(cè)器自身的復(fù)雜屬性，需要有很強(qiáng)的啟發(fā)信息。正是基于這種思想，提出一種基于空間簡(jiǎn)化的深空大角度機(jī)動(dòng)自主規(guī)劃器，在考慮探測(cè)器的約束的前提下進(jìn)行探測(cè)器姿態(tài)大角度機(jī)動(dòng)的路徑規(guī)劃和優(yōu)化。
在已有的大角度機(jī)動(dòng)規(guī)劃技術(shù)方案中，在先技術(shù)[1](參見(jiàn)G.Singh，G.Macala，E.Wong，and R.Rasmussen.A Constraint Monitor Algorithm for the Cassini Spacecraft.InProceedings of the AIAA Guidance，Navigation，and Control Conference，272-282，1997)中的Cassini探測(cè)器采用的是一種被動(dòng)模式，即利用約束管理器，通過(guò)實(shí)時(shí)檢查來(lái)確保姿態(tài)機(jī)動(dòng)指令的可行性。但這是一種局部的被動(dòng)判斷，不能從整個(gè)機(jī)動(dòng)過(guò)程中考慮規(guī)劃。
在先技術(shù)[2](參見(jiàn)C.McInnes.Large Angle Slew Maneuver with AutonomousSun Vector Avoidance.AIAAJ.on Guidance，Control and Dynamics，17(4)875-877，1994)中通過(guò)定義勢(shì)能函數(shù)的方法，該方法的主要缺點(diǎn)是容易陷入局部最小，而且對(duì)于復(fù)雜問(wèn)題很難定義勢(shì)能函數(shù)。
在先技術(shù)[3](參見(jiàn)E.Frazzoli，M.A.Dahleh and E.Feron.A RandomizedAttitude Slew Planning Algorithm for Autonomous spacecraft.AIAA Guidance，Navigation and Control Conf.，AIAA，2001-4155)中結(jié)合概率路徑圖法(PRM，Probabilistic RoadMap)和快速擴(kuò)展樹(shù)法(RRT，Rapidly-Exploring Random Trees)，把自主探測(cè)器大角度機(jī)動(dòng)當(dāng)作一個(gè)三維規(guī)劃問(wèn)題。在RRT樹(shù)的擴(kuò)展中，隨機(jī)變量為探測(cè)器姿態(tài)的俯仰、偏航、滾轉(zhuǎn)三個(gè)角，該方法能完成大角度機(jī)動(dòng)規(guī)劃，但由于其還是一個(gè)三維規(guī)劃問(wèn)題，求解過(guò)程的搜索空間很大。
并且在先技術(shù)[2]和在先技術(shù)[3]中，都只是針對(duì)障礙完成路徑規(guī)劃，要完成星上的自主大角度機(jī)動(dòng)規(guī)劃，還需要解決自主的生成規(guī)劃問(wèn)題。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題是克服上述已有技術(shù)的困難，提供一種能夠自主完成探測(cè)器大角度機(jī)動(dòng)的方法，該實(shí)現(xiàn)方法主要為通過(guò)給定的探測(cè)器軌道信息，對(duì)環(huán)境進(jìn)行判斷并自動(dòng)的生成障礙，基于這些障礙通過(guò)規(guī)劃算法生成姿態(tài)機(jī)動(dòng)的路徑，這里，規(guī)劃算法采用我們提出的改進(jìn)的RRT算法，在隨機(jī)樹(shù)的擴(kuò)展中通過(guò)單軸隨機(jī)算法，使屬于三維規(guī)劃的大角度機(jī)動(dòng)規(guī)劃問(wèn)題簡(jiǎn)化成為一個(gè)二維規(guī)劃問(wèn)題，并引入啟發(fā)因子，加快算法的收斂。并利用前向搜索對(duì)規(guī)劃出來(lái)的路徑進(jìn)行優(yōu)化。
本發(fā)明的深空探測(cè)器大角度機(jī)動(dòng)路徑自主生成方法按照如下步驟進(jìn)行一、障礙自主生成需要的初始參數(shù)(1)用戶提供的探測(cè)器載荷安裝；(2)在給定的時(shí)刻和探測(cè)器當(dāng)前的位置。(3)大角度機(jī)動(dòng)的初始、終止姿態(tài)。
1)、根據(jù)探測(cè)器載荷的安裝，確定相關(guān)矢量，限制相關(guān)矢量Ri與圓錐體的軸線方向Rc的夾角滿足下述條件Ri·Rc＜cosαmin或Ri·Rc＞cosαmax；2)、根據(jù)給定時(shí)刻，探測(cè)器自主生成地球-天線障礙區(qū)；3)、根據(jù)探測(cè)器當(dāng)前位置，確定太陽(yáng)-導(dǎo)航相機(jī)障礙區(qū)、太陽(yáng)-星敏感器障礙區(qū)；4)、根據(jù)給定時(shí)刻和探測(cè)器當(dāng)前位置，自主確定需要考慮的大型行星，并確定相應(yīng)的星敏感器-行星障礙區(qū)，其中當(dāng)前行星在星敏感器中的成像大小滿足下述條件L＝R/Dsp-planet，R為行星的半徑，Dsp-planet為探測(cè)器與行星的距離；二、大角度機(jī)動(dòng)規(guī)劃(一)、利用單軸隨機(jī)擴(kuò)展的RRT規(guī)劃算法規(guī)劃姿態(tài)規(guī)劃路徑，具體方法如下1)、規(guī)劃空間采用Euler軸角或四元數(shù)表示探測(cè)器的姿態(tài)；2)、姿態(tài)制導(dǎo)律為滿足約束條件，一次完整的大角度機(jī)動(dòng)要分成多次小的大角度機(jī)動(dòng)，定義小的機(jī)動(dòng)間的狀態(tài)為節(jié)點(diǎn)，假定節(jié)點(diǎn)處探測(cè)器的姿態(tài)角速率為零，且節(jié)點(diǎn)間的姿態(tài)運(yùn)動(dòng)為固定Euler軸旋轉(zhuǎn)，則相鄰節(jié)點(diǎn)間的姿態(tài)制導(dǎo)律為qi+1＝qiqei，其中qi為第i個(gè)節(jié)點(diǎn)對(duì)應(yīng)的姿態(tài)四元數(shù)，qei為第i個(gè)節(jié)點(diǎn)與第i+1個(gè)節(jié)點(diǎn)間的偏差四元數(shù)，對(duì)應(yīng)兩節(jié)點(diǎn)間的固定Euler軸角旋轉(zhuǎn)；3)、二維的概率啟發(fā)因子在隨機(jī)生成節(jié)點(diǎn)的過(guò)程中，引入啟發(fā)因子會(huì)加快算法的收斂，假設(shè)S、T分別為平面上的兩點(diǎn)，兩者中點(diǎn)為C，則隨機(jī)生成點(diǎn)R可取概率為 其中A為常數(shù)，Dis(R，C)表示R、C之間的距離；4)、單軸隨機(jī)擴(kuò)展算法(1)隨機(jī)生成兩個(gè)角α、β，求出其在單位球面上對(duì)應(yīng)的一點(diǎn)Rrz(xrz，yrz，zrz)，令Rrz為隨機(jī)Euler軸角Rr時(shí)探測(cè)器體坐標(biāo)系的Z軸指向矢量；(2)按照二維的概率啟發(fā)因子決定此點(diǎn)是否可取，如果可取則繼續(xù)下一步驟，否則返回步驟(1)；(3)在已有的Euler軸角中，找出Z軸指向矢量與Rrz夾角最小的Euler軸角Rn，夾角為θ；(4)如果θ＞0.2弧度，返回步驟(1)；如果θ≤0.2弧度，繼續(xù)下一步驟；(5)Rn的X軸指向矢量為Rnx(xnx，ynx，znx)，Rn的Y軸指向矢量為Rny(xny，yny，zny)，如果znx≥zny，則執(zhí)行步驟(6)，否則執(zhí)行步驟(7)；(6)通過(guò)下式求解Rr的X軸指向矢量Rrx(xrx，yrx，zrx)zrx=znxxrx·xrz+yrx·yrz+zrx·zrz=0xrx·xrx+yrx·yrx+zrx·zrx=1;]]>(7)通過(guò)下式求解Rr的Y軸指向矢量Rry(xry，yry，zry)zry=znyxry·xrz+yry·yrz+zry·zrz=0xry·xry+yry·yry+zry·zry=1;]]>(8)由Rrz(xrz，yrz，zrz)、Rrx(xrx，yrx，zrx)或Rry(xry，yry，zry)即可確定此次隨機(jī)生成的Euler軸角；(9)判斷生成的Euler軸角是否滿足約束條件，如果不滿足約束條件，返回步驟(1)；5)、利用上述單軸隨機(jī)擴(kuò)展算法，得到改進(jìn)的RRT規(guī)劃算法如下(1)把起始姿態(tài)加入路徑列表；(2)判斷路徑列表中的最后一個(gè)點(diǎn)是否能夠直接到達(dá)終止姿態(tài)，如果可以，結(jié)束，否則繼續(xù)；(3)利用單軸隨機(jī)擴(kuò)展算法生成一個(gè)隨機(jī)姿態(tài)；(4)在當(dāng)前路徑中找到離隨機(jī)姿態(tài)最近的姿態(tài)；(5)判斷隨機(jī)姿態(tài)和最近姿態(tài)之間的路徑是否通過(guò)障礙區(qū)，如果是，則返回步驟(3)，否則，建立兩者的聯(lián)系；(6)把隨機(jī)點(diǎn)加入路徑列表末尾，跳轉(zhuǎn)至步驟(2)；(二)、利用前向搜索對(duì)路徑優(yōu)化；(三)、最終生成可行大角度機(jī)動(dòng)規(guī)劃路徑。
本發(fā)明所述方法的基本原理
在探測(cè)器大角度機(jī)動(dòng)過(guò)程中，探測(cè)器姿態(tài)必須滿足敏感儀器、保持通信鏈路等的約束。而這些約束是變化的，如對(duì)星敏感器來(lái)說(shuō)，如果某個(gè)大行星的在其上的成像很小，則可以不考慮其影響，這樣，我們就需要根據(jù)當(dāng)前的時(shí)間和探測(cè)器所處的位置來(lái)確定當(dāng)前的障礙，另外行星都是在運(yùn)動(dòng)的，所以其障礙在不同的時(shí)刻也是不同的，我們根據(jù)其成像大小來(lái)判斷其是否需要考慮，從而能夠自主的生成障礙。
在機(jī)動(dòng)過(guò)程中，把探測(cè)器視作一個(gè)剛體，則其姿態(tài)為在三維空間的旋轉(zhuǎn)。其規(guī)劃問(wèn)題構(gòu)造空間為三維。用Euler軸角或四元數(shù)表示探測(cè)器的姿態(tài)。探測(cè)器的姿態(tài)有三個(gè)隨機(jī)參數(shù)，即這是一個(gè)三維空間運(yùn)動(dòng)規(guī)劃問(wèn)題，而規(guī)劃問(wèn)題的搜索空間與規(guī)劃空間的維數(shù)成指數(shù)增長(zhǎng)。為了減少求解過(guò)程的搜索空間則可以減少規(guī)劃問(wèn)題的維數(shù)?；谶@種目的，在每一步搜索我們采用一種單軸隨機(jī)的算法，把此規(guī)劃問(wèn)題簡(jiǎn)化成一個(gè)二維空間的規(guī)劃問(wèn)題，具體辦法是先隨機(jī)生成兩個(gè)變量，其對(duì)應(yīng)探測(cè)器姿態(tài)的一個(gè)軸的指向矢量，而再充分利用臨近點(diǎn)的信息得到另一軸的信息，從而確定當(dāng)前隨機(jī)的姿態(tài)。算法中只需要兩個(gè)隨機(jī)變量，即規(guī)劃空間為二維，減少了規(guī)劃問(wèn)題構(gòu)造空間的維數(shù)，從而減少問(wèn)題求解的搜索空間，并在單軸隨機(jī)過(guò)程中引入啟發(fā)因子，加快算法的收斂。應(yīng)用上述單軸隨機(jī)算法進(jìn)行隨機(jī)擴(kuò)展，直到找到可行的路徑，并利用前向搜索的方式對(duì)路徑進(jìn)行優(yōu)化。
這樣，在給定時(shí)刻和給定探測(cè)器的位置的情況下，我們就能夠自主的生成當(dāng)前的障礙，并在這些障礙的情況下，進(jìn)行探測(cè)器的大角度機(jī)動(dòng)規(guī)劃，最終形成一條可行的規(guī)劃路徑。
本發(fā)明能夠自主的生成姿態(tài)機(jī)動(dòng)障礙，并自主進(jìn)行姿態(tài)機(jī)動(dòng)規(guī)劃生成大角度機(jī)動(dòng)路徑，并具有交互性強(qiáng)、直觀、快捷等特點(diǎn)。在先技術(shù)[1]，先技術(shù)[2]，先技術(shù)[3]的不同之處在于，本發(fā)明的新穎之處在于能夠自主的生成當(dāng)前的姿態(tài)障礙，并能自主的避開(kāi)障礙生成路徑。從而實(shí)現(xiàn)了星上自主大角度機(jī)動(dòng)，避免了傳統(tǒng)的通過(guò)地面進(jìn)行規(guī)劃，然后給探測(cè)器上傳機(jī)動(dòng)指令，或探測(cè)器進(jìn)行被動(dòng)的檢測(cè)地面上傳指令是否可行方式。規(guī)劃器可以作為深空探測(cè)器自主任務(wù)規(guī)劃系統(tǒng)的一部分，也可以作為地面探測(cè)器姿態(tài)大角度機(jī)動(dòng)路徑規(guī)劃的工具，形成地面上傳的機(jī)動(dòng)指令。
具體實(shí)施例方式
具體實(shí)施方式
一本實(shí)施方式按照如下步驟實(shí)現(xiàn)深空探測(cè)器自主生成大角度機(jī)動(dòng)路徑一、障礙自主生成大角度機(jī)動(dòng)中要考慮裝置、載荷等對(duì)外部環(huán)境的限制，主要的外部環(huán)境約束可以分成兩類——指向約束和積分約束，指向約束要求探測(cè)器的某一矢量方向不能進(jìn)入或離開(kāi)某一區(qū)域，如果忽略機(jī)動(dòng)過(guò)程中約束的變化，這類約束的障礙區(qū)域是一個(gè)圓錐體，我們只需要限制相關(guān)的矢量Ri與圓錐體的軸線方向Rc的夾角就行，可表示為Ri·Rc＜cosαmin或Ri·Rc＞cosαmax。
積分約束是一種時(shí)變的約束，如散熱面暴露在太陽(yáng)光下的時(shí)間不能太長(zhǎng)，具體由下式?jīng)Q定∫0tg(θ,d)dτ<H]]>其中H為一常值，即探測(cè)器最大能夠承受的輻射量；g(θ，d)為輻射強(qiáng)度函數(shù)，與受照射面與太陽(yáng)光矢量的夾角、探測(cè)器與太陽(yáng)的距離等相關(guān)。
在這些約束中，指向約束是一個(gè)跟探測(cè)器軌道相關(guān)的參數(shù)，其可能考慮的對(duì)象有水星、金星、地球、火星、木星、土星，但探測(cè)器在不同的位置會(huì)有些不同，即有時(shí)只需要考慮其中的某幾顆，我們是通過(guò)當(dāng)前行星在星敏感器中的成像大小來(lái)決定的，為了簡(jiǎn)便，我們定義簡(jiǎn)單的成像大小為L(zhǎng)，具體如下式L＝R/Dsp-planet其中R為行星的半徑(公里)，Dsp-planet為探測(cè)器與其的距離(天文單位)。
當(dāng)某行星的L大于1000時(shí)，就認(rèn)為其為需要考慮的障礙。
二、大角度機(jī)動(dòng)規(guī)劃上述已經(jīng)自主的生成障礙，下一步的工作就是在這些障礙存在的情況下，自主的規(guī)劃出一條可行的機(jī)動(dòng)路徑?；趯?duì)規(guī)劃問(wèn)題的規(guī)劃空間、約束的描述以及機(jī)動(dòng)姿態(tài)的制導(dǎo)律，本實(shí)施方式引入改進(jìn)的RRT算法進(jìn)行大角度機(jī)動(dòng)規(guī)劃，對(duì)RRT算法改進(jìn)的具體步驟為應(yīng)用單軸隨機(jī)算法生成探測(cè)器姿態(tài)的一個(gè)軸，然后利用臨近點(diǎn)的信息確定當(dāng)前的隨機(jī)姿態(tài)。其中單軸隨機(jī)算法中還引入了啟發(fā)因子。
(一)規(guī)劃空間自主探測(cè)器大角度機(jī)動(dòng)問(wèn)題是一個(gè)含動(dòng)態(tài)約束的動(dòng)力學(xué)規(guī)劃問(wèn)題，通過(guò)簡(jiǎn)化，我們可以把問(wèn)題變成一個(gè)運(yùn)動(dòng)學(xué)規(guī)劃問(wèn)題，而其也是一個(gè)PSPACE難題。算法的復(fù)雜度與問(wèn)題的維數(shù)成指數(shù)增長(zhǎng)。在機(jī)動(dòng)過(guò)程中，把探測(cè)器視作一個(gè)剛體，則其姿態(tài)為在三維空間的旋轉(zhuǎn)，即此規(guī)劃問(wèn)題的構(gòu)造空間的維數(shù)為3。這里，我們用Euler軸角或四元數(shù)表示探測(cè)器的姿態(tài)。
(二)姿態(tài)制導(dǎo)律為滿足約束條件，一次完整的大角度機(jī)動(dòng)可能要分成多次小的大角度機(jī)動(dòng)，我們稱小的機(jī)動(dòng)間的狀態(tài)為節(jié)點(diǎn)，為了問(wèn)題的簡(jiǎn)化，我們假定節(jié)點(diǎn)處探測(cè)器的姿態(tài)角速率為零，且節(jié)點(diǎn)間的姿態(tài)運(yùn)動(dòng)為固定Euler軸旋轉(zhuǎn)，可知相鄰節(jié)點(diǎn)間的姿態(tài)制導(dǎo)律為qi+1＝qiqei其中qi為第i個(gè)節(jié)點(diǎn)對(duì)應(yīng)的姿態(tài)四元數(shù)qei為第i個(gè)節(jié)點(diǎn)與第i+1個(gè)節(jié)點(diǎn)間的偏差四元數(shù)，對(duì)應(yīng)兩節(jié)點(diǎn)間的固定Euler軸角旋轉(zhuǎn)。
(三)單軸隨機(jī)擴(kuò)展的RRT規(guī)劃算法對(duì)于姿態(tài)大角度機(jī)動(dòng)規(guī)劃問(wèn)題，采用三軸RRT方法，每一次樹(shù)的擴(kuò)展需要三個(gè)隨機(jī)量，我們應(yīng)用單軸隨機(jī)擴(kuò)展算法，即每次擴(kuò)展只需要兩個(gè)隨機(jī)變量。而單軸隨機(jī)時(shí)又采用二維的概率啟發(fā)因子。
1、二維的概率啟發(fā)因子在隨機(jī)生成節(jié)點(diǎn)的過(guò)程中，引入啟發(fā)因子會(huì)加快算法的收斂，這里我們以簡(jiǎn)單的勢(shì)能函數(shù)作為啟發(fā)因子，用于表示接受此次隨機(jī)生成的節(jié)點(diǎn)的概率，假設(shè)S、T分別為平面上的兩點(diǎn)，兩者中點(diǎn)為C，則隨機(jī)生成點(diǎn)R可取概率為 其中A為常數(shù)，根據(jù)障礙選取，路徑中的障礙越多，A值越大，一般取0.4；Dis(R，C)表示R、C之間的距離；2、單軸隨機(jī)擴(kuò)展算法在大角度機(jī)動(dòng)規(guī)劃中，此時(shí)的構(gòu)造空間維數(shù)為3，我們應(yīng)用單軸隨機(jī)的方法生成節(jié)點(diǎn)，則問(wèn)題簡(jiǎn)化為一個(gè)構(gòu)造空間維數(shù)為2，具體方法如下(1)隨機(jī)生成兩個(gè)角α、β，求出其在單位球面上對(duì)應(yīng)的一點(diǎn)Rrz(xrz，yrz，zrz)，令Rrz為隨機(jī)Euler軸角Rr時(shí)探測(cè)器體坐標(biāo)系的Z軸指向矢量；(2)按照二維的概率啟發(fā)因子決定此點(diǎn)是否可取，如果可取則繼續(xù)下一步驟，否則返回步驟(1)；(3)在已有的Euler軸角中，找出Z軸指向矢量與Rrz夾角最小的Euler軸角Rn，夾角為θ；(4)如果θ＞0.2弧度，返回步驟(1)；如果θ≤0.2弧度，繼續(xù)下一步驟；(5)得到Rn的X軸指向矢量Rnx(xnx，ynx，znx)，Rn的Y軸指向矢量Rny(xny，yny，zny)，如果znx≥zny，則執(zhí)行步驟(6)，否則執(zhí)行步驟(7)；(6)通過(guò)下式求解Rr的X軸指向矢量Rrx(xrx，yrx，zrx)zrx=znxxrx·xrz+yrx·yrz+zrx·zrz=0xrx·xrx+yrx·yrx+zrx·zrx=1;]]>(7)通過(guò)下式求解Rr的Y軸指向矢量Rry(xry，yry，zry)zry=znyxry·xrz+yry·yrz+zry·zrz=0xry·xry+yry·yry+zry·zry=1;]]>(8)由Rrz(xrz，yrz，zrz)、Rrx(xrx，yrx，zrx)或Rry(xry，yry，zry)即可確定此次隨機(jī)生成的Euler軸角。
(9)判斷生成的Euler軸角是否滿足約束條件，如果否，返回步驟(1)。
可以看出，在算法中隨機(jī)Euler軸角僅一個(gè)軸是隨機(jī)生成的，再利用此軸臨近Euler軸角的信息，即可確定當(dāng)前隨機(jī)生成的Euler軸角，從而減少了對(duì)隨機(jī)Euler軸角另一軸的搜索。
隨機(jī)生成節(jié)點(diǎn)算法保證了路徑的起點(diǎn)和終點(diǎn)不在障礙區(qū)，但其并不能保證路徑上所有點(diǎn)均不在障礙區(qū)，因此還需要檢測(cè)路徑是否通過(guò)了障礙區(qū)，我們可以采用取樣檢測(cè)的方法，只需要檢測(cè)路徑中的取樣點(diǎn)，而點(diǎn)的幾何約束檢測(cè)可利用前面的約束表達(dá)式。
3、單軸隨機(jī)擴(kuò)展的RRT規(guī)劃算法利用上面的隨機(jī)算法，可以得到改進(jìn)的RRT規(guī)劃算法如下
(1)把起始姿態(tài)加入路徑列表；(2)判斷路徑列表中的最后一個(gè)點(diǎn)是否能夠直接到達(dá)終止姿態(tài)，如果可以，結(jié)束，否則繼續(xù)；(3)利用單軸隨機(jī)擴(kuò)展算法生成一個(gè)隨機(jī)姿態(tài)；(4)在當(dāng)前路徑中找到離隨機(jī)姿態(tài)最近的姿態(tài)，(5)判斷隨機(jī)姿態(tài)和最近姿態(tài)之間的路徑是否通過(guò)障礙區(qū)，如果是，則返回3，否則，建立兩者的聯(lián)系；(6)把隨機(jī)點(diǎn)加入路徑列表末尾；跳轉(zhuǎn)至2；H-RRT(Ns，Nt){Nini＝Ns；R.add(Ns)；∥初始化，起點(diǎn)加入路徑序列while！Arrive(Nini，Nt)；{do{Nrand＝RandNode()；Nnear＝LeastD(Nrand，R)；}while！AvoidRouter(Nrand，Nnear)；R.add(Nrand)；Nini＝Nrand；∥更新路徑最后一個(gè)點(diǎn)}R.add(Nt)；} 。
其中Arrive(Nini，Nt)用于判斷從Nini是否能夠直接到達(dá)Nt；RandNode()為根據(jù)單軸隨機(jī)擴(kuò)展算法生成一個(gè)節(jié)點(diǎn)。LeastD(Nrand，R)，找到已有節(jié)點(diǎn)集合中離Nrand最近的節(jié)點(diǎn)，AvoidRouter(Nrand，Nnear)檢測(cè)Nrand、Nnear之間的路徑是否在障礙區(qū)。
算法中需要用到兩個(gè)狀態(tài)間的距離信息，而在非平面規(guī)劃過(guò)程中，沒(méi)有直觀的距離，因此需要通過(guò)定義一個(gè)變量來(lái)表示距離，稱之為偽距離，F(xiàn)razzoli等人是通過(guò)求解最優(yōu)控制問(wèn)題，從而利用代價(jià)函數(shù)作為偽距離。我們這里通過(guò)兩個(gè)Euler軸角之間的機(jī)動(dòng)時(shí)間當(dāng)作偽距離，機(jī)動(dòng)時(shí)間估計(jì)方法可以參照Clementine上采用的方法。
(四)路徑優(yōu)化及積分約束由隨機(jī)生成樹(shù)構(gòu)成的路徑往往需要優(yōu)化，才能形成最優(yōu)或次優(yōu)的路徑，優(yōu)化的準(zhǔn)則是整個(gè)機(jī)動(dòng)時(shí)間最短。
路徑優(yōu)化采用前向搜索的方法，如下以起點(diǎn)為初始點(diǎn)，依次搜索，找到其能直接到達(dá)的最遠(yuǎn)節(jié)點(diǎn)，條件是他們之間直接到達(dá)的路徑能夠滿足路徑檢測(cè)。刪除中間節(jié)點(diǎn)，連接起點(diǎn)和最遠(yuǎn)點(diǎn)。然后以最遠(yuǎn)點(diǎn)為起點(diǎn)，進(jìn)行同樣的操作。如此反復(fù)搜索，直到起點(diǎn)為路徑終點(diǎn)的前一個(gè)點(diǎn)。這樣，依次連接所有被選中的起點(diǎn)，加上路徑終點(diǎn)構(gòu)成最終的優(yōu)化路徑，具體如下Optimal(R){for i＝0→size(R)-2 ∥size(R)初始路徑R的節(jié)點(diǎn)數(shù)；{tag＝i+1；//初始化下一個(gè)連接點(diǎn)的編號(hào)tag；for j＝size(R)-1→i+1{∥依次搜索If AvoidRouter(R[i]，R[j])；{tag＝j(luò)；L_Node＝R[j]；}}Result_R.add(L_Node)；//加入連接的節(jié)點(diǎn)；i＝tag；}}。
積分約束是在路徑生成之后進(jìn)行檢測(cè)，如果不滿足則需要重新規(guī)劃，這里把路徑按節(jié)點(diǎn)分成離散的段，則積分約束變成下式Σi=0n-2(g(i)+g(i+1))Δt/2≤H]]>其中Δt為從i點(diǎn)機(jī)動(dòng)到i+1點(diǎn)的時(shí)間；n為路徑被離散成的點(diǎn)數(shù)，包含起點(diǎn)和終點(diǎn)。
權(quán)利要求
1.深空探測(cè)器大角度機(jī)動(dòng)路徑自主生成方法，其特征在于所述方法按照如下步驟進(jìn)行一、障礙自主生成1)、根據(jù)探測(cè)器載荷的安裝，確定相關(guān)矢量，限制相關(guān)矢量Ri與圓錐體的軸線方向Rc的夾角滿足下述條件Ri·Rc＜cosαmin或Ri·Rc＞cosαmax；2)、根據(jù)給定時(shí)刻，探測(cè)器自主生成地球-天線障礙區(qū)；3)、根據(jù)探測(cè)器當(dāng)前位置，確定太陽(yáng)-導(dǎo)航相機(jī)障礙區(qū)、太陽(yáng)-星敏感器障礙區(qū)；4)、根據(jù)給定時(shí)刻和探測(cè)器當(dāng)前位置，自主確定需要考慮的大型行星，并確定相應(yīng)的星敏感器-行星障礙區(qū)，其中當(dāng)前行星在星敏感器中的成像大小滿足下述條件L＝R/Dsp-planet，R為行星的半徑，Dsp-planet為探測(cè)器與行星的距離；二、大角度機(jī)動(dòng)規(guī)劃(一)、利用單軸隨機(jī)擴(kuò)展的RRT規(guī)劃算法規(guī)劃姿態(tài)規(guī)劃路徑，具體方法如下1)、規(guī)劃空間采用Euler軸角或四元數(shù)表示探測(cè)器的姿態(tài)；2)、姿態(tài)制導(dǎo)律為滿足約束條件，一次完整的大角度機(jī)動(dòng)要分成多次小的大角度機(jī)動(dòng)，定義小的機(jī)動(dòng)間的狀態(tài)為節(jié)點(diǎn)，假定節(jié)點(diǎn)處探測(cè)器的姿態(tài)角速率為零，且節(jié)點(diǎn)間的姿態(tài)運(yùn)動(dòng)為固定Euler軸旋轉(zhuǎn)，則相鄰節(jié)點(diǎn)間的姿態(tài)制導(dǎo)律為qi+1＝qiqei，其中qi為第i個(gè)節(jié)點(diǎn)對(duì)應(yīng)的姿態(tài)四元數(shù)，qei為第i個(gè)節(jié)點(diǎn)與第i+1個(gè)節(jié)點(diǎn)間的偏差四元數(shù)，對(duì)應(yīng)兩節(jié)點(diǎn)間的固定Euler軸角旋轉(zhuǎn)；3)、二維的概率啟發(fā)因子在隨機(jī)生成節(jié)點(diǎn)的過(guò)程中，引入啟發(fā)因子會(huì)加快算法的收斂，假設(shè)S、T分別為平面上的兩點(diǎn)，兩者中點(diǎn)為C，則隨機(jī)生成點(diǎn)R可取概率為 其中A為常數(shù)，Dis(R，C)表示R、C之間的距離；4)、單軸隨機(jī)擴(kuò)展算法(1)隨機(jī)生成兩個(gè)角α、β，求出其在單位球面上對(duì)應(yīng)的一點(diǎn)Rrz(xrz，yrz，zrz)，令Rrz為隨機(jī)Euler軸角Rr時(shí)探測(cè)器體坐標(biāo)系的Z軸指向矢量；(2)按照二維的概率啟發(fā)因子決定此點(diǎn)是否可取，如果可取則繼續(xù)下一步驟，否則返回步驟(1)；(3)在已有的Euler軸角中，找出Z軸指向矢量與Rrz夾角最小的Euler軸角Rn，夾角為θ；(4)如果θ＞0.2弧度，返回步驟(1)；如果θ≤0.2弧度，繼續(xù)下一步驟；(5)Rn的X軸指向矢量為Rnx(xnx，ynx，znx)，Rn的Y軸指向矢量為Rny(xny，yny，zny)，如果znx≥zny，則執(zhí)行步驟(6)，否則執(zhí)行步驟(7)；(6)通過(guò)下式求解Rr的X軸指向矢量Rrx(xrx，yrx，zrx)zrx=znxxrx·xrz+yrx·yrz+zrx·zrz=0xrx·xrx+yrx·yrx+zrx·zrx=1;]]>(7)通過(guò)下式求解Rr的Y軸指向矢量Rry(xry，yry，zry)zry=znyxry·xrz+yry·yrz+zry·zrz=0xry·xry+yry·yry+zry·zry=1;]]>(8)由Rrz(xrz，yrz，zrz)、Rrx(xrx，yrx，zrx)或Rry(xry，yry，zry)即可確定此次隨機(jī)生成的Euler軸角；(9)判斷生成的Euler軸角是否滿足約束條件，如果不滿足約束條件，返回步驟(1)；5)、利用上述單軸隨機(jī)擴(kuò)展算法，得到改進(jìn)的RRT規(guī)劃算法如下(1)把起始姿態(tài)加入路徑列表；(2)判斷路徑列表中的最后一個(gè)點(diǎn)是否能夠直接到達(dá)終止姿態(tài)，如果可以，結(jié)束，否則繼續(xù)；(3)利用單軸隨機(jī)擴(kuò)展算法生成一個(gè)隨機(jī)姿態(tài)；(4)在當(dāng)前路徑中找到離隨機(jī)姿態(tài)最近的姿態(tài)；(5)判斷隨機(jī)姿態(tài)和最近姿態(tài)之間的路徑是否通過(guò)障礙區(qū)，如果是，則返回步驟(3)，否則，建立兩者的聯(lián)系；(6)把隨機(jī)點(diǎn)加入路徑列表末尾，跳轉(zhuǎn)至步驟(2)；(二)、利用前向搜索對(duì)路徑優(yōu)化；(三)、最終生成可行大角度機(jī)動(dòng)規(guī)劃路徑。
全文摘要
深空探測(cè)器大角度機(jī)動(dòng)路徑自主生成方法，涉及一種實(shí)現(xiàn)深空探測(cè)器自主生成大角度機(jī)動(dòng)路徑的方法。本發(fā)明要解決的技術(shù)問(wèn)題是克服已有技術(shù)的困難，提供一種能夠自主完成探測(cè)器大角度機(jī)動(dòng)的方法，方法主要實(shí)現(xiàn)為通過(guò)給定的探測(cè)器軌道信息，對(duì)環(huán)境進(jìn)行判斷并自動(dòng)的生成障礙，基于這些障礙通過(guò)規(guī)劃算法生成姿態(tài)機(jī)動(dòng)的路徑，這里，規(guī)劃算法采用我們提出的改進(jìn)的RRT算法，在隨機(jī)樹(shù)的擴(kuò)展中通過(guò)單軸隨機(jī)算法，使屬于三維規(guī)劃的大角度機(jī)動(dòng)規(guī)劃問(wèn)題簡(jiǎn)化成為一個(gè)二維規(guī)劃問(wèn)題，并引入啟發(fā)因子，加快算法的收斂。并利用前向搜索對(duì)規(guī)劃出來(lái)的路徑進(jìn)行優(yōu)化。本發(fā)明具有交互性強(qiáng)、直觀、快捷等特點(diǎn)。
文檔編號(hào)G05D1/00GK1851598SQ20061001011
公開(kāi)日2006年10月25日 申請(qǐng)日期2006年6月2日 優(yōu)先權(quán)日2006年6月2日
發(fā)明者徐瑞, 崔祜濤, 崔平遠(yuǎn), 徐文明, 史雪巖 申請(qǐng)人:哈爾濱工業(yè)大學(xué)