第四章、自平衡控制

0 前言

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

注：这里主要面临的问题是根据给定的课程代码没办法实现自稳定，因此只能在整个代码中拆解找原因。这个部分可能一次性更新不完会持续更新。

1 理论部分

理论部分很简单实际上就是计算一个恢复力矩，下面我们简单对公式和理论做一定的叙述，以及会对自己的想法做部分记录

首先需要有一个前提假设，假设我们的轮子装的完全理想化，也就是说足高度定了的情况下我们用上节课的知识通过逆运动学求解了舵机角度，然后控制舵机运动到相关位置。假设此时左腿结构与右腿完全相同（为什么要一而再再而三的强调这个问题，是因为实际的机械结构基本上没办法达到这个要求，那么达不到这个要求会带来什么影响呢，我们着重放在机身的base上，如果满足要求那么在等腿高情况下运动实际上只会有pitch（俯仰角）变化，但如果不满足实际上我们会有roll（滚转角）的，那么恢复力矩除了要处理pitch之外还要处理roll）

1 假设只有pitch变化的时候，实际上自平衡我们就是期望pitch是0，那给一个相反方向的力矩就可以了
$torque=k_p\times (0-pitch)$
2 假设除了pitch之外我们还有roll的变化
$torque=k_p\times (0-pitch)+k_d\times (0-roll)$
3 假如我想前后运动怎么办呢？指令部分肯定就是告诉机器人往前或者往后的速度。那自然而然的我希望轮子的速度达到这个速度，并计算我达到这种情况下的目标角度是怎样的。
$target\_angle=k_{p}new\times (targetspeed-speedavg)$
$torque=k_p\times (target\_angle-pitch)+k_d\times (0-roll)$
暂时不考虑自平衡补偿部分，那么对应代码就是

float targetAngle = PID_VEL(targetSpeed - speedAvg);
float torque = kp1*(targetAngle - pitch)- Kd * robotPose.GyroY;

所以实际上拆分的代码没法让我的机器人平衡的原因就在roll上，尤其是自己拼装的。

2 代码部分

1 高度自适应PID参数调整

	//目的：适应不同重心高度下的动力学特性。if(robotPose.height != robotLastHeight){//速度环PID（PID_VEL）的比例项 P 分两段线性调整。if(70 <= robotPose.height < 110)PID_VEL.P = -0.0067 * robotPose.height + 1.12;else if(110 <= robotPose.height <= 130)PID_VEL.P = 0.4;PID_VEL.I = 0.00153 * 0.6;  //1PID_VEL.D = (0.0000002 * robotPose.height * robotPose.height - 0.00001 * robotPose.height - 0.01) * 0.1;//其他参数（I/D项、直立环PID、转向限制等）通过二次函数拟合高度变化。PID_Stb.Kp = (0.0003 * robotPose.height * robotPose.height - 0.0488 * robotPose.height + 3.5798) * 0.8;     //0.8PID_Stb.Kd = (-0.000002 * robotPose.height * robotPose.height + 0.0005 * robotPose.height - 0.0043) * 1.6;  //1.6PID_Roll.Kp = (0.001 * robotPose.height * robotPose.height - 0.2281 * robotPose.height + 17.495) * 1.3;PID_Gyrox.Kp = (0.0000009 * robotPose.height * robotPose.height - 0.0005 * robotPose.height + 0.091) * GyroXPModify;robotMotion.turnLimit = (0.000009 * robotPose.height * robotPose.height - 0.005 * robotPose.height + 120.5104) * 1;robotLastHeight = robotPose.height;}

2 当前两个轮的速度，计算考虑电机方向（M0SpdDir/M1SpdDir）。

robotPose.speedAvg = (motorStatus.M0SpdDir * motorStatus.M0Speed + motorStatus.M1SpdDir * motorStatus.M1Speed) / 2;

3 自平衡补偿
当遥控器通道2无变化时，调用重心自校准函数selfCaliCentroid()。

if(RCLastCH2Value == RCValue[1]){controlTarget.centerAngleOffset = selfCaliCentroid(controlTarget.centerAngleOffset);}RCLastCH2Value = RCValue[1];

自平衡补偿函数

float selfCaliGain = 0.8;
float selfcaliOffset = 0;
#define SELF_CALI_RANGE 7
#define CENTER_ANGLE_OFFSET 3
float selfCaliCentroid(float central){static int i = 0;// 静态计数器，用于控制校准执行频率// 每40次调用执行一次校准逻辑if(i == 40){// 仅当机器人平均速度大于1时进行校准if(fabs(robotPose.speedAvg) > 1){// 计算速度补偿偏移量：增益 * (-1) * 平均速度（其实就是速度相反方向补偿，我感觉可能是这个的问题）selfcaliOffset = selfCaliGain * -1 * robotPose.speedAvg;// 将偏移量限制在[-0.5, 0.5]范围内，防止过度补偿selfcaliOffset = _constrain(selfcaliOffset, -0.5, 0.5);// 应用偏移量到中心位置central += selfcaliOffset;}i = 0;}else{++i;   }// 将central约束在有效范围内：// [CENTER_ANGLE_OFFSET - SELF_CALI_RANGE, CENTER_ANGLE_OFFSET + SELF_CALI_RANGE]central = _constrain(central, CENTER_ANGLE_OFFSET-SELF_CALI_RANGE, CENTER_ANGLE_OFFSET+SELF_CALI_RANGE);return central;
}

4 核心控制环

//遥控器前进指令（robotMotion.forward）与当前速度的偏差。
//输出：目标俯仰角（用于平衡速度跟踪）
controlTarget.velocity = PID_VEL(robotMotion.forward*0.5f - robotPose.speedAvg);//速度环   forward是遥控器前进后退参数
//遥控器转向指令（turn）与Z轴角速度（GyroZ）的偏差。
//输出：左右轮速差（differVel）。
controlTarget.differVel = PID_Streeing.Kp*(robotMotion.turn-robotPose.GyroZ);//转向        turn是遥控器左右控制参数
//关键公式：直立环输出 = Kp*(目标角度+补偿-实际俯仰角) - Kd*俯仰角速度。
targetVoltage = PID_Stb.Kp*(controlTarget.velocity + controlTarget.centerAngleOffset - robotPose.pitch) - PID_Stb.Kd * robotPose.GyroY;//直立环 输出控制的电机的目标速度

5 点击输出合成

//左电机：基础电压（targetVoltage） + 转向差速（differVel）。
motorsTarget.motorLeft = motorStatus.M0Dir * (targetVoltage + controlTarget.differVel);
//右电机：基础电压 - 转向差速。
motorsTarget.motorRight = motorStatus.M1Dir * (targetVoltage - controlTarget.differVel);

6 安全约束

//俯仰角在安全范围（-35° ~ 40°），急停条件：俯仰角超限或电机未使能时立即停止。
if (robotMode.motorEnable == 1 && robotPose.pitch <= 40 && robotPose.pitch >= -35) {motorsTarget.motorLeft = _constrain(motorsTarget.motorLeft, -5.7, 5.7); // 速度钳位motorsTarget.motorRight = _constrain(..., -5.7, 5.7);motors.setTargets(motorsTarget.motorLeft, motorsTarget.motorRight); // 驱动电机
} else {motors.setTargets(0, 0); // 急停条件
}