衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器的制造方法
【專利摘要】本發(fā)明提供一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，包括：GNC模擬器、機(jī)械臂模擬器、動(dòng)力學(xué)仿真機(jī)、星務(wù)遙測模擬器和綜合接口箱?？蓪?shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂之間的耦合仿真，可分析機(jī)械臂運(yùn)動(dòng)與平臺(tái)控制之間的相互影響關(guān)系，所得結(jié)果較模擬器獨(dú)立仿真具有更好的可信度；另外，該模擬器中衛(wèi)星平臺(tái)和機(jī)械臂分系統(tǒng)的相對獨(dú)立，模擬器間的數(shù)據(jù)格式與實(shí)際衛(wèi)星系統(tǒng)中的一致，因而可以接入真實(shí)的星上計(jì)算機(jī)，不僅可以驗(yàn)證系統(tǒng)算法的有效性，也可以驗(yàn)證系統(tǒng)設(shè)計(jì)的可靠性，具有可信度高、可擴(kuò)展性好、靈活性強(qiáng)的優(yōu)點(diǎn)。
【專利說明】衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于衛(wèi)星仿真測試【技術(shù)領(lǐng)域】，具體涉及一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真。
【背景技術(shù)】
[0002]空間機(jī)器人在未來空間任務(wù)中具有廣闊的應(yīng)用前景，是一種重要的空間飛行器。由于空間機(jī)器人技術(shù)復(fù)雜、工作環(huán)境特殊，地面測試驗(yàn)證是其系統(tǒng)設(shè)計(jì)中的必需環(huán)節(jié)，有效的空間機(jī)器人地面測試驗(yàn)證方法對于空間機(jī)器人系統(tǒng)至關(guān)重要。空間機(jī)器人主要由衛(wèi)星平臺(tái)和機(jī)械臂兩部分組成，兩部分之間的相互作用和影響對空間機(jī)器人的特性具有較大影響，是地面仿真測試必須考慮的因素。
[0003]目前，國內(nèi)外在進(jìn)行空間機(jī)器人地面測試驗(yàn)證時(shí)，一般有幾種(I)全數(shù)字仿真方法。該方法的優(yōu)點(diǎn)是簡單、快速、成本低，但系統(tǒng)的接口無法與真實(shí)系統(tǒng)一致，無法接入真實(shí)系統(tǒng)樣機(jī)進(jìn)行仿真，系統(tǒng)的有效性難以充分驗(yàn)證；(2)獨(dú)立模擬器仿真，針對衛(wèi)星平臺(tái)和模擬器分別開展仿真，只能從局部了解分系統(tǒng)的特性，無法考慮空間平臺(tái)和機(jī)械臂之間的動(dòng)力學(xué)耦合關(guān)系，不能反映系統(tǒng)的整體特性；(3)氣浮臺(tái)試驗(yàn)，雖然能較為真實(shí)的測試系統(tǒng)的有效性，但試驗(yàn)設(shè)計(jì)系統(tǒng)復(fù)雜，試驗(yàn)成本較高，且該方法適用范圍較窄，僅能對特定條件下系統(tǒng)情況進(jìn)行試驗(yàn)。
[0004]因此，需要一種能夠模擬空間平臺(tái)與機(jī)械臂相互作用影響的可信度高、容易實(shí)施和擴(kuò)展、可接入真實(shí)星上計(jì)算機(jī)的地面驗(yàn)證測試的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器。
[0005]目前國內(nèi) 外文獻(xiàn)尚未有相關(guān)報(bào)道。

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

