[發(fā)明專利]衛(wèi)星平臺與機械臂協(xié)同仿真模擬器

有效

• 申請?zhí)枺?/td>: 201410209200.8
• 申請日: 2014-05-16
• 公開（公告）號: CN103970032A
• 公開（公告）日: 2014-08-06
• 發(fā)明（設(shè)計）人: 李智;宋旭民;陳勇;林琪;劉海洋;樊鵬山
• 申請（專利權(quán)）人: 中國人民解放軍裝備學(xué)院
• 主分類號: G05B17/02
• 分類號: G05B17/02;G05B23/02
• 代理公司: 北京科迪生專利代理有限責(zé)任公司 11251
• 代理人: 楊學(xué)明;顧煒

1.一種衛(wèi)星平臺與機械臂協(xié)同仿真模擬器，其特征在于，包括：GNC模擬器、機械臂模擬器、動力學(xué)仿真機、星務(wù)遙測模擬器和綜合接口箱；按照飛行任務(wù)流程，衛(wèi)星平臺設(shè)置了姿態(tài)捕獲、對地定向、逼近停靠、機械臂操作和快速穩(wěn)定5個任務(wù)模式，GNC模擬器預(yù)存了每個任務(wù)模式的轉(zhuǎn)入條件、姿態(tài)和軌道控制算法、每個任務(wù)模式下所使用的執(zhí)行機構(gòu)列表，執(zhí)行機構(gòu)包括飛輪、磁力矩計和推力器，同時也預(yù)存了所有執(zhí)行機構(gòu)的安裝位置、控制力和控制力矩大小信息；

所述的GNC模擬器，用于模擬星載GNC系統(tǒng)的功能；GNC模擬器具體實現(xiàn)如下：

(11)GNC模擬器啟動后，首先進入姿態(tài)捕獲任務(wù)模式；在仿真過程中，GNC模擬器采用獨立線程監(jiān)控CAN總線，隨時接收由星務(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)部時間作為基準進行周期控制，等待時間達到預(yù)定控制時間時，開始一個控制周期的仿真；在GNC模擬器的每一控制周期內(nèi)，首先通過CAN總線采集動力學(xué)仿真機發(fā)送的衛(wèi)星狀態(tài)信息，所述衛(wèi)星狀態(tài)信息包括姿態(tài)敏感器、交會測量敏感器、機械臂關(guān)節(jié)狀態(tài)敏感器的測量信息；所述機械臂關(guān)節(jié)狀態(tài)包括關(guān)節(jié)角度和角速度；

(13)根據(jù)當前控制周期的任務(wù)模式，確定姿態(tài)和軌道控制算法，以衛(wèi)星狀態(tài)信息為輸入，進行衛(wèi)星平臺姿態(tài)和軌道控制計算，求解所需要的控制力和控制力矩；

(14)根據(jù)當前控制周期的任務(wù)模式，確定衛(wèi)星執(zhí)行機構(gòu)列表，獲得執(zhí)行機構(gòu)列表中包含的飛輪、磁力矩器和推力器的安裝位置、控制力和控制力矩，進行執(zhí)行機構(gòu)分配求解，確定需要執(zhí)行機構(gòu)列表中各執(zhí)行機構(gòu)的開關(guān)時間及控制力和力矩大小，將步驟(13)產(chǎn)生的推力和力矩指令轉(zhuǎn)化為飛輪、磁力矩器和推力器的開/關(guān)指令和動作電流，即形成執(zhí)行機構(gòu)指令，并發(fā)送至CAN總線；

(15)按照衛(wèi)星遙測數(shù)據(jù)格式，生成GNC狀態(tài)信息發(fā)送至CAN總線，完成本控制周期的仿真，等待內(nèi)部始終到達下一控制周期預(yù)定的控制時間后，開始下一周期的仿真；

所述的機械臂模擬器，用于模擬機械臂任務(wù)規(guī)劃系統(tǒng)的功能；按照飛行任務(wù)流程，實際系統(tǒng)設(shè)置了多個預(yù)先設(shè)定的機械臂關(guān)節(jié)期望動作序列，稱為機械臂任務(wù)模式；機械臂模擬器預(yù)存了機械臂任務(wù)模式信息。機械臂模擬器具體實現(xiàn)如下：

(21)在機械臂模擬器每個控制周期，首先從任務(wù)模式列表讀取下一模式開始時間和任務(wù)模式字，如果時間到達模式開始時間，則切換機械臂任務(wù)模式至對應(yīng)的任務(wù)模式；

(22)然后從CAN總線接收由動力學(xué)仿真機發(fā)送的衛(wèi)星平臺位置和姿態(tài)、當前機械臂關(guān)節(jié)狀態(tài)測量值；根據(jù)當前機械臂關(guān)節(jié)角和角速度，利用機械臂運動學(xué)關(guān)系，計算得到機械臂位于機械臂末端的手爪在衛(wèi)星平臺坐標系中的相對位置、姿態(tài)、速度和角速度；

(23)根據(jù)衛(wèi)星平臺的位置和姿態(tài)、手爪在衛(wèi)星平臺坐標系中的相對位置和姿態(tài)，通過坐標變換，計算當前時刻太陽、手爪以及衛(wèi)星平臺之間的相對位置關(guān)系，確定手爪的光照條件；然后根據(jù)安裝于機械臂手爪的手眼相機的視場角范圍、手爪的光照條件，基于Open Scene Graph模擬手眼相機對安裝于平臺上的手柄成像進行模擬，生成手眼相機模擬圖像；利用圖像識別算法，對所成圖像進行處理，求解得到手爪相對于手柄的相對位置測量值；

