小型雙足機(jī)器人設(shè)計(jì)方案

1五湖四海1

6 u/ r; N; ]; O7 k) H" F

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

( G0 Y: W. Z* s5 N
) U( W9 K" u, p4 Z, g1 s7 m

硬件控制板大體分為主控板和主機(jī)板

% Q) H" }- R7 t7 S* _

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

& [3 \5 f* s" h' j

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

. W! A  t- v( Y8 O2 T2 N4 V
3 n: o4 X8 U% G: y

腿部結(jié)構(gòu)部分是采用12個(gè)舵機(jī)作為關(guān)節(jié)提供動(dòng)力,，舵機(jī)是RS485總線方式與主控板進(jìn)行通信,。

, r# Z: l. L% y* l% i

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

" c) Z  o! }# \5 t
7 k! Z2 i# O# i! ?  l

傳感器分布

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

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

0 G7 u- T- A9 T/ }8 l1 F! q

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

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

6 _9 G# w# A6 ?4 G' F$ R/ m5 h

1五湖四海1

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

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

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

/ O3 l# ~. g0 r0 H

7 h# x+ N7 Q4 z0 k- R$ @$ O

1五湖四海1

. e" N" ~! w. I7 l0 j* h5 @: r

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

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

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

步驟1.已知d和h通過勾補(bǔ)定理計(jì)算出L2

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

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

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

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

. D: O/ ^/ \$ D/ R& {# ^

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

) Q9 T: h( r  o1 M' F( ~

如果已知的三角計(jì)算同樣可以計(jì)算出步長(zhǎng)d和高度h,。

3 A* }! \; h  z+ b( o) t" `& U6 o

1五湖四海1

1 m6 x. P  u$ l  Q7 u

1五湖四海1 發(fā)表于 2021-12-24 13:15
) E- r/ ~# x# l- W# K7 `: J- w上圖是站立姿態(tài)腿部膝蓋俯仰角,，髖俯仰角，踝俯仰角的示意圖,。最終需要計(jì)算出a,，b,，c三個(gè)角度。已知d,，h ...

1 o* E+ d4 U9 j5 t3 c$ u

模塊化設(shè)計(jì)

機(jī)械需要模塊化設(shè)計(jì)軟件同樣使用模塊設(shè)計(jì)思想,，設(shè)計(jì)出面向?qū)ο蟮哪�塊設(shè)計(jì)，抽象出需要使用的對(duì)象,，提供對(duì)外部使用的API方法,。

雙足機(jī)器人運(yùn)動(dòng)主控的軟件架構(gòu)主要分舵機(jī)驅(qū)動(dòng)模塊，通訊模塊,，關(guān)節(jié)模塊,，腿部模塊，寄存器模塊,，整體應(yīng)用模塊組成,。

舵機(jī)驅(qū)動(dòng)模塊

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

2 Y! a- A8 b0 b3 p- g; Y4 R) Y* r

通訊模塊

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

運(yùn)動(dòng)相關(guān)

上位機(jī)通過寫入0x68到0x9f區(qū)域內(nèi)存,，主控解析出關(guān)節(jié)信息去執(zhí)行舵機(jī)運(yùn)動(dòng)。

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

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

get_joint_init                                 主控端啟動(dòng)后通過這個(gè)函數(shù)讀取內(nèi)存中每個(gè)關(guān)節(jié)對(duì)應(yīng)舵機(jī)id數(shù)                                                據(jù)

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

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

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

set_move_id                                 主控執(zhí)行運(yùn)動(dòng)隊(duì)列的id寫入寄存器里

陀螺儀相關(guān)

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

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

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

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

控制相關(guān)

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

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

關(guān)節(jié)模塊

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

腿部模塊

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

leg_set_attribute                            設(shè)置小腿,，大腿長(zhǎng)度,，踝到重心長(zhǎng)度等屬性,。

leg_get_h_step_len                         獲取當(dāng)前腿的高度和步長(zhǎng),。

leg_get_barycnter_off                     獲取當(dāng)前腿的重心偏移距離

leg_stretch                                    此函數(shù)提供了對(duì)腿部上下前后和重心移動(dòng)等運(yùn)動(dòng)

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

整體應(yīng)用模塊

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

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

2模塊的注冊(cè)與初始化

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

feet_robot_sms_dev_init                                      初始化舵機(jī)設(shè)備

