小型双足机器人设计方案

1五湖四海1

! T4 m$ ?# k& A, U

仿生机器人主要是模仿人的步伐进行行走，从最开始的静态步伐发展到了像人一样的动态步伐，开发者们经历了漫长的时间研发，最终实现了接近人类的漂亮步伐，下面我来介绍下我们研发发双机器人软件架构和硬件构成。

7 y9 P/ G- E: R

硬件控制板大体分为主控板和主机板

8 T4 u9 @5 o8 b( Y
; u G" m" ` Z( r7 Z, P

主控负责运动控制和采集传感器数据，采用stm32h743 跑rtthread RTOS系统。下面是主控上电启动的效果。

U$ x$ F4 [1 A \
$ ?$ @4 A/ W9 ]- Z! X: S- J

主机板主要负责运动具体控制，采用树莓派4b运行官方 Raspbian系统，基于树莓派平台运行控制软件采用QT编写，并配有5寸hdmi显示屏和两个小音箱提供了多媒体功能。

2 | b* M: n4 x
" p; ^+ d" a }2 E9 _

腿部结构部分是采用12个舵机作为关节提供动力，舵机是RS485总线方式与主控板进行通信。

主机与主控通信采用Mobus协议通信，主机作为客户端，主控作为服务端，在主控上的文件系统映射一个modbus寄存器区域。

传感器分布

主控上有九轴陀螺传感器用于获取身体姿态

树莓派接8个tof激光测距传感器通过i2c总线连接，安装在双足的四个角上面用了测量双足触地情况。

整体框架是PC机电脑和树莓派开发板上运行QT运动控制软件控制主控板完成各个关节运动控制，主控板映射一个寄存器内存需要，建立两个modbus服务器，一个用来连接PC机一个用来连接树莓派开发板。

具体模块介绍待续....

9 t0 x; n# t- M8 [" L1 _* h

1五湖四海1

最早的控制策略是开环控制，根据控制模式计算出理论的步伐数据去执行，但实际中会受到地面不平等因素影响机器人走稳，所以需要采用闭环控制很有必要。下面的思路是通过陀螺仪传感器获取机器人姿态，然后通过 pid 控制器输出需要调整的角度，在把目标角度重新调整下，最后控制伺服电机带动关节转动。

+ v5 D* I8 v) `0 y* J/ |

上位机需要根据用户输入的运动动作和速度参数转换为时间相等的小段运动动作，这个相等的时间就是 pid 控制周期。转换后的小段数据依次发送到主控端，运动数据需要做流控等待主控运动队列空闲空余时进行发送数据。

8 g6 c% }1 w; R$ g2 u% J

主控端接收运动队列的每一段运动数据，进行获取陀螺仪数据后进行 pid 控制器输出需要调整的角度，俯仰角是根据 pid 控制器的输出调整臀俯仰关节。横滚角是根据 pid 控制器的输出调整双足的高度重而调整身体横滚姿态。

7 e( y+ D* g" {2 r0 }

1五湖四海1

) e, K; o; n. H, z# J- F

上图是站立姿态腿部膝盖俯仰角，髋俯仰角，踝俯仰角的示意图。

最终需要计算出a，b，c三个角度。已知d，h，L1，L3参数。

L1，L3是腿长固定的数据，d踝到重心的垂直距离。根据用户输入高度参数h输出三个关节角度。

步骤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)

' _. G8 Q! ]9 Q# U

