專利名稱：空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，可用于空間機(jī)器人系統(tǒng)
的機(jī)械、電氣、控制、軟件等多領(lǐng)域一體化的建模，并進(jìn)行閉環(huán)控制仿真。
背景技術(shù)：
空間機(jī)器人系統(tǒng)涉及的學(xué)科領(lǐng)域很多，包括機(jī)械、電氣、自動(dòng)控制、計(jì)算機(jī)、航天器 軌道及姿態(tài)動(dòng)力學(xué)，等等。整個(gè)系統(tǒng)的動(dòng)力學(xué)特性為多個(gè)領(lǐng)域交互作用的結(jié)果。在以往的 工程實(shí)踐中，不同階段——部件級(jí)、分系統(tǒng)級(jí)、系統(tǒng)級(jí)等建模和仿真的側(cè)重點(diǎn)不同。在部件 開發(fā)階段，設(shè)計(jì)師往往強(qiáng)調(diào)的是部件自身的細(xì)節(jié)，而該部件與其他部件的交互作用卻往往 被忽略或進(jìn)行粗略的近似。相反地，在分系統(tǒng)/系統(tǒng)級(jí)的開發(fā)階段，部件間的耦合作用卻是 主要考慮的因素，部件自身的很多細(xì)節(jié)又被大大地簡(jiǎn)化了 (Agrawal， S. K. ， Chen， M. Y. and A皿apragada， M. ， et al. Modelling and Simulation of Assembly in a Free—floating WorkEnvironment by a Free—floating Robot. Transactions of ASME Journal of Mechanical Design, 1996， 118 (1):卯.115-120)。所有的簡(jiǎn)化或近似都是基于一定的假設(shè) 條件的，當(dāng)條件滿足時(shí)，不會(huì)對(duì)分析結(jié)果產(chǎn)生影響；但如果實(shí)際系統(tǒng)在工作中出現(xiàn)了超出假 設(shè)條件的情況，則其模型不能準(zhǔn)確反映分系統(tǒng)/系統(tǒng)的行為，基于此模型設(shè)計(jì)的控制算法 將會(huì)失效。舉例來說，當(dāng)設(shè)計(jì)一套機(jī)構(gòu)的控制系統(tǒng)時(shí)，一般認(rèn)為機(jī)械的頻響與電氣相比慢很 多，由此按經(jīng)典控制理論設(shè)計(jì)PID控制器，但當(dāng)機(jī)構(gòu)高速運(yùn)動(dòng)或機(jī)構(gòu)本身的質(zhì)量很輕時(shí)，控 制器的帶寬就會(huì)與對(duì)象的一階振動(dòng)頻率耦合，導(dǎo)致控制對(duì)象發(fā)生共振，造成災(zāi)難性的后果。 因此，設(shè)計(jì)性能良好的控制器，必須將機(jī)械、電氣及控制系統(tǒng)納入統(tǒng)一框架內(nèi)(Samin J C， Brills O，Collard J F，et al. Multiphysicsmodeling and optimization of mechatronic multibody systems. Multibody System Dynamic. 2007 (18) :345-373)，開展多領(lǐng)域統(tǒng)一建 模與仿真石開究，以實(shí)現(xiàn)多學(xué)利j尤化設(shè)計(jì)(Multidisciplinary Design Optimization, MD0) 的目標(biāo)(Sobieszczanski-Sobieski J， Haftka T. Multidisciplinary aerospace design optimization :Survey of recentdevelopments. 1 996，AIAA 9620711)。
多領(lǐng)域建模與仿真方法主要有三種基于接口的方法，基于高層體系結(jié)構(gòu) (HighLevel Architecture,HLA)的方法，以及基于統(tǒng)一建模語言的方法?；诮y(tǒng)一建模語 言的方法對(duì)來自不同領(lǐng)域的系統(tǒng)構(gòu)件采用統(tǒng)一方式進(jìn)行描述，徹底實(shí)現(xiàn)了不同領(lǐng)域模型之 間的無縫集成和數(shù)據(jù)交換。Modelica語言是目前盛行的一種多領(lǐng)域物理系統(tǒng)建模語言，它 具有模型重用性高、建模簡(jiǎn)單方便、無須符號(hào)處理等許多優(yōu)點(diǎn)。M. Lovera等利用Modelica 語言進(jìn)行了衛(wèi)星姿態(tài)和軌道控制的仿真，但對(duì)于執(zhí)行機(jī)構(gòu)——飛輪、磁力矩器等的建模仍 然采用簡(jiǎn)化的數(shù)學(xué)描述(Lovera M. Control-oriented modelling and simulation of spacecraft attitude and orbitdynamics. Mathematical and Computer Modelling of Dynamical Systems. 2006， 12 (1) :73-88)。目前文獻(xiàn)中尚未見到對(duì)于空間機(jī)器人系統(tǒng)的多 領(lǐng)域建模與仿真方面的研究。因此，開發(fā)一套空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)是非 常必要和迫切的。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供了一種空間機(jī)器人多領(lǐng)域統(tǒng)一建模 與仿真系統(tǒng)。利用該系統(tǒng)，可建立包括機(jī)械、電氣、控制、軟件等多個(gè)領(lǐng)域的統(tǒng)一多領(lǐng)域模 型，基于該模型開展的閉環(huán)控制仿真，充分地體現(xiàn)了系統(tǒng)內(nèi)容的耦合關(guān)系，實(shí)現(xiàn)多學(xué)科設(shè)計(jì) 的優(yōu)化。 空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，由空間機(jī)器人路徑規(guī)劃器(1)、關(guān)節(jié)軸模 塊(2)、空間機(jī)器人手眼相機(jī)測(cè)量模塊(3)、空間機(jī)器人機(jī)構(gòu)模塊(4)、世界坐標(biāo)系及中心體 重力場(chǎng)(5)、軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)、空間機(jī)器人基座敏感器模塊(7)、推進(jìn)模塊 (8)、反作用飛輪組件(10)及空間機(jī)器人基座姿軌控模塊(9)組成。其中
空間機(jī)器人路徑規(guī)劃器(1)，接收來自于手眼視覺測(cè)量模塊(3)的相對(duì)位置、姿態(tài) 測(cè)量結(jié)果，自主規(guī)劃?rùn)C(jī)械臂和基座的運(yùn)動(dòng)軌跡——期望關(guān)節(jié)角、角速度、角加速度、基座姿 態(tài)角、角速度，作為關(guān)節(jié)軸模塊(2)和空間機(jī)器人基座姿軌控模塊(9)的輸入?？臻g機(jī)器 人路徑規(guī)劃器(1)還能實(shí)現(xiàn)各種笛卡爾空間、關(guān)節(jié)空間的規(guī)劃算法，包括梯形插值、三次樣 條、多項(xiàng)式插值等常規(guī)路徑規(guī)劃，以及機(jī)械臂與基座的協(xié)調(diào)規(guī)劃等，根據(jù)不同的任務(wù)要求， 可選擇合適的路徑規(guī)劃算法； 關(guān)節(jié)軸模塊(2)由機(jī)械臂所有關(guān)節(jié)軸組成，每個(gè)關(guān)節(jié)軸包括關(guān)節(jié)控制器、關(guān)節(jié)控 制器、電機(jī)及其驅(qū)動(dòng)器、諧波減速器、關(guān)節(jié)敏感器。關(guān)節(jié)控制器接收任務(wù)規(guī)劃器(1)輸出的 期望關(guān)節(jié)角、角速度、角加速度，以及關(guān)節(jié)敏感器的當(dāng)前關(guān)節(jié)角、角速度、電流，實(shí)現(xiàn)位置環(huán)、 速度環(huán)、電流環(huán)的控制算法，產(chǎn)生關(guān)節(jié)控制力矩，通過諧波減速器環(huán)節(jié)后作用在空間機(jī)器人 機(jī)構(gòu)模型(4)上； 空間機(jī)器人機(jī)構(gòu)模塊(4)包括空間機(jī)器人系統(tǒng)多剛體機(jī)構(gòu)模型、目標(biāo)衛(wèi)星單剛體 模型。該模塊接收關(guān)節(jié)軸模塊(2)輸出的關(guān)節(jié)驅(qū)動(dòng)力拒、反作用飛輪組件(10)輸出的基座 姿態(tài)控制力矩、以及軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)輸出的干擾力矩，計(jì)算作用后的機(jī)械 臂各關(guān)節(jié)角、角速度，基座姿態(tài)、角速度，以及目標(biāo)衛(wèi)星姿態(tài)、位置，輸出作為手眼視覺測(cè)量 模塊(3)、軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)/基座敏感器(6)以及關(guān)節(jié)軸模塊(2)中的關(guān)節(jié) 敏感器的輸入； 手眼視覺測(cè)量模塊(3)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的機(jī)械臂末端位置、姿 態(tài)，以及目標(biāo)衛(wèi)星的位置、姿態(tài)，計(jì)算目標(biāo)衛(wèi)星相對(duì)于機(jī)械臂末端坐標(biāo)系的位置、姿態(tài)，該位 置、姿態(tài)數(shù)據(jù)疊加上相機(jī)測(cè)量噪聲數(shù)據(jù)后成為手眼視覺測(cè)量數(shù)據(jù)，作為該模塊的輸出、空間 機(jī)器人路徑規(guī)劃器(1)的輸入； 世界坐標(biāo)系及中心體重力場(chǎng)(5)，建立世界坐標(biāo)系與系統(tǒng)本體系的關(guān)系——指向、 原點(diǎn)相對(duì)位置，以及中心體的重力場(chǎng)，可實(shí)現(xiàn)不同中心體下的空間機(jī)器人系統(tǒng)多領(lǐng)域統(tǒng)一 動(dòng)力學(xué)的建模與仿真研究。該模塊與空間機(jī)器人機(jī)構(gòu)模塊(4)相連。 軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的機(jī)器人基 座本體系相對(duì)于慣性系的姿態(tài)，以及推進(jìn)模塊(8)輸出的推力脈沖，計(jì)算空間機(jī)器人系統(tǒng) 質(zhì)心的位置、基座本體系相對(duì)于軌道坐標(biāo)系的姿態(tài)、角速度、所處軌道位置的磁場(chǎng)強(qiáng)度以及 環(huán)境干擾力矩，作為敏感器模塊(7)、空間機(jī)器人基座姿軌控模塊(9)，以及空間機(jī)器人機(jī) 構(gòu)模塊(4)的輸入；
空間機(jī)器人基座敏感器模塊(7)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的基座本體 系相對(duì)于慣性坐標(biāo)系的姿態(tài)、角速度，以及軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)輸出的基座本 體系相對(duì)于軌道坐標(biāo)系的姿態(tài)、角速度，疊加測(cè)量噪聲后作為敏感器的輸出，該輸出是空間 機(jī)器入基座姿軌控模塊(9)的輸入； 空間機(jī)器人基座姿軌控模塊(9)，接收基座姿態(tài)敏感器(7)輸出的當(dāng)前姿態(tài)角、 角速度，以及空間機(jī)器人路徑規(guī)劃器(1)輸出的期望姿態(tài)角、角速度，執(zhí)行航天器的各種導(dǎo) 航、制導(dǎo)與控制算法，如對(duì)目標(biāo)衛(wèi)星進(jìn)行跟蹤、接近、繞飛、軌道保持等的GNC算法，以及常 規(guī)模式下自身姿態(tài)、軌道的控制算法等，生成反作用飛輪組件(10)和推進(jìn)系統(tǒng)(8)的控制 指令，其中反作用飛輪組件的控制指令為控制電壓、推進(jìn)系統(tǒng)的控制指令為推力脈沖；
推進(jìn)系統(tǒng)模塊(8)，接收空間機(jī)器人基座姿軌控模塊(9)輸出的推力脈沖，產(chǎn)生各 推力器的作用力，作用于軌道動(dòng)力學(xué)及空間環(huán)境模塊(6); 反作用飛輪組件(10)，接收空間機(jī)器人基座姿軌控模塊(9)輸出的各推力器控制 電壓，產(chǎn)生各個(gè)飛輪的作用力矩，作用于空間機(jī)器人機(jī)構(gòu)模塊(4)。 所述的空間機(jī)器人路徑規(guī)劃器(1)采用多領(lǐng)域統(tǒng)一建模語言Modelica實(shí)現(xiàn)了一 種空間機(jī)器人目標(biāo)捕獲的自主路徑規(guī)劃方法，該方法利用手眼相機(jī)的相對(duì)位姿測(cè)量值，實(shí) 時(shí)規(guī)劃空間機(jī)器人的運(yùn)動(dòng)，以最終捕獲目標(biāo)。主要包括如下步驟位姿偏差計(jì)算、目標(biāo)運(yùn) 動(dòng)的預(yù)測(cè)、空間機(jī)器人末端運(yùn)動(dòng)速度規(guī)劃、空間機(jī)器人避奇異的路徑規(guī)劃、基座運(yùn)動(dòng)的預(yù)測(cè) 等。首先，根據(jù)手眼測(cè)量數(shù)據(jù)判斷相對(duì)位姿偏差ep和e。是否小于設(shè)定的閾值^和e。，若 小于，則閉合手爪、捕獲目標(biāo)；反之，則根據(jù)相對(duì)位姿偏差，實(shí)時(shí)估計(jì)目標(biāo)的運(yùn)動(dòng)狀態(tài)，并將 估計(jì)的結(jié)果反應(yīng)到機(jī)械臂末端速度的規(guī)劃中，以保證機(jī)械臂末端時(shí)刻朝最近的方向趨近目 標(biāo)，機(jī)械臂末端能自主跟蹤目標(biāo)的運(yùn)動(dòng)，直到最后捕獲目標(biāo)。規(guī)劃出末端運(yùn)動(dòng)速度后，即調(diào) 用自主奇異回避算法，以解算關(guān)節(jié)的期望角速度，并據(jù)此預(yù)測(cè)機(jī)械臂運(yùn)動(dòng)對(duì)基座的擾動(dòng)，當(dāng) 擾動(dòng)超出容許的范圍時(shí)，則自動(dòng)調(diào)整機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)速度，以保證期望的偏轉(zhuǎn)在許可的 范圍內(nèi)。整個(gè)過程一直持續(xù)到機(jī)械臂捕獲到目標(biāo)為止。 所述的關(guān)節(jié)軸模塊(2)采用多領(lǐng)域統(tǒng)一建模語言Modelica建立了機(jī)械臂各關(guān)節(jié) 的機(jī)械、電氣、控制等多學(xué)科領(lǐng)域一體的模型，每個(gè)關(guān)節(jié)軸模型由關(guān)節(jié)控制器、電機(jī)及其驅(qū) 動(dòng)器、關(guān)節(jié)傳動(dòng)機(jī)構(gòu)、關(guān)節(jié)敏感器等組成。關(guān)節(jié)控制器實(shí)現(xiàn)了位置環(huán)和速度環(huán)的控制，其中， 位置環(huán)采用PD控制，而速度環(huán)采用PI控制；電機(jī)模型中包含了電樞電阻Ra、電樞電感La、 反電動(dòng)勢(shì)emf 、電機(jī)軸Jmotor等環(huán)節(jié)；驅(qū)動(dòng)器部分由電阻R、電容C、運(yùn)算放大器0p、電壓源 Vs及接地g等組成；關(guān)節(jié)傳動(dòng)機(jī)構(gòu)包括諧波減速器、齒輪減速箱等，是連接電機(jī)軸和關(guān)節(jié)軸 的中間部分，該部分的建模由庫倫摩擦bearingFrition、彈性阻尼器springDamper，以及 理想減速模型idearGear三部分組成； 所述的空間機(jī)器人手眼相機(jī)測(cè)量模塊(3)采用多領(lǐng)域統(tǒng)一建模語言Modelica的 多體敏感器庫MultiBody. Sensors中的相對(duì)運(yùn)動(dòng)敏感器RelativeSensor，疊加上相機(jī)測(cè)量 噪聲，作為手眼相機(jī)的測(cè)量數(shù)據(jù)； 所述的空間機(jī)器人機(jī)構(gòu)模塊(4)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫空間機(jī) 器人系統(tǒng)及目標(biāo)衛(wèi)星的多個(gè)剛體屬性，以及剛體間的約束實(shí)現(xiàn)。每個(gè)剛體的屬性包括質(zhì)量、 慣量、質(zhì)心位置、坐標(biāo)系a和坐標(biāo)系b，其中質(zhì)量、慣量、質(zhì)心位置為剛體的質(zhì)量特性參數(shù)，坐 標(biāo)系a、坐標(biāo)系b則用于定義該剛體與相應(yīng)約束的連接關(guān)系；剛體間的約束用于描述相連
7剛體間的相對(duì)運(yùn)動(dòng)關(guān)系，空間機(jī)器人基座與機(jī)械臂第一個(gè)連桿，以及機(jī)械臂各連桿之間為 旋轉(zhuǎn)關(guān)節(jié)，而目標(biāo)衛(wèi)星與慣性坐標(biāo)系之間為6D0F的自由運(yùn)動(dòng)，通過Modelica多體庫中的 FreeMotion實(shí)現(xiàn)； 所述的世界坐標(biāo)系及中心體重力場(chǎng)(5)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫， 建立世界坐標(biāo)系與系統(tǒng)本體系的關(guān)系(指向、原點(diǎn)相對(duì)位置)，以及地球的微重力場(chǎng)；
所述的軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫， 實(shí)現(xiàn)兩星的相對(duì)軌道動(dòng)力學(xué)方程——Hill方程，以及軌道環(huán)境干擾力、干擾力矩，包括太陽 光壓力/力矩、大氣拖動(dòng)力/力矩、剩磁力矩等； 所述的空間機(jī)器人基座敏感器模塊(7)采用多領(lǐng)域統(tǒng)一建模語言Modelica的多
體敏感器庫(MultiBody. Sensors)中的相對(duì)運(yùn)動(dòng)敏感器RelativeSensor，通過設(shè)置輸出
項(xiàng)、再疊加上相應(yīng)姿態(tài)敏感器的測(cè)量噪聲，作為基座敏感器的測(cè)量數(shù)據(jù)； 所述的推進(jìn)模塊(8)、反作用飛輪組件(10)采用多領(lǐng)域統(tǒng)一建模語言Modelica建
立，其中反作用飛輪組件(10)包括4個(gè)反作用飛輪，采用"三軸正交安裝+ —等傾角斜裝"，
通過設(shè)置可工作于整星零動(dòng)量或偏置動(dòng)量模式，單個(gè)飛輪的建模由驅(qū)動(dòng)電路、電機(jī)及輪體
組成； 所述的空間機(jī)器人基座姿軌控模塊(9)采用多領(lǐng)域統(tǒng)一建模語言Modelica實(shí)現(xiàn) 相應(yīng)的姿態(tài)、軌道控制算法，產(chǎn)生執(zhí)行機(jī)構(gòu)的控制指令——四個(gè)飛輪的控制電壓，以及推進(jìn) 系統(tǒng)的推力脈沖，對(duì)基座進(jìn)行6D0F的控制，控制基座姿態(tài)、軌道按期望的軌跡運(yùn)動(dòng)。
本發(fā)明與現(xiàn)有技術(shù)相比具有如下優(yōu)點(diǎn)(l)建立的模型包括了機(jī)械、電氣、控制、 軟件等多個(gè)學(xué)科領(lǐng)域，全面反映多個(gè)領(lǐng)域交互作用的整體效果；(2)建模與仿真系統(tǒng)中 的各模塊重用性好，可方便地建立任意自由度的、串/并聯(lián)、單臂/多臂空間機(jī)器人系統(tǒng) 的多領(lǐng)域模型；(3)該建模與仿真系統(tǒng)支持全數(shù)學(xué)的、半物理的仿真實(shí)驗(yàn)，還可方便的實(shí) 現(xiàn)實(shí)時(shí)系統(tǒng)的仿真；(4)該建模與仿真系統(tǒng)具有和Simulink的接口，其所建模型可轉(zhuǎn)換 為Simulink的模塊，與其他Simulink模塊一樣可在Simulink環(huán)境中被隨意使用；同時(shí)， Simulink模塊亦可轉(zhuǎn)換為該模型庫所支持的模塊，作為模型庫的一員；(5)該建模與仿真 系統(tǒng)支持模型的校驗(yàn)和更新，即可將實(shí)際的實(shí)驗(yàn)數(shù)據(jù)導(dǎo)入到該建模與仿真平臺(tái)中，與仿真 數(shù)據(jù)進(jìn)行校核，并根據(jù)仿真數(shù)據(jù)與實(shí)驗(yàn)數(shù)據(jù)之差更新模型參數(shù)，使得所建模型與真實(shí)情況 更加接近。


