機器人航天員雙臂操作仿真系統(tǒng)的研究與實現(xiàn)

熊明華，張曉東，肖 濤，王耀兵

（空間智能機器人系統(tǒng)技術(shù)與應(yīng)用北京市重點實驗室北京空間飛行器總體設(shè)計部，北京100094）

機器人航天員雙臂操作仿真系統(tǒng)的研究與實現(xiàn)

熊明華，張曉東，肖 濤，王耀兵

（空間智能機器人系統(tǒng)技術(shù)與應(yīng)用北京市重點實驗室北京空間飛行器總體設(shè)計部，北京100094）

針對機器人航天員復(fù)雜操作場景下的任務(wù)規(guī)劃和仿真驗證問題，基于仿真模塊集成化的設(shè)計思想，集成通信模塊、任務(wù)調(diào)度管理模塊、路徑規(guī)劃模塊、力控制模塊、碰撞檢測模塊、雙臂3D幾何模型，通過OSG建立了實體模型及仿真場景，并建立插拔、抓握、對準(zhǔn)、旋擰、搬運、轉(zhuǎn)移等典型操作庫，實現(xiàn)了機器人航天員雙臂多種復(fù)雜操作任務(wù)的仿真。最后，通過機器人航天員搬運物體任務(wù)仿真實現(xiàn)雙臂協(xié)調(diào)操作任務(wù)規(guī)劃的仿真，驗證了該系統(tǒng)的有效性和任務(wù)規(guī)劃驗證的合理性。

雙臂機器人；路徑規(guī)劃；碰撞檢測；一體化仿真

1 引言

隨著計算機圖形處理能力的提升，實時三維可視化仿真成為機器人任務(wù)規(guī)劃和運動控制仿真驗證的重要工具［1］?；诜抡婺K集成化設(shè)計思想而開發(fā)的機器人航天員雙臂操作一體化仿真系統(tǒng)，可有效指導(dǎo)空間機器人系統(tǒng)任務(wù)的仿真驗證［2］。在該領(lǐng)域已經(jīng)有許多研究，Burdea［3］基于虛擬現(xiàn)實技術(shù)建立了Puma560模型并進行目標(biāo)抓取力的仿真驗證；Xu Hua［4］設(shè)計了一種雙臂機器人的圖形化編程和仿真控制系統(tǒng)，用戶通過圖形編程對機器人進行任務(wù)規(guī)劃，可在三維仿真環(huán)境中分析規(guī)劃結(jié)果；周軍［5］針對冗余度雙臂機器人進行軸孔裝配的運動學(xué)約束關(guān)系，對冗余雙臂機器人的軸孔裝配作業(yè)進行三維動態(tài)仿真驗證；Univ Malaya［6］建立一個集成的三維模擬軟件和虛擬現(xiàn)實系統(tǒng)以優(yōu)化機器人的運動時間并解決機器人的編程問題，具備在虛擬現(xiàn)實和三維機器人真實環(huán)境中傳遞和優(yōu)化數(shù)據(jù)的功能。

以上研究都是針對雙臂機器人某單一作業(yè)任務(wù)進行規(guī)劃仿真，而實際的機器人航天員雙臂操作任務(wù)是對機械臂單臂運動和雙臂協(xié)調(diào)操作的綜合，是由一系列基本動作序列組成的，這些基本序列涉及到單臂的路徑規(guī)劃、抓取物體和雙臂之間的協(xié)調(diào)操作。機器人的動作很多具有相同的性質(zhì)，因此將相似的動作看作一個基本動作單元，通過調(diào)用基本動作單元，并按特定順序組合就可完成復(fù)雜的任務(wù)，從而基于仿真模塊集成化實現(xiàn)任務(wù)仿真。因此，本文基于機器人航天員雙臂操作任務(wù)分解和動作規(guī)劃為基礎(chǔ)，在 Visual Studio 2010的開發(fā)平臺下，以C語言編程實現(xiàn)各主要算法，利用開源三維引擎OSG（Open Scene Graph）建立雙臂機器人模型、基本動作庫及工作環(huán)境，通過模塊化的設(shè)計思想，集成了機器人航天員雙臂操作的幾大關(guān)鍵模塊，開發(fā)出一套集協(xié)調(diào)操作規(guī)劃、三維可視化處理、機器人力控制及碰撞檢測于一體化的仿真系統(tǒng)，并進行了仿真驗證。

