小型雙足機器人設計方案

1五湖四海1

仿生機器人主要是模仿人的步伐進行行走,，從最開始的靜態(tài)步伐發(fā)展到了像人一樣的動態(tài)步伐,，開發(fā)者們經(jīng)歷了漫長的時間研發(fā),，最終實現(xiàn)了接近人類的漂亮步伐,，下面我來介紹下我們研發(fā)發(fā)雙機器人軟件架構和硬件構成,。

硬件控制板大體分為主控板和主機板

! {2 c3 O7 F* `- a3 M3 B% j( v

主控負責運動控制和采集傳感器數(shù)據(jù),，采用stm32h743 跑rtthread RTOS系統(tǒng)。下面是主控上電啟動的效果,。

% _7 k- c/ g4 n$ w) v

主機板主要負責運動具體控制,，采用樹莓派4b運行官方Raspbian系統(tǒng)，基于樹莓派平臺運行控制軟件采用QT編寫,，并配有5寸hdmi顯示屏和兩個小音箱提供了多媒體功能,。

. v- ?/ T0 s3 U8 d- F7 _6 s, c' y
/ y7 Q- `  O* O

腿部結構部分是采用12個舵機作為關節(jié)提供動力，舵機是RS485總線方式與主控板進行通信,。

0 A* T; l9 m6 j

主機與主控通信采用Mobus協(xié)議通信，主機作為客戶端,，主控作為服務端,，在主控上的文件系統(tǒng)映射一個modbus寄存器區(qū)域,。

1 }% Z' ~# Y5 ]: a* S

傳感器分布

主控上有九軸陀螺傳感器用于獲取身體姿態(tài)

樹莓派接8個tof激光測距傳感器通過i2c總線連接，安裝在雙足的四個角上面用了測量雙足觸地情況,。

1 f; e1 }5 c& K2 B- u. Q/ h! r/ e

整體框架是PC機電腦和樹莓派開發(fā)板上運行QT運動控制軟件控制主控板完成各個關節(jié)運動控制,，主控板映射一個寄存器內存需要，建立兩個modbus服務器,，一個用來連接PC機一個用來連接樹莓派開發(fā)板,。

具體模塊介紹待續(xù)....

: W' [# i( U# i; @

1五湖四海1

最早的控制策略是開環(huán)控制,，根據(jù)控制模式計算出理論的步伐數(shù)據(jù)去執(zhí)行，但實際中會受到地面不平等因素影響機器人走穩(wěn),，所以需要采用閉環(huán)控制很有必要,。下面的思路是通過陀螺儀傳感器獲取機器人姿態(tài)，然后通過pid控制器輸出需要調整的角度,，在把目標角度重新調整下,，最后控制伺服電機帶動關節(jié)轉動。

上位機需要根據(jù)用戶輸入的運動動作和速度參數(shù)轉換為時間相等的小段運動動作,，這個相等的時間就是pid控制周期,。轉換后的小段數(shù)據(jù)依次發(fā)送到主控端，運動數(shù)據(jù)需要做流控等待主控運動隊列空閑空余時進行發(fā)送數(shù)據(jù),。

主控端接收運動隊列的每一段運動數(shù)據(jù),，進行獲取陀螺儀數(shù)據(jù)后進行pid控制器輸出需要調整的角度，俯仰角是根據(jù)pid控制器的輸出調整臀俯仰關節(jié),。橫滾角是根據(jù)pid控制器的輸出調整雙足的高度重而調整身體橫滾姿態(tài),。

1五湖四海1

4 f) [/ W% p2 H1 C  R" U3 i) S+ k
4 M9 M" c% X4 j& _) L5 B- o$ J
! j3 p; J/ F# G. {& ?

上圖是站立姿態(tài)腿部膝蓋俯仰角,，髖俯仰角，踝俯仰角的示意圖,。

最終需要計算出a,，b，c三個角度,。已知d,，h，L1,，L3參數(shù),。

L1,，L3是腿長固定的數(shù)據(jù)，d踝到重心的垂直距離,。根據(jù)用戶輸入高度參數(shù)h輸出三個關節(jié)角度,。

步驟1.已知d和h通過勾補定理計算出L2

步驟2.已知L1，L3和L2,，通過公式計算出三角,。

a = arcos (L12+L32-L22 / 2 * L1 * L3)

b = arcos (L12+L22-L32 / 2 * L1 * L2)

c = arcos (L22+L32-L12 / 2 * L2 * L3)

3 U* I3 z6 b# I' A* R- O: s" d; U

根據(jù)上面的思路加入腿部的前后的移動，計算方法也類似首先計算出L2再根據(jù)三邊計算出三角,，不同的是需要根據(jù)三個關節(jié)的角度計算出d是正負,，上圖是d為負數(shù)的情況。

) z9 _7 |4 X2 f! d

