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

羅文濤 鄧金標 李先開

關(guān)鍵詞：Python;v-rep;RRT算法;四足機器人

1 V-rep仿真平臺

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

2仿真搭建

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

進行服務(wù)端配置需要先導(dǎo)入12自由度的四足機器人模型，完成添加關(guān)節(jié)和提取凸面體結(jié)構(gòu)的工作。配置完成后層級關(guān)系圖和仿真環(huán)境中的模型圖1如下。

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

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

3四足機器人步態(tài)規(guī)劃

使用Xuanqi Zeng的足端軌跡規(guī)劃方案，用貝塞爾函數(shù)

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

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

4構(gòu)建迷宮環(huán)境并獲取信息

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

單擊鼠標左鍵選擇Add-Vision sensor-Perspective type生成視覺傳感器，移動視覺傳感器到世界坐標為（0，0，4）的位置世界，其姿態(tài)角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，這樣四足機器人只能被默認攝像頭看見，就不會出現(xiàn)在視覺傳感器。在Python的控制文件添加攝像頭的控制句柄，然后通過vrep.simxGetVisionSensorlmage（）獲取圖像信息并用opencv進行二值化、邊緣提取。把圖像轉(zhuǎn)化為512*512的柵格地圖。

5 RRT算法規(guī)劃路徑

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

6循路方案

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

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

7仿真結(jié)果

為了方便對四足機器人仿真結(jié)果的觀察，添加物體的運動軌跡，點擊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一欄統(tǒng)一選擇Graph，拖放到四足機器人層級關(guān)系圖的身體目錄下。點擊播放鍵開始仿真，仿真過程如下，紅色線為四足機器人的運動軌跡。

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

8結(jié)論

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