[0006]本發(fā)明技術(shù)解決問題:克服現(xiàn)有技術(shù)的不足，提供一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，可實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂之間的耦合仿真，仿真具有更好的可信度；而且不僅可以驗(yàn)證系統(tǒng)算法的有效性，也可以驗(yàn)證系統(tǒng)設(shè)計(jì)的可靠性，具有可信度高、可擴(kuò)展性好、靈活性強(qiáng)的優(yōu)點(diǎn)。
[0007]本發(fā)明所采用的技術(shù)方案如下:一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，包括:GNC模擬器、機(jī)械臂模擬器、動(dòng)力學(xué)仿真機(jī)、星務(wù)遙測模擬器和綜合接口箱；按照飛行任務(wù)流程，衛(wèi)星平臺(tái)設(shè)置了姿態(tài)捕獲、對地定向、逼近?？?、機(jī)械臂操作和和快速穩(wěn)定5個(gè)任務(wù)模式，GNC模擬器預(yù)存了每個(gè)任務(wù)模式的轉(zhuǎn)入條件、姿態(tài)和軌道控制算法、每個(gè)任務(wù)模式下所使用的執(zhí)行機(jī)構(gòu)列表，執(zhí)行機(jī)構(gòu)包括飛輪、磁力矩計(jì)和推力器，同時(shí)也預(yù)存了所有執(zhí)行機(jī)構(gòu)的安裝位置、控制力和控制力矩大小信息；
[0008]所述的GNC模擬器，用于模擬星載GNC系統(tǒng)的功能；GNC模擬器具體實(shí)現(xiàn)如下:
[0009](I)GNC模擬器啟動(dòng)后，首先進(jìn)入姿態(tài)捕獲任務(wù)模式；在仿真過程中，GNC模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的GNC指令；收到GNC指令后，根據(jù)指令中包含的任務(wù)模式字獲取預(yù)存的任務(wù)模式轉(zhuǎn)入條件，并判斷轉(zhuǎn)入條件是否滿足；如果滿足轉(zhuǎn)入條件，則在下一控制周期開始切換到對應(yīng)的任務(wù)模式，并載入任務(wù)模式對應(yīng)的姿態(tài)和軌道控制算法；如果不滿足轉(zhuǎn)入條件，則丟棄收到的遙控指令；
[0010](2)GNC模擬器利用模擬器內(nèi)部時(shí)間作為基準(zhǔn)進(jìn)行周期控制，等待時(shí)間達(dá)到預(yù)定控制時(shí)間時(shí)，開始一個(gè)控制周期的仿真；在GNC模擬器的每一控制周期內(nèi)，首先通過CAN總線采集動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星狀態(tài)信息，所述衛(wèi)星狀態(tài)信息包括姿態(tài)敏感器、交會(huì)測量敏感器、機(jī)械臂關(guān)節(jié)狀態(tài)敏感器的測量信息；所述機(jī)械臂關(guān)節(jié)狀態(tài)包括關(guān)節(jié)角度和角速度；
[0011](3)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定姿態(tài)和軌道控制算法，以衛(wèi)星狀態(tài)信息為輸入，進(jìn)行衛(wèi)星平臺(tái)姿態(tài)和軌道控制計(jì)算，求解所需要的控制力和控制力矩；
[0012](4)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定衛(wèi)星執(zhí)行機(jī)構(gòu)列表，獲得執(zhí)行機(jī)構(gòu)列表中包含的飛輪、磁力矩器和推力器的安裝位置、控制力和控制力矩，進(jìn)行執(zhí)行機(jī)構(gòu)分配求解，確定需要執(zhí)行機(jī)構(gòu)列表中各執(zhí)行機(jī)構(gòu)的開關(guān)時(shí)間及控制力和力矩大小，將步驟(3)產(chǎn)生的推力和力矩指令轉(zhuǎn)化為飛輪、磁力矩器和推力器的開/關(guān)指令和動(dòng)作電流，即形成執(zhí)行機(jī)構(gòu)指令，并發(fā)送至CAN總線；
[0013](5)按照衛(wèi)星遙測數(shù)據(jù)格式，生成GNC狀態(tài)信息發(fā)送至CAN總線，完成本控制周期的仿真，等待內(nèi)部始終到達(dá)下一控制周期預(yù)定的控制時(shí)間后，開始下一周期的仿真；
[0014]所述的機(jī)械臂模擬器，用于模擬機(jī)械臂任務(wù)規(guī)劃系統(tǒng)的功能；按照飛行任務(wù)流程，實(shí)際系統(tǒng)設(shè)置了多個(gè)預(yù)先設(shè)定的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列，稱為機(jī)械臂任務(wù)模式；機(jī)械臂模擬器預(yù)存了機(jī)械臂任務(wù)模式信息。機(jī)械臂模擬器具體實(shí)現(xiàn)如下:
[0015](I)在機(jī)械臂模擬器每個(gè)控制周期，首先從任務(wù)模式列表讀取下一模式開始時(shí)間和任務(wù)模式字，如果時(shí)間到達(dá)模式開始時(shí)間，則切換機(jī)械臂任務(wù)模式至對應(yīng)的任務(wù)模式；
[0016](2)然后從CAN總線接收由動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星平臺(tái)位置和姿態(tài)、當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值；根據(jù)當(dāng)前機(jī)械臂關(guān)節(jié)角和角速度，利用機(jī)械臂運(yùn)動(dòng)學(xué)關(guān)系，計(jì)算得到機(jī)械臂位于機(jī)械臂末端的手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置、姿態(tài)、速度和角速度；
[0017](3)根據(jù)衛(wèi)星平臺(tái)的位置和姿態(tài)、手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置和姿態(tài)，通過坐標(biāo)變換，計(jì)算當(dāng)前時(shí)刻太陽、手爪以及衛(wèi)星平臺(tái)之間的相對位置關(guān)系，確定手爪的光照條件；然后根據(jù)安裝于機(jī)械臂手爪的手眼相機(jī)的視場角范圍、手爪的光照條件，基于OpenScene Graph模擬手眼相機(jī)對安裝于平臺(tái)上的手柄成像進(jìn)行模擬，生成手眼相機(jī)模擬圖像；利用圖像識(shí)別算法，對所成圖像進(jìn)行處理，求解得到手爪相對于手柄的相對位置測量值；
[0018](4)機(jī)械臂模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的機(jī)械臂動(dòng)作指令，并記錄指令中包含的任務(wù)模式字和任務(wù)開始時(shí)間，形成任務(wù)模式列表；根據(jù)當(dāng)前的機(jī)械臂任務(wù)模式，從預(yù)存的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列讀取本控制周期結(jié)束時(shí)的期望關(guān)節(jié)狀態(tài)；將期望關(guān)節(jié)狀態(tài)與當(dāng)前機(jī)械臂的關(guān)節(jié)狀態(tài)測量值作差值，得到本控制周期的機(jī)械臂關(guān)節(jié)狀態(tài)的變化量；將控制周期時(shí)間內(nèi)均勻取若干個(gè)時(shí)間點(diǎn)，從當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值到期望關(guān)節(jié)狀態(tài)進(jìn)行插值，得到各時(shí)間點(diǎn)的期望關(guān)節(jié)狀態(tài)，形成當(dāng)前控制周期的期望關(guān)節(jié)狀態(tài)序列，并發(fā)送至CAN總線。
[0019]所述動(dòng)力學(xué)仿真機(jī)，用于實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂復(fù)合動(dòng)力學(xué)計(jì)算，軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量仿真，以及機(jī)械臂控制過程仿真功能；具體實(shí)現(xiàn)如下:
[0020](I)動(dòng)力學(xué)仿真機(jī)啟動(dòng)后，首先通過以太網(wǎng)與綜合接口箱建立連接，之后開始監(jiān)控以太網(wǎng)，接收由綜合接口箱發(fā)送的數(shù)據(jù)。
[0021](2)接收綜合接口箱轉(zhuǎn)發(fā)的執(zhí)行機(jī)構(gòu)指令后，計(jì)算飛輪、推力器和磁力矩器的輸出力和力矩；
[0022](3)接收綜合接口箱轉(zhuǎn)發(fā)的本周期機(jī)械臂期望關(guān)節(jié)狀態(tài)序列后，利用機(jī)械臂關(guān)節(jié)角度測量結(jié)果求得誤差信息，采用ro控制算法，計(jì)算機(jī)械臂各關(guān)節(jié)的控制力矩；
[0023](4)考慮6X6階攝動(dòng)項(xiàng)、大氣阻力攝動(dòng)、光壓攝動(dòng)因素計(jì)算衛(wèi)星所受的環(huán)境力和力矩，與執(zhí)行機(jī)構(gòu)輸出力和力矩合成，得到衛(wèi)星所受的外力和外力矩，進(jìn)行衛(wèi)星平臺(tái)軌道動(dòng)力學(xué)計(jì)算，生成衛(wèi)星平臺(tái)軌道參數(shù)；
[0024](5)采用拉格朗日方法建立平臺(tái)和機(jī)械臂復(fù)合動(dòng)力學(xué)模型，將衛(wèi)星所受的外力和外力矩及機(jī)械臂各關(guān)節(jié)控制力矩作為輸入，采用8階龍格-庫塔方法進(jìn)行數(shù)值積分求解。由于衛(wèi)星的姿態(tài)控制周期較大，為了提高復(fù)合動(dòng)力學(xué)計(jì)算的精度，對每個(gè)控制周期內(nèi)分為100步作為動(dòng)力學(xué)計(jì)算的積分步長，得到控制周期內(nèi)各時(shí)間點(diǎn)的衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)；
[0025](6)將衛(wèi)星平臺(tái)軌道參數(shù)、衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)輸入至軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量模型，計(jì)算得到衛(wèi)星狀態(tài)信息，按照通訊格式的要求組成數(shù)據(jù)幀并通過以太網(wǎng)發(fā)送至綜合接口箱。
[0026]所述星務(wù)遙測模擬器，用于為仿真模擬器提供用戶輸入輸出界面，實(shí)現(xiàn)地面指令的輸入和發(fā)送，并完成遙測數(shù)據(jù)的打包存儲(chǔ)；具體實(shí)現(xiàn)如下:
[0027](I)星務(wù)遙測模擬器通過用戶界面設(shè)置遙控指令輸入，當(dāng)收到新指令后，首先完成遙控指令解析，分析指令的內(nèi)容和參數(shù)，并將指令按照衛(wèi)星實(shí)際遙控指令格式進(jìn)行打包；
[0028](2)如果是GNC指令，則通過CAN總線發(fā)送至GNC模擬器；如果是機(jī)械臂動(dòng)作指令，則通過CAN總線發(fā)送至機(jī)械臂模擬器；
[0029](3)在仿真過程中，星務(wù)遙測模擬器通過輪詢方式檢測CAN總線的數(shù)據(jù)，由于CAN總線具有廣播特性，因此GNC模擬器、機(jī)械臂模擬器和綜合接口箱發(fā)送的信息都被星務(wù)遙測模擬器收到，包括執(zhí)行機(jī)構(gòu)指令、GNC狀態(tài)信息、衛(wèi)星狀態(tài)信息和期望關(guān)節(jié)狀態(tài)序列；星務(wù)遙測模擬器收到上述數(shù)據(jù)后，從中挑選構(gòu)建遙測數(shù)據(jù)包需要的數(shù)據(jù)，并按照星上遙測數(shù)據(jù)格式進(jìn)行重新打包，形成遙測數(shù)據(jù)包存儲(chǔ)在星務(wù)遙測模擬器硬盤中，用于仿真后查閱；
[0030]所述綜合接口箱，用于完成所述的動(dòng)力學(xué)仿真機(jī)的數(shù)據(jù)格式轉(zhuǎn)換；它一端連接于CAN總線，通過CAN總線與GNC模擬器、機(jī)械臂模擬器及星務(wù)遙測模擬器進(jìn)行數(shù)據(jù)交換；另一端通過以太網(wǎng)連接于動(dòng)力學(xué)仿真機(jī)；綜合接口箱啟動(dòng)后，首先監(jiān)聽以太網(wǎng)端口，等待動(dòng)力學(xué)仿真機(jī)的連接，建立連接后，通過兩個(gè)線程分別監(jiān)聽CAN總線和以太網(wǎng)的數(shù)據(jù)，當(dāng)收到以太網(wǎng)數(shù)據(jù)，則按照CAN總線協(xié)議進(jìn)行打包，并發(fā)送至CAN總線；當(dāng)收到CAN總線數(shù)據(jù)，則按照以太網(wǎng)協(xié)議進(jìn)行打包，通過以太網(wǎng)發(fā)送至動(dòng)力學(xué)仿真機(jī)。
[0031]所述仿真模擬器還包括:網(wǎng)絡(luò)集線器，所述網(wǎng)絡(luò)集線器用于連接所述綜合接口箱和所述動(dòng)力學(xué)仿真機(jī)。
[0032]所述星務(wù)遙測模擬器采用windows操作系統(tǒng)，具有CAN總線接口。
[0033]所述GNC模擬器、機(jī)械臂模擬器均采用實(shí)時(shí)操作系統(tǒng)，具有CAN總線接口。所述綜合接口箱采用實(shí)時(shí)操作系統(tǒng)，具有CAN總線和以太網(wǎng)接口。所述動(dòng)力學(xué)仿真機(jī)采用實(shí)時(shí)操作系統(tǒng)，與所述的綜合接口箱之間采用以太網(wǎng)連接，數(shù)據(jù)符合以太網(wǎng)協(xié)議。[0034]本發(fā)明的有益效果如下:
[0035](I)本發(fā)明提供的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，可實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂之間的耦合仿真，分析機(jī)械臂運(yùn)動(dòng)與平臺(tái)控制之間的相互影響，所得結(jié)果較模擬器獨(dú)立仿真具有更好的可信性；
[0036](2)本發(fā)明該模擬器中衛(wèi)星平臺(tái)和機(jī)械臂分系統(tǒng)的相對獨(dú)立性，模擬器間的數(shù)據(jù)格式又與實(shí)際衛(wèi)星系統(tǒng)中的一致，因而可以接入真實(shí)的星上計(jì)算機(jī)，不僅可以驗(yàn)證系統(tǒng)算法的有效性，也可以驗(yàn)證系統(tǒng)設(shè)計(jì)的可靠性，具有可信度高、可擴(kuò)展性好、靈活性強(qiáng)的優(yōu)
占.[0037](3)本發(fā)明綜合利用了物理仿真和數(shù)學(xué)仿真的優(yōu)點(diǎn)，相比氣浮臺(tái)試驗(yàn)適用范圍更廣，相比數(shù)字仿真可信度更高。
【專利附圖】

