Delta型腿机器狗的设计与实现-机器谱-robotway

机器谱

【R322】Delta型腿机器狗

作者：机器谱

图文展示3264(1)

副标题

全动作展示

原地动作

行进动作

机器狗制作、Delta型腿机器狗制作、Delta机器人制作、仿生机器人、Delta型腿、仿生机器人设计、开源

1. 功能说明

本文示例将实现R322样机Delta型腿机器狗维持身体平衡、原地圆形摆动、原地踏步、蹲起、站立、前进、后退、转向、横向移动、斜向移动等功能。

2. 电子硬件

本实验中采用了以下硬件：

全动作展示

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
扩展板	SH-SR舵机扩展板
传感器	近红外传感器
传感器	六轴陀螺仪
电池	7.4v锂电池、11.1V动力电池
其它	电压显示器

/*------------------------------------------------------------------------------------

Distributed under MIT license.See file LICENSE for detail or copy at

https://opensource.org/licenses/MIT

by 机器谱 2023-06-07 https://www.robotway.com/

------------------------------*/

/*****

全动作展示例程

*****/

#include "Tlc5940.h"

#include "tlc_servos.h"

#include <math.h>

#include "types.h"

#include "config.h"

// 相关函数声明

/***** 红外相关函数 *****/

void IRInit(); //红外初始化

void enableIR(); //红外使能

void disableIR(); //关闭红外

void updateIR(); //红外避障更新动作

/***** 平衡相关函数 *****/

void switchAdjustStat(uint stat); //切换平衡调节模式不调节/原地调节/行进间调节

void readGyroSerial(); //读取陀螺仪串口消息

void adjustAct(); //平衡调节动作

/****** 腿部动作相关函数 *****/

void setTurnLeftFlag(bool flag); //修改左转状态标志位

void setTurnRightFlag(bool flag); //修改右转状态标志位

void leg1(); //更新1号腿（左前）位置

void leg2(); //更新2号腿（左后）位置

void leg3(); //更新3号腿（右前）位置

void leg4(); //更新4号腿（右后）位置

bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数

/***** 整机动作相关函数 *****/

void dogReset(Point3d initPos, uint waitTime); //复位动作

void dogInit(); //初始化动作

void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作

void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作

void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作

void liftShoulder(uint height, uint times); //原地摆臂动作

//动作周期计数器

int cycleCount;

//复位计数器

void resetCycleCount()

{

cycleCount = -1;

}

void updateCycleCount()

{

cycleCount++;

}

//当前运动状态

dogMode currentMode;

//切换运动状态

void setMode(dogMode mode)

{

if (mode == currentMode) return;

if (mode == DOG_MODE_TURN_LEFT)

{

setTurnLeftFlag(true);

setTurnRightFlag(false);

} else if (mode == DOG_MODE_TURN_RIGHT)

{

setTurnLeftFlag(false);

setTurnRightFlag(true);

} else {

setTurnLeftFlag(false);

setTurnRightFlag(false);

}

if (mode == DOG_MODE_BACK) //后退时关闭红外传感器

{

disableIR();

} else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节

{

switchAdjustStat(ADJUST_STAT_LEG);

dogReset({0, 0, Leg_Init_Z_Pos}, 200);

}

currentMode = mode;

}

void updateMode()

{

if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);

if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);

if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);

if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);

if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);

if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);

if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);

if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);

if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);

if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);

}

void setup()

{

//陀螺仪连接串口，波特率115200

Serial.begin(115200);

//舵机驱动板初始化

Tlc.init(0);

tlc_initServos(); // Note: this will drop the PWM freqency down to 50Hz.

//红外传感器初始化

IRInit();

//大狗身体初始化

dogInit();

//原地摆臂动作一次

liftShoulder(40, 1);

delay(500);

//原地做圆形摆动3周

drawCircle(0, 0, -120, 60, 3);

delay(500);

//原地蹲起3次

upDown(0, 0, -160, -90, 3);

delay(500);

//原地踏步6次

stepping(0, 0, -150, -100, 6);

delay(500);

resetCycleCount();

enableIR();

switchAdjustStat(ADJUST_STAT_TRACK);

setMode(DOG_MODE_FRONT);

}

void loop()

{

//姿态调节

adjustAct();

if (currentMode == DOG_MODE_STOP) return; //静止模式不进行后续动作

updateMode(); //切换运动模式

//计算4条腿运动位置

leg1();

leg4();

leg2();

leg3();

//更新所有舵机位置

Tlc.update();

//检测红外传感器信息

updateIR();

}

