当前位置：首页 > news >正文

第三章、运动学逆解（双足轮根据腿高求舵机角度）

news 来源：原创 2025/5/29 13:10:37

0 前言

该部分作为开发过程中的学习记录，可能有错误及不足欢迎小伙伴们一起讨论～

注：该部分是根据灯哥开源的代码来进行学习，具体数学计算部分不做过多叙述，bilibili的教学视频中已经非常清晰了，本节只对个人学习过程中的代码做对应和解释。

1 腿高控制代码

首先，在上一章中我们已经了解了标准Arduino代码由setup()和loop()两个函数组成。

void setup() {// put your setup code here, to run once:// 在这里加入你的setup代码，它只会运行一次：
}void loop() {// put your main code here, to run repeatedly://  在这里加入你的loop代码,它会不断重复运行：
}

本章我们首先来看setup()和loop()两个函数

通过loop()主函数我们可以得到整个流程：

获取控制器数据（可以理解为每个通道都对应一个按钮，每个按钮不同位置对应不同的数字，比如说下面代码中用到的2通道实际上对应了给定的手柄左边的摇杆，摇杆从下推到上的值从200到1800）
将手柄数据范围映射到腿高数据范围（手柄推动高低即我们期望的腿高低，也就是所谓的腿高期望值。摇杆从下推到上的值从200到1800，腿高是70到130，这个映射的数学公式也很简单假设我们现在控制器数据为 $x$ 则映射后的腿高为 $70+\frac{x-200}{1800-200}\cdot(130-70)$ ）
根据腿高计算舵机角度值
控制舵机运动到相应的角度

void setup() {Serial.begin(921600);// 初始化串口通信，用于调试输出，波特率921600Wire.begin(1,2,400000UL);// 初始化I2C总线（SDA=GPIO1, SCL=GPIO2），时钟频率400kHzservos.init();// 初始化舵机控制器servos.setAngleRange(0,300);// 设置舵机角度范围（0-300度）servos.setPluseRange(500,2500);// 设置舵机PWM脉宽范围（500-2500μs）sbusRx.Begin(SBUSPIN,-1);// 初始化SBUS接收机（指定信号引脚，无反向）
}void loop() {getRCValue();// 读取遥控器通道数据/* 高度控制逻辑：* 将遥控器通道3的值映射到高度范围[lowest, highest]* RCCHANNEL3_MIN/MAX: 遥控器通道3的原始最小/最大值* Y_demand: 目标高度需求值*/Y_demand = ((int)map(RCValue[2], RCCHANNEL3_MIN, RCCHANNEL3_MAX, lowest, highest));// 调试输出：原始通道值 -> 映射后高度Serial.printf("%d\n",RCValue[2]);Serial.printf("%f\n",Y_demand);// 位置控制参数X = 0;// 水平位置保持0（中心位置）/* 高度平滑过渡：* 使用一阶惯性环节 Y = Y + Kp*(Y_demand - Y)* Kp_Y=0.1 为比例系数，数值越大响应越快但可能振荡*/Y = Y + Kp_Y * (Y_demand - Y);// 设置逆运动学参数（左右腿对称）IKParam.XLeft = X; // 左腿X坐标IKParam.XRight = X;// 右腿X坐标 IKParam.YLeft = Y;// 左腿Y坐标（高度）IKParam.YRight = Y;// 右腿Y坐标Serial.printf("%f,%f,%f\n",X,Y,Y_demand);// 执行逆运动学计算并驱动舵机inverseKinematics();
}

接下来再看实际逆运动学求解部分，该图片来自灯哥教学。
在这里插入图片描述