圖1是典型的空間機(jī)器人在軌服務(wù)系統(tǒng)； 圖2是空間機(jī)器人系統(tǒng)功能模塊； 圖3是空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)組成圖； 圖4是空間機(jī)器人系統(tǒng)幾何參數(shù)及坐標(biāo)系； 圖5是所建立的空間機(jī)器人及目標(biāo)的多體系統(tǒng)模型； 圖6是所建立的空間機(jī)器人及目標(biāo)的多體系統(tǒng)的3D視圖； 圖7是機(jī)械臂單個(gè)關(guān)節(jié)軸模型的總體組成圖 圖8是電機(jī)及其驅(qū)動(dòng)器模型； 圖9是關(guān)節(jié)傳動(dòng)機(jī)構(gòu)(gear)模型；
圖10是飛輪系統(tǒng)的多領(lǐng)域模型； 圖11是"3正交+1斜裝"飛輪系統(tǒng)的3D模型； 圖12是推力器系統(tǒng)的模型； 圖13是姿態(tài)控制結(jié)構(gòu)圖； 圖14是手眼視覺測(cè)量的模型圖； 圖15是雙臂空間機(jī)器人系統(tǒng)的多領(lǐng)域模型； 圖16是雙臂空間機(jī)器人系統(tǒng)的多領(lǐng)域模型的3D圖示。
具體實(shí)施例方式
—、空間機(jī)器人系統(tǒng)多領(lǐng)域功能模塊劃分與建模仿真系統(tǒng)的組成
典型的空間機(jī)器人在軌服務(wù)系統(tǒng)由一飛行基座和空間機(jī)械手組成，如圖1所示。其中，飛行基座上安裝了目標(biāo)測(cè)量系統(tǒng)、對(duì)接機(jī)構(gòu)、姿軌控系統(tǒng)等，空間機(jī)械手可由6D0F機(jī)械臂、抓捕手爪及手眼視覺組成?？臻g目標(biāo)可能是故障衛(wèi)星(如太陽帆板未展開)、廢棄衛(wèi)星或空間碎片等。要實(shí)現(xiàn)完整的空間機(jī)器人在軌服務(wù)系統(tǒng)的建模，需包含如下功能模塊
(1)空間機(jī)器人系統(tǒng)動(dòng)力學(xué)模塊，包括空間機(jī)器人系統(tǒng)的多體動(dòng)力學(xué)模型、軌道動(dòng)力學(xué)模型、軌道環(huán)境模型等； (2)關(guān)節(jié)模型包括關(guān)節(jié)i(i = l，. . . ，6)控制器、電機(jī)及其驅(qū)動(dòng)器模型、關(guān)節(jié)傳動(dòng)機(jī)構(gòu)模型等； (3)空間機(jī)械臂路徑規(guī)劃器包括視覺伺服控制、機(jī)械臂逆運(yùn)動(dòng)學(xué)、關(guān)節(jié)插值等算法； (4)基座姿態(tài)及軌道控制器AOCS :根據(jù)敏感器測(cè)量信息，對(duì)基座的位置、姿態(tài)進(jìn)行控制； (5)敏感器模型包括關(guān)節(jié)傳感器模型(根據(jù)不同的應(yīng)用情況，提供各個(gè)關(guān)節(jié)的位
置、速度、力矩等測(cè)量信息)、手眼視覺測(cè)量模型、基座姿態(tài)敏感器模型等。 各功能模塊之間的連接關(guān)系如圖2所示。本發(fā)明的空間機(jī)器人動(dòng)力學(xué)建模與仿真
系統(tǒng)組成如圖3所示。 二、單臂空間機(jī)器人系統(tǒng)多領(lǐng)域統(tǒng)一模型的建立 不失一般性，以由六自由度串聯(lián)機(jī)械臂和作為其基座的衛(wèi)星組成的單臂空間機(jī)器人為例，整個(gè)系統(tǒng)由七個(gè)剛體組成，分別記為B。 Be，其中B。為基座』6為末端執(zhí)行器。B卜工與Bi(i二l，…，6)之間通過旋轉(zhuǎn)關(guān)節(jié)二(i = 1，…，6)連接。所建立的空間機(jī)器人系統(tǒng)多領(lǐng)域統(tǒng)一模型如圖3所示，包括空間機(jī)器人系統(tǒng)的多剛體動(dòng)力學(xué)模型SpaceRobot、關(guān)節(jié)軸模型Axisl Axis6、空間機(jī)器人路徑規(guī)劃器Planner、基座的姿態(tài)軌道控制器A0CS、敏感器Sensors、飛輪Flywheel、推力器Thruster、軌道動(dòng)力學(xué)0rbitDynAndDis等模塊組成。
(1)空間機(jī)器人系統(tǒng)的機(jī)構(gòu)模型 為方便空間機(jī)器人系統(tǒng)的建模，首先建立如圖4所示的坐標(biāo)系(相應(yīng)于機(jī)械臂的折疊位置，此時(shí)定義為關(guān)節(jié)角的零位)，其中坐標(biāo)系E b，即坐標(biāo)系0bXbYbZb為基座的參考坐標(biāo)系，三軸對(duì)地模式下0bXb指向飛行方向、0bZb指向地心，0bYb根據(jù)右手定則確定；E 。為基座的質(zhì)心坐標(biāo)系，指向與E b—致；E Ji = 1，…，6)為桿件i的固連坐標(biāo)系，原點(diǎn)位于第i個(gè)關(guān)節(jié)二，折疊狀態(tài)下指向與E b —致；E e為機(jī)械臂末端工具坐標(biāo)系，折疊狀態(tài)下0eXe垂直于基座+Z面并指向Z軸，0eZe沿手爪軸向指向外。 按下面的步驟建立空間機(jī)器人系統(tǒng)機(jī)構(gòu)部分的模型 (a)建立重力場(chǎng)和世界坐標(biāo)系 任何機(jī)械模型的建立，都首先要建立慣性坐標(biāo)系和重力場(chǎng)。將MultiBody庫中的World圖標(biāo)拖到當(dāng)前模型編輯窗中，雙擊該圖標(biāo)可對(duì)相關(guān)參數(shù)進(jìn)行設(shè)置i)重力類型"gravityType"選擇"UniformGravity"，ii)重力加速度g賦值為0，即g = 0 ;iii)重力矢量方向定義為慣性系的Z軸。World模塊同時(shí)建立了一個(gè)慣性系，后續(xù)各桿件的建立以此為參考。 (b)建立基座及其與慣性系的約束 基座是整個(gè)多體系統(tǒng)中的第一個(gè)剛體，通過定義其質(zhì)量、慣量、質(zhì)心位置、與慣性系的約束關(guān) 系和與下一個(gè)剛體的連接關(guān)系，可完整反映其運(yùn)動(dòng)狀態(tài)。首先創(chuàng)建一個(gè)剛體，命名為B。，并給B。的相關(guān)參數(shù)賦值。雙擊該圖標(biāo)，彈出的對(duì)話框中，"General"界面定義質(zhì)量特性(矢量r相當(dāng)于iLi ;rJM相當(dāng)于、；m即為Mass ;1_11為Ixx， 1_22為Iyy， 1_33為Izz， I_21為Iyx， I_31為Izx， I_32為Izy ;"Animation"界面可定義幾何外形，在〃 sh即eType" —欄選擇B。的形狀，標(biāo)準(zhǔn)形狀包括矩形〃 box"、球形〃 sphere"、圓柱形〃 cylinder"等；對(duì)于復(fù)雜的形狀，可以由用戶自行定義?；男螤钣晌募?.dxf確定，因此〃 sh即eType"一欄輸入"0〃即可。 由于基座在空間具有6自由度運(yùn)動(dòng)能力，因此，BO與慣性系之間通過約束"FreeMotion"連接，表明基座的位置、姿態(tài)均可改變，同時(shí)，定義基座控制力、力矩的輸入接口 Tb、fb，其中fb作用于基座質(zhì)心，因此需要定義一個(gè)固定平移的坐標(biāo)變換，引出基座質(zhì)心坐標(biāo)系。 (c)建立機(jī)械臂各關(guān)節(jié)及連桿的模型 基座定義好后，即可定義關(guān)節(jié)衛(wèi)，其旋轉(zhuǎn)軸為Z軸；由于衛(wèi)是可驅(qū)動(dòng)的旋轉(zhuǎn)關(guān)節(jié)，故用〃 ActuatedRevolute〃進(jìn)行定義，并引出其驅(qū)動(dòng)軸接口 axisl，該接口包含了力矩、旋轉(zhuǎn)角信息。然后定義B"其動(dòng)力學(xué)參數(shù)、幾何外形的定義與B。的定義類似，機(jī)械臂各桿的形狀分別由文件l.dxf，…，6. dxf等定義。 接著定義J2 (旋轉(zhuǎn)軸為-Y軸)、B2 ; J3 (旋轉(zhuǎn)軸為-Y軸)、B3 ; J4 (旋轉(zhuǎn)軸為_X軸)、B4 ;J5(旋轉(zhuǎn)軸為-Y軸)、B5 ;J6(旋轉(zhuǎn)軸為X軸)、B6，以及各關(guān)節(jié)的驅(qū)動(dòng)軸接口 axis2 axis6。
目標(biāo)衛(wèi)星按類似于空間機(jī)器人基座的方法建立。最后所建立的空間機(jī)機(jī)器人機(jī)構(gòu)部分的模型如圖5所示，折疊狀態(tài)下機(jī)械臂的3D視圖如圖6所示。[OO64] (2)空間機(jī)器人系統(tǒng)軌道動(dòng)力學(xué)及環(huán)境[OOes] (a)軌道動(dòng)力學(xué)的建模 記質(zhì)點(diǎn)在慣性系下的位置為巧，在中心天體體固系下的慣性加速度 ，慣性系到
中心天體體固系的坐標(biāo)旋轉(zhuǎn)矩陣為C^，反之為C^，則有 ^C正flE((Vt) (1) 上式中，求導(dǎo)為在慣性系下的導(dǎo)數(shù)，括號(hào)表示aE為CEiri的函數(shù)。將上式積分即可得到質(zhì)點(diǎn)在慣性系下的位置速度。 追蹤星和目標(biāo)星的軌道坐標(biāo)系分別記為0。iHZ。i、 0。2X。2Y。2Z。2，兩航天器的相對(duì)追蹤星質(zhì)心在目標(biāo)星軌道坐標(biāo)系0。2X。2Y。2Z。2中的坐標(biāo))r。 = [x， y， z]，相對(duì)速度
。在兩航天器相距較近，且均運(yùn)行在近圓軌道的條件下，相對(duì)運(yùn)動(dòng)可用Hill方程
^簡(jiǎn)化描述
i + 2必少=,/附i
j)-2^:-3"2"y;//^ (2)
S +必22 = /2 / W
其中，(fx，fy， fz)為施加在追蹤星上的控制力(在目標(biāo)星軌道坐標(biāo)系下的投影)。 追蹤星的質(zhì)量，"為軌道角速度，對(duì)圓軌道來說，"為常值。 (b)空間環(huán)境的建模
對(duì)于低軌道航天器，主要的空間環(huán)境干擾包括氣動(dòng)力矩、太陽輻射力矩、剩磁力 力梯度力矩。氣動(dòng)力、力矩表示為
式中
￡B -—質(zhì)心到衛(wèi)星中心體壓心的矢徑 SB-—星體的迎流面積
V， P -—?dú)饬魉俾蚀笮『蛠砹鞣较虻膯挝皇? 太陽輻射壓力、力矩
(3)
(4)
(5)
—F 2
式中
Fe—太陽常數(shù)1358W/m2 C-一真空中光速 6 -—受曬面外法向的單位矢量 e i—_入射角 Si-—受曬面面積 N-—受曬面?zhèn)€數(shù)
《-一 整星質(zhì)心到第i個(gè)受曬面壓心的矢徑 剩磁力矩表示為
(6)
(7)
式中
-一整星的剩磁矩
5 -—衛(wèi)星所處位置上的地磁場(chǎng)強(qiáng)度，由星上實(shí)時(shí)軌道計(jì)算和地磁場(chǎng)模型計(jì)算得到。




