本文示例将实现R322样机Delta型腿机器狗维持身体平衡、原地圆形摆动、原地踏步、蹲起、站立、前进、后退、转向、横向移动、斜向移动等功能。
本实验中采用了以下硬件：
电路连接说明：为了便于识别控制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
3. 功能实现
3.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号舵机舵角：
圆
计算2号舵机舵角：
圆
计算3号舵机舵角：
圆
代码中对应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），具体实验效果可参考演示视频。
/*------------------------------------------------------------------------------------
版权说明：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-06-07 https://www.robotway.com/
------------------------------*/
/*****
Copyright 2017 Robot TIme
全动作展示例程
*****/
#include "Tlc5940.h"
#include "tlc_servos.h"
#include
#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();
}
程序源代码及样机3D文件详见 Delta型腿机器狗-全动作展示
