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 电子硬件
在这个示例中,我们采用了以下硬件,请大家参考:
主控板 | |
扩展板 | SH-SR舵机扩展板 |
舵机 | 标准舵机 |
电池 | 7.4V锂电池 |
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(增加的动作组名称[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例程源代码
资料内容详见 12自由度六足-步态规划