如果已知的三角計算同樣可以計算出步長d和高度h,。

2 A% |* t5 F1 I  t1 d: Y

1五湖四海1

1五湖四海1 發(fā)表于 2021-12-24 13:15
" y& q) S2 \  U; }- s. b4 r上圖是站立姿態(tài)腿部膝蓋俯仰角,，髖俯仰角,，踝俯仰角的示意圖。最終需要計算出a,，b,，c三個角度。已知d,，h ...

" \6 c& \, f& O/ ~8 ^

模塊化設計

機械需要模塊化設計軟件同樣使用模塊設計思想,，設計出面向對象的模塊設計，抽象出需要使用的對象,，提供對外部使用的API方法,。

雙足機器人運動主控的軟件架構主要分舵機驅動模塊，通訊模塊,，關節(jié)模塊,，腿部模塊，寄存器模塊,，整體應用模塊組成,。

8 m  q  `. v3 f- n  w9 D0 M( o5 B- u

舵機驅動模塊

該模塊提供了舵機當前位置的初始化，舵機在線設備搜索,，獲取舵機是否運動,，獲取舵機溫度，設置舵機pid參數(shù),，舵機的運動控制等功能,。舵機設備對象創(chuàng)建后，這些方法注冊到舵機設備對象中供外部模塊使用,。

通訊模塊

主機與主控采用標準modbus通訊協(xié)議 ,，通過共享內存區(qū)的數(shù)據(jù)進行通訊,，主控作為服務端存儲內存數(shù)據(jù),，主機作為客戶端可以對內存進行寫入與讀取,。下面是基于modbus實現(xiàn)的功能。

運動相關

上位機通過寫入0x68到0x9f區(qū)域內存,，主控解析出關節(jié)信息去執(zhí)行舵機運動,。

get_joint_move_cmd                      這個函數(shù)是輪詢獲取主機是否有新數(shù)據(jù)到來。

get_joint_move_data                      如果有新數(shù)據(jù)到來通過這個函數(shù)解析出關節(jié)角度數(shù)據(jù),。

get_joint_init                                 主控端啟動后通過這個函數(shù)讀取內存中每個關節(jié)對應舵機id數(shù)                                                據(jù)

set_joint_init                                 主控端設置關節(jié)參數(shù)到寄存器內存里

set_joint_pos_last                          設置舵機位置數(shù)據(jù)到寄存器里

get_joint_pos_last                          獲取存儲在寄存器里的舵機位置數(shù)據(jù)

set_move_id                                 主控執(zhí)行運動隊列的id寫入寄存器里

5 ^3 @% @' l0 B5 o/ B' z8 P; ]/ W

陀螺儀相關

get_body_euler_angle                   獲取存儲在寄存器里的陀螺儀姿態(tài)數(shù)據(jù)

set_body_euler_angle                   硬件陀螺儀數(shù)據(jù)寫入到寄存器內存里

get_body_euler_pid                      獲取存儲在寄存器里的pid參數(shù)

set_body_euler_pid                      主控設置pid參數(shù)到寄存器里

控制相關

get_master_ctl_cmd                     獲取內存中控制位數(shù)據(jù)

set_master_ctl_cmd                     設置控制數(shù)據(jù)到內存里

6 T" G( ?! Q0 U3 U

關節(jié)模塊

主要提供了和舵機ID的綁定與獲取函數(shù),，關節(jié)角度與舵機位置之間的相互轉換功能，關節(jié)運動數(shù)據(jù)的生成與位置更新等功能,。

$ w3 k% C  Y7 g' P0 \4 [+ U1 n6 U; F

腿部模塊

腿部模塊的創(chuàng)建需要先創(chuàng)建好關節(jié)模塊然后在腿部模塊里初始化各個關節(jié),。

leg_set_attribute                            設置小腿，大腿長度,，踝到重心長度等屬性,。

leg_get_h_step_len                         獲取當前腿的高度和步長。

leg_get_barycnter_off                     獲取當前腿的重心偏移距離

leg_stretch                                    此函數(shù)提供了對腿部上下前后和重心移動等運動

leg_d_foot_touch_refactor               根據(jù)陀螺儀反饋數(shù)據(jù)調整雙足支撐時身體姿態(tài)需要變化的關節(jié)角度

整體應用模塊

1.創(chuàng)建各個模塊

其中包括,，與上位機的modbus總線創(chuàng)建,，紅外遙控設備創(chuàng)建，舵機模塊,，寄存器模塊,，管模塊，腿部設備模塊,，姿態(tài)模塊的創(chuàng)建

0 U# `9 N4 I8 K