根据上面的思路加入腿部的前后的移动，计算方法也类似首先计算出 L2再根据三边计算出三角，不同的是需要根据三个关节的角度计算出d是正负，上图是d为负数的情况。

) |4 w5 `* b, q

如果已知的三角计算同样可以计算出步长d和高度h。

0 J1 @, J: ~+ K, c

1五湖四海1

) Y/ |) I5 A' g5 C/ \5 q

1五湖四海1 发表于 2021-12-24 13:15
/ e+ T5 u& J+ u$ E; n6 N上图是站立姿态腿部膝盖俯仰角，髋俯仰角，踝俯仰角的示意图。最终需要计算出a，b，c三个角度。已知d，h ...

模块化设计

机械需要模块化设计软件同样使用模块设计思想，设计出面向对象的模块设计，抽象出需要使用的对象，提供对外部使用的API方法。

双足机器人运动主控的软件架构主要分舵机驱动模块，通讯模块，关节模块，腿部模块，寄存器模块，整体应用模块组成。

# V( w8 v! D" M

舵机驱动模块

该模块提供了舵机当前位置的初始化，舵机在线设备搜索，获取舵机是否运动，获取舵机温度，设置舵机pid参数，舵机的运动控制等功能。舵机设备对象创建后，这些方法注册到舵机设备对象中供外部模块使用。

2 o$ w# n" f. n& j g( O

通讯模块

主机与主控采用标准modbus通讯协议 ，通过共享内存区的数据进行通讯，主控作为服务端存储内存数据，主机作为客户端可以对内存进行写入与读取。下面是基于modbus实现的功能。

运动相关

上位机通过写入0x68到0x9f区域内存，主控解析出关节信息去执行舵机运动。

get_joint_move_cmd 这个函数是轮询获取主机是否有新数据到来。

get_joint_move_data 如果有新数据到来通过这个函数解析出关节角度数据。

get_joint_init 主控端启动后通过这个函数读取内存中每个关节对应舵机id数 据

set_joint_init 主控端设置关节参数到寄存器内存里

set_joint_pos_last 设置舵机位置数据到寄存器里

get_joint_pos_last 获取存储在寄存器里的舵机位置数据

set_move_id 主控执行运动队列的id写入寄存器里

7 G- ?5 V% a7 h- G5 S) m: j

陀螺仪相关

get_body_euler_angle 获取存储在寄存器里的陀螺仪姿态数据

set_body_euler_angle 硬件陀螺仪数据写入到寄存器内存里

get_body_euler_pid 获取存储在寄存器里的pid参数

set_body_euler_pid 主控设置pid参数到寄存器里

控制相关

get_master_ctl_cmd 获取内存中控制位数据

set_master_ctl_cmd 设置控制数据到内存里

/ t. Z' ^ x7 F

关节模块

主要提供了和舵机ID的绑定与获取函数，关节角度与舵机位置之间的相互转换功能，关节运动数据的生成与位置更新等功能。

腿部模块

腿部模块的创建需要先创建好关节模块然后在腿部模块里初始化各个关节。

leg_set_attribute 设置小腿，大腿长度，踝到重心长度等属性。

leg_get_h_step_len 获取当前腿的高度和步长。

leg_get_barycnter_off 获取当前腿的重心偏移距离

leg_stretch 此函数提供了对腿部上下前后和重心移动等运动

leg_d_foot_touch_refactor 根据陀螺仪反馈数据调整双足支撑时身体姿态需要变化的关节角度

整体应用模块

1.创建各个模块

其中包括，与上位机的modbus总线创建，红外遥控设备创建，舵机模块，寄存器模块，管模块，腿部设备模块，姿态模块的创建

, e' a, g$ U; v0 G

2模块的注册与初始化

需要用到的模块创建后依次注册到机器人robot_api_dev中，然后按照顺序依次完成初始化工作。

feet_robot_sms_dev_init 初始化舵机设备

robot_regs_data_read_joint_init_param

robot_leg_create 在寄存器中读取关节初始参数后创建腿部模块

robot_leg_set_attribute 设置大腿小腿长度和踝到重心的距离

feet_robot_serch_dev 对舵机模块进行在线舵机搜索

robot_regs_data_set_joint_postion_last 设置舵机位置数据到寄存器中

3业务逻辑

有两个任务处理业务逻辑，一个是主应用用于轮询处理函数，一个是运动执行任务，该任务负责接收运动队列并执行运动控制

上面是主任务需要处理的函数

robot_regs_data_get_joint_move_cmd 在寄存器内存中获取运动标志

robot_regs_data_joint_move_cmd_handle 如果运动标志置位进行解析运动数据并发送 到运动队列

robot_servo_monitoring 每五秒进行一次舵机温度检查

robot_pid_controller_loop 轮询上位机是否打开pid控制，如果打开进行 pid参数设置

robot_wait_move_fin_loop 运动等待处理轮询函数

上面是运行执行任务处理逻辑

joint_move_data_recv_mq 阻塞接收运动队列数据

robot_regs_data_set_move_id 运动id写入到寄存器

robot_regs_data_joint_move_refactor 姿态控制打开时进行关节角度微调

robot_move_wait_timer_set_period 根据运动队列参数设定下一次运动等待时间

robot_joint_update_pos 更新舵机位置

robot_regs_data_joint_move_exeute 开始执行运动

8 D. ~0 `8 x' r2 n7 d

5.4 遥控调试功能

红外遥控框架包括解码层，红外中间件和红外驱动层，中间件提供了红外事件回调

" y0 H9 E) [% `6 A0 G

注册好时间回调后，有按键按下后进行回调到处理函数，上面图中对按键进行了对应的运动控制

+ 双腿支撑身体上升30mm

- 双腿支撑身体下降30mm

← 身体向左重心移动 30mm

→ 身体向右重心移动30mm

0 双腿向前移动30mm

C 双腿向后移动30mm

1 左腿伸展30mm

4 左腿弯曲30mm

2 右腿伸展30mm

5 有腿弯曲30mm

7 加速运动200mm/s

8 加速运动200mm/s

+ a( P+ S9 b1 v% M1 o
; Z) M/ G! x6 Z2 ?, c

Hi@

大牛

1五湖四海1

. J! |" ^7 A9 O' u+ p7 j0 G: v足底着地检测传感器

" E6 }( c2 ~# a1 r" `- R6 Z9 Q

一共采用了8个tof 时间飞行测距传感器型号是st的vl530。

下面是编写的上位机测量模块和实际安装位置。

- T( x5 l4 |' M& Q. F) A8 n
! a3 D+ \4 A0 x7 ?; q

这八个tof模块初始化比较耗时放在了树莓派端进行采集，tof vl530类中初始化tof的步骤是，首先使能第一个tof传感器电源重新配置tof模块地址完成初始化，然后基于这个地址加一作为下一个tof模块地址，然后使能第二个tof模块电源进行初始化，后面的模块安装上面的方法一次初始化tof模块。tof模块I2C物理通讯接口采用树莓派的BCM2835库进行读写控制。

+ C) U# B- M0 a% z8 V1 l" i: X% z2 O

西街不卖可乐

可以

1五湖四海1

1五湖四海1 发表于 2021-12-24 15:59
- A( ], t, e; r& q. {/ y足底着地检测传感器

一共采用了8个tof 时间飞行测距传感器型号是st的vl530。下面是编写的上位机测量模块 ...

, _$ H: \7 |0 b1 n
树莓派开发板获取8个 tof vl530传感器代码
* h# T7 Y$ I) q

东门吹牛2022

楼主大牛 能否加个好友qq2074772922

YIDA.

赞