重力梯度力矩為
f,3叫v(/.g (8)
式中
W。-一軌道角速度 -一地垂單位矢量 I-一衛(wèi)星轉(zhuǎn)動(dòng)慣量陣
在對(duì)地指向三軸穩(wěn)定姿態(tài)情況下，衛(wèi)星重力梯度力矩可根據(jù)下式計(jì)算<formula>formula see original document page 12</formula> 式中(c13， c23， c33) = (_sin 9 ， sin 4> cos 9 ， cos 4> cos 9 )。
(3)機(jī)械臂關(guān)節(jié)軸的建模 機(jī)械臂關(guān)節(jié)軸包含了關(guān)節(jié)控制器、電機(jī)及其驅(qū)動(dòng)器、諧波減速器、關(guān)節(jié)位置敏感器 等，模型的總體組成如圖7所示?？刂破鲗?shí)現(xiàn)了位置環(huán)和速度環(huán)的控制，電流環(huán)的控制在 "電機(jī)及其驅(qū)動(dòng)器模型"中實(shí)現(xiàn)。其中，位置環(huán)采用PD控制，而速度環(huán)采用PI控制，各模塊可 從Modelica. Blocks. Continuous庫中選取后，對(duì)相關(guān)參數(shù)進(jìn)行賦值來實(shí)現(xiàn)。電機(jī)模型中包 含了電樞電阻Ra、電樞電感La、反電動(dòng)勢(shì)emf 、電機(jī)軸Jmotor等環(huán)節(jié)，驅(qū)動(dòng)器部分由電阻R、 電容C、運(yùn)算放大器0p、電壓源Vs及接地g等組成，除Jmotor夕卜，其它部分可在Modelica. Electrical. Analog. Basic中選取。電機(jī)相關(guān)參數(shù)及模型如圖8所示。關(guān)節(jié)傳動(dòng)機(jī)構(gòu)部分 一般為諧波減速器、齒輪減速箱等，是連接電機(jī)軸和關(guān)節(jié)軸的中間部分。為反映真實(shí)情況， 該部分的建模由庫倫摩擦bearingFrition、彈性阻尼器springDamper，以及理想減速模型 idearGear三部分組成，如圖9所示。
(4)機(jī)械臂路徑規(guī)劃器 機(jī)械臂的路徑規(guī)劃器用于規(guī)劃期望的關(guān)節(jié)角、角速度軌跡，作為關(guān)節(jié)控制器的輸 入。根據(jù)不同的任務(wù)，可采用不同的路徑規(guī)劃方法，如關(guān)節(jié)空間點(diǎn)到點(diǎn)PTP路徑規(guī)劃、關(guān)節(jié) 空間連續(xù)CP路徑規(guī)劃、笛卡爾點(diǎn)到點(diǎn)路徑規(guī)劃、笛卡爾空間連續(xù)路徑規(guī)劃，以及基于視覺 的自主路徑規(guī)劃(視覺伺服控制)等。以關(guān)節(jié)空間點(diǎn)到點(diǎn)路徑規(guī)劃為例，采用五次多項(xiàng)式 對(duì)規(guī)劃關(guān)節(jié)i(i = 1，…，6)在
時(shí)間段內(nèi)的運(yùn)動(dòng)，即:
9 i = aists+a^+a^ts+a^+ant+aw (10) 其中，9i為關(guān)節(jié)i的運(yùn)動(dòng)角度，ai。 ^為五次多項(xiàng)數(shù)的待定參數(shù)，t為時(shí)間。相 應(yīng)的關(guān)節(jié)角速度和角加速度分別為 <formula>formula see original document page 12</formula> (11)
<formula>formula see original document page 12</formula> (12)
有下列約束條件<formula>formula see original document page 12</formula>(13) <formula>formula see original document page 12</formula>