(24)機械臂模擬器采用獨立線程監(jiān)控CAN總線，隨時接收由星務(wù)遙測模擬器發(fā)送的機械臂動作指令，并記錄指令中包含的任務(wù)模式字和任務(wù)開始時間，形成任務(wù)模式列表；根據(jù)當前的機械臂任務(wù)模式，從預(yù)存的機械臂關(guān)節(jié)期望動作序列讀取本控制周期結(jié)束時的期望關(guān)節(jié)狀態(tài)；將期望關(guān)節(jié)狀態(tài)與當前機械臂的關(guān)節(jié)狀態(tài)測量值作差值，得到本控制周期的機械臂關(guān)節(jié)狀態(tài)的變化量；將控制周期時間內(nèi)均勻取若干個時間點，從當前機械臂關(guān)節(jié)狀態(tài)測量值到期望關(guān)節(jié)狀態(tài)進行插值，得到各時間點的期望關(guān)節(jié)狀態(tài)，形成當前控制周期的期望關(guān)節(jié)狀態(tài)序列，并發(fā)送至CAN總線；

所述動力學(xué)仿真機，用于實現(xiàn)衛(wèi)星平臺與機械臂復(fù)合動力學(xué)計算，軌道動力學(xué)、姿態(tài)敏感器、交會敏感器和機械臂關(guān)節(jié)狀態(tài)敏感器測量仿真，以及機械臂控制過程仿真功能；具體實現(xiàn)如下：

(31)動力學(xué)仿真機啟動后，首先通過以太網(wǎng)與綜合接口箱建立連接，之后開始監(jiān)控以太網(wǎng)，接收由綜合接口箱發(fā)送的數(shù)據(jù)；

(32)接收綜合接口箱轉(zhuǎn)發(fā)的執(zhí)行機構(gòu)指令后，計算飛輪、推力器和磁力矩器的輸出力和力矩；

(33)接收綜合接口箱轉(zhuǎn)發(fā)的本周期機械臂期望關(guān)節(jié)狀態(tài)序列后，利用機械臂關(guān)節(jié)角度測量結(jié)果求得誤差信息，采用PD控制算法，計算機械臂各關(guān)節(jié)的控制力矩；

(34)考慮6×6階攝動項、大氣阻力攝動、光壓攝動因素計算衛(wèi)星所受的環(huán)境力和力矩，與執(zhí)行機構(gòu)輸出力和力矩合成，得到衛(wèi)星所受的外力和外力矩，進行衛(wèi)星平臺軌道動力學(xué)計算，生成衛(wèi)星平臺軌道參數(shù)；

(35)采用拉格朗日方法建立平臺和機械臂復(fù)合動力學(xué)模型，將衛(wèi)星所受的外力和外力矩及機械臂各關(guān)節(jié)控制力矩作為輸入，采用8階龍格-庫塔方法進行數(shù)值積分求解。由于衛(wèi)星的姿態(tài)控制周期較大，為了提高復(fù)合動力學(xué)計算的精度，對每個控制周期內(nèi)分為100步作為動力學(xué)計算的積分步長，得到控制周期內(nèi)各時間點的衛(wèi)星平臺姿態(tài)、機械臂關(guān)節(jié)角和角速度參數(shù)；

(36)將衛(wèi)星平臺軌道參數(shù)、衛(wèi)星平臺姿態(tài)、機械臂關(guān)節(jié)角和角速度參數(shù)輸入至軌道動力學(xué)、姿態(tài)敏感器、交會敏感器和機械臂關(guān)節(jié)狀態(tài)敏感器測量模型，計算得到衛(wèi)星狀態(tài)信息，按照通訊格式的要求組成數(shù)據(jù)幀并通過以太網(wǎng)發(fā)送至綜合接口箱；

所述星務(wù)遙測模擬器，用于為仿真模擬器提供用戶輸入輸出界面，實現(xiàn)地面指令的輸入和發(fā)送，并完成遙測數(shù)據(jù)的打包存儲；具體實現(xiàn)如下：

(41)星務(wù)遙測模擬器通過用戶界面設(shè)置遙控指令輸入，當收到新指令后，首先完成遙控指令解析，分析指令的內(nèi)容和參數(shù)，并將指令按照衛(wèi)星實際遙控指令格式進行打包；

(42)如果是GNC指令，則通過CAN總線發(fā)送至GNC模擬器；如果是機械臂動作指令，則通過CAN總線發(fā)送至機械臂模擬器；

(43)在仿真過程中，星務(wù)遙測模擬器通過輪詢方式檢測CAN總線的數(shù)據(jù)，由于CAN總線具有廣播特性，因此GNC模擬器、機械臂模擬器和綜合接口箱發(fā)送的信息都被星務(wù)遙測模擬器收到，包括執(zhí)行機構(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ù)格式進行重新打包，形成遙測數(shù)據(jù)包存儲在星務(wù)遙測模擬器硬盤中，用于仿真后查閱；

所述綜合接口箱，用于完成所述的動力學(xué)仿真機的數(shù)據(jù)格式轉(zhuǎn)換；它一端連接于CAN總線，通過CAN總線與GNC模擬器、機械臂模擬器及星務(wù)遙測模擬器進行數(shù)據(jù)交換；另一端通過以太網(wǎng)連接于動力學(xué)仿真機；綜合接口箱啟動后，首先監(jiān)聽以太網(wǎng)端口，等待動力學(xué)仿真機的連接，建立連接后，通過兩個線程分別監(jiān)聽CAN總線和以太網(wǎng)的數(shù)據(jù)，當收到以太網(wǎng)數(shù)據(jù)，則按照CAN總線協(xié)議進行打包，并發(fā)送至CAN總線；當收到CAN總線數(shù)據(jù)，則按照以太網(wǎng)協(xié)議進行打包，通過以太網(wǎng)發(fā)送至動力學(xué)仿真機。