2 一體化仿真系統(tǒng)總體方案

通過機器人航天員雙臂操作任務(wù)需求分析，在覆蓋所需功能需求的基礎(chǔ)上，建立了完整的機器人航天員雙臂操作的一體化仿真系統(tǒng)，主要包括任務(wù)調(diào)度管理模塊、路徑規(guī)劃模塊、力控制模塊、碰撞檢測模塊、通信模塊、視覺模塊及三維仿真模塊等，集成仿真系統(tǒng)結(jié)構(gòu)如圖1所示。

各模塊功能如下：

1）任務(wù)調(diào)度管理模塊：該仿真系統(tǒng)最主要的模塊之一，在實際控制中，主要負(fù)責(zé)底層任務(wù)規(guī)劃；任務(wù)調(diào)度管理模塊從指令接收模塊中獲取當(dāng)前任務(wù)的信息，結(jié)合接收到的機器人視覺／力測量數(shù)據(jù)和規(guī)劃庫來完成操作人員設(shè)定的相關(guān)任務(wù)；

2）路徑規(guī)劃模塊：主要包括單臂笛卡爾空間路徑規(guī)劃、關(guān)節(jié)空間路徑規(guī)劃、雙臂協(xié)調(diào)操作及避障路徑規(guī)劃，路徑規(guī)劃算法采用C語言進行編寫，形成雙臂機器人動作的基本規(guī)劃庫，并以動態(tài)鏈接庫形式供任務(wù)調(diào)度管理模塊進行調(diào)用；

3）力控制模塊：主要完成機器人的柔順控制任務(wù)，根據(jù)機器人力傳感器反饋信息與任務(wù)規(guī)劃中理想力矩的差值，調(diào)用自適應(yīng)阻抗控制算法庫，實現(xiàn)對機器人的力位混合控制；

4）碰撞檢測模塊：根據(jù)整個系統(tǒng)的當(dāng)前位置與狀態(tài)，判斷是否與自身及周圍物體碰撞；

5）通信模塊：數(shù)據(jù)通信模塊主要完成對機器人航天員控制過程中指令的收發(fā)，以及實現(xiàn)與數(shù)據(jù)管理、三維仿真、視覺模塊等外部單元之間的數(shù)據(jù)交互，操作輸入端、數(shù)據(jù)管理以及三維仿真通過以太網(wǎng)與任務(wù)調(diào)度管理模塊進行通信；

6）視覺模塊：機器人航天員的視覺處理模塊，一方面用于拍攝視覺圖像，作為操作人員的操作輸入；另一方面用于測量目標(biāo)位姿，作為路徑規(guī)劃的依據(jù)；

7）三維仿真模塊：實現(xiàn)三維仿真環(huán)境建模，增強臨場感，提升人機交互體驗。

3 關(guān)鍵模塊的設(shè)計與實現(xiàn)

為完成搭建機器人航天員雙臂操作任務(wù)仿真，本文重點對操作任務(wù)仿真所必需的三維仿真模塊、任務(wù)調(diào)度管理模塊、雙臂協(xié)調(diào)操作路徑規(guī)劃模塊進行論述，力控制模塊、視覺等模塊可參考文獻［5］等進行設(shè)計。

3.1三維仿真模塊

OSG是一套基于C＋＋平臺的應(yīng)用程序接口，能夠快速、便捷地創(chuàng)建高性能、跨平臺的交互式圖形程序。通過OSG可繪制出逼真的三維仿真環(huán)境，增強操作員的臨場感，提升控制單元的人機交互體驗。因此，本模塊基于OSG接口，采用Open?GL技術(shù)進行開發(fā)，模型創(chuàng)建流程如下：

1）將基于PROE軟件繪制出的機器人航天員模型導(dǎo)出至3DMAX中，并進行渲染；

2）完成渲染后，保存并導(dǎo)出3ds格式文件；

