基于Matlab與Modelsim 的四足機(jī)器人步態(tài)算法的協(xié)同仿真

羅慶生，李超，韓寶玲，司世才，牛鍇

（1.北京理工大學(xué) 機(jī)電學(xué)院，北京100081；2.北京理工大學(xué) 機(jī)械與車(chē)輛學(xué)院）

羅慶生（教授）、李超（碩士研究生），研究方向?yàn)樘胤N機(jī)器人技術(shù)、機(jī)器人伺服驅(qū)動(dòng)技術(shù)。

引 言

我國(guó)廣大山區(qū)地勢(shì)險(xiǎn)峻、地形復(fù)雜，輪式或履帶式機(jī)器人很難在這些地區(qū)進(jìn)行軍事偵察、資源勘探、貨物運(yùn)載等工作。四足機(jī)器人具有較強(qiáng)的機(jī)動(dòng)性和靈活性，能夠適應(yīng)非結(jié)構(gòu)化的地面，有望在這些區(qū)域發(fā)揮機(jī)動(dòng)性能，已成為移動(dòng)機(jī)器人研究領(lǐng)域的熱點(diǎn)之一。運(yùn)動(dòng)控制算法是四足機(jī)器人研究的重點(diǎn)和難點(diǎn)，如何驗(yàn)證控制算法的有效性，更好地應(yīng)用于四足機(jī)器人的實(shí)際行走中是本文研究的重點(diǎn)。

筆者所在實(shí)驗(yàn)室設(shè)計(jì)了一種液壓驅(qū)動(dòng)的四足機(jī)器人，該機(jī)器人采用FPGA 作為步態(tài)算法的硬件平臺(tái)，如何將步態(tài)算法快速有效地應(yīng)用于FPGA 硬件板中成為研究的核心。傳統(tǒng)的驗(yàn)證方法需要花費(fèi)大量的時(shí)間將步態(tài)算法轉(zhuǎn)換為硬件語(yǔ)言，并且在后期驗(yàn)證過(guò)程中需要系統(tǒng)開(kāi)發(fā)人員以及FPGA 設(shè)計(jì)人員多次返工。而Simulink 仿真庫(kù)里提供的HDL Coder功能，能夠把Matlab／Simulink直接轉(zhuǎn)化為Verilog 語(yǔ)言程序，同時(shí)可以生成仿真用的Test Bench，包括FPGA 和ASIC 的硬件設(shè)計(jì)流程無(wú)縫連接起來(lái)的聯(lián)合仿真接口擴(kuò)展模塊，它的快速雙向連接將Matlab／Simulink和硬件描述語(yǔ)言仿真軟件Modelsim 連接起來(lái)，使二者之間直接的聯(lián)合仿真成為可能，并且更高效地在Matlab／Simulink 中驗(yàn)證Modelsim 中的寄存器傳輸級(jí)（RTL）模型。因此筆者在文中采用了Matlab與Modelsim 協(xié)同仿真的方法，既降低了系統(tǒng)級(jí)開(kāi)發(fā)的復(fù)雜度，又提高了設(shè)計(jì)的驗(yàn)證效率。

1 四足機(jī)器人控制模型建立

1.1 液壓四足機(jī)器人物理樣機(jī)介紹

筆者所在團(tuán)隊(duì)設(shè)計(jì)的四足機(jī)器人采用液壓伺服驅(qū)動(dòng)，現(xiàn)已設(shè)計(jì)并加工出該機(jī)器人的物理樣機(jī)（見(jiàn)圖1）。該四足機(jī)器人采用兩腿節(jié)設(shè)計(jì)，共12個(gè)主動(dòng)自由度，腿部含有3個(gè)旋轉(zhuǎn)關(guān)節(jié)，分別為側(cè)擺關(guān)節(jié)、髖關(guān)節(jié)和膝關(guān)節(jié)。3 個(gè)關(guān)節(jié)均采用液壓缸驅(qū)動(dòng)，通過(guò)對(duì)液壓缸活塞桿的位置控制來(lái)實(shí)現(xiàn)機(jī)器人的各種步態(tài)。該機(jī)器人的總體長(zhǎng)度為1m，寬度為0.5m，站立時(shí)高度為1m，其中足端距機(jī)體中心高度為0.74m。機(jī)器人的腿部采用前肘后膝的結(jié)構(gòu)形式［7］，四條腿采用模塊化設(shè)計(jì)，結(jié)構(gòu)尺寸完全一致，其中大腿和小腿的長(zhǎng)度均為400mm，側(cè)擺關(guān)節(jié)到髖關(guān)節(jié)的距離為140mm。

