基于Python的四足機器人RRT路徑算法仿真

羅文濤 鄧金標 李先開

關鍵詞：Python;v-rep;RRT算法;四足機器人

1 V-rep仿真平臺

在仿真平臺上仿真相比于在真實的機器人上直接測試更能夠節省時間和科研成本，v-rep仿真平臺是一款多功能的仿真軟件，擁有逼真的3D渲染，可以用多種腳本語言進行仿真，像c++、maflab、Python等等。這里我們使用Python進行聯合仿真。

2仿真搭建

首先進行客戶端配置，在v-rep的安裝文件夾找到vrep.py，vrepConst.py還有window平臺需要使用的remoteApi.dll放在控制程序的文件夾中。在控制文件中使用Vrep.simxFinish（-1）關閉現有通信，simxStart（19999）獲得v-rep中scene的clientID。

進行服務端配置需要先導入12自由度的四足機器人模型，完成添加關節和提取凸面體結構的工作。配置完成后層級關系圖和仿真環境中的模型圖1如下。

這里只展示四足機器人身體和右前腿部分，其他部分大同小異。各個零件在Local respondable mask設定不同的碰撞掩碼，避免零件產生碰撞，設置四足機器人總重量為1.8kg。設定關節為力矩控制模式，打開關節的動力學屬性按照mg996r舵機的參數設置最大力矩為1.5N*m和速度上限為333.33des。

最后在v-rep四足模型的內置腳本中添加simRemoteApi.start（19999）命令。

3四足機器人步態規劃

使用Xuanqi Zeng的足端軌跡規劃方案，用貝塞爾函數

改變x和Y值對四足機器人末端x和y的坐標進行規劃，調整L值可以控制四足機器人的步長，t在零到一內取值就能得到貝塞爾函數上離散的點。當L=1，x=1，Y=O時的足端軌跡規劃如下。再運用D-H參數描述法。

用Muhammed Arif SenL的逆運動學分析的方法對這些四足末端點進行逆運動學分析，求出側擺關節，髖關節，膝關節的角度，用vrep.simxGetObjectHandle（）給要控制的關節添加控制句柄，再使用vrep.simxSetJointTargetPosition（）控制關節角度。

4構建迷宮環境并獲取信息

在v-rep的model browser中選擇墻體模型，移入仿真界面后，選擇Scene 0bject Properties-Adjust color-Ambient/diffuse component把亮度（Luminosity）一欄的值調為零，墻體變成黑色，這樣方便opencv對地圖進行二值化處理。

單擊鼠標左鍵選擇Add-Vision sensor-Perspective type生成視覺傳感器，移動視覺傳感器到世界坐標為（0，0，4）的位置世界，其姿態角Alpha、Beta和Gamma分別為180度、0度和-180度，打開Scene 0bject.Properties把分辨率（Resolu-tion）X/Y改為512/512。選擇四足機器人模型再打開Scene0bject.Properties-Common在Can be seen by欄中選擇[Cam-era]DefaultCamera，這樣四足機器人只能被默認攝像頭看見，就不會出現在視覺傳感器。在Python的控制文件添加攝像頭的控制句柄，然后通過vrep.simxGetVisionSensorlmage（）獲取圖像信息并用opencv進行二值化、邊緣提取。把圖像轉化為512*512的柵格地圖。

5 RRT算法規劃路徑

使用Sertac Karaman的RRT算法，RRT算法是相對高效的路徑規劃方法，算法原理大致如下：將起點作為根節點，在工作空間中，隨機生成一個位姿點。在位姿樹上尋找位姿點的最近鄰位姿點，判斷它們是否與障礙物碰撞。如不碰撞，將位姿點加入位姿樹。如到達目標點，溯源獲得路徑。減少四足機器人碰撞障礙物的可能性，增大障礙物邊界點的半徑為30。起點為（120，120），終點為（480，480），位姿點間的距離設置為30，下圖為RRT算法的結果。位姿點保存在txt文本文件。

6循路方案

生成一個方塊作為四足機器人的姿態角和位置的感應器，單擊鼠標左鍵選擇Add-Primitive-Cuboid，將其旋轉至和世界姿態角一致，這樣可以解決許多坐標轉換的麻煩。方塊移動到四足機器人的身體中心。Scene 0bject Properties-Show dynamic properties dialog，去掉Body is respondable和Body is dynamic，關閉了它的物理學屬性，避免方塊與四足機器人產生碰撞。Scene hierarehy中把方塊移動到四足機器人層級關系圖的目錄下，方塊就可以和四足機器人一起移動旋轉。在控制文件用vrep.simxGetO bjectHandle（）給要方塊添加句柄。vrep.simxGetObjectOrientation（）和vrep.simxGetOb-jectPosition（）分別獲取方塊的姿態角和位置。由起點到終點的順序調用txt文本文件里的柵格地圖位姿點通過以下公式坐標轉化為世界坐標。

把柵格地圖位姿點的縱坐標和橫坐標分別帶入公式計算，算出f（a）的值分別就是對應的世界坐標的縱坐標和橫坐標。再和方塊的位置求出目標角度和與目標位姿點的距離，和方塊的當前航向軸相比較可得出四足機器人需要轉動的角度。在四足機器人到達離位姿點一定距離以內切換到下一個位姿點，因為trot步態容易走偏，所以移動時需要修正四足機器人的航向軸直到與目標角度一致。到達終點后結束任務。

7仿真結果

為了方便對四足機器人仿真結果的觀察，添加物體的運動軌跡，點擊Add～Graph，再選擇Scene 0 bject Properties-Add new data stream to record，Data stream type一欄分別選擇0bject：x-position;0bject：absolute y-position;0bject：absolute z-position，而0bject/items to record一欄統一選擇Graph，拖放到四足機器人層級關系圖的身體目錄下。點擊播放鍵開始仿真，仿真過程如下，紅色線為四足機器人的運動軌跡。

四足機器人順利走到終點，耗時約1分40秒，過程中并無與障礙物碰撞，證明了代碼的可行性。

8結論

通過仿真軟件的幫助，在沒有真實的四足機器人和場地的情況下，不但能檢驗機構設計的合理性，而且還能很好的驗證代碼。在四足機器人的研發也是幫助莫大，通過仿真所得的數據，能夠更準確地選擇合適的硬件制作四足機器人，給四足機器人更多功能的應用奠定了基礎。