柚子快報(bào)邀請(qǐng)碼778899分享：四足機(jī)器人算法驗(yàn)證流程框架

http://yzkb.51969.com/

一、四足機(jī)器人基礎(chǔ)介紹

1.1 四足機(jī)器人項(xiàng)目系統(tǒng)框圖

圖1.1 自足機(jī)器人軟件硬件控制流程圖

（1）操作員通過遙控器給機(jī)械狗下發(fā)平動(dòng)速度 和轉(zhuǎn)向率命令；

（2）機(jī)械狗接收到命令，質(zhì)心COM生成參考軌跡并傳送給身體和腿部控制器。

（3）控制器根據(jù)用戶輸入命令和機(jī)器人狀態(tài)，如果腿處于腿在擺動(dòng)中，則使用“擺動(dòng)腿控制器”，如果腿處于腿在支撐中，則使用“力控支撐腿控制器”

（4）力和位置指令被發(fā)送到STM32微控制器，用于將電機(jī)指令傳遞給機(jī)器人的每條腿。

（5）更高的要求，比如自主導(dǎo)航，視覺輔助等，則可以給計(jì)算機(jī)板卡加個(gè)工控機(jī)運(yùn)行SLAM或者AI算法。

（一）腿部硬件組成

腿部硬件組成以1條腿進(jìn)行設(shè)計(jì)，其它腿也一樣，嵌入硬件設(shè)計(jì)部分見圖2.2所示：

圖1.2 四足機(jī)器人單腿通信控制流程

腿部控制器基于Cortex-A8的處理器，執(zhí)行腿部控制任務(wù)，如笛卡爾阻抗的控制或聯(lián)合PD控制。負(fù)責(zé)完成較高的腿部頻率和高速關(guān)節(jié)速度搖擺時(shí)的濾波、高帶寬跟蹤相位以及其他位置控制動(dòng)作。最后，每條腿控制器發(fā)送扭矩指令并接收無刷電機(jī)上的編碼器測(cè)量值?？梢钥闯鯱P board計(jì)算機(jī)板卡的核心功能就是進(jìn)行運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)運(yùn)算。使用第二代酷睿i7處理器，運(yùn)行UbuntuLinux和配置搶占RT補(bǔ)丁。UP board通過以太網(wǎng)與筆記本電腦通信，UP board使用輕量級(jí)接收用戶命令并記錄數(shù)據(jù)通信和編組（LCM）。

（二）數(shù)據(jù)轉(zhuǎn)換板

其中數(shù)據(jù)轉(zhuǎn)換板在四足機(jī)器人的腿部使用多個(gè)電機(jī)來實(shí)現(xiàn)運(yùn)動(dòng)和步態(tài)控制。每個(gè)腿部電機(jī)都需要根據(jù)控制指令進(jìn)行旋轉(zhuǎn)，以驅(qū)動(dòng)腿部關(guān)節(jié)的運(yùn)動(dòng)。腿部電機(jī)數(shù)據(jù)轉(zhuǎn)換板起到了信號(hào)轉(zhuǎn)換和驅(qū)動(dòng)功率放大的作用。

圖1 數(shù)據(jù)轉(zhuǎn)換板

使用的是GD32F150R8T6，一個(gè)GD32F4通過CAN總線負(fù)責(zé)兩條腿，一個(gè)腿由3個(gè)GD32控制板通過FOC算法控制腿部三個(gè)電機(jī)，而且只運(yùn)行了FOC算法。四足機(jī)器人腿部電機(jī)數(shù)據(jù)轉(zhuǎn)換板是一種用于控制四足機(jī)器人腿部電機(jī)的電路板。主要功能是將來自主控制器的信號(hào)轉(zhuǎn)換為適合驅(qū)動(dòng)腿部電機(jī)的電流和電壓信號(hào)。

在四足機(jī)器人腿部電機(jī)數(shù)據(jù)轉(zhuǎn)換板上，通常包含以下主要組件：

控制信號(hào)接口：用于接收來自主控制器的控制信號(hào)，通常是PWM（脈沖寬度調(diào)制）信號(hào)或者其他數(shù)字信號(hào)。

電機(jī)驅(qū)動(dòng)器：將控制信號(hào)轉(zhuǎn)換為適合驅(qū)動(dòng)腿部電機(jī)的電流和電壓信號(hào)。電機(jī)驅(qū)動(dòng)器可以采用各種技術(shù)，如MOSFET（金屬氧化物半導(dǎo)體場(chǎng)效應(yīng)晶體管）驅(qū)動(dòng)器、H橋驅(qū)動(dòng)器等。