2模塊的注冊與初始化

需要用到的模塊創(chuàng)建后依次注冊到機器人robot_api_dev中,，然后按照順序依次完成初始化工作。

feet_robot_sms_dev_init                                      初始化舵機設備

robot_regs_data_read_joint_init_param        

robot_leg_create                                                 在寄存器中讀取關節(jié)初始參數(shù)后創(chuàng)建腿部模塊

robot_leg_set_attribute                                        設置大腿小腿長度和踝到重心的距離

feet_robot_serch_dev                                          對舵機模塊進行在線舵機搜索

robot_regs_data_set_joint_postion_last                 設置舵機位置數(shù)據(jù)到寄存器中

/ _+ C* K- |! |

3業(yè)務邏輯

有兩個任務處理業(yè)務邏輯,，一個是主應用用于輪詢處理函數(shù),，一個是運動執(zhí)行任務，該任務負責接收運動隊列并執(zhí)行運動控制

上面是主任務需要處理的函數(shù)

robot_regs_data_get_joint_move_cmd                        在寄存器內存中獲取運動標志

robot_regs_data_joint_move_cmd_handle                   如果運動標志置位進行解析運動數(shù)據(jù)并發(fā)送                                                                                                                                                         到運動隊列

robot_servo_monitoring                                             每五秒進行一次舵機溫度檢查

robot_pid_controller_loop                                           輪詢上位機是否打開pid控制,，如果打開進行                                                                                                                                                         pid參數(shù)設置

robot_wait_move_fin_loop                                         運動等待處理輪詢函數(shù)

上面是運行執(zhí)行任務處理邏輯

joint_move_data_recv_mq                                         阻塞接收運動隊列數(shù)據(jù)

robot_regs_data_set_move_id                                    運動id寫入到寄存器

robot_regs_data_joint_move_refactor                         姿態(tài)控制打開時進行關節(jié)角度微調

robot_move_wait_timer_set_period                             根據(jù)運動隊列參數(shù)設定下一次運動等待時間

robot_joint_update_pos                                             更新舵機位置

robot_regs_data_joint_move_exeute                           開始執(zhí)行運動

5.4 遙控調試功能

紅外遙控框架包括解碼層,，紅外中間件和紅外驅動層，中間件提供了紅外事件回調

注冊好時間回調后,，有按鍵按下后進行回調到處理函數(shù),，上面圖中對按鍵進行了對應的運動控制

+                  雙腿支撐身體上升30mm

-                   雙腿支撐身體下降30mm

←                 身體向左重心移動 30mm

→                 身體向右重心移動30mm

0                   雙腿向前移動30mm

C                   雙腿向后移動30mm

1                  左腿伸展30mm

4                  左腿彎曲30mm

2                  右腿伸展30mm

5                  有腿彎曲30mm

7                  加速運動200mm/s

8                  加速運動200mm/s

* k3 H8 S, B' j) P

Hi@

大牛

1五湖四海1

* m# x5 D. c! x) r4 E* X6 T2 z: G3 P足底著地檢測傳感器
8 ]: c6 k5 D! |) U' Y

一共采用了8個tof 時間飛行測距傳感器型號是st的vl530,。

下面是編寫的上位機測量模塊和實際安裝位置。

# J- V' U: P# |
$ P" q! U# I8 ]! p' q1 U

這八個tof模塊初始化比較耗時放在了樹莓派端進行采集,，tof vl530類中初始化tof的步驟是,，首先使能第一個tof傳感器電源重新配置tof模塊地址完成初始化,，然后基于這個地址加一作為下一個tof模塊地址，然后使能第二個tof模塊電源進行初始化,，后面的模塊安裝上面的方法一次初始化tof模塊,。tof模塊I2C物理通訊接口采用樹莓派的BCM2835庫進行讀寫控制。

+ {' M& K  ^$ f; s+ A) ?; i4 L/ ^4 x4 B

西街不賣可樂

可以

1五湖四海1

1五湖四海1 發(fā)表于 2021-12-24 15:59
- t) l9 I, J3 J足底著地檢測傳感器

一共采用了8個tof 時間飛行測距傳感器型號是st的vl530。下面是編寫的上位機測量模塊 ...

. b2 C0 a) R" J8 T$ ]9 V% D
樹莓派開發(fā)板獲取8個 tof vl530傳感器代碼

東門吹牛2022

樓主大牛 能否加個好友qq2074772922

YIDA.

贊