【附圖說明】
[0038]圖1為本發(fā)明衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器的結(jié)構(gòu)示意圖；
[0039]圖2為本發(fā)明中GNC模擬器實(shí)現(xiàn)流程圖；
[0040]圖3為本發(fā)明中機(jī)械臂模擬器實(shí)現(xiàn)流程圖；
[0041]圖4為本發(fā)明中動(dòng)力學(xué)仿真機(jī)實(shí)現(xiàn)流程圖；
[0042]圖5為本發(fā)明中星務(wù)遙測模擬器實(shí)現(xiàn)流程圖。
【具體實(shí)施方式】
[0043]如圖1所示，本發(fā)明一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，包括:GNC模擬器、機(jī)械臂模擬器、動(dòng)力學(xué)仿真機(jī)、星務(wù)遙測模擬器和綜合接口箱。
[0044]下面詳細(xì)介紹本發(fā)明各組成部分的實(shí)現(xiàn)過程。
[0045]1.GNC 模擬器
[0046]本發(fā)明實(shí)施提供的GNC模擬器，主要用于模擬衛(wèi)星平臺(tái)GNC系統(tǒng)的功能。GNC模擬器外部接口與真實(shí)星上計(jì)算機(jī)相同，因此可以直接接入星上計(jì)算機(jī)進(jìn)行仿真，則整個(gè)星上計(jì)算機(jī)硬件和軟件均可以得到測試，既可以反映GNC系統(tǒng)的設(shè)計(jì)，又可以反映GNC系統(tǒng)的實(shí)現(xiàn)，提聞了系統(tǒng)的有效性。
[0047]GNC模擬器軟件可以以星上軟件為基礎(chǔ)，設(shè)計(jì)運(yùn)行在VxWorks系統(tǒng)上，采用C語言進(jìn)行編碼，采用中斷方式響應(yīng)綜合接口箱的通訊要求，在中斷服務(wù)程序中調(diào)用CAN總線通訊中斷處理函數(shù)，完成數(shù)據(jù)收發(fā)。
[0048]按照飛行任務(wù)流程，衛(wèi)星平臺(tái)設(shè)置了姿態(tài)捕獲、對地定向、逼近?？?、機(jī)械臂操作和快速穩(wěn)定5個(gè)任務(wù)模式，GNC模擬器預(yù)存了每個(gè)任務(wù)模式的轉(zhuǎn)入條件、姿態(tài)和軌道控制算法、每個(gè)任務(wù)模式下所使用的執(zhí)行機(jī)構(gòu)列表，執(zhí)行機(jī)構(gòu)包括飛輪、磁力矩計(jì)、推力器3類，同時(shí)也預(yù)存了所有執(zhí)行機(jī)構(gòu)的安裝位置、控制力和控制力矩大小信息。
[0049]姿態(tài)捕獲模式用于衛(wèi)星平臺(tái)入軌后快速建立對地姿態(tài)。在星箭分離后自動(dòng)轉(zhuǎn)入姿態(tài)捕獲模式，無需發(fā)送指令。此模式下采用飛輪、推力器組合對衛(wèi)星平臺(tái)進(jìn)行姿態(tài)控制，不進(jìn)行軌道控制。姿態(tài)控制算法采用ro控制方法。
[0050]對地定向模式用于實(shí)現(xiàn)衛(wèi)星平臺(tái)的對地三軸穩(wěn)定控制。姿態(tài)捕獲模式、快速穩(wěn)定模式下、逼近?？磕Ｊ胶蜋C(jī)械臂操作模式均可自動(dòng)轉(zhuǎn)入對地定向模式。此模式下執(zhí)行機(jī)構(gòu)采用飛輪和磁力矩器組合對衛(wèi)星平臺(tái)進(jìn)行姿態(tài)控制，不進(jìn)行軌道控制。姿態(tài)控制算法采用相平面控制方法。
[0051]逼近?？磕Ｊ接糜趯?shí)現(xiàn)衛(wèi)星平臺(tái)對目標(biāo)衛(wèi)星的接近過程的軌道和姿態(tài)控制。僅當(dāng)衛(wèi)星平臺(tái)處于對地定向模式下時(shí)，才可以轉(zhuǎn)入逼近?？磕Ｊ?。此模式下采用飛輪、推力器組合對衛(wèi)星平臺(tái)進(jìn)行姿態(tài)和軌道控制，姿態(tài)控制算法采用ro控制方法，軌道控制采用c-w方法。
[0052]機(jī)械臂操作模式用于機(jī)械臂動(dòng)作期間對衛(wèi)星平臺(tái)的姿態(tài)控制。僅當(dāng)衛(wèi)星平臺(tái)處于對地定向模式下時(shí)，才可以轉(zhuǎn)入機(jī)械臂操作模式。此模式下采用推力器對衛(wèi)星平臺(tái)進(jìn)行姿態(tài)控制，不進(jìn)行軌道控制，姿態(tài)控制算法采用ro控制方法。
[0053]快速穩(wěn)定模式用于快速消除平臺(tái)的姿態(tài)角和角速度偏差，回到對地定向狀態(tài)。僅當(dāng)衛(wèi)星平臺(tái)處于對地定向模式下時(shí)，才可以轉(zhuǎn)入快速穩(wěn)定模式。此模式下采用推力器對衛(wèi)星平臺(tái)進(jìn)行姿態(tài)控制，不進(jìn)行軌道控制，姿態(tài)控制算法采用滑?？刂品椒ā?br> [0054]如圖2所示，GNC模擬器具體實(shí)現(xiàn)如下:
[0055](I)GNC模擬器啟動(dòng)后，首先進(jìn)入姿態(tài)捕獲任務(wù)模式；在仿真過程中，GNC模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的GNC指令；收到GNC指令后，根據(jù)指令中包含的任務(wù)模式字獲取預(yù)存的任務(wù)模式轉(zhuǎn)入條件，并判斷轉(zhuǎn)入條件是否滿足；如果滿足轉(zhuǎn)入條件，則在下一控制周期開始切換到對應(yīng)的任務(wù)模式，并載入任務(wù)模式對應(yīng)的姿態(tài)和軌道控制算法；如果不滿足轉(zhuǎn)入條件，則丟棄收到的遙控指令；
[0056](2)GNC模擬器利用模擬器內(nèi)部時(shí)間作為基準(zhǔn)進(jìn)行周期控制，等待時(shí)間達(dá)到預(yù)定控制時(shí)間時(shí)，開始一個(gè)控制周期的仿真。在GNC模擬器的每一控制周期內(nèi)，首先通過CAN總線采集動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星狀態(tài)信息，包括姿態(tài)敏感器、交會(huì)測量敏感器、機(jī)械臂關(guān)節(jié)狀態(tài)(包括關(guān)節(jié)角度和角速度)敏感器的測量信息；
[0057](3)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定姿態(tài)和軌道控制算法，以衛(wèi)星狀態(tài)信息為輸入，進(jìn)行衛(wèi)星平臺(tái)姿態(tài)和軌道控制計(jì)算，求解所需要的控制力和控制力矩；
[0058](4)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定衛(wèi)星執(zhí)行機(jī)構(gòu)列表，獲得執(zhí)行機(jī)構(gòu)列表中包含的飛輪、磁力矩器和推力器的安裝位置、控制力和控制力矩，進(jìn)行執(zhí)行機(jī)構(gòu)分配求解，確定需要執(zhí)行機(jī)構(gòu)列表中各執(zhí)行機(jī)構(gòu)的開關(guān)時(shí)間及控制力和力矩大小，將步驟(3)產(chǎn)生的推力和力矩指令轉(zhuǎn)化為飛輪、磁力矩器和推力器的開/關(guān)指令和動(dòng)作電流，即形成執(zhí)行機(jī)構(gòu)指令，并發(fā)送至CAN總線，完成本控制周期的仿真，等待內(nèi)部始終到達(dá)下一控制周期預(yù)定的控制時(shí)間后，開始下一周期的仿真。
[0059]2.機(jī)械臂模擬器
[0060]所述的機(jī)械臂模擬器，用于模擬機(jī)械臂任務(wù)規(guī)劃系統(tǒng)的功能；機(jī)械臂模擬器外部接口與真實(shí)星上計(jì)算機(jī)相同，因此可以直接接入星上計(jì)算機(jī)進(jìn)行仿真，則整個(gè)星上計(jì)算機(jī)硬件和軟件均可以得到測試，提高了系統(tǒng)的有效性。同時(shí)，在系統(tǒng)設(shè)計(jì)的初期，星上計(jì)算機(jī)還沒有完成之前，可以構(gòu)建模擬系統(tǒng)進(jìn)行測試。隨著系統(tǒng)設(shè)計(jì)的進(jìn)展，模擬系統(tǒng)逐漸完善，直至替換為真實(shí)系統(tǒng)。在這個(gè)過程中，所有的改變僅涉及機(jī)械臂模擬器和綜合接口箱，系統(tǒng)其它部分不需要改變，體現(xiàn)了系統(tǒng)具有較好的擴(kuò)展性。
[0061]按照飛行任務(wù)流程，實(shí)際系統(tǒng)設(shè)置了多個(gè)預(yù)先設(shè)定的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列，稱為機(jī)械臂任務(wù)模式；機(jī)械臂模擬器預(yù)存了機(jī)械臂任務(wù)模式信息。[0062]如圖3所示，機(jī)械臂模擬器具體實(shí)現(xiàn)如下:
[0063](I)在機(jī)械臂模擬器每個(gè)控制周期，首先從任務(wù)模式列表讀取下一模式開始時(shí)間和任務(wù)模式字，如果時(shí)間到達(dá)模式開始時(shí)間，則切換機(jī)械臂任務(wù)模式至對應(yīng)的任務(wù)模式；
[0064](2)然后從CAN總線接收由動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星平臺(tái)位置和姿態(tài)、當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值；根據(jù)當(dāng)前機(jī)械臂關(guān)節(jié)角和角速度，利用機(jī)械臂運(yùn)動(dòng)學(xué)關(guān)系，計(jì)算得到機(jī)械臂位于機(jī)械臂末端的手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置、姿態(tài)、速度和角速度；
[0065](3)根據(jù)衛(wèi)星平臺(tái)的位置和姿態(tài)、手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置和姿態(tài)，通過坐標(biāo)變換，計(jì)算當(dāng)前時(shí)刻太陽、手爪以及衛(wèi)星平臺(tái)之間的相對位置關(guān)系，確定手爪的光照條件；然后根據(jù)安裝于機(jī)械臂手爪的手眼相機(jī)的視場角范圍、手爪的光照條件，基于OpenScene Graph模擬手眼相機(jī)對安裝于平臺(tái)上的手柄成像進(jìn)行模擬，生成手眼相機(jī)模擬圖像；利用圖像識(shí)別算法，對所成圖像進(jìn)行處理，求解得到手爪相對于手柄的相對位置測量值；
[0066](4)機(jī)械臂模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的機(jī)械臂動(dòng)作指令，并記錄指令中包含的任務(wù)模式字和任務(wù)開始時(shí)間，形成任務(wù)模式列表；根據(jù)當(dāng)前的機(jī)械臂任務(wù)模式，從預(yù)存的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列讀取本控制周期結(jié)束時(shí)的期望關(guān)節(jié)狀態(tài)；將期望關(guān)節(jié)狀態(tài)與當(dāng)前機(jī)械臂的關(guān)節(jié)狀態(tài)測量值作差值，得到本控制周期的機(jī)械臂關(guān)節(jié)狀態(tài)的變化量；將控制周期時(shí)間內(nèi)均勻取10個(gè)時(shí)間點(diǎn)，從當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值到期望關(guān)節(jié)狀態(tài)進(jìn)行插值，得到各時(shí)間點(diǎn)的期望關(guān)節(jié)狀態(tài)，形成當(dāng)前控制周期的期望關(guān)節(jié)狀態(tài)序列，并發(fā)送至CAN總線。
[0067]3.綜合接口箱
[0068]本發(fā)明實(shí)施提供的綜合接口箱安裝以太網(wǎng)卡和多個(gè)CAN總線接口卡，內(nèi)部為實(shí)時(shí)工控機(jī)，系統(tǒng)采用VxWorks實(shí)時(shí)操作系統(tǒng)。其主要作用為:完成所述的動(dòng)力學(xué)仿真機(jī)的數(shù)據(jù)格式轉(zhuǎn)換；它一端連接于CAN總線，可通過CAN總線與GNC模擬器、機(jī)械臂模擬器及星務(wù)遙測模擬器進(jìn)行數(shù)據(jù)交換；另一端通過以太網(wǎng)連接于動(dòng)力學(xué)仿真機(jī)。綜合接口箱啟動(dòng)后，首先監(jiān)聽以太網(wǎng)端口，等待動(dòng)力學(xué)仿真機(jī)的連接。建立連接后，通過兩個(gè)線程分別監(jiān)聽CAN總線和以太網(wǎng)的數(shù)據(jù)，一旦收到以太網(wǎng)數(shù)據(jù)，則按照CAN總線協(xié)議進(jìn)行打包，并發(fā)送至CAN總線。反之一旦收到CAN總線數(shù)據(jù)，則按照以太網(wǎng)協(xié)議進(jìn)行打包，通過以太網(wǎng)發(fā)送至動(dòng)力學(xué)仿真機(jī)。
[0069]經(jīng)過綜合接口箱的轉(zhuǎn)換，GNC模擬器、機(jī)械臂模擬器的接口與各自的星上計(jì)算機(jī)一致，解決了模擬器之間數(shù)據(jù)格式匹配問題，降低了模擬器之間的接口和數(shù)據(jù)耦合，提高了系統(tǒng)的可擴(kuò)展性。
[0070]4.動(dòng)力學(xué)仿真機(jī)
[0071]本發(fā)明實(shí)施提供的動(dòng)力學(xué)仿真機(jī)，用于實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂復(fù)合動(dòng)力學(xué)計(jì)算，軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量仿真，以及機(jī)械臂控制過程仿真功能。其中復(fù)合動(dòng)力學(xué)模塊基于多剛體動(dòng)力學(xué)建模，完成衛(wèi)星平臺(tái)和機(jī)械臂動(dòng)力學(xué)耦合計(jì)算，分析機(jī)械臂運(yùn)動(dòng)和平臺(tái)姿態(tài)運(yùn)動(dòng)之間的相互干擾作用，是實(shí)現(xiàn)GNC模擬器與機(jī)械臂模擬器進(jìn)行協(xié)同仿真的基礎(chǔ)。
[0072]動(dòng)力學(xué)仿真機(jī)設(shè)計(jì)運(yùn)行在VxWorks系統(tǒng)上，用C/C++進(jìn)行編碼，實(shí)現(xiàn)執(zhí)行機(jī)構(gòu)模型計(jì)算和敏感器模型計(jì)算、衛(wèi)星平臺(tái)和機(jī)械臂復(fù)合動(dòng)力學(xué)計(jì)算以及機(jī)械臂關(guān)節(jié)控制計(jì)算。干擾動(dòng)力學(xué)計(jì)算模塊建立了空間機(jī)器人的擬動(dòng)力學(xué)模型，用于分析機(jī)械臂運(yùn)動(dòng)產(chǎn)生的干擾力矩。
[0073]如圖4所示，動(dòng)力學(xué)仿真機(jī)具體實(shí)現(xiàn)如下:
[0074](I)動(dòng)力學(xué)仿真機(jī)啟動(dòng)后，首先通過以太網(wǎng)與綜合接口箱建立連接，之后開始監(jiān)控以太網(wǎng)，接收由綜合接口箱發(fā)送的數(shù)據(jù)；
[0075](2)接收綜合接口箱轉(zhuǎn)發(fā)的執(zhí)行機(jī)構(gòu)指令后，計(jì)算飛輪、推力器和磁力矩器的輸出力和力矩；
[0076](3)接收綜合接口箱轉(zhuǎn)發(fā)的本周期機(jī)械臂期望關(guān)節(jié)狀態(tài)序列后，利用機(jī)械臂關(guān)節(jié)角度測量結(jié)果求得誤差信息，采用ro控制算法，計(jì)算機(jī)械臂各關(guān)節(jié)的控制力矩；
[0077](4)考慮6X6階攝動(dòng)項(xiàng)、大氣阻力攝動(dòng)、光壓攝動(dòng)因素計(jì)算衛(wèi)星所受的環(huán)境力和力矩，與執(zhí)行機(jī)構(gòu)輸出力和力矩合成，得到衛(wèi)星所受的外力和外力矩，進(jìn)行衛(wèi)星平臺(tái)軌道動(dòng)力學(xué)計(jì)算，生成衛(wèi)星平臺(tái)軌道參數(shù)；
[0078](5)采用拉格朗日方法建立平臺(tái)和機(jī)械臂復(fù)合動(dòng)力學(xué)模型，將衛(wèi)星所受的外力和外力矩及機(jī)械臂各關(guān)節(jié)控制力矩作為輸入，采用8階龍格-庫塔方法進(jìn)行數(shù)值積分求解。由于衛(wèi)星的姿態(tài)控制周期較大，為了提高復(fù)合動(dòng)力學(xué)計(jì)算的精度，對每個(gè)控制周期內(nèi)分為100步作為動(dòng)力學(xué)計(jì)算的積分步長，得到控制周期內(nèi)各時(shí)間點(diǎn)的衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)；
[0079](6)將衛(wèi)星平臺(tái)軌道參數(shù)、衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)輸入至軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量模型，計(jì)算得到衛(wèi)星狀態(tài)信息，按照通訊格式的要求組成數(shù)據(jù)幀并通過以太網(wǎng)發(fā)送至綜合接口箱。
[0080]5.星務(wù)遙測模擬器
[0081]本發(fā)明實(shí)施提供的星務(wù)遙測模擬器的，用于為仿真模擬器提供用戶輸入輸出界面，實(shí)現(xiàn)地面指令的輸入和發(fā)送，并完成遙測數(shù)據(jù)的打包存儲(chǔ)。星務(wù)遙測模擬器具有遙控指令輸入界面，可以模擬地面遙控指令注入和注入?yún)?shù)指令，并通過CAN總線發(fā)送給GNC模擬器和機(jī)械手模擬器，控制模擬器的運(yùn)行流程。
[0082]選擇NI 公司的 Labview (Laboratory Virtual instrument Engineering)開發(fā)，運(yùn)行在Windows平臺(tái)。LabView是一款方便的可視化界面開發(fā)工具，其自身可提供多種圖形組件，數(shù)據(jù)通訊和數(shù)據(jù)庫接口，可以方便的開發(fā)模擬器監(jiān)控及數(shù)據(jù)顯示人機(jī)交互界面。其編譯生成的可執(zhí)行文件可直接在Windows環(huán)境下運(yùn)行。
[0083]如圖5所示，星務(wù)遙測模擬器具體實(shí)現(xiàn)如下:
[0084](I)星務(wù)遙測模擬器通過用戶界面設(shè)置遙控指令輸入，當(dāng)收到新指令后，首先完成遙控指令解析，分析指令的內(nèi)容和參數(shù)，并將指令按照衛(wèi)星實(shí)際遙控指令格式進(jìn)行打包；
[0085](2)如果是GNC指令，則通過CAN總線發(fā)送至GNC模擬器；如果是機(jī)械臂動(dòng)作指令，則通過CAN總線發(fā)送至機(jī)械臂模擬器；
[0086](3)在仿真過程中，星務(wù)遙測模擬器通過輪詢方式檢測CAN總線的數(shù)據(jù)，由于CAN總線具有廣播特性，因此GNC模擬器、機(jī)械臂模擬器和綜合接口箱發(fā)送的信息都可以被星務(wù)遙測模擬器收到，包括執(zhí)行機(jī)構(gòu)指令、衛(wèi)星狀態(tài)信息和期望關(guān)節(jié)狀態(tài)序列星務(wù)遙測模擬器收到上述數(shù)據(jù)后，從中挑選構(gòu)建遙測數(shù)據(jù)包需要的數(shù)據(jù)，并按照星上遙測數(shù)據(jù)格式進(jìn)行重新打包，形成遙測數(shù)據(jù)包存儲(chǔ)在星務(wù)遙測模擬器硬盤中，用于仿真后查閱。
[0087]由于現(xiàn)有實(shí)際空間機(jī)器人系統(tǒng)中，星上計(jì)算機(jī)是通過CAN總線進(jìn)行數(shù)據(jù)通訊的，因此，本實(shí)施例提供的星上計(jì)算機(jī)輸入的數(shù)據(jù)格式以符合CAN協(xié)議的數(shù)據(jù)格式為例進(jìn)行說明。但需要說明的是，本發(fā)明并不局限于此，當(dāng)星上實(shí)際系統(tǒng)間采用其他數(shù)據(jù)格式，例如:A類數(shù)據(jù)格式進(jìn)行通訊時(shí)，本發(fā)明提供的執(zhí)行器模擬器中，其輸入的數(shù)據(jù)格式也可以進(jìn)行相應(yīng)的變化，凡是符合該種構(gòu)想的情況均在本發(fā)明保護(hù)范圍之內(nèi)。
[0088]以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對于本發(fā)明【技術(shù)領(lǐng)域】的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視本發(fā)明的保護(hù)范圍。
【權(quán)利要求】
1.一種衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于，包括:GNC模擬器、機(jī)械臂模擬器、動(dòng)力學(xué)仿真機(jī)、星務(wù)遙測模擬器和綜合接口箱；按照飛行任務(wù)流程，衛(wèi)星平臺(tái)設(shè)置了姿態(tài)捕獲、對地定向、逼近停靠、機(jī)械臂操作和快速穩(wěn)定5個(gè)任務(wù)模式，GNC模擬器預(yù)存了每個(gè)任務(wù)模式的轉(zhuǎn)入條件、姿態(tài)和軌道控制算法、每個(gè)任務(wù)模式下所使用的執(zhí)行機(jī)構(gòu)列表，執(zhí)行機(jī)構(gòu)包括飛輪、磁力矩計(jì)和推力器，同時(shí)也預(yù)存了所有執(zhí)行機(jī)構(gòu)的安裝位置、控制力和控制力矩大小信息； 所述的GNC模擬器，用于模擬星載GNC系統(tǒng)的功能；GNC模擬器具體實(shí)現(xiàn)如下: (11)GNC模擬器啟動(dòng)后，首先進(jìn)入姿態(tài)捕獲任務(wù)模式；在仿真過程中，GNC模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的GNC指令；收到GNC指令后，根據(jù)指令中包含的任務(wù)模式字獲取預(yù)存的任務(wù)模式轉(zhuǎn)入條件，并判斷轉(zhuǎn)入條件是否滿足；如果滿足轉(zhuǎn)入條件，則在下一控制周期開始切換到對應(yīng)的任務(wù)模式，并載入任務(wù)模式對應(yīng)的姿態(tài)和軌道控制算法；如果不滿足轉(zhuǎn)入條件，則丟棄收到的遙控指令； (12)GNC模擬器利用模擬器內(nèi)部時(shí)間作為基準(zhǔn)進(jìn)行周期控制，等待時(shí)間達(dá)到預(yù)定控制時(shí)間時(shí)，開始一個(gè)控制周期的仿真；在GNC模擬器的每一控制周期內(nèi)，首先通過CAN總線采集動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星狀態(tài)信息，所述衛(wèi)星狀態(tài)信息包括姿態(tài)敏感器、交會(huì)測量敏感器、機(jī)械臂關(guān)節(jié)狀態(tài)敏感器的測量信息；所述機(jī)械臂關(guān)節(jié)狀態(tài)包括關(guān)節(jié)角度和角速度； (13)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定姿態(tài)和軌道控制算法，以衛(wèi)星狀態(tài)信息為輸入，進(jìn)行衛(wèi)星平臺(tái)姿態(tài)和軌道控制計(jì)算，求解所需要的控制力和控制力矩； (14)根據(jù)當(dāng)前控制周期的任務(wù)模式，確定衛(wèi)星執(zhí)行機(jī)構(gòu)列表，獲得執(zhí)行機(jī)構(gòu)列表中包含的飛輪、磁力矩器和推力器的安裝位置、控制力和控制力矩，進(jìn)行執(zhí)行機(jī)構(gòu)分配求解，確定需要執(zhí)行機(jī)構(gòu)列表中各執(zhí)行機(jī)構(gòu)的開關(guān)時(shí)間及控制力和力矩大小，將步驟(13)產(chǎn)生的推力和力矩指令轉(zhuǎn)化為飛輪、磁力矩器和推力器的開/關(guān)指令和動(dòng)作電流，即形成執(zhí)行機(jī)構(gòu)指令，并發(fā)送至CAN總線； (15)按照衛(wèi)星遙測數(shù)據(jù)格式，生成GNC狀態(tài)信息發(fā)送至CAN總線，完成本控制周期的仿真，等待內(nèi)部始終到達(dá)下一控制周期預(yù)定的控制時(shí)間后，開始下一周期的仿真； 所述的機(jī)械臂模擬器，用于模擬機(jī)械臂任務(wù)規(guī)劃系統(tǒng)的功能；按照飛行任務(wù)流程，實(shí)際系統(tǒng)設(shè)置了多個(gè)預(yù)先設(shè)定的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列，稱為機(jī)械臂任務(wù)模式；機(jī)械臂模擬器預(yù)存了機(jī)械臂任務(wù)模式信息。機(jī)械臂模擬器具體實(shí)現(xiàn)如下: (21)在機(jī)械臂模擬器每個(gè)控制周期，首先從任務(wù)模式列表讀取下一模式開始時(shí)間和任務(wù)模式字，如果時(shí)間到達(dá)模式開始時(shí)間，則切換機(jī)械臂任務(wù)模式至對應(yīng)的任務(wù)模式； (22)然后從CAN總線接收由動(dòng)力學(xué)仿真機(jī)發(fā)送的衛(wèi)星平臺(tái)位置和姿態(tài)、當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值；根據(jù)當(dāng)前機(jī)械臂關(guān)節(jié)角和角速度，利用機(jī)械臂運(yùn)動(dòng)學(xué)關(guān)系，計(jì)算得到機(jī)械臂位于機(jī)械臂末端的手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置、姿態(tài)、速度和角速度； (23)根據(jù)衛(wèi)星平臺(tái)的位置和姿態(tài)、手爪在衛(wèi)星平臺(tái)坐標(biāo)系中的相對位置和姿態(tài)，通過坐標(biāo)變換，計(jì)算當(dāng)前時(shí)刻太陽、手爪以及衛(wèi)星平臺(tái)之間的相對位置關(guān)系，確定手爪的光照條件；然后根據(jù)安裝于機(jī)械臂手爪的手眼相機(jī)的視場角范圍、手爪的光照條件，基于OpenScene Graph模擬手眼相機(jī)對安裝于平臺(tái)上的手柄成像進(jìn)行模擬，生成手眼相機(jī)模擬圖像；利用圖像識(shí)別算法，對所成圖像進(jìn)行處理，求解得到手爪相對于手柄的相對位置測量值； (24)機(jī)械臂模擬器采用獨(dú)立線程監(jiān)控CAN總線，隨時(shí)接收由星務(wù)遙測模擬器發(fā)送的機(jī)械臂動(dòng)作指令，并記錄指令中包含的任務(wù)模式字和任務(wù)開始時(shí)間，形成任務(wù)模式列表；根據(jù)當(dāng)前的機(jī)械臂任務(wù)模式，從預(yù)存的機(jī)械臂關(guān)節(jié)期望動(dòng)作序列讀取本控制周期結(jié)束時(shí)的期望關(guān)節(jié)狀態(tài)；將期望關(guān)節(jié)狀態(tài)與當(dāng)前機(jī)械臂的關(guān)節(jié)狀態(tài)測量值作差值，得到本控制周期的機(jī)械臂關(guān)節(jié)狀態(tài)的變化量；將控制周期時(shí)間內(nèi)均勻取若干個(gè)時(shí)間點(diǎn)，從當(dāng)前機(jī)械臂關(guān)節(jié)狀態(tài)測量值到期望關(guān)節(jié)狀態(tài)進(jìn)行插值，得到各時(shí)間點(diǎn)的期望關(guān)節(jié)狀態(tài)，形成當(dāng)前控制周期的期望關(guān)節(jié)狀態(tài)序列，并發(fā)送至CAN總線； 所述動(dòng)力學(xué)仿真機(jī)，用于實(shí)現(xiàn)衛(wèi)星平臺(tái)與機(jī)械臂復(fù)合動(dòng)力學(xué)計(jì)算，軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量仿真，以及機(jī)械臂控制過程仿真功能；具體實(shí)現(xiàn)如下: (31)動(dòng)力學(xué)仿真機(jī)啟動(dòng)后，首先通過以太網(wǎng)與綜合接口箱建立連接，之后開始監(jiān)控以太網(wǎng)，接收由綜合接口箱發(fā)送的數(shù)據(jù)； (32)接收綜合接口箱轉(zhuǎn)發(fā)的執(zhí)行機(jī)構(gòu)指令后，計(jì)算飛輪、推力器和磁力矩器的輸出力和力矩； (33)接收綜合接口箱轉(zhuǎn)發(fā)的本周期機(jī)械臂期望關(guān)節(jié)狀態(tài)序列后，利用機(jī)械臂關(guān)節(jié)角度測量結(jié)果求得誤差信息，采用ro控制算法，計(jì)算機(jī)械臂各關(guān)節(jié)的控制力矩； (34)考慮6X6階攝動(dòng)項(xiàng)、大氣阻力攝動(dòng)、光壓攝動(dòng)因素計(jì)算衛(wèi)星所受的環(huán)境力和力矩，與執(zhí)行機(jī)構(gòu)輸出力和力矩合成，得到衛(wèi)星所受的外力和外力矩，進(jìn)行衛(wèi)星平臺(tái)軌道動(dòng)力學(xué)計(jì)算，生成衛(wèi)星平臺(tái)軌道參數(shù)； (35)采用拉格朗日方法建立平臺(tái)和機(jī)械臂復(fù)合動(dòng)力學(xué)模型，將衛(wèi)星所受的外力和外力矩及機(jī)械臂各關(guān)節(jié)控制力矩作為輸入，采用8階龍格-庫塔方法進(jìn)行數(shù)值積分求解。由于衛(wèi)星的姿態(tài)控制周期較大，為了提高復(fù)合動(dòng)力學(xué)計(jì)算的精度，對每個(gè)控制周期內(nèi)分為100步作為動(dòng)力學(xué)計(jì)算的積分步長，得到控制周期內(nèi)各時(shí)間點(diǎn)的衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)； (36)將衛(wèi)星平臺(tái)軌道參數(shù)、衛(wèi)星平臺(tái)姿態(tài)、機(jī)械臂關(guān)節(jié)角和角速度參數(shù)輸入至軌道動(dòng)力學(xué)、姿態(tài)敏感器、交會(huì)敏感器和機(jī)械臂關(guān)節(jié)狀態(tài)敏感器測量模型，計(jì)算得到衛(wèi)星狀態(tài)信息，按照通訊格式的要求組成數(shù)據(jù)幀并通過以太網(wǎng)發(fā)送至綜合接口箱； 所述星務(wù)遙測模擬器，用于為仿真模擬器提供用戶輸入輸出界面，實(shí)現(xiàn)地面指令的輸入和發(fā)送，并完成遙測數(shù)據(jù)的打包存儲(chǔ)；具體實(shí)現(xiàn)如下: (41)星務(wù)遙測模擬器通過用戶界面設(shè)置遙控指令輸入，當(dāng)收到新指令后，首先完成遙控指令解析，分析指令的內(nèi)容和參數(shù)，并將指令按照衛(wèi)星實(shí)際遙控指令格式進(jìn)行打包； (42)如果是GNC指令，則通過CAN總線發(fā)送至GNC模擬器；如果是機(jī)械臂動(dòng)作指令，則通過CAN總線發(fā)送至機(jī)械臂模擬器； (43)在仿真過程中，星務(wù)遙測模擬器通過輪詢方式檢測CAN總線的數(shù)據(jù)，由于CAN總線具有廣播特性，因此GNC模擬器、機(jī)械臂模擬器和綜合接口箱發(fā)送的信息都被星務(wù)遙測模擬器收到，包括執(zhí)行機(jī)構(gòu)指令、GNC狀態(tài)信息、衛(wèi)星狀態(tài)信息和期望關(guān)節(jié)狀態(tài)序列；星務(wù)遙測模擬器收到上述數(shù) 據(jù)后，從中挑選構(gòu)建遙測數(shù)據(jù)包需要的數(shù)據(jù)，并按照星上遙測數(shù)據(jù)格式進(jìn)行重新打包，形成遙測數(shù)據(jù)包存儲(chǔ)在星務(wù)遙測模擬器硬盤中，用于仿真后查閱； 所述綜合接口箱，用于完成所述的動(dòng)力學(xué)仿真機(jī)的數(shù)據(jù)格式轉(zhuǎn)換；它一端連接于CAN總線，通過CAN總線與GNC模擬器、機(jī)械臂模擬器及星務(wù)遙測模擬器進(jìn)行數(shù)據(jù)交換；另一端通過以太網(wǎng)連接于動(dòng)力學(xué)仿真機(jī)；綜合接口箱啟動(dòng)后，首先監(jiān)聽以太網(wǎng)端口，等待動(dòng)力學(xué)仿真機(jī)的連接，建立連接后，通過兩個(gè)線程分別監(jiān)聽CAN總線和以太網(wǎng)的數(shù)據(jù)，當(dāng)收到以太網(wǎng)數(shù)據(jù)，則按照CAN總線協(xié)議進(jìn)行打包，并發(fā)送至CAN總線；當(dāng)收到CAN總線數(shù)據(jù)，則按照以太網(wǎng)協(xié)議進(jìn)行打包，通過以太網(wǎng)發(fā)送至動(dòng)力學(xué)仿真機(jī)。
2.根據(jù)權(quán)利要求1所述的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于:所述仿真模擬器還包括:網(wǎng)絡(luò)集線器，所述網(wǎng)絡(luò)集線器用于連接所述綜合接口箱和所述動(dòng)力學(xué)仿真機(jī)。
3.根據(jù)權(quán)利要求1所述的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于:所述星務(wù)遙測模擬器采用windows操作系統(tǒng)，具有CAN總線接口。
4.根據(jù)權(quán)利要求1所述的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于:所述GNC模擬器、機(jī)械臂模擬器均采用實(shí)時(shí)操作系統(tǒng)，具有CAN總線接口。
5.根據(jù)權(quán)利要求1所述的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于:所述綜合接口箱采用實(shí)時(shí)操作系統(tǒng)，具有CAN總線和以太網(wǎng)接口。
6.根據(jù)權(quán)利要求1所述的衛(wèi)星平臺(tái)與機(jī)械臂協(xié)同仿真模擬器，其特征在于，所述動(dòng)力學(xué)仿真機(jī)采用實(shí)時(shí)操作系統(tǒng)，與所述的綜合接口箱之間采用以太網(wǎng)連接，數(shù)據(jù)符合以太網(wǎng)協(xié)議。
【文檔編號(hào)】G05B17/02GK103970032SQ201410209200
【公開日】2014年8月6日 申請日期:2014年5月16日 優(yōu)先權(quán)日:2014年5月16日
【發(fā)明者】李智, 宋旭民, 陳勇, 林琪, 劉海洋, 樊鵬山 申請人:中國人民解放軍裝備學(xué)院