電源接口：提供電源輸入接口，連接外部電源供應(yīng)，并為腿部電機(jī)提供所需的電流和電壓。

保護(hù)電路：用于保護(hù)電機(jī)和電路板免受過電流、過熱和電源過壓等因素的損害。常見的保護(hù)電路包括過電流保護(hù)、過溫保護(hù)和瞬態(tài)電壓抑制器等。

反饋接口：腿部電機(jī)數(shù)據(jù)轉(zhuǎn)換板還可以包含反饋接口，用于讀取電機(jī)的位置、速度或電流等信息，以提供閉環(huán)控制。

腿部電機(jī)數(shù)據(jù)轉(zhuǎn)換板的設(shè)計(jì)和功能會(huì)根據(jù)具體的四足機(jī)器人應(yīng)用而有所不同。一些高級(jí)的轉(zhuǎn)換板可能還會(huì)集成額外的功能，如通信接口（如CAN、UART等）和編碼器接口等，以更好地與其他系統(tǒng)進(jìn)行連接和協(xié)作。

（三）IMU傳感器

四足機(jī)器人IMU模塊的原理是基于牛頓力學(xué)原理和運(yùn)動(dòng)學(xué)原理，通過測(cè)量機(jī)器人的加速度和角速度來計(jì)算機(jī)器人的位置、速度和姿態(tài)等數(shù)據(jù)。這些數(shù)據(jù)可以用于控制機(jī)器人的運(yùn)動(dòng)和姿態(tài)調(diào)整，例如跳躍、爬行、行走和平衡等。

圖2 IMU傳感器

用的是串口通信，每個(gè)STM32還有獨(dú)立串口通信，用于模塊的設(shè)置與debug。IMU模塊通常由以下主要組件組成：

加速度計(jì)：用于測(cè)量機(jī)器人的加速度，可以檢測(cè)機(jī)器人的靜態(tài)或動(dòng)態(tài)加速度，并將其轉(zhuǎn)換為電信號(hào)輸出。

陀螺儀：用于測(cè)量機(jī)器人的旋轉(zhuǎn)速度，可以檢測(cè)機(jī)器人的角速度，并將其轉(zhuǎn)換為電信號(hào)輸出。

磁力計(jì)：可用于檢測(cè)地球磁場(chǎng)方向，以提供機(jī)器人的定位信息。通常情況下，磁力計(jì)會(huì)與加速度計(jì)和陀螺儀配合使用，以提供更為準(zhǔn)確的位置估計(jì)。

四足機(jī)器人IMU模塊的用途非常廣泛，主要包括以下幾個(gè)方面：

姿態(tài)控制：IMU模塊可以測(cè)量機(jī)器人的姿態(tài)和旋轉(zhuǎn)速度，從而為機(jī)器人提供實(shí)時(shí)的姿態(tài)反饋，使機(jī)器人能夠更加精確地控制的姿態(tài)和避免傾覆。

運(yùn)動(dòng)控制：IMU模塊可以測(cè)量機(jī)器人的加速度和角速度，從而為機(jī)器人提供實(shí)時(shí)的運(yùn)動(dòng)狀態(tài)反饋，以便控制機(jī)器人的移動(dòng)、速度和方向等。

環(huán)境感知：IMU模塊可以檢測(cè)機(jī)器人所處的環(huán)境信息，例如地面坡度、震動(dòng)等，這些信息可以為機(jī)器人提供更準(zhǔn)確的運(yùn)動(dòng)預(yù)測(cè)和控制。

（四）電機(jī)控制器與磁編碼器

四足機(jī)器人電機(jī)控制器與磁編碼器是電機(jī)控制器接收數(shù)據(jù)轉(zhuǎn)換板轉(zhuǎn)發(fā)來的信息（力矩、位置、速度及位置和速度的增益），并且上傳反饋信息（位置、速度以及基于電流環(huán)測(cè)量的扭矩）最大通信頻率：4kHz/驅(qū)動(dòng)器數(shù)目 （可通過增加CAN總線數(shù)目擴(kuò)大通信帶寬）

圖3 磁編碼器

四足機(jī)器人電機(jī)控制板中的磁編碼器是一種可以測(cè)量電機(jī)旋轉(zhuǎn)角度和速度的傳感器?？梢酝ㄟ^檢測(cè)磁場(chǎng)變化，計(jì)算出電機(jī)旋轉(zhuǎn)的精確位置和角度，并將這些數(shù)據(jù)反饋給電機(jī)控制板，以實(shí)現(xiàn)更加精準(zhǔn)的電機(jī)控制。

