12自由度六足机器人实现步态规划功能
本帖最后由 机器谱 于 2023-3-20 11:27 编辑1. 运动功能描述
R111样机是一款拥有12个自由度的串联仿生六足机器人。本文示例实现12自由度六足机器人的行走功能，包括前进、后退、左转、右转。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgpKeroAYo6NCZkgMw5gU4rAQ!400x400.png.webp
2. 结构说明
R111样机由六个2自由度串联结构组成，中间由舵机双折弯和螺柱结合固定，从而达到一个仿生机器人的结构。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgl6iroAYoqJDi5gEwuhI43Q0!450x450.png.webphttps://28846868.s21i.faiusr.com/4/ABUIABAEGAAgu6iroAYo9vHUvQQwngQ4hwQ!300x300.png.webp
该2自由度串联结构由舵机1和舵机2驱动，其中舵机1实现腿部前后摆动，舵机2实现腿部的上下抬伸。其中抬伸通过一个平行四连杆ABCD作为传动结构以增加腿部的行程和增强腿部运动的稳定性。
3. 步态原理
12自由度六足机器人的运动步态有两种：一种是采用三角步态进行运动，一种是采用波动步态运动。
① 三角步态
12自由度六足仿生机器人的三角步态是将机器人六足分成两组腿（身体一侧的前足、后足与另一侧的中足）分别进行摆动和支撑，即处于三角形上的三条腿的动作一样，均处于摆动相或均处于支撑相。如图：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAglb6roAYopPSHgwcwngc42gQ!600x600.png.webp
② 波动步态
12自由度六足仿生机器人的波动步态是机器人每条腿两侧依次运动，即左（右）侧一条腿先迈步，再右（左）侧腿迈步，再左（右）侧下一条腿运动，如此循环完成波动步态的运动。如图：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgsqqroAYopK7GwwEwmgY4_Qc!800x800.png.webp
提示：可以参考蜈蚣的步态。
本文讲一下三角步态的实现案例，波动步态大家可以自己研究。
4. 基于STM32主控板实现
4.1电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板STM32主控板
扩展板STM32扩展板
舵机标准舵机
电池7.4V锂电池
按下图进行电路连接： 我们先对机构的舵机进行编号（见下图）
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgkKyroAYozOLlmQYwoQY4-wM!600x600.png.webp
接下来与扩展板进行连接（见下图）
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgvq6roAYolK6yhgEwtQY4jAM!600x600.png.webp
相应的引脚编号见下表，这些引脚号在编程中将会用到。
物料引脚号物料引脚号
舵机0PE9舵机6PD15
舵机1PE11舵机7
PD14
舵机2PE13舵机8
PD13
舵机3PE14舵机9
PB15
舵机4PC6舵机10
PB14
舵机5PC7舵机11
PB9
4.2 示例程序
编程环境：keil5
使用Controller获取姿态参数，并拷贝到下面的程序中。
三角步态前进的参考例程（USER\test.uvprojx），main.c的代码：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2023-02-16 https://www.robotway.com/
------------------------------*/
#include "sys.h"
#include "led.h"
#include "stdio.h"
#include "delay.h"
#include "usart.h"
#include "stdio.h"
#include "core_cm4.h"
#include "string.h"
#include "stdlib.h"
#include "pwm.h"
#include "timer.h"
#include "math.h"
/*
7 1
| |
6----| |----0
| |
9 3
| |
8----| |----2
| |
11 5
| |
10----| |----4
| |
*/
float Ang = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
int Ang_Init = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
float data_f = {0};
void split(char *src,const char *separator,char **dest,int *num);//字符串拆分函数
float sort(float data);//排序函数（小->大）
float find_min(float *de);//寻找最小值函数
void Set_Ang(float, float, float, float, float, float, float, float, float, float, float, float);//设置舵机旋转角度函数
void Steering_Gear_Init(void);//机械臂初始化位置函数
void Steering_Gear_Angle(u8 num, float ang);//单个舵机控制函数
void Steering_Gear_Ang_Pwm1(float data);//机械臂移动函数
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168);//初始化延时，168为CPU运行频率
usart_init(115200); //串口初始化
LED_Init();//初始化LED灯
TIM1_PWM_Init(20000-1, 168-1);
TIM4_PWM_Init(20000-1,84-1);
TIM3_PWM_Init(20000-1,84-1);
TIM11_PWM_Init(20000-1,168-1);
TIM12_PWM_Init(20000-1,84-1);
Steering_Gear_Init();//六足位置初始化
while(1)
{
Set_Ang(80,85,110,90,110,80,80,90,115,90,70,95);
Steering_Gear_Ang_Pwm1(data_f);
delay_ms(1000);
while(1){
Set_Ang(90,100,105,75,110,95,90,105,105,75,80,110);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(70,100,70,75,130,95,120,105,120,75,120,110);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(70,70,70,105,130,65,120,75,120,105,120,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(90,70,105,105,110,65,90,75,105,105,80,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(120,70,120,105,60,65,80,75,70,105,70,80);
Steering_Gear_Ang_Pwm1(data_f);
Set_Ang(120,100,120,75,60,95,80,105,70,75,70,110);
Steering_Gear_Ang_Pwm1(data_f);
}
}
}
void split(char *src,const char *separator,char **dest,int *num)
{
char *pNext;
int count = 0;
if (src == NULL || strlen(src) == 0)//字符串为空或遇到结束标志
return;
if (separator == NULL || strlen(separator) == 0)//分隔符为空
return;
pNext = strtok(src,separator);//字符串分割
while(pNext != NULL) {
*dest++ = pNext;
++count;
pNext = strtok(NULL,separator);
}
*num = count;
}
//对数组内素有成员按从小到大的顺序从新排列，并返回最小值
float sort(float data1)
{
float array = {0};
int i = 0;
int j = 0;
int temp;
for(int i=0; i < 12; i++){
array = data1;
}
for ( i = 0; i < 12; i++){
for ( j = 0; j < 11-i; j++){
if(array>array){
temp=array;
array=array;
array=temp;
}
}
}
return find_min(array);
}
//寻找数组中的最小值
float find_min(float *de)
{
if((int)*de != 0)
return *de;
find_min(++de);
}
//给数组赋值
void Set_Ang(float ang1, float ang2, float ang3, float ang4, float ang5, float ang6, float ang7, float ang8, float ang9, float ang10, float ang11, float ang12)
{
data_f = ang1;
data_f = ang2;
data_f = ang3;
data_f = ang4;
data_f = ang5;
data_f = ang6;
data_f = ang7;
data_f = ang8;
data_f = ang9;
data_f = ang10;
data_f = ang11;
data_f = ang12;
}
//将两个位置之间需要舵机移动的度数转换为多步，然后分步完成
void Steering_Gear_Ang_Pwm1(float data)
{
float den = 0;
float dif = {0};
floatang = {0};
//计算两个位置之间的差值
for(int i=0; i < 12; i++){
dif = fabs(data - Ang);
}
//得到最小差值
den = sort(dif);
//计算12个舵机每次移动的最小距离
if((int)den == 0){
for(int i=0; i < 12; i++)
ang = 0;
}else{
for(int i=0; i < 12; i++){
ang = (data - Ang)/den;
}
for(int i=0; i<den; i++){
for(int j=0; j < 12; j++)
Ang += ang;
//对多个舵机进行位置限制，防止舵机堵转
for(int g=0; g < 12; g++){
if(Ang < 0)
Ang = 0;
else if(Ang > 180)
Ang = 180;
}
//舵机控制
for(int k=0; k<12; k++){
Steering_Gear_Angle(k, Ang);
}
}
}
}
void Steering_Gear_Angle(u8 num, float ang)
{
u32 Ang = 0;
Ang = 500 + ang*2000/180;
switch(num){
case 0 :
TIM_SetCompare1(TIM1, Ang);//修改比较值，修改占空比
break;
case 1 :
TIM_SetCompare2(TIM1, Ang);//修改比较值，修改占空比
break;
case 2 :
TIM_SetCompare3(TIM1, Ang);//修改比较值，修改占空比
break;
case 3 :
TIM_SetCompare4(TIM1, Ang);//修改比较值，修改占空比
break;
case 4 :
TIM_SetCompare1(TIM3, Ang);//修改比较值，修改占空比
break;
case 5 :
TIM_SetCompare2(TIM3, Ang);//修改比较值，修改占空比
break;
case 6 :
TIM_SetCompare4(TIM4, Ang);//修改比较值，修改占空比
break;
case 7 :
TIM_SetCompare3(TIM4, Ang);//修改比较值，修改占空比
break;
case 8 :
TIM_SetCompare2(TIM4, Ang);//修改比较值，修改占空比
break;
case 9 :
TIM_SetCompare2(TIM12, Ang);//修改比较值，修改占空比
break;
case 10 :
TIM_SetCompare1(TIM12, Ang);//修改比较值，修改占空比
break;
case 11 :
TIM_SetCompare1(TIM11, Ang);//修改比较值，修改占空比
break;
default:
break;
}
delay_ms(1);
}
//六足起始位置
void Steering_Gear_Init(void)
{
for(int i=0; i < 12; i++){
Steering_Gear_Angle(i,Ang_Init);
}
delay_ms(1000);
}
5. 基于Arduino主控板实现
5.1 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板Basra主控板（兼容Arduino Uno）
扩展板STM32扩展板
舵机标准舵机
电池7.4V锂电池
电路连接说明：
因为12自由度六足机器人结构上装有12个舵机，所以将采用 SH-SR舵机扩展板 。
舵机接线顺序：1、3； 4、 5； 6、 8；11、13；15、 21；24、26。
https://28846868.s21i.faiusr.com/2/ABUIABACGAAgoLiroAYoj4rkjQYwgB84oBc!600x600.jpg.webp
5.2 示例程序
使用Controller获取姿态参数，并拷贝到下面的12自由度六足机器人三角步态的参考例程（robot_walk.ino）之中，并下载到主控板：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2023-02-16 https://www.robotway.com/
------------------------------*/
#include <Arduino.h>
#include <avr/pgmspace.h>
#include "Config.h"
#include <Tlc5940.h>
#include <tlc_servos.h>
int count_input = 0;
boolean _b = false;
/**************+++++++++++动作组数组，请将转换后的动作组数据粘贴到相应的动作组中+++++++***************************/
const PROGMEM int actionInit[] = {
1090, 1790, 1220, 980, 650, 1800, 1020, 1330, 520, 1155, 1260, 2020,
1090, 1790, 1220, 980, 650, 1800, 1020, 1330, 520, 1155, 1260, 2020,
};
const PROGMEM int actionMove[] = {
1660, 1230, 1220, 980, 1280, 1190, 1020, 1330, 1090, 1155, 1260, 1540,
1660, 1790, 1320, 1080, 1280, 1800, 920, 1230, 1090, 1255, 1360, 2020,
1660, 1230, 1320, 1080, 1280, 1190, 920, 1230, 1090, 1255, 1360, 1540,
1090, 1300, 1120, 880, 650, 900, 1120, 1430, 520, 1055, 1160, 1300,
1660, 1300, 1120, 880, 1280, 900, 1120, 1430, 1090, 1055, 1160, 1300,
};
const PROGMEM int actionLeft[] = {
};
const PROGMEM int actionRight[] = {
};
const PROGMEM int actionBack[] = {
};
const PROGMEM int actionDance[] = {
};
/**************************+++++---------分割线--------++++++*******************************************************/
//动作组数组长度获取函数，增加动作组时需要按如下格式添加：actPwmNum[增加的动作组序号] = sizeof(增加的动作组名称) / sizeof(增加的动作组名称);
void act_length()
{
actPwmNum = (sizeof(actionMove) / sizeof(actionMove))/servo_num;
actPwmNum = (sizeof(actionLeft) / sizeof(actionLeft))/servo_num;
actPwmNum = (sizeof(actionRight) / sizeof(actionRight))/servo_num;
actPwmNum = (sizeof(actionBack) / sizeof(actionBack))/servo_num;
actPwmNum = (sizeof(actionDance) / sizeof(actionDance))/servo_num;
actPwmNum = (sizeof(actionInit) / sizeof(actionInit))/servo_num;
}
//map映射函数
long map_servo(long x, long in_min, long in_max, long out_min, long out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
//PWM 转换为舵机角度的函数，增加动作组时需要修改
void vlaue2angle(int p, int act)
{
switch(act)
{
case 0: value_cur = map_servo(pgm_read_word_near(actionMove + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 1: value_cur = map_servo(pgm_read_word_near(actionLeft + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 2: value_cur = map_servo(pgm_read_word_near(actionRight + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 3: value_cur = map_servo(pgm_read_word_near(actionBack + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 4: value_cur = map_servo(pgm_read_word_near(actionDance + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 5: value_cur = map_servo(pgm_read_word_near(actionInit + p + servo_num * count_input), 500, 2500, 0, 180); break;
default: break;
}
}
//舵机初始化函数，动作组第一行为舵机初始化值
void servo_init(int act, int num)
{
if(!_b)
{
for(int i=0;i<servo_num;i++)
{
vlaue2angle(i, act);
tlc_setServo(servo_pin, value_cur);
value_pre = value_cur;
}
Tlc.update();
//delay(1000);
}
num == 1 ? _b = true : _b = false;
}
//舵机移动函数，参数： act：动作组宏定义名称 ；num：动作组执行的次数，num > 0 ；
void servo_move(int act, int num)
{
float value_delta = {};
float in_value = {};
servo_init(act, num);
for(int i=0;i< num * actPwmNum;i++)
{
count_input++;
if(count_input == actPwmNum)
{
count_input = 0;
continue;
}
for(int i=0;i<servo_num;i++)
{
vlaue2angle(i, act);
in_value = value_pre;
value_delta = (value_cur - value_pre) / frequency;
}
for(int i=0;i<frequency;i++)
{
for(int k=0;k<servo_num;k++)
{
in_value += value_delta;
value_pre = in_value;
}
for(int j=0;j<servo_num;j++)
{
tlc_setServo(servo_pin, in_value);
delayMicroseconds(servo_speed);
}
Tlc.update();
}
delayMicroseconds(action_delay);
}
}
/********************************************************-------分割线--------****************************************************/
//初始化函数
void setup() {
Serial.begin(9600); //开启串口通信，波特率9600
Tlc.init(0);
tlc_initServos();
act_length();
delay(action_delay);
}
//主函数，walk
void loop() {
servo_move(action_move, 6);
delay(100);
}注意：
上面虽然给大家提供了一个参考例程，但并不意味着直接将程序烧录成功后机器人即可正常运行，还需要大家对一些参数或者结构进行调整。调试提示：
（1）安装时将每个关节的舵机角度调成与程序一致；
（2）程序里面初始角度调整为安装时的角度，然后根据之前运动的角度差调整其它动作的角度；
（3）注意每条腿上舵机对应的端口号，运动需要与三角步态对应；
（4）可以通过调整延迟参数来控制机器人的运动快慢。
6. 资料下载
资料内容：
①步态规划-STM32例程源代码
②步态规划-Arduino例程源代码
资料下载地址：https://www.robotway.com/h-col-197.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
页:
[1]