12自由度六足机器人实现步态规划功能

机器谱 · 发表于 2023-3-20 11:27:42

本帖最后由机器谱于 2023-3-20 11:27 编辑

1. 运动功能描述
R111样机是一款拥有12个自由度的串联仿生六足机器人。本文示例实现12自由度六足机器人的行走功能，包括前进、后退、左转、右转。

2. 结构说明
R111样机由六个2自由度串联结构组成，中间由舵机双折弯和螺柱结合固定，从而达到一个仿生机器人的结构。

   该2自由度串联结构由舵机1和舵机2驱动，其中舵机1实现腿部前后摆动，舵机2实现腿部的上下抬伸。其中抬伸通过一个平行四连杆ABCD作为传动结构以增加腿部的行程和增强腿部运动的稳定性。

3. 步态原理
   12自由度六足机器人的运动步态有两种：一种是采用三角步态进行运动，一种是采用波动步态运动。
① 三角步态
   12自由度六足仿生机器人的三角步态是将机器人六足分成两组腿（身体一侧的前足、后足与另一侧的中足）分别进行摆动和支撑，即处于三角形上的三条腿的动作一样，均处于摆动相或均处于支撑相。如图：

② 波动步态
12自由度六足仿生机器人的波动步态是机器人每条腿两侧依次运动，即左（右）侧一条腿先迈步，再右（左）侧腿迈步，再左（右）侧下一条腿运动，如此循环完成波动步态的运动。如图：

提示：可以参考蜈蚣的步态。
本文讲一下三角步态的实现案例，波动步态大家可以自己研究。

4. 基于STM32主控板实现
4.1电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：

主控板	STM32主控板
扩展板	STM32扩展板
舵机	标准舵机
电池	7.4V锂电池

按下图进行电路连接：我们先对机构的舵机进行编号（见下图）

接下来与扩展板进行连接（见下图）

相应的引脚编号见下表，这些引脚号在编程中将会用到。

物料	引脚号	物料	引脚号
舵机0	PE9	舵机6	PD15
舵机1	PE11	舵机7	PD14
舵机2	PE13	舵机8	PD13
舵机3	PE14	舵机9	PB15
舵机4	PC6	舵机10	PB14
舵机5	PC7	舵机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[12] = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
int Ang_Init[12] = {100, 85, 90, 90, 90, 80, 100, 90, 95, 90, 90, 95};
float data_f[12] = {0};
void split(char *src,const char *separator,char **dest,int *num);//字符串拆分函数
float sort(float data[12]);//排序函数（小->大）
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[12]);//机械臂移动函数
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[12])
{
float array[12] = {0};
int i = 0;
int j = 0;
int temp;
for(int i=0; i < 12; i++){
array[i] = data1[i];
}
for ( i = 0; i < 12; i++){
for ( j = 0; j < 11-i; j++){
if(array[j]>array[j+1]){
temp=array[j];
array[j]=array[j+1];
array[j+1]=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[0] = ang1;
data_f[1] = ang2;
data_f[2] = ang3;
data_f[3] = ang4;
data_f[4] = ang5;
data_f[5] = ang6;
data_f[6] = ang7;
data_f[7] = ang8;
data_f[8] = ang9;
data_f[9] = ang10;
data_f[10] = ang11;
data_f[11] = ang12;
}
//将两个位置之间需要舵机移动的度数转换为多步，然后分步完成
void Steering_Gear_Ang_Pwm1(float data[12])
{
float den = 0;
float dif[12] = {0};
floatang[12] = {0};
//计算两个位置之间的差值
for(int i=0; i < 12; i++){
dif[i] = fabs(data[i] - Ang[i]);
}
//得到最小差值
den = sort(dif);
//计算12个舵机每次移动的最小距离
if((int)den == 0){
for(int i=0; i < 12; i++)
ang[i] = 0;
}else{
for(int i=0; i < 12; i++){
ang[i] = (data[i] - Ang[i])/den;
}
for(int i=0; i<den; i++){
for(int j=0; j < 12; j++)
Ang[j] += ang[j];
//对多个舵机进行位置限制，防止舵机堵转
for(int g=0; g < 12; g++){
if(Ang[g] < 0)
Ang[g] = 0;
else if(Ang[g] > 180)
Ang[g] = 180;
}
//舵机控制
for(int k=0; k<12; k++){
Steering_Gear_Angle(k, Ang[k]);
}
}
}
}
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[i]);
}
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。

5.2 示例程序
使用Controller获取姿态参数，并拷贝到下面的12自由度六足机器人三角步态的参考例程（robot_walk.ino）之中，并下载到主控板：

/*------------------------------------------------------------------------------------
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(增加的动作组名称[0]);
void act_length()
{
actPwmNum[0] = (sizeof(actionMove) / sizeof(actionMove[0]))/servo_num;
actPwmNum[1] = (sizeof(actionLeft) / sizeof(actionLeft[0]))/servo_num;
actPwmNum[2] = (sizeof(actionRight) / sizeof(actionRight[0]))/servo_num;
actPwmNum[3] = (sizeof(actionBack) / sizeof(actionBack[0]))/servo_num;
actPwmNum[4] = (sizeof(actionDance) / sizeof(actionDance[0]))/servo_num;
actPwmNum[5] = (sizeof(actionInit) / sizeof(actionInit[0]))/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[p] = map_servo(pgm_read_word_near(actionMove + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 1: value_cur[p] = map_servo(pgm_read_word_near(actionLeft + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 2: value_cur[p] = map_servo(pgm_read_word_near(actionRight + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 3: value_cur[p] = map_servo(pgm_read_word_near(actionBack + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 4: value_cur[p] = map_servo(pgm_read_word_near(actionDance + p + servo_num * count_input), 500, 2500, 0, 180); break;
case 5: value_cur[p] = 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[i], value_cur[i]);
value_pre[i] = value_cur[i];
}
Tlc.update();
//delay(1000);
}
num == 1 ? _b = true : _b = false;
}
//舵机移动函数，参数： act：动作组宏定义名称；num：动作组执行的次数，num > 0 ；
void servo_move(int act, int num)
{
float value_delta[servo_num] = {};
float in_value[servo_num] = {};
servo_init(act, num);
for(int i=0;i< num * actPwmNum[act];i++)
{
count_input++;
if(count_input == actPwmNum[act])
{
count_input = 0;
continue;
}
for(int i=0;i<servo_num;i++)
{
vlaue2angle(i, act);
in_value[i] = value_pre[i];
value_delta[i] = (value_cur[i] - value_pre[i]) / frequency;
}
for(int i=0;i<frequency;i++)
{
for(int k=0;k<servo_num;k++)
{
in_value[k] += value_delta[k];
value_pre[k] = in_value[k];
}
for(int j=0;j<servo_num;j++)
{
tlc_setServo(servo_pin[j], in_value[j]);
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

帐号		自动登录	找回密码
密码			注册