磁編碼器通常由以下主要組件組成：

磁鐵：固定在電機(jī)軸上，作為參考點(diǎn)，以提供電機(jī)旋轉(zhuǎn)的基準(zhǔn)點(diǎn)。

磁編碼器感應(yīng)器：通常使用霍爾效應(yīng)傳感器或光電傳感器，用于檢測(cè)磁鐵的磁場(chǎng)變化，并將其轉(zhuǎn)換為電信號(hào)輸出。

信號(hào)處理器：用于處理磁編碼器產(chǎn)生的電信號(hào)，以確定電機(jī)旋轉(zhuǎn)的位置和角度。

反饋接口：將磁編碼器的位置和角度數(shù)據(jù)反饋給電機(jī)控制板，以實(shí)現(xiàn)閉環(huán)控制。

磁編碼器的原理是基于磁場(chǎng)感應(yīng)原理，利用磁鐵和磁編碼器感應(yīng)器之間的磁場(chǎng)變化來確定電機(jī)旋轉(zhuǎn)的位置和角度。具體地說，當(dāng)電機(jī)旋轉(zhuǎn)時(shí)，磁鐵會(huì)改變周圍的磁場(chǎng)分布，磁編碼器感應(yīng)器會(huì)檢測(cè)到這種變化，并將其轉(zhuǎn)換為電信號(hào)輸出。信號(hào)處理器會(huì)將這些輸出信號(hào)處理成電機(jī)旋轉(zhuǎn)的位置和角度數(shù)據(jù)，并將其反饋給電機(jī)控制板。

在四足機(jī)器人中，通過使用磁編碼器可以實(shí)現(xiàn)對(duì)腿部電機(jī)的精確控制和運(yùn)動(dòng)驅(qū)動(dòng)。磁編碼器可以用于檢測(cè)電機(jī)的位置、速度和加速度等參數(shù)，從而提高機(jī)器人的定位精度和運(yùn)動(dòng)控制能力。同時(shí)，磁編碼器還可以用于檢測(cè)動(dòng)態(tài)負(fù)載和防止電機(jī)過載，以提高機(jī)器人的安全性和穩(wěn)定性。

二、算法設(shè)計(jì)簡(jiǎn)介

2.1 控制算法簡(jiǎn)介

基礎(chǔ)的有電機(jī)上運(yùn)行的FOC，是一種對(duì)無刷電機(jī)的驅(qū)動(dòng)控制方法，通過精確地控制磁場(chǎng)大小與方向，使得電機(jī)的運(yùn)動(dòng)轉(zhuǎn)矩平穩(wěn)、噪聲小、效率高，并且具有高速的動(dòng)態(tài)響應(yīng)?？梢宰寣?duì)無刷電機(jī)進(jìn)行“像素級(jí)”控制。

圖2.1 FOC控制框圖

機(jī)器狗腿部無刷電機(jī)FOC控制算法程序，F(xiàn)OC算法的優(yōu)劣直接影響機(jī)械狗的運(yùn)動(dòng)性能，可以說是底層最重要的一環(huán)。

圖2.2 ZMP原理構(gòu)型

經(jīng)典控制算法ZMP算法：ZMP (Zero Moment Point)是一個(gè)用于機(jī)器人平衡的方法，通過確保經(jīng)過重心垂直的力矩線在支撐面之上，來保證機(jī)器人的穩(wěn)定性。

圖2.3 MPC原理構(gòu)型

主流控制算法MPC算法：模型預(yù)測(cè)控制（MPC）是四足機(jī)器人中一種普遍采用的控制策略。MPC利用機(jī)器人的動(dòng)力學(xué)模型，通過預(yù)測(cè)和優(yōu)化目標(biāo)函數(shù)，實(shí)時(shí)規(guī)劃最優(yōu)的步行和運(yùn)動(dòng)策略，有效處理多步穩(wěn)定性和系統(tǒng)約束問題。

2.2 ?控制算法開發(fā)過程

(一)四足機(jī)器人建模

圖2.4 機(jī)器狗仿真模型

圖2.5 simulink仿真模塊

首先通過Matlab建利機(jī)器狗物理模、?simulink模塊拖拽、?綁定、?設(shè)定參數(shù)、附加功能、正弦波輸入23腿、反向輸入14腿?控制機(jī)器測(cè)試模型

圖2.6 正逆運(yùn)動(dòng)學(xué)理論與simulink仿真模塊