3）將3ds格式文件導(dǎo)入OSG，創(chuàng)建組織節(jié)點，并獲取其參考坐標(biāo)系，通過確定各節(jié)點之間的父子關(guān)系即矩陣變換，使其在三維空間中組裝成期望的仿真模型；

4）設(shè)置關(guān)節(jié)角度的驅(qū)動變量，建立模型驅(qū)動接口。

其中，三維模型建立過程為首先建立運動學(xué)參數(shù)矩陣，之后進行模型導(dǎo)入及裝配，其基本流程簡述如下：

1）根據(jù)機器人航天員各組合體坐標(biāo)系創(chuàng)建運動學(xué)參數(shù)矩陣，并關(guān)聯(lián)坐標(biāo)系模型，使關(guān)聯(lián)的坐標(biāo)系模型與建立的運動學(xué)參數(shù)坐標(biāo)系完全一致；

2）將所有三維模型讀入到三維可視化軟件中，使之關(guān)聯(lián)到對應(yīng)的模型節(jié)點，讀入所有模型成功后轉(zhuǎn)入第三步，否則給出提示；

3）為每個模型節(jié)點關(guān)聯(lián)一個姿態(tài)調(diào)整矩陣，通過其調(diào)整各個模型的初始姿態(tài)，直至所有模型的位置都符合要求；

4）將每個模型節(jié)點姿態(tài)調(diào)整矩陣關(guān)聯(lián)到對應(yīng)的運動學(xué)參數(shù)矩陣；

5）根據(jù)裝配關(guān)系組裝運動學(xué)參數(shù)矩陣；

6）將運動學(xué)參數(shù)矩陣關(guān)聯(lián)到OSG根節(jié)點中，使之能夠在OSG仿真窗口中顯示。

3.2任務(wù)調(diào)度管理模塊

任務(wù)調(diào)度管理模塊是仿真系統(tǒng)最主要的模塊之一，在實際的控制中，主要負(fù)責(zé)底層任務(wù)規(guī)劃。對于機器人航天員系統(tǒng)的某個任務(wù)，其控制過程被分解為多個步驟，任務(wù)調(diào)度管理模塊根據(jù)當(dāng)前機械臂的執(zhí)行情況來規(guī)劃下一步的執(zhí)行內(nèi)容，此時需要一個定時器定時啟動規(guī)劃下一步內(nèi)容來完成任務(wù)規(guī)劃。在雙臂機器人的任務(wù)規(guī)劃階段，需要根據(jù)機器人的當(dāng)前位姿和目標(biāo)物的狀態(tài)等信息做出合適的路徑規(guī)劃方案。任務(wù)規(guī)劃過程中，任務(wù)調(diào)度管理模塊從指令收發(fā)模塊中獲取當(dāng)前任務(wù)的信息，并結(jié)合接收到的機械臂實時數(shù)據(jù)來完成操作人員設(shè)定的相關(guān)任務(wù)。

對于操作人員設(shè)置的具體任務(wù)，每次根據(jù)任務(wù)期望結(jié)果與當(dāng)前機械臂的狀態(tài)及當(dāng)前執(zhí)行的時間來規(guī)劃下一步操作。由于任務(wù)規(guī)劃過程中需要等待機械臂返回的實時狀態(tài)和數(shù)據(jù)以規(guī)劃下一步操作，進而保證機械臂準(zhǔn)確安全地執(zhí)行工作，故底層任務(wù)規(guī)劃采用“等待－判別－執(zhí)行”機制來完成每份指令或數(shù)據(jù)的發(fā)送，任務(wù)調(diào)度管理模塊流程邏輯圖如2所示。

該系統(tǒng)中的控制平臺可以提前對任務(wù)進行規(guī)劃和調(diào)整，通過任務(wù)調(diào)度管理模塊接口，完成任務(wù)信息的規(guī)劃。任務(wù)調(diào)度管理模塊能夠在障礙環(huán)境中規(guī)劃多個無障礙動作序列，利用簡單直線路徑規(guī)劃算法拼接復(fù)雜任務(wù)，使機械臂在軌跡規(guī)劃階段前就可以提前檢測出任務(wù)路徑中可能出現(xiàn)的障礙物并進行躲閃。