圖1 液壓四足機(jī)器人物理樣機(jī)

1.2 CPG控制方法簡(jiǎn)介

液壓四足機(jī)器人采用基于CPG 和生物反射的控制方法。CPG（Central Pattern Generator，中樞模式發(fā)生器）是由中間神經(jīng)元構(gòu)成的局部網(wǎng)絡(luò)，能夠通過(guò)神經(jīng)元之間的相互抑制產(chǎn)生穩(wěn)定的相位互鎖關(guān)系，并通過(guò)自激振蕩激發(fā)肢體的節(jié)律運(yùn)動(dòng)（如跑、跳、游等），通過(guò)對(duì)其進(jìn)行數(shù)學(xué)建模以產(chǎn)生控制信號(hào)實(shí)現(xiàn)四足機(jī)器人的典型步態(tài)。該方法避免了對(duì)動(dòng)力學(xué)研究精確度的依賴(lài)，控制相對(duì)簡(jiǎn)單，具有良好的自穩(wěn)定性，生物對(duì)復(fù)雜環(huán)境的適應(yīng)性主要是通過(guò)各種反射機(jī)制實(shí)現(xiàn)的，觀(guān)察生物反射行為，探索其反射機(jī)理，對(duì)反射動(dòng)作進(jìn)行規(guī)劃和建模是提高四足機(jī)器人環(huán)境適應(yīng)性的主要方法。

對(duì)四足機(jī)器人的運(yùn)動(dòng)進(jìn)行控制，最基本的是建立合理的CPG 網(wǎng)絡(luò)模型，產(chǎn)生持續(xù)穩(wěn)定并且具有一定相位關(guān)系的多路周期性振蕩信號(hào)，以控制四足機(jī)器人的各關(guān)節(jié)運(yùn)動(dòng)，使其可以在平地上完成幾種典型步態(tài)下的運(yùn)動(dòng)。

1.3 液壓四足機(jī)器人控制算法模型的建立

本文選擇了加入反饋的Hopf振

蕩器作為CPG單元模型，Hopf振蕩器結(jié)構(gòu)較簡(jiǎn)單，且能在一定程度上反映神經(jīng)元間的關(guān)系，調(diào)節(jié)方便，參數(shù)對(duì)輸出的影響簡(jiǎn)單，其數(shù)學(xué)表達(dá)式如式（1）所示。式中x、y為兩個(gè)相互耦合的變量，使振蕩器模型的輸出在狀態(tài)空間形成穩(wěn)定極限環(huán)，a、b為定義極限環(huán)收斂時(shí)間的正常數(shù)，t是一個(gè)取較大值的正常數(shù)為x、y振蕩的幅值，w為振蕩器的振蕩頻率，wstance和wswing分別控制振蕩器x輸出上升和下降區(qū)間的持續(xù)時(shí)間（即分別代表支撐相和擺動(dòng)相的頻率）。

2 從Simulink模型到硬件描述語(yǔ)言的轉(zhuǎn)換

2.1 Simulink模型的建立

Simulink是Matlab軟件重要的軟件包，在動(dòng)態(tài)系統(tǒng)仿真中，它對(duì)于連續(xù)系統(tǒng)和離散系統(tǒng)、線(xiàn)性系統(tǒng)和非線(xiàn)性系統(tǒng)同時(shí)適用，采用模塊直觀(guān)地描述系統(tǒng)典型環(huán)節(jié)，可以十分方便地建立系統(tǒng)模型而不需要花大量時(shí)間編程。所建立的CPG 模型能否輸出具有確定相位關(guān)系的控制信號(hào)，還需要進(jìn)一步仿真驗(yàn)證，本文利用Simulink工具包對(duì)模型進(jìn)行了數(shù)值仿真，采用odb3（Bogacki－Shampine）算法，仿真步長(zhǎng)固定為0.005s。

建立如圖2所示的CPG 單元模型，并且建立如圖3所示的CPG 整體模型。