其中利用正逆運(yùn)動(dòng)學(xué)解算來編寫四足機(jī)器人Matlab的simulink控制器與規(guī)劃器：

（1）正運(yùn)動(dòng)學(xué)解算：

正運(yùn)動(dòng)學(xué)解算用于將關(guān)節(jié)角度映射到機(jī)器人的末端執(zhí)行器位置?？梢酝ㄟ^機(jī)器人的幾何模型來實(shí)現(xiàn)。在Simulink中，可以使用機(jī)器人工具箱（Robotics Toolbox）來進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算。根據(jù)機(jī)器人的幾何模型和關(guān)節(jié)角度輸入，輸出機(jī)器人的末端執(zhí)行器位置。

（2）逆運(yùn)動(dòng)學(xué)解算：

逆運(yùn)動(dòng)學(xué)解算用于根據(jù)期望的末端執(zhí)行器位置來計(jì)算關(guān)節(jié)角度。這對(duì)于控制器和規(guī)劃器來說非常重要，因?yàn)榭梢詫⑵谕哪┒藞?zhí)行器軌跡轉(zhuǎn)化為關(guān)節(jié)角度軌跡。逆運(yùn)動(dòng)學(xué)解算通常涉及求解非線性方程組，可以使用數(shù)值方法（例如牛頓-拉夫遜法）或符號(hào)計(jì)算方法來實(shí)現(xiàn)。

（3）控制器設(shè)計(jì)：

控制器的設(shè)計(jì)是根據(jù)機(jī)器人的動(dòng)力學(xué)模型和控制目標(biāo)來確定的?？梢园ㄎ恢每刂啤⒘?力矩控制、姿態(tài)控制等。在Simulink中，使用建模和仿真工具來設(shè)計(jì)和驗(yàn)證控制器的性能。通過將正逆運(yùn)動(dòng)學(xué)解算與控制器結(jié)合起來，實(shí)現(xiàn)機(jī)器人的閉環(huán)控制。

（4）規(guī)劃器設(shè)計(jì)：

規(guī)劃器用于生成機(jī)器人的軌跡，使其能夠完成各種任務(wù)，例如行走、躲避障礙物等。規(guī)劃器可以基于運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)模型進(jìn)行設(shè)計(jì)，同時(shí)考慮機(jī)器人的約束條件。在Simulink中，使用路徑規(guī)劃工具箱（Path Planning Toolbox）來設(shè)計(jì)和生成機(jī)器人的軌跡。規(guī)劃器通常與逆運(yùn)動(dòng)學(xué)解算相結(jié)合，以生成關(guān)節(jié)角度軌跡。

理解機(jī)器人的幾何和動(dòng)力學(xué)模型是至關(guān)重要的，這有助于進(jìn)行正逆運(yùn)動(dòng)學(xué)解算和控制器設(shè)計(jì)。

使用合適的數(shù)值方法或符號(hào)計(jì)算方法實(shí)現(xiàn)逆運(yùn)動(dòng)學(xué)解算，并對(duì)求解過程進(jìn)行優(yōu)化，以提高計(jì)算效率和準(zhǔn)確性。在設(shè)計(jì)控制器時(shí)，考慮機(jī)器人的動(dòng)力學(xué)特性和控制目標(biāo)，并進(jìn)行模擬和實(shí)驗(yàn)驗(yàn)證，以確?？刂破鞯男阅芎头€(wěn)定性。規(guī)劃器的設(shè)計(jì)需要考慮機(jī)器人的約束條件和任務(wù)要求，同時(shí)結(jié)合逆運(yùn)動(dòng)學(xué)解算生成關(guān)節(jié)角度軌跡。采用模塊化的設(shè)計(jì)方法，將正逆運(yùn)動(dòng)學(xué)解算、控制器和規(guī)劃器分開實(shí)現(xiàn)，并進(jìn)行集成測(cè)試，以確保整個(gè)系統(tǒng)的一致性和正確性。

圖3.7 Linux環(huán)境下Mujoco四足機(jī)器人建模

同樣在后期在Linux系統(tǒng)Mujoco仿真軟件中建立機(jī)器狗模型測(cè)試算法：

（1）建立機(jī)器狗模型：

在Mujoco仿真軟件中，使用MJCF文件格式來定義機(jī)器狗的模型。指定機(jī)器狗的各個(gè)部分（例如身體、腿、關(guān)節(jié)等）的參數(shù)，以及之間的連接方式和約束條件。手動(dòng)編寫MJCF文件，也可以使用Mujoco官方提供的模型編輯器（Model Editor）來進(jìn)行可視化編輯。