任務(wù)調(diào)度管理模塊提供多種規(guī)劃方案，并以任務(wù)中間點的形式給出，以此達到躲避障礙目標(biāo)、優(yōu)化路徑的目的，降低機械臂在運行過程中發(fā)生碰撞的可能性，提高機械臂在不同環(huán)境下的工作能力和適應(yīng)性。

任務(wù)規(guī)劃算法總體思路遵循簡單路徑拼接復(fù)雜任務(wù)、添加任務(wù)中間點數(shù)量最小化。算法從點到點直線運動方案嘗試，逐步增加任務(wù)中間點，任務(wù)中間點只在遇到障礙的任務(wù)階段添加，無障礙階段保證動作精簡。具體算法如下：

1）對任務(wù)目標(biāo)點反解，如果目標(biāo)構(gòu)型有碰撞，則返回任務(wù)失?。?/p>

2）對任務(wù)始末點的末端軌跡進行碰撞檢測，如果有碰撞則根據(jù)末端軌跡碰撞算法插入任務(wù)中間點。如果無碰撞，則返回任務(wù)規(guī)劃結(jié)果；

3）通過碰撞算法檢測任務(wù)中間點碰撞情況，如果無碰撞，則將當(dāng)前動作按中間點劃分為兩個動作，遞歸調(diào)用step2。如果有碰撞，對中間點增加安全距離進行調(diào)整；

4）調(diào)整后的中間點如果依然碰撞，則任務(wù)失敗。否則返回規(guī)劃結(jié)果；

5）路徑規(guī)劃中，當(dāng)遇到障礙，則增加中間點，并進入step3。

3.3雙臂協(xié)調(diào)路徑規(guī)劃模塊

機器人航天員雙臂操作具有類人雙臂的特點，可以代替或協(xié)助人類完成工具組裝、更換等復(fù)雜作業(yè)。通過對雙臂系統(tǒng)任務(wù)操作分析，可得出雙臂系統(tǒng)在執(zhí)行任務(wù)過程中需進行的基本操作包括：插拔、抓握、對準(zhǔn)、旋擰、搬運、轉(zhuǎn)移等。通過對雙臂機器人系統(tǒng)的基本操作特點進行分析，可以將基本操作劃分為兩大類：

第一類：雙臂機器人系統(tǒng)雙臂末端有相對運動方式，比如插拔、旋擰等；

第二類：雙臂機器人系統(tǒng)雙臂末端無相對運動方式，比如搬運、轉(zhuǎn)移等。不同的運動方式將得到不同的位置、速度和加速度約束方程。

路徑規(guī)劃模塊主要包括：單臂笛卡爾空間路徑規(guī)劃、關(guān)節(jié)空間路徑規(guī)劃；雙臂協(xié)調(diào)操作及避障路徑規(guī)劃。路徑規(guī)劃算法采用標(biāo)準(zhǔn)C語言進行編寫，以動態(tài)鏈接庫形式進行調(diào)用。各路徑規(guī)劃函數(shù)的輸入輸出概括如下：

1）單臂笛卡爾空間路徑規(guī)劃以機械臂初始和目標(biāo)位姿為輸入，以機械臂關(guān)節(jié)角序列為輸出；關(guān)節(jié)空間路徑規(guī)劃函數(shù)以機械臂初始及目標(biāo)關(guān)節(jié)角序列為輸入，以規(guī)劃后的關(guān)節(jié)角序列為輸出；

2）雙臂協(xié)調(diào)操作包括雙臂笛卡爾空間及雙臂關(guān)節(jié)空間路徑規(guī)劃。其中，雙臂笛卡爾空間路徑規(guī)劃以雙臂運動模式，初始位姿及目標(biāo)位姿為輸入，以雙臂關(guān)節(jié)角序列為輸出；雙臂關(guān)節(jié)空間路徑規(guī)劃以左右臂初始及目標(biāo)關(guān)節(jié)角序列為輸入，以規(guī)劃后的關(guān)節(jié)角序列為輸出；

3）避障路徑規(guī)劃算法以避碰關(guān)鍵點、有障礙下運動方程、避碰速度為輸入，以機械臂關(guān)節(jié)角序列為輸出。