robot_regs_data_read_joint_init_param        

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

robot_leg_set_attribute                                        設(shè)置大腿小腿長(zhǎng)度和踝到重心的距離

feet_robot_serch_dev                                          對(duì)舵機(jī)模塊進(jìn)行在線舵機(jī)搜索

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

( [# ~$ \5 C/ R! [. k7 W) A: O) S: L

3業(yè)務(wù)邏輯

有兩個(gè)任務(wù)處理業(yè)務(wù)邏輯，一個(gè)是主應(yīng)用用于輪詢處理函數(shù),，一個(gè)是運(yùn)動(dòng)執(zhí)行任務(wù),，該任務(wù)負(fù)責(zé)接收運(yùn)動(dòng)隊(duì)列并執(zhí)行運(yùn)動(dòng)控制

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

robot_regs_data_get_joint_move_cmd                        在寄存器內(nèi)存中獲取運(yùn)動(dòng)標(biāo)志

robot_regs_data_joint_move_cmd_handle                   如果運(yùn)動(dòng)標(biāo)志置位進(jìn)行解析運(yùn)動(dòng)數(shù)據(jù)并發(fā)送                                                                                                                                                         到運(yùn)動(dòng)隊(duì)列

robot_servo_monitoring                                             每五秒進(jìn)行一次舵機(jī)溫度檢查

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

robot_wait_move_fin_loop                                         運(yùn)動(dòng)等待處理輪詢函數(shù)

4 }4 c& M/ k7 q4 b% \: x9 z

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

joint_move_data_recv_mq                                         阻塞接收運(yùn)動(dòng)隊(duì)列數(shù)據(jù)

robot_regs_data_set_move_id                                    運(yùn)動(dòng)id寫入到寄存器

robot_regs_data_joint_move_refactor                         姿態(tài)控制打開時(shí)進(jìn)行關(guān)節(jié)角度微調(diào)

robot_move_wait_timer_set_period                             根據(jù)運(yùn)動(dòng)隊(duì)列參數(shù)設(shè)定下一次運(yùn)動(dòng)等待時(shí)間

robot_joint_update_pos                                             更新舵機(jī)位置

robot_regs_data_joint_move_exeute                           開始執(zhí)行運(yùn)動(dòng)

5.4 遙控調(diào)試功能

紅外遙控框架包括解碼層,，紅外中間件和紅外驅(qū)動(dòng)層,，中間件提供了紅外事件回調(diào)

$ H5 H9 s* E) s2 U; }5 E4 b

注冊(cè)好時(shí)間回調(diào)后，有按鍵按下后進(jìn)行回調(diào)到處理函數(shù),，上面圖中對(duì)按鍵進(jìn)行了對(duì)應(yīng)的運(yùn)動(dòng)控制

+                  雙腿支撐身體上升30mm

-                   雙腿支撐身體下降30mm

←                 身體向左重心移動(dòng) 30mm

→                 身體向右重心移動(dòng)30mm

0                   雙腿向前移動(dòng)30mm

C                   雙腿向后移動(dòng)30mm

1                  左腿伸展30mm

4                  左腿彎曲30mm

2                  右腿伸展30mm

5                  有腿彎曲30mm

7                  加速運(yùn)動(dòng)200mm/s

8                  加速運(yùn)動(dòng)200mm/s

Hi@

大牛

1五湖四海1

( A: e& l" ?6 C0 \! {* F
4 K+ m  E& r7 Y6 \2 @足底著地檢測(cè)傳感器

! t; y# Z5 \- ^

一共采用了8個(gè)tof 時(shí)間飛行測(cè)距傳感器型號(hào)是st的vl530,。

下面是編寫的上位機(jī)測(cè)量模塊和實(shí)際安裝位置,。

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

西街不賣可樂

可以

1五湖四海1

1五湖四海1 發(fā)表于 2021-12-24 15:59
足底著地檢測(cè)傳感器

一共采用了8個(gè)tof 時(shí)間飛行測(cè)距傳感器型號(hào)是st的vl530。下面是編寫的上位機(jī)測(cè)量模塊 ...

& P: t1 X* j$ l  d/ R3 g
樹莓派開發(fā)板獲取8個(gè) tof vl530傳感器代碼
6 H, J: _% V# K5 a4 O

東門吹牛2022

樓主大牛 能否加個(gè)好友qq2074772922

YIDA.

贊