柚子快報邀請碼778899分享：四足機器人算法驗證流程框架

http://yzkb.51969.com/

一、四足機器人基礎介紹

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

圖1.1 自足機器人軟件硬件控制流程圖

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

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

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

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

（5）更高的要求，比如自主導航，視覺輔助等，則可以給計算機板卡加個工控機運行SLAM或者AI算法。

（一）腿部硬件組成

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

圖1.2 四足機器人單腿通信控制流程

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

（二）數據轉換板

其中數據轉換板在四足機器人的腿部使用多個電機來實現(xiàn)運動和步態(tài)控制。每個腿部電機都需要根據控制指令進行旋轉，以驅動腿部關節(jié)的運動。腿部電機數據轉換板起到了信號轉換和驅動功率放大的作用。

圖1 數據轉換板

使用的是GD32F150R8T6，一個GD32F4通過CAN總線負責兩條腿，一個腿由3個GD32控制板通過FOC算法控制腿部三個電機，而且只運行了FOC算法。四足機器人腿部電機數據轉換板是一種用于控制四足機器人腿部電機的電路板。主要功能是將來自主控制器的信號轉換為適合驅動腿部電機的電流和電壓信號。

在四足機器人腿部電機數據轉換板上，通常包含以下主要組件：

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

電機驅動器：將控制信號轉換為適合驅動腿部電機的電流和電壓信號。電機驅動器可以采用各種技術，如MOSFET（金屬氧化物半導體場效應晶體管）驅動器、H橋驅動器等。

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

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

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

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

（三）IMU傳感器

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

圖2 IMU傳感器

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

加速度計：用于測量機器人的加速度，可以檢測機器人的靜態(tài)或動態(tài)加速度，并將其轉換為電信號輸出。

陀螺儀：用于測量機器人的旋轉速度，可以檢測機器人的角速度，并將其轉換為電信號輸出。

磁力計：可用于檢測地球磁場方向，以提供機器人的定位信息。通常情況下，磁力計會與加速度計和陀螺儀配合使用，以提供更為準確的位置估計。

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

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

運動控制：IMU模塊可以測量機器人的加速度和角速度，從而為機器人提供實時的運動狀態(tài)反饋，以便控制機器人的移動、速度和方向等。

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

（四）電機控制器與磁編碼器

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

圖3 磁編碼器

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

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

磁鐵：固定在電機軸上，作為參考點，以提供電機旋轉的基準點。

磁編碼器感應器：通常使用霍爾效應傳感器或光電傳感器，用于檢測磁鐵的磁場變化，并將其轉換為電信號輸出。

信號處理器：用于處理磁編碼器產生的電信號，以確定電機旋轉的位置和角度。

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

磁編碼器的原理是基于磁場感應原理，利用磁鐵和磁編碼器感應器之間的磁場變化來確定電機旋轉的位置和角度。具體地說，當電機旋轉時，磁鐵會改變周圍的磁場分布，磁編碼器感應器會檢測到這種變化，并將其轉換為電信號輸出。信號處理器會將這些輸出信號處理成電機旋轉的位置和角度數據，并將其反饋給電機控制板。

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

二、算法設計簡介

2.1 控制算法簡介

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

圖2.1 FOC控制框圖

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

圖2.2 ZMP原理構型

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

圖2.3 MPC原理構型

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

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

(一)四足機器人建模

圖2.4 機器狗仿真模型

圖2.5 simulink仿真模塊

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

圖2.6 正逆運動學理論與simulink仿真模塊

其中利用正逆運動學解算來編寫四足機器人Matlab的simulink控制器與規(guī)劃器：

（1）正運動學解算：

正運動學解算用于將關節(jié)角度映射到機器人的末端執(zhí)行器位置。可以通過機器人的幾何模型來實現(xiàn)。在Simulink中，可以使用機器人工具箱（Robotics Toolbox）來進行正運動學計算。根據機器人的幾何模型和關節(jié)角度輸入，輸出機器人的末端執(zhí)行器位置。

（2）逆運動學解算：

逆運動學解算用于根據期望的末端執(zhí)行器位置來計算關節(jié)角度。這對于控制器和規(guī)劃器來說非常重要，因為可以將期望的末端執(zhí)行器軌跡轉化為關節(jié)角度軌跡。逆運動學解算通常涉及求解非線性方程組，可以使用數值方法（例如牛頓-拉夫遜法）或符號計算方法來實現(xiàn)。

（3）控制器設計：

控制器的設計是根據機器人的動力學模型和控制目標來確定的?？梢园ㄎ恢每刂?、力/力矩控制、姿態(tài)控制等。在Simulink中，使用建模和仿真工具來設計和驗證控制器的性能。通過將正逆運動學解算與控制器結合起來，實現(xiàn)機器人的閉環(huán)控制。

（4）規(guī)劃器設計：

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

理解機器人的幾何和動力學模型是至關重要的，這有助于進行正逆運動學解算和控制器設計。

使用合適的數值方法或符號計算方法實現(xiàn)逆運動學解算，并對求解過程進行優(yōu)化，以提高計算效率和準確性。在設計控制器時，考慮機器人的動力學特性和控制目標，并進行模擬和實驗驗證，以確保控制器的性能和穩(wěn)定性。規(guī)劃器的設計需要考慮機器人的約束條件和任務要求，同時結合逆運動學解算生成關節(jié)角度軌跡。采用模塊化的設計方法，將正逆運動學解算、控制器和規(guī)劃器分開實現(xiàn)，并進行集成測試，以確保整個系統(tǒng)的一致性和正確性。

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

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

（1）建立機器狗模型：

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

（2）編寫C算法代碼：

可以使用Python語言編寫算法代碼，并調用Mujoco Python包中的API來實現(xiàn)與Mujoco仿真軟件的交互。在VSMC算法中，需要定義神經元網絡的結構和參數，并將其與機器狗模型進行耦合，以實現(xiàn)運動控制。

（3）進行仿真測試：

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

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

建模都是從復雜到簡單:RBD->SRDB/CD->ZMP/CoP->LIP，而實際應用中控制器是從簡單到復雜:阻抗(無模型)->VMC/Balance QP/MPC(質心動力學模型)->WBC(浮動基座動力學模型+優(yōu)先級的任務空間控制)

（1）研讀四足機器人論文

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

（2）編寫控制代碼

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

（3）進行仿真驗證

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

（4）實驗測試

在完成仿真驗證后，將控制代碼上傳到實際的四足機器人系統(tǒng)中進行實驗測試。需要將控制代碼與機器人的硬件系統(tǒng)結合起來，并進行實時控制。根據實驗結果，可以評估控制算法的實際應用效果，并進行調試和優(yōu)化。在編寫和調試四足機器人控制控制代碼時，需要具備一定的機器人學、控制理論和編程經驗，同時需要進行充分的仿真驗證和實驗測試，以確保控制算法的有效性和可靠性。

柚子快報邀請碼778899分享：四足機器人算法驗證流程框架

http://yzkb.51969.com/

精彩文章