圖2 CPG單元模型

2.2 高效轉(zhuǎn)換工具

傳統(tǒng)的FPGA 開(kāi)發(fā)流程，要將已經(jīng)在Simulink中實(shí)現(xiàn)的算法轉(zhuǎn)換為FPGA 可識(shí)別的代碼。首先需要將算法中的數(shù)據(jù)進(jìn)行定點(diǎn)化處理，將浮點(diǎn)型變量轉(zhuǎn)換為定點(diǎn)型；其次將算法用HDL代碼形式實(shí)現(xiàn)出來(lái)，并對(duì)代碼進(jìn)行測(cè)試，發(fā)現(xiàn)問(wèn)題并改正相應(yīng)代碼，直到測(cè)試通過(guò)；再次將HDL代碼綜合成邏輯網(wǎng)表，映射到FPGA 上的資源；最后在FPGA 上完成布局布線(xiàn)，并再次測(cè)試。傳統(tǒng)的FPGA開(kāi)發(fā)需要很長(zhǎng)的開(kāi)發(fā)周期，很難滿(mǎn)足快速更新要求。

圖3 CPG整體模型

基于模型設(shè)計(jì)，利用Matlab中的HDL Coder工具實(shí)現(xiàn)代碼轉(zhuǎn)換過(guò)程，如圖4 所示。首先利用Mathworks的Matlab／Simulink的系統(tǒng)設(shè)計(jì)能力和無(wú)縫分析能力，以及已有的MegaCore完成頂層系統(tǒng)設(shè)計(jì)；接著通過(guò)轉(zhuǎn)換工具配置Simulink中的IP核，即將靜態(tài)參數(shù)做定點(diǎn)化處理；再將Simulink模型文件（.mdl）轉(zhuǎn)換成VHDL 的RTL 表述和工具命令語(yǔ)言（Tcl）腳本，同時(shí)還可進(jìn)行RTL級(jí)的功能仿真；然后通過(guò)SOPC 設(shè)計(jì)工具進(jìn)行綜合、適配與時(shí)序仿真；最后形成對(duì)指定FPGA 進(jìn)行編程配置的文件，實(shí)現(xiàn)硬件系統(tǒng)的仿真測(cè)試模型。

圖4 采用基于模型設(shè)計(jì)加速的整個(gè)過(guò)程

2.3 Verilog代碼生成

轉(zhuǎn)換需要兩個(gè)過(guò)程：第一個(gè)過(guò)程，審察整個(gè)設(shè)計(jì)并確定模型的公用模塊，每一個(gè)公用模塊都將被描述成一個(gè)單獨(dú)的VHDL文件。從Simulink的基本模塊中區(qū)分模型的次級(jí)模塊，并采集每一個(gè)模塊端口及端口類(lèi)型的相關(guān)信息，在編寫(xiě)VHDL文件進(jìn)行內(nèi)容描述時(shí)將會(huì)用到這些信息。第二個(gè)過(guò)程，算法反復(fù)地檢查模型的全部層次，從最高層次到底層，為每一個(gè)在第一個(gè)過(guò)程中找到的模塊生成結(jié)構(gòu)描述。HDL Coder不支持矩陣，對(duì)原模型做適當(dāng)修改，將內(nèi)部的矩陣相乘改為相乘相加的形式，運(yùn)行模型中的.m 文件，并在Simulink中完成仿真。右鍵點(diǎn)擊模型選擇Fixed－Point Tool進(jìn)入對(duì)話(huà)框，按照?qǐng)D5中右面步驟一步步完成定點(diǎn)轉(zhuǎn)換。點(diǎn)擊Fixed－Point Advisor打開(kāi)定點(diǎn)向?qū)?，右鍵選擇Run to Failure運(yùn)行向?qū)б徊讲津?yàn)證，當(dāng)出現(xiàn)錯(cuò)誤或者警告時(shí)，按照提示做出相應(yīng)調(diào)整。

完成定點(diǎn)轉(zhuǎn)換以后，右鍵點(diǎn)擊模型選擇HDL Code下的HDL Code Properties進(jìn)入圖6所示對(duì)話(huà)框。HDL Coder支持代碼自動(dòng)生成，同時(shí)還可以生成仿真用的Test Bench，可以大大縮短驗(yàn)證時(shí)間，從而縮短設(shè)計(jì)周期。