可解出
<formula>formula see original document page 13</formula>
(16)
(17)
(18)
利用Modelica語言實(shí)現(xiàn)如上的路徑規(guī)劃算法。 (5)基座姿態(tài)控制執(zhí)行機(jī)構(gòu)的建模
飛輪實(shí)質(zhì)上是一個(gè)帶有大轉(zhuǎn)動(dòng)慣量的力矩馬達(dá)，由驅(qū)動(dòng)電路、電機(jī)及輪體組成。以
右
"三軸正交安裝+ —等傾角斜裝"的飛輪組件為例，其多領(lǐng)域模型如圖ll所示，由驅(qū)動(dòng)電沒 電機(jī)及其驅(qū)動(dòng)電路(Motor 1 Motor4)、軸承摩擦(bearingFrictionl bearingFriction4)、 轉(zhuǎn)動(dòng)關(guān)節(jié)(Jx， Jy，Jz， Js)、輪體(Bx，By，Bz，Bs)，以及坐標(biāo)轉(zhuǎn)換關(guān)系(T1 T4)。其中，T1 T4 分別建立了各飛輪安裝坐標(biāo)系相對(duì)于基座質(zhì)心坐標(biāo)系的位置和姿態(tài)(frame_al與系統(tǒng)質(zhì) 心坐標(biāo)系CM直接連接)，而旋轉(zhuǎn)關(guān)節(jié)定義了各輪體與基座之間的旋轉(zhuǎn)關(guān)系，電機(jī)及其軸承 的輸出軸與關(guān)節(jié)的驅(qū)動(dòng)軸相連。飛輪系統(tǒng)的3D模型如圖ll所示。 推力器用于基座姿態(tài)、軌道的6D0F控制。每一個(gè)推力器的推力矢量表示為fi、作 用點(diǎn)矢量表示為ri，推力脈沖表示為t i，則推力器的作用等效于作用于質(zhì)心的作用力及力 矩。其中作用力
<formula>formula see original document page 13</formula>
相應(yīng)的作用力矩
Ti"i) = riXFi (20) 多個(gè)推力器的合成作用按矢量和計(jì)算 柳=-

