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

羅文濤 鄧金標(biāo) 李先開

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

1 V-rep仿真平臺

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

2仿真搭建

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

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

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

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

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

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

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

用Muhammed Arif SenL的逆運(yùn)動學(xué)分析的方法對這些四足末端點(diǎn)進(jìn)行逆運(yùn)動學(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對地圖進(jìn)行二值化處理。

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

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

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

6循路方案

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

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

7仿真結(jié)果

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

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

8結(jié)論

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