void inverseKinematics(){float alpha1,alpha2,beta1,beta2;uint16_t servoLeftFront,servoLeftRear,servoRightFront,servoRightRear;/*------------------------ 左腿计算 ------------------------*/// 可以对应上面的图片来看基本一致（左腿参数）float aLeft = 2 * IKParam.XLeft * L1;float bLeft = 2 * IKParam.YLeft * L1;float cLeft = IKParam.XLeft * IKParam.XLeft + IKParam.YLeft * IKParam.YLeft + L1 * L1 - L2 * L2;float dLeft = 2 * L4 * (IKParam.XLeft - L5);float eLeft = 2 * L4 * IKParam.YLeft;float fLeft = ((IKParam.XLeft - L5) * (IKParam.XLeft - L5) + L4 * L4 + IKParam.YLeft * IKParam.YLeft - L3 * L3);// 解二次方程得到大腿关节角度 alpha 和 beta（两个可能解）alpha1 = 2 * atan((bLeft + sqrt((aLeft * aLeft) + (bLeft * bLeft) - (cLeft * cLeft))) / (aLeft + cLeft));alpha2 = 2 * atan((bLeft - sqrt((aLeft * aLeft) + (bLeft * bLeft) - (cLeft * cLeft))) / (aLeft + cLeft));beta1 = 2 * atan((eLeft + sqrt((dLeft * dLeft) + eLeft * eLeft - (fLeft * fLeft))) / (dLeft + fLeft));beta2 = 2 * atan((eLeft - sqrt((dLeft * dLeft) + eLeft * eLeft - (fLeft * fLeft))) / (dLeft + fLeft));// 角度规范化（确保在0-2π范围内）alpha1 = (alpha1 >= 0)?alpha1:(alpha1 + 2 * PI);alpha2 = (alpha2 >= 0)?alpha2:(alpha2 + 2 * PI);/* 解的选择策略：* 左后腿选择大于π/4（45度）的解* 左前选择0~π/4范围内的解*/if(alpha1 >= PI/4) IKParam.alphaLeft = alpha1;else IKParam.alphaLeft = alpha2;if(beta1 >= 0 && beta1 <= PI/4) IKParam.betaLeft = beta1;else IKParam.betaLeft = beta2;/*------------------------ 右腿计算 ------------------------*/// 可以对应上面的图片来看基本一致（右腿参数）float aRight = 2 * IKParam.XRight * L1;float bRight = 2 * IKParam.YRight * L1;float cRight = IKParam.XRight * IKParam.XRight + IKParam.YRight * IKParam.YRight + L1 * L1 - L2 * L2;float dRight = 2 * L4 * (IKParam.XRight - L5);float eRight = 2 * L4 * IKParam.YRight;float fRight = ((IKParam.XRight - L5) * (IKParam.XRight - L5) + L4 * L4 + IKParam.YRight * IKParam.YRight - L3 * L3);IKParam.alphaRight = 2 * atan((bRight + sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));IKParam.betaRight = 2 * atan((eRight - sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));// 解二次方程得到大腿关节角度 alpha 和 beta（两个可能解）alpha1 = 2 * atan((bRight + sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));alpha2 = 2 * atan((bRight - sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));beta1 = 2 * atan((eRight + sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));beta2 = 2 * atan((eRight - sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));// 角度规范化（确保在0-2π范围内）alpha1 = (alpha1 >= 0)?alpha1:(alpha1 + 2 * PI);alpha2 = (alpha2 >= 0)?alpha2:(alpha2 + 2 * PI);/* 解的选择策略：* 右后腿选择大于π/4（45度）的解* 右前选择0~π/4范围内的解*/if(alpha1 >= PI/4) IKParam.alphaRight = alpha1;else IKParam.alphaRight = alpha2;if(beta1 >= 0 && beta1 <= PI/4) IKParam.betaRight = beta1;else IKParam.betaRight = beta2;// 弧度转角度alphaLeftToAngle = (int)((IKParam.alphaLeft / 6.28) * 360);betaLeftToAngle = (int)((IKParam.betaLeft / 6.28) * 360);alphaRightToAngle = (int)((IKParam.alphaRight / 6.28) * 360);betaRightToAngle = (int)((IKParam.betaRight / 6.28) * 360);// 角度矫正servoLeftFront = 90 + betaLeftToAngle;servoLeftRear = 90 + alphaLeftToAngle;servoRightFront = 270 - betaRightToAngle;servoRightRear = 270 - alphaRightToAngle;setServoAngle(servoLeftFront,servoLeftRear,servoRightFront,servoRightRear);
}