圖5 定點(diǎn)化處理

圖6 HDL Coder代碼自動(dòng)生成

3 協(xié)同仿真驗(yàn)證

為了綜合驗(yàn)證算法的有效性，生成代碼以后還需要驗(yàn)證生成代碼與Simulink模型的一致性。文中采用了協(xié)同仿真驗(yàn)證，本文借助Matlab內(nèi)的HDL Coder功能協(xié)助生成測(cè)試腳本，完成相應(yīng)的仿真驗(yàn)證。選擇HDL Code Generation中的Test Bench，勾選HDL Test Bench和Cosimulation兩個(gè)選項(xiàng)用于聯(lián)合仿真，點(diǎn)擊右下角的Generate Test Bench，生成測(cè)試腳本，同時(shí)產(chǎn)生用于聯(lián)合仿真的模塊，如圖7所示。圖7（a）為原Simulink模型，圖7（b）是用于調(diào)用Modesim 對(duì)HDL代碼進(jìn)行仿真。

點(diǎn)擊圖（a）右上角模塊打開(kāi)Modesim，點(diǎn)擊Simulink中的運(yùn)行進(jìn)行仿真。在Simulink適配器中產(chǎn)生數(shù)據(jù)波形如圖8所示。上面為Modesim 輸出的波形，下面為Simulink中輸出波形，最后一個(gè)為兩個(gè)之間的差值。如圖能清晰地看出兩個(gè)波形輸出完全一致，驗(yàn)證了HDL 代碼與Simulink模型完全一致。

圖7 聯(lián)合仿真模塊

圖8 輸出效果對(duì)比圖

結(jié) 語(yǔ)

為了驗(yàn)證四足機(jī)器人步態(tài)算法的可行性，采用Matlab與Modelsim 協(xié)同仿真的方法，力求通過(guò)聯(lián)合仿真的方式實(shí)現(xiàn)驗(yàn)證的完整性。在Simulink中建立系統(tǒng)模型，完成初步仿真，使用了Matlab內(nèi)的HDL Coder將Simulink模型自動(dòng)轉(zhuǎn)換為Verilog，實(shí)現(xiàn)FPGA 系統(tǒng)級(jí)的仿真。這既保證了代碼的準(zhǔn)確性，又節(jié)省了復(fù)雜的Test Bench編寫(xiě)過(guò)程，有效加快了仿真的速度，縮短了設(shè)計(jì)周期。最后借助Modelsim 實(shí)現(xiàn)仿真數(shù)據(jù)的前后對(duì)比，直觀(guān)有效地驗(yàn)證了步態(tài)算法的有效性。

［1］郭云飛，孫福海.Modelsim 與Matlab／Simulink聯(lián)合仿真技術(shù)研究及應(yīng)用［J］.光電技術(shù)應(yīng)用，2013，28（6）.

［2］劉杰.基于模型的設(shè)計(jì)—Qsys篇［M］.北京：機(jī)械工業(yè)出版社，2012.

［3］陳永春.從Matlab／Simulink模型到代碼實(shí)現(xiàn)［M］.北京：清華大學(xué)出版社，2002.

［4］諸葛俊貴.基于Matlab模糊控制器HDL 代碼的自動(dòng)生成［J］.電子科技，2012，25（1）：42－44.

［5］楊守良.Matlab／simulink在FPGA 設(shè)計(jì)中的應(yīng)用［J］.微計(jì)算機(jī)信息，2005，21（23）.

［6］Ying－Shieh Kung，Nguyen Vu Quynh.Simulink／ModelSim co－simulation of sensorless PMSM speed controller［J］.Proceedings 2011IEEE Symposium on Industrial Electronics and Applications，2011，24（9）.

［7］Hatnik U，Altmann S.Using ModelSim，Matlab／Simulink and NS for simulation of distributed systems［J］.International Conference on Parallel Computing in Electrical Engineering，2004（9）：7－10.

［8］杜勇，劉帝英.MATLAB在FPGA 設(shè)計(jì)中的應(yīng)用［J］.電子工程師，2007（1）.

［9］胡迎剛.基于FPGA的DSP設(shè)計(jì)方法的研究［J］.工會(huì)博覽·理論研究，2010（5）.