本文將雙臂復(fù)雜的運動過程分解為基本的路徑規(guī)劃，在需要時進行調(diào)用。在雙臂機器人系統(tǒng)運動學(xué)建模的基礎(chǔ)上，得到系統(tǒng)的正逆解運動學(xué)方程，進而可以按照圖3所示的基本流程調(diào)用軌跡規(guī)劃模塊，實現(xiàn)雙臂協(xié)調(diào)操作。

3.4碰撞檢測模塊

碰撞檢測模塊作為仿真驗證平臺的重要組成單元，主要檢測在仿真環(huán)境中檢測場景內(nèi)各個物體之間是否會發(fā)生碰撞，完善仿真驗證環(huán)境的真實性，從而避免機器人航天員雙臂在路徑規(guī)劃過程中可能會發(fā)生雙臂之間、臂與基座以及所在空間外部障礙物之間的碰撞情況。

為簡化模型，采用長方體和圓柱體兩種簡化模型代替桿件，球形簡化模型代替障礙物，繼而運用空間幾何和最優(yōu)化理論中的可行方向法得出圓柱體與球體、圓柱體與圓柱體、長方體與球體、長方體與長方體四種類型的最短距離計算方法，最后將其應(yīng)用于桿件與障礙物、桿件與桿件之間的碰撞檢測中。

整個機器人航天員雙臂操作的碰撞檢測流程如圖4所示。

4 仿真驗證

為驗證機器人航天員仿真平臺的有效性及在機械臂研究應(yīng)用方面的可行性，設(shè)計機器人航天員雙臂協(xié)調(diào)操作搬運物體任務(wù)。任務(wù)要求如下：機器人航天員將一個物體從一個位置移動到另一個位置，保證雙臂的協(xié)調(diào)運動，即需要位置控制實現(xiàn)雙臂末端保持一定的相對位姿；為了保證物體不會掉落、損壞，通過力控制保持接觸力在某一范圍內(nèi)，任務(wù)過程如圖5所示。

根據(jù)機器人航天員雙臂協(xié)調(diào)操作搬運物體任務(wù)需求，首先利用基于模塊化組件的通用仿真平臺搭建雙臂協(xié)調(diào)機械臂及虛擬仿真環(huán)境，根據(jù)任務(wù)路徑規(guī)劃需求，添加路徑規(guī)劃功能模塊、碰撞檢測模塊及力控制模塊。通過設(shè)計任務(wù)的仿真，驗證模塊化通用仿真平臺的有效性。具體步驟如下：

1）創(chuàng)建模塊化組件庫。收集機器人對象及其工作環(huán)境信息，利用模塊化預(yù)處理軟件創(chuàng)建相關(guān)節(jié)點信息問題，導(dǎo)入模塊化組件庫；

2）搭建平臺虛擬仿真工作環(huán)境。根據(jù)機器人工作環(huán)境布置需求，設(shè)置環(huán)境節(jié)點安裝參數(shù)，完成平臺環(huán)境節(jié)點布置；

3）搭建仿真機器人對象。利用機器人節(jié)點信息文件，搭建平臺機器人對象；

4）仿真平臺環(huán)境檢驗。在平臺仿真環(huán)境搭建完成后，對機器人及工作環(huán)境信息進行校驗，利用靜態(tài)分析單元對各結(jié)構(gòu)進行分析，保證虛擬仿真環(huán)境的正確性；

5）添加路徑規(guī)劃、碰撞檢測及力控制模塊，并進行相應(yīng)的功能測試。

在仿真環(huán)境搭建完成后，利用本平臺進行機器人航天員雙臂協(xié)調(diào)搬運物體任務(wù)驗證。首先，機器人航天員雙臂運動到夾持位置，夾緊搬運物體，采用雙臂協(xié)調(diào)路徑規(guī)劃算法，將其搬運至目標(biāo)點，此過程需要考慮雙臂力位混合控制，同時雙臂末端無相對運動，保證在搬運過程中不會產(chǎn)生內(nèi)力，任務(wù)仿真過程如圖6、圖7所示。

構(gòu)型規(guī)劃總時間為20 s，時間步長為0.05 s。規(guī)劃過程中的關(guān)節(jié)角速度如圖8所示。