(19)
(21)
(22) 推力器組件的模型如圖12所示。
(6)基座A0CS系統(tǒng)的建模 姿態(tài)控制采用PID+前饋補(bǔ)償?shù)牟呗?，控制框圖如圖13所示?？刂坡扇缦?
Tc = Kp qe+K! / qe+Kd (" d_ " b) +TB (23) 其中，Kp、 K工、Kd分別為控制器的比例、積分、微分控制參數(shù)，qe為姿態(tài)四元數(shù)誤差， "d為期望的姿態(tài)角速度，"b為實(shí)際通過姿態(tài)敏感器測(cè)出角速度；Te為補(bǔ)償力矩；T。為期望 的作用于基座的控制力矩，該力矩通過飛輪的反作用力矩來實(shí)現(xiàn)。飛輪組的方向矩陣為
<formula>formula see original document page 14</formula>
(24)令四個(gè)飛輪的控制電流分別為^ i4，組成的矢量為U二 [i"i2，i3，i4]1。飛輪組
中僅選擇三個(gè)參與控制，如果第i個(gè)飛輪不參與控制，則由三軸姿態(tài)控制的指令力矩i;分 配各個(gè)飛輪的控制電壓如下(其中負(fù)號(hào)表示作用于飛輪的力矩為作用于星體的力矩符號(hào)
相反) <formula>formula see original document page 14</formula> -(25) 其中，K為電機(jī)的力矩常數(shù)組成的4X4對(duì)角陣，Ci為令矩陣(24)中的第i列全為 0后得到的矩陣，C「1為&的廣義逆。例如，若第四個(gè)飛輪不用于控制，則
即三個(gè)正交飛輪分別完成三個(gè)軸的姿態(tài)控制。當(dāng)其中一個(gè)出現(xiàn)故障時(shí)，斜裝飛輪
<formula>formula see original document page 14</formula>(26)
將用于備份。如假設(shè)X軸飛輪出現(xiàn)故障，則按下式分配各飛輪的控制電沒
右<formula>formula see original document page 14</formula>(27)

-我^ —
其中T。x， T。y， T。z為控制力矩T。在各軸的分量。 (7)敏感器的建模
敏感器用于提供控制器的測(cè)量信息。在Modelica的多體庫中有一些現(xiàn)成的敏感 器包MultiBody. Sensors,但提供的是理想的相對(duì)/絕對(duì)位置、姿態(tài)、線速度、角速度等，而 實(shí)際中的敏感器是有測(cè)量誤差的，因此，通過在理想敏感器的基礎(chǔ)上疊加測(cè)量數(shù)據(jù)，實(shí)現(xiàn)真 實(shí)敏感器的建模。以機(jī)械臂的手眼相機(jī)為例，首先用RelativeSensor (路徑為MultiBody. Sensors. RelativeSensor)提供理想的位置、姿態(tài)測(cè)量，然后疊加相機(jī)的測(cè)量噪聲，其中測(cè) 量噪聲為零均值的高斯噪聲，位置、姿態(tài)測(cè)量的標(biāo)準(zhǔn)差分別為
op = 1. 2X10—3 (28)
o。 = 0.25 (29) 隨機(jī)數(shù)采用自定義的隨機(jī)函數(shù)RandomNormal (Time)實(shí)現(xiàn)，該函數(shù)的Modelica程 序如下 function randn" random" /氺__y = randn(seed， std)__氺/0151]
0152]
0153]
0154]
0155]
0156]
0157]
0158]
0159]
0160] position
0161] attitude
0162]
input Real seed ; input Real std ; output Real y ; algorithm
y : = RandomNormal(seed)*std ; end :randn j model randnBlk import Modelica. Constants, pi ; parameter Integer皿m = 6 ;
parameter Real stdPose =1.2'
The stendard deviation of the
parameter Real stdAtt = 0. 25〃 The standard deviation of the
(stdPose氺le—3， stdPose氺le—3，
皿m
SpaceRobotLibNew. MathFcn.
final parameter Real std[皿m] stdPose*le_3， stdAtt*pi/180. 0， 0163] stdAtt*pi/180. 0， stdAtt*pi/180. 0}; 0164] Modelica. Blocks. Interfaces. 0utPort OutSig(n
0165] annotation(extent = [100， _10 ;120，10]);
0166] equation 0167] for i in 1 :皿m loop
0168] OutSig. signal [i]
屋dn(time+(i-l)承10， std[i]); 0169] end for ;
0170] end randnBlk ;
0171] 最后建立的手眼相機(jī)敏感器模型如圖14所示。其它敏感器的建模過程與此類似。 0172] 三、多臂空間機(jī)器人系統(tǒng)的多領(lǐng)域統(tǒng)一模型
0173] 上面所建立的機(jī)械模型、關(guān)節(jié)軸模型、規(guī)劃器模型等具有可重用性，因此，在建立 單臂空間機(jī)器人系統(tǒng)多領(lǐng)域模型的基礎(chǔ)上，可方便的建立雙臂空間機(jī)器人系統(tǒng)的多領(lǐng)域模 型。 假設(shè)在飛行基座上對(duì)稱安裝了兩套完全一致的空間機(jī)械臂，除圖5所示的連桿關(guān) 系外，另一臂的第一個(gè)關(guān)節(jié)位于基座參考系下(1. 1，0， -0.712)的位置，其安裝坐標(biāo)系相對(duì) 于基座參考系的姿態(tài)為(0，0，180° )(本文的所有姿態(tài)角采用3-2-1歐拉角的形式)。因 此，將臂1的第一個(gè)桿件B工坐標(biāo)系(E》進(jìn)行平移、旋轉(zhuǎn)后，可得到臂2的第一個(gè)桿件87 坐標(biāo)系(E 7)，艮P定義平移MultiBody. Parts. FixedTranslation、旋轉(zhuǎn)MultiBody. Parts. FixedRotation可實(shí)現(xiàn)將E !到E 7的變換-Translate (Z，-0. 712X 2) 、Rotate (X， 180° )。 由此，將原臂1的BrBe、jrJe、Axisl Axis6同時(shí)復(fù)制，并進(jìn)行合適的連線，可完成雙臂空間 機(jī)器人機(jī)構(gòu)部分的建模?；诟髂K的可重用性，所建立的包含關(guān)節(jié)各軸模型、路徑規(guī)劃模 ±央，以及基座GNC模塊的整個(gè)雙臂空間機(jī)器人系統(tǒng)的多領(lǐng)域模型如圖15所示，相應(yīng)的3D圖 示如圖16所示。
1權(quán)利要求
空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于包括空間機(jī)器人路徑規(guī)劃器(1)、關(guān)節(jié)軸模塊(2)、空間機(jī)器人手眼相機(jī)測(cè)量模塊(3)、空間機(jī)器人機(jī)構(gòu)模塊(4)、世界坐標(biāo)系及中心體重力場(chǎng)(5)、軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)、空間機(jī)器人基座敏感器模塊(7)、推進(jìn)模塊(8)、反作用飛輪組件(10)、空間機(jī)器人基座姿軌控模塊(9)。其中空間機(jī)器人路徑規(guī)劃器(1)，接收來自于手眼視覺測(cè)量模塊(3)的相對(duì)位置、姿態(tài)測(cè)量結(jié)果，自主規(guī)劃?rùn)C(jī)械臂和基座的運(yùn)動(dòng)軌跡——期望關(guān)節(jié)角、角速度、角加速度、基座姿態(tài)角、角速度，作為關(guān)節(jié)軸模塊(2)和空間機(jī)器人基座姿軌控模塊(9)的輸入?？臻g機(jī)器人路徑規(guī)劃器(1)還能實(shí)現(xiàn)各種笛卡爾空間、關(guān)節(jié)空間的規(guī)劃算法，包括梯形插值、三次樣條、多項(xiàng)式插值等常規(guī)路徑規(guī)劃，以及機(jī)械臂與基座的協(xié)調(diào)規(guī)劃等，根據(jù)不同的任務(wù)要求，可選擇合適的路徑規(guī)劃算法；關(guān)節(jié)軸模塊(2)由機(jī)械臂所有關(guān)節(jié)軸組成，每個(gè)關(guān)節(jié)軸包括關(guān)節(jié)控制器、關(guān)節(jié)控制器、電機(jī)及其驅(qū)動(dòng)器、諧波減速器、關(guān)節(jié)敏感器。關(guān)節(jié)控制器接收任務(wù)規(guī)劃器(1)輸出的期望關(guān)節(jié)角、角速度、角加速度，以及關(guān)節(jié)敏感器的當(dāng)前關(guān)節(jié)角、角速度、電流，實(shí)現(xiàn)位置環(huán)、速度環(huán)、電流環(huán)的控制算法，產(chǎn)生關(guān)節(jié)控制力矩，通過諧波減速器環(huán)節(jié)后作用在空間機(jī)器人機(jī)構(gòu)模型(4)上；空間機(jī)器人機(jī)構(gòu)模塊(4)包括空間機(jī)器人系統(tǒng)多剛體機(jī)構(gòu)模型、目標(biāo)衛(wèi)星單剛體模型。該模塊接收關(guān)節(jié)軸模塊(2)輸出的關(guān)節(jié)驅(qū)動(dòng)力拒、反作用飛輪組件(10)輸出的基座姿態(tài)控制力矩、以及軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)輸出的干擾力矩，計(jì)算作用后的機(jī)械臂各關(guān)節(jié)角、角速度，基座姿態(tài)、角速度，以及目標(biāo)衛(wèi)星姿態(tài)、位置，輸出作為手眼視覺測(cè)量模塊(3)、軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)/基座敏感器(6)以及關(guān)節(jié)軸模塊(2)中的關(guān)節(jié)敏感器的輸入；手眼視覺測(cè)量模塊(3)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的機(jī)械臂末端位置、姿態(tài)，以及目標(biāo)衛(wèi)星的位置、姿態(tài)，計(jì)算目標(biāo)衛(wèi)星相對(duì)于機(jī)械臂末端坐標(biāo)系的位置、姿態(tài)，該位置、姿態(tài)數(shù)據(jù)疊加上相機(jī)測(cè)量噪聲數(shù)據(jù)后成為手眼視覺測(cè)量數(shù)據(jù)，作為該模塊的輸出、空間機(jī)器人路徑規(guī)劃器(1)的輸入；世界坐標(biāo)系及中心體重力場(chǎng)(5)，建立世界坐標(biāo)系與系統(tǒng)本體系的關(guān)系——指向、原點(diǎn)相對(duì)位置，以及中心體的重力場(chǎng)，可實(shí)現(xiàn)不同中心體下的空間機(jī)器人系統(tǒng)多領(lǐng)域統(tǒng)一動(dòng)力學(xué)的建模與仿真研究。該模塊與空間機(jī)器人機(jī)構(gòu)模塊(4)相連。軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的機(jī)器人基座本體系相對(duì)于慣性系的姿態(tài)，以及推進(jìn)模塊(8)輸出的推力脈沖，計(jì)算空間機(jī)器人系統(tǒng)質(zhì)心的位置、基座本體系相對(duì)于軌道坐標(biāo)系的姿態(tài)、角速度、所處軌道位置的磁場(chǎng)強(qiáng)度以及環(huán)境干擾力矩，作為敏感器模塊(7)、空間機(jī)器人基座姿軌控模塊(9)，以及空間機(jī)器人機(jī)構(gòu)模塊(4)的輸入；空間機(jī)器人基座敏感器模塊(7)，接收空間機(jī)器人機(jī)構(gòu)模塊(4)輸出的基座本體系相對(duì)于慣性坐標(biāo)系的姿態(tài)、角速度，以及軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)輸出的基座本體系相對(duì)于軌道坐標(biāo)系的姿態(tài)、角速度，疊加測(cè)量噪聲后作為敏感器的輸出，該輸出是空間機(jī)器人基座姿軌控模塊(9)的輸入；空間機(jī)器人基座姿軌控模塊(9)，接收基座姿態(tài)敏感器(7)輸出的當(dāng)前姿態(tài)角、角速度，以及空間機(jī)器人路徑規(guī)劃器(1)輸出的期望姿態(tài)角、角速度，執(zhí)行航天器的各種導(dǎo)航、制導(dǎo)與控制算法，如對(duì)目標(biāo)衛(wèi)星進(jìn)行跟蹤、接近、繞飛、軌道保持等的GNC算法，以及常規(guī)模式下自身姿態(tài)、軌道的控制算法(三軸穩(wěn)定、自旋轉(zhuǎn)定、軌道機(jī)動(dòng)、姿態(tài)機(jī)動(dòng)等)等，生成反作用飛輪組件(10)和推進(jìn)系統(tǒng)(8)的控制指令，其中反作用飛輪組件的控制指令為控制電壓、推進(jìn)系統(tǒng)的控制指令為推力脈沖；推進(jìn)系統(tǒng)模塊(8)，接收空間機(jī)器人基座姿軌控模塊(9)輸出的推力脈沖，產(chǎn)生各推力器的作用力，作用于軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)；反作用飛輪組件(10)，接收空間機(jī)器人基座姿軌控模塊(9)輸出的各推力器控制電壓，產(chǎn)生各個(gè)飛輪的作用力矩，作用于空間機(jī)器人機(jī)構(gòu)模塊(4)。
2. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的空間機(jī)器人路徑規(guī)劃器(1)采用多領(lǐng)域統(tǒng)一建模語言Modelica實(shí)現(xiàn)了一種空間機(jī)器人 目標(biāo)捕獲的自主路徑規(guī)劃方法，該方法利用手眼相機(jī)的相對(duì)位姿測(cè)量值，實(shí)時(shí)規(guī)劃空間機(jī) 器人的運(yùn)動(dòng)，以最終捕獲目標(biāo)。主要包括如下步驟位姿偏差計(jì)算、目標(biāo)運(yùn)動(dòng)的預(yù)測(cè)、空間 機(jī)器人末端運(yùn)動(dòng)速度規(guī)劃、空間機(jī)器人避奇異的路徑規(guī)劃、基座運(yùn)動(dòng)的預(yù)測(cè)等。首先，根據(jù) 手眼測(cè)量數(shù)據(jù)判斷相對(duì)位姿偏差ep和e。是否小于設(shè)定的閾值e。和e。，若小于，則閉合手 爪、捕獲目標(biāo)；反之，則根據(jù)相對(duì)位姿偏差，實(shí)時(shí)估計(jì)目標(biāo)的運(yùn)動(dòng)狀態(tài)，并將估計(jì)的結(jié)果反應(yīng) 到機(jī)械臂末端速度的規(guī)劃中，以保證機(jī)械臂末端時(shí)刻朝最近的方向(直線)趨近目標(biāo)，機(jī)械 臂末端能自主跟蹤目標(biāo)的運(yùn)動(dòng)，直到最后捕獲目標(biāo)。規(guī)劃出末端運(yùn)動(dòng)速度后，即調(diào)用自主奇 異回避算法，以解算關(guān)節(jié)的期望角速度，并據(jù)此預(yù)測(cè)機(jī)械臂運(yùn)動(dòng)對(duì)基座的擾動(dòng)，當(dāng)擾動(dòng)超出 容許的范圍時(shí)，則自動(dòng)調(diào)整機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)速度，以保證期望的偏轉(zhuǎn)在許可的范圍內(nèi)。整 個(gè)過程一直持續(xù)到機(jī)械臂捕獲到目標(biāo)為止。
3. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的關(guān)節(jié)軸模塊(2)采用多領(lǐng)域統(tǒng)一建模語言Modelica建立了機(jī)械臂各關(guān)節(jié)的機(jī)械、電氣、 控制等多學(xué)科領(lǐng)域一體的模型，每個(gè)關(guān)節(jié)軸模型由關(guān)節(jié)控制器、電機(jī)及其驅(qū)動(dòng)器、關(guān)節(jié)傳動(dòng) 機(jī)構(gòu)、關(guān)節(jié)敏感器等組成。關(guān)節(jié)控制器實(shí)現(xiàn)了位置環(huán)和速度環(huán)的控制，其中，位置環(huán)采用PD 控制，而速度環(huán)采用PI控制；電機(jī)模型中包含了電樞電阻Ra、電樞電感La、反電動(dòng)勢(shì)emf 、 電機(jī)軸Jmotor等環(huán)節(jié)；驅(qū)動(dòng)器部分由電阻R、電容C、運(yùn)算放大器Op、電壓源(Vs)及接地 (g)等組成；關(guān)節(jié)傳動(dòng)機(jī)構(gòu)包括諧波減速器、齒輪減速箱等，是連接電機(jī)軸和關(guān)節(jié)軸的中間 部分，該部分的建模由庫倫摩擦bearingFrition、彈性阻尼器springDamper，以及理想減 速模型三部分組成；
4. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的空間機(jī)器人手眼相機(jī)測(cè)量模塊(3)采用多領(lǐng)域統(tǒng)一建模語言Modelica的多體敏感器庫 MultiBody. Sensors中的相對(duì)運(yùn)動(dòng)敏感器RelativeSensor，疊加上相機(jī)測(cè)量噪聲，作為手 眼相機(jī)的測(cè)量數(shù)據(jù)；
5. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的空間機(jī)器人機(jī)構(gòu)模塊(4)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫空間機(jī)器人系統(tǒng)及目 標(biāo)衛(wèi)星的多個(gè)剛體屬性，以及剛體間的約束實(shí)現(xiàn)。每個(gè)剛體的屬性包括質(zhì)量、慣量、質(zhì)心位 置、坐標(biāo)系a和坐標(biāo)系b，其中質(zhì)量、慣量、質(zhì)心位置為剛體的質(zhì)量特性參數(shù)，坐標(biāo)系a、坐標(biāo) 系b則用于定義該剛體與相應(yīng)約束的連接關(guān)系；剛體間的約束用于描述相連剛體間的相對(duì) 運(yùn)動(dòng)關(guān)系，空間機(jī)器人基座與機(jī)械臂第一個(gè)連桿，以及機(jī)械臂各連桿之間為旋轉(zhuǎn)關(guān)節(jié)，而目標(biāo)衛(wèi)星與慣性坐標(biāo)系之間為6D0F的自由運(yùn)動(dòng)，通過Modelica多體庫中的FreeMotion實(shí) 現(xiàn)；
6. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的世界坐標(biāo)系及中心體重力場(chǎng)(5)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫，建立世界坐標(biāo) 系與系統(tǒng)本體系的關(guān)系，以及地球的微重力場(chǎng)；
7. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)采用多領(lǐng)域統(tǒng)一建模語言Modelica編寫，實(shí)現(xiàn)兩星的 相對(duì)軌道動(dòng)力學(xué)方程——Hill方程，以及軌道環(huán)境干擾力、干擾力矩，包括太陽光壓力/力 矩、大氣拖動(dòng)力/力矩、剩磁力矩等；
8. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的空間機(jī)器人基座敏感器模塊(7)采用多領(lǐng)域統(tǒng)一建模語言Modelica的多體敏感器庫 MultiBody. Sensors中的相對(duì)運(yùn)動(dòng)敏感器RelativeSensor,通過設(shè)置輸出項(xiàng)、再疊加上相 應(yīng)姿態(tài)敏感器的測(cè)量噪聲，作為基座敏感器的測(cè)量數(shù)據(jù)；
9. 根據(jù)權(quán)利要求1所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的推進(jìn)模塊(8)、反作用飛輪組件(10)采用多領(lǐng)域統(tǒng)一建模語言Modelica建立，其中反作 用飛輪組件(10)包括4個(gè)反作用飛輪，采用"三軸正交安裝+ —等傾角斜裝"，通過設(shè)置可 工作于整星零動(dòng)量或偏置動(dòng)量模式，單個(gè)飛輪的建模由驅(qū)動(dòng)電路、電機(jī)及輪體組成；
10. 根據(jù)權(quán)利要求l所述的空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，其特征在于所述 的空間機(jī)器人基座姿軌控模塊(9)采用多領(lǐng)域統(tǒng)一建模語言Modelica實(shí)現(xiàn)相應(yīng)的姿態(tài)、軌 道控制算法，產(chǎn)生執(zhí)行機(jī)構(gòu)的控制指令——四個(gè)飛輪的控制電壓，以及推進(jìn)系統(tǒng)的推力脈 沖，對(duì)基座進(jìn)行6D0F的控制，控制基座姿態(tài)、軌道按期望的軌跡運(yùn)動(dòng)。
全文摘要
空間機(jī)器人多領(lǐng)域統(tǒng)一建模與仿真系統(tǒng)，由空間機(jī)器人路徑規(guī)劃器(1)、關(guān)節(jié)軸模塊(2)、空間機(jī)器人手眼相機(jī)測(cè)量模塊(3)、空間機(jī)器人機(jī)構(gòu)模塊(4)、世界坐標(biāo)系及中心體重力場(chǎng)(5)、軌道動(dòng)力學(xué)及空間環(huán)境模塊(6)、空間機(jī)器人基座敏感器模塊(7)、推進(jìn)模塊(8)、反作用飛輪組件(10)及空間機(jī)器人基座姿軌控模塊(9)組成。各模型庫采用多領(lǐng)域物理系統(tǒng)建模語言Modelica開發(fā)，徹底實(shí)現(xiàn)了機(jī)械、電氣、軟件、控制等不同領(lǐng)域模型之間的無縫集成和數(shù)據(jù)交換，實(shí)現(xiàn)多學(xué)科優(yōu)化設(shè)計(jì)的目標(biāo)?；谠摻Ｅc仿真系統(tǒng)，可方便地實(shí)現(xiàn)自由飛行、自由漂浮模式下，單臂、多臂空間機(jī)器人的建模與仿真。
文檔編號(hào)B25J9/00GK101733749SQ200910073470
公開日2010年6月16日 申請(qǐng)日期2009年12月22日 優(yōu)先權(quán)日2009年12月22日
發(fā)明者徐文福, 李成, 梁斌, 王學(xué)謙, 齊海萍 申請(qǐng)人:哈爾濱工業(yè)大學(xué)