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

【發(fā)明內容】

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

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