雙臂將搬運物體要求誤差為0.1 mm，根據(jù)圖9所示仿真的結(jié)果，雙臂將搬運物體搬運到目標(biāo)位置后，最大角度誤差為0.68×10－7。產(chǎn)生的機械臂末端誤差在允許范圍內(nèi)。雙臂機器人的末端到達了指定位姿，完成了雙臂機器人系統(tǒng)搬運物體的全部任務(wù)。

5 結(jié)論

本文采用仿真模塊集成化設(shè)計思想建立了一體化仿真系統(tǒng)，集成了任務(wù)調(diào)度管理、雙臂協(xié)調(diào)操作規(guī)劃、三維可視化、力控制和碰撞檢測等功能模塊，通過調(diào)用雙臂協(xié)調(diào)典型操作庫，實現(xiàn)了機器人航天員雙臂協(xié)調(diào)搬運操作任務(wù)仿真，仿真結(jié)果表明，本文建立的機器人航天員任務(wù)仿真驗證平臺可有效的實現(xiàn)機械臂應(yīng)用可行性驗證和任務(wù)規(guī)劃合理性驗證。

（References）

［1］ 鄭潔，張衛(wèi)民，吳昊，等.基于Vega的柔性雙臂空間機器人的三維動畫仿真［J］.計算機工程與科學(xué)，2004，26（10）：72?74. Zheng Jie，Zhang Weimin，Wu Hao，et al.3?D animational simulation of a flexible dual?manipulator space robot based on vega［J］.Computer Engeneering＆Science，2004，26（10）：72?74.（in Chinese）

［2］ Shin S Y，Kim C H.Human?like motion generation and con?trol for humanoid＇s dual arm object manipulation［J］.IEEE Transactions on Industrial Electronics，2015，62（4）：2265?2276.

［3］ Burdea G C.Invited review：the synergy between virtual real?ity and robotics［J］.IEEE Transactions on Robotics and Au?tomation，1999，15（3）：400?410.

［4］ Cao Qixin，Zhang Zhen，Gu Jiajun.Graphical programming，simulation and control system for dual?arm mobile robot［J］. Journal of System Simulation，2006，18（9）：2542?2546.

［5］ 周軍，丁希侖，陸震.冗余度雙臂機器人軸孔裝配的三維動態(tài)仿真與實驗［J］.機器人，2006，28（4）：422?427. Zhou Jun，Ding Xilun，Lu zhen.3?D dynamic simulation and experiment for shaft and hole mating of redundant dual?arm ro?bot［J］.Robot，2006，28（4）：422?427.（in Chinese）

［6］ Tahriri F，Mousavi M，Yap H J，et al.Optimizing the robot arm movement time using virtual reality robotic teaching sys?tem［J］.International Journal of Simulation Modelling，2015，14（1）：28?38.

（責(zé)任編輯：龍晉偉）

Research and Realization of Integrated Simulation System for Robonaut Dual?arm Manipulation

XIONG Minghua，ZHANG Xiaodong，XIAO Tao，WANG Yaobing

（Beijing Key Laboratory of Intelligent Space Robotic Systems Technology and Applications，Institute of Spacecraft System Engineering CAST，Beijing 100094，China）

An integrated simulation system for dual?arm robonaut was developed in the paper to deal with the mission planning and validation problems in the complicated scenarios.Based on the modu?lar design concept，the simulation system was composed of the communication module，task schedu?ling management module，path planning module，dynamic control module，3D geometric model and collision detection module.The simulation scenarios were created by OSG（Open Scene Graph）. Then，the missions of the dual?arm manipulator were divided into a series of basic movements such as the plug，grasp，screw，transport，and the simulation of the complicated mission was realized by using the special combination of the basic movements.Finally，the algorithms of coordinated dual?arm robot was verified by the mission of transporting objects.The results demonstrated the effective?ness of the system and verified the feasibility of mission planning.

dual?arm robot；path planning；collision detection；integrated simulation

TP242.3

：A

：1674?5825（2017）02?0150?06

2015?11?02；

2016?08?08

國家自然科學(xué)基金（61573058）

熊明華，男，碩士，高級工程師，研究方向為空間機器人技術(shù)。E?mail：13910830761＠139.com