//串口与陀螺仪通信

void serialEvent() {

readGyroSerial();

}

电路连接说明：为了便于识别控制Delta型腿机器狗，我们先对机器狗的腿位置编号（如下图所示）：

① 硬件连接：

② 电压显示器与大电池连接：

③ 舵机接线位置：上面3个舵机分别连接在Bigfish扩展板的D4、D3、D8端口。

Delta型腿机器狗每条腿有4个舵机，4条腿上总共有16个舵机，将这16个舵机分别连接在SH-SR舵机扩展板的舵机接口上。

1号腿：s1连接口9 s2连接口8 s3连接口5 s4连接口6

2号腿：s1连接口18 s2连接口19 s3连接口20 s4连接口21

3号腿：s1连接口0 s2连接口2 s3连接口1 s4连接口3

4号腿：s1连接口27 s2连接口25 s3连接口26 s4连接口24

4. 资料清单

序号	内容
1	R322-程序源代码
2	R322-样机3D文件

机械狗的操作说明详见下方视频：

3. 功能实现

3.1 算法说明

（1）并联狗腿脚底位置逆解计算方法

代码中对应parallelLeg标签

u 已知(单位mm)：

3个舵机分别以相同连杆结构连接脚底平台，并联驱动

数学模型将脚底简化成平台点，3套连杆结构相应平移

给定的脚底空间位置Fo(x,y,z)

与舵机相连的连杆长度l1 = 60

与脚底相连的连杆长度l2 = 105

x-z平面(y=0)内两对称的舵机轴简化位置：O1(20,0,0),O2(-20,0,0)

y-z平面(x=0)内的舵机轴简化位置：O3(0, 40, 0)

u 计算过程描述：

以脚底坐标Fo(x,y,z)为球心，以l2为半径做球

以3个舵机轴位置为圆心，以l1为半径，分别在3组连杆所在平面内做3个圆

求出球面与3个圆的交点坐标

由交点坐标和舵机轴坐标共同计算出舵机角度

u 实际计算过程：

计算1号舵机舵角：

圆<Fo,l2><O1,l1>的交点，取x值较大的点

计算2号舵机舵角：

圆<Fo,l2><O2,l1>的交点，取x值较小的点

计算3号舵机舵角：

圆<Fo,l2><O3,l1>的交点，取y值较大的点

（2）运动差值算法

代码中对应legControl标签

① 确定单腿运动的矩形轨迹(x1,y1) - (x2,y2) (左下角坐标 - 右上角坐标)

② 分配一个周期T内运动到矩形各个顶点的时间t1 , t2, t3, t4

③ 为保证在矩形边上匀速运行，用当前边的两顶点的到达时间ti,tj及位置(xi,yi),(xj,yj)，以当前时间t做线性插值，计算t时刻的实际位置：

④ 按时间实时更新腿部位置，保证并联腿部的平滑运动

3.2 示例程序

编程环境：Arduino 1.8.0

下面提供一个Delta型腿机器狗全动作展示（维持身体平衡、原地圆形摆动、原地踏步、蹲起、站立、前进、后退、转向、横向移动、斜向移动）的参考例程（parallel_dog_display.ino），具体实验效果可参考演示视频。

文件下载

【整体打包】-【R322】Delta型腿机器狗-全动作展示-资料附件.zip

2.61MB

下载8次

下载

原地动作

1. 功能说明

本文示例将实现R322样机Delta型腿机器狗原地摆臂、原地圆形摆动、原地蹲起、原地踏步的功能。

机器狗制作、Delta型腿机器狗制作、Delta机器人制作、仿生机器人、Delta型腿、仿生机器人设计、开源、机器狗原地摆臂图

机器狗制作、Delta型腿机器狗制作、Delta机器人制作、仿生机器人、Delta型腿、仿生机器人设计、开源、机器狗原地蹲起

机器狗制作、Delta型腿机器狗制作、Delta机器人制作、仿生机器人、Delta型腿、仿生机器人设计、开源、原地踏步

原地摆臂

原地圆形摆动

原地蹲起

原地踏步

2. 电子硬件

本实验中采用了以下硬件：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
扩展板	SH-SR舵机扩展板
传感器	近红外传感器
传感器	六轴陀螺仪
电池	7.4v锂电池、11.1V动力电池
其它	电压显示器

/*------------------------------------------------------------------------------------

Distributed under MIT license.See file LICENSE for detail or copy at

https://opensource.org/licenses/MIT

by 机器谱 2023-06-26 https://www.robotway.com/

------------------------------*/

/*****

摆臂例程