（2）編寫C算法代碼：

可以使用Python語言編寫算法代碼，并調(diào)用Mujoco Python包中的API來實(shí)現(xiàn)與Mujoco仿真軟件的交互。在VSMC算法中，需要定義神經(jīng)元網(wǎng)絡(luò)的結(jié)構(gòu)和參數(shù)，并將其與機(jī)器狗模型進(jìn)行耦合，以實(shí)現(xiàn)運(yùn)動(dòng)控制。

（3）進(jìn)行仿真測(cè)試：

在完成機(jī)器狗模型和算法的編寫后，使用Mujoco仿真軟件進(jìn)行仿真測(cè)試。在Python代碼中調(diào)用Mujoco?Python包中的API來加載機(jī)器狗模型、運(yùn)行算法，并獲取仿真結(jié)果（例如機(jī)器狗的位置、姿態(tài)、速度等）。根據(jù)仿真結(jié)果，評(píng)估算法的性能，并進(jìn)行調(diào)試和優(yōu)化。在Mujoco仿真軟件中建立機(jī)器狗模型并測(cè)試VSMC算法需要一定的編程和仿真經(jīng)驗(yàn)，但是可以幫助快速驗(yàn)證算法的有效性，并且大大減少了實(shí)際物理實(shí)驗(yàn)的成本和時(shí)間。

（二）四足機(jī)器人控制算法開發(fā)

建模都是從復(fù)雜到簡(jiǎn)單:RBD->SRDB/CD->ZMP/CoP->LIP，而實(shí)際應(yīng)用中控制器是從簡(jiǎn)單到復(fù)雜:阻抗(無模型)->VMC/Balance QP/MPC(質(zhì)心動(dòng)力學(xué)模型)->WBC(浮動(dòng)基座動(dòng)力學(xué)模型+優(yōu)先級(jí)的任務(wù)空間控制)

（1）研讀四足機(jī)器人論文

在編寫控制代碼前，先研究相關(guān)的四足機(jī)器人論文，了解其結(jié)構(gòu)、運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性，以及控制的基本思想和實(shí)現(xiàn)方法。這將幫助更好地理解控制算法的原理和優(yōu)缺點(diǎn)，并為后續(xù)的代碼編寫和調(diào)試提供指導(dǎo)。

（2）編寫控制代碼

根據(jù)控制的思想和實(shí)現(xiàn)方法，使用編程語言（例如MATLAB或Python）編寫控制代碼。在編寫代碼時(shí)，考慮機(jī)器人的動(dòng)力學(xué)模型和控制目標(biāo)，并設(shè)計(jì)合適的滑模面和控制律。使用數(shù)值方法（例如Euler法或Runge-Kutta法）來進(jìn)行離散化和數(shù)值求解。

（3）進(jìn)行仿真驗(yàn)證

在完成控制代碼編寫后，使用仿真軟件（MATLAB Simulink或Mujoco）對(duì)四足機(jī)器人進(jìn)行仿真驗(yàn)證。需要將機(jī)器人的動(dòng)力學(xué)模型和控制代碼結(jié)合起來，并進(jìn)行集成測(cè)試。根據(jù)仿真結(jié)果，評(píng)估控制算法的性能和穩(wěn)定性，并進(jìn)行調(diào)試和優(yōu)化。

（4）實(shí)驗(yàn)測(cè)試

在完成仿真驗(yàn)證后，將控制代碼上傳到實(shí)際的四足機(jī)器人系統(tǒng)中進(jìn)行實(shí)驗(yàn)測(cè)試。需要將控制代碼與機(jī)器人的硬件系統(tǒng)結(jié)合起來，并進(jìn)行實(shí)時(shí)控制。根據(jù)實(shí)驗(yàn)結(jié)果，可以評(píng)估控制算法的實(shí)際應(yīng)用效果，并進(jìn)行調(diào)試和優(yōu)化。在編寫和調(diào)試四足機(jī)器人控制控制代碼時(shí)，需要具備一定的機(jī)器人學(xué)、控制理論和編程經(jīng)驗(yàn)，同時(shí)需要進(jìn)行充分的仿真驗(yàn)證和實(shí)驗(yàn)測(cè)試，以確?？刂扑惴ǖ挠行院涂煽啃浴?/p>

柚子快報(bào)邀請(qǐng)碼778899分享：四足機(jī)器人算法驗(yàn)證流程框架

http://yzkb.51969.com/

精彩文章