代码结构概览
整个项目采用**有限状态机(FSM)**架构，主要模块包括：
plainText
main.cpp # 主入口，创建控制框架
├── ControlFrame.cpp # 控制框架，调用FSM
├── FSM/
│ ├── FSM.cpp # 状态机核心，管理状态切换
│ ├── State_Trotting.cpp # 小跑步态状态（主要运动模式）
│ ├── State_FreeStand.cpp # 自由站立状态
│ └── State_FixedStand.cpp# 固定站立状态
├── Gait/
│ ├── GaitGenerator.cpp # 步态轨迹生成器
│ └── FeetEndCal.cpp # 足端位置计算
├── control/
│ └── BalanceCtrl.cpp # 平衡控制
└── interface/
└── WirelessHandle.cpp # 无线手柄输入
左右转动代码位置分析
1. Trotting 小跑状态（核心转动控制）
文件: /home/wd/micu_unitreedog_base/src/unitree_guide/unitree_guide/src/FSM/State_Trotting.cpp
关键代码：
读取用户转动指令 (第136-146行)：
cpp
void State_Trotting::getUserCmd(){
/* Movement */
_vCmdBody(0) = invNormalize(_userValue.ly, _vxLim(0), _vxLim(1)); // 前进/后退
_vCmdBody(1) = -invNormalize(_userValue.lx, _vyLim(0), _vyLim(1)); // 左右平移
/* Turning */
_dYawCmd = -invNormalize(_userValue.rx, _wyawLim(0), _wyawLim(1)); // 左右转动！
_dYawCmd = 0.9*_dYawCmdPast + (1-0.9) * _dYawCmd; // 低通滤波
_dYawCmdPast = _dYawCmd;
}
计算转动命令 (第148-165行)：
cpp
void State_Trotting::calcCmd(){
/* Movement */
_vCmdGlobal = _B2G_RotMat * _vCmdBody;
/* Turning */
_yawCmd = _yawCmd + _dYawCmd * _ctrlComp->dt; // 累积计算目标偏航角
_Rd = rotz(_yawCmd); // 生成旋转矩阵
_wCmdGlobal(2) = _dYawCmd; // 设置全局角速度（绕Z轴）
}
设置命令到步态生成器 (第89行)：
cpp
_gait->setGait(_vCmdGlobal.segment(0,2), _wCmdGlobal(2), _gaitHeight);
2. 自由站立状态的转动控制
文件: /home/wd/micu_unitreedog_base/src/unitree_guide/unitree_guide/src/FSM/State_FreeStand.cpp
关键代码 (第44-47行)：
cpp
void State_FreeStand::run(){
Vec34 vecOP;
_userValue = _lowState->userValue;
vecOP = _calcOP( invNormalize(_userValue.lx, _rowMin, _rowMax), // 横滚
invNormalize(_userValue.ly, _pitchMin, _pitchMax), // 俯仰
-invNormalize(_userValue.rx, _yawMin, _yawMax), // 偏航（左右转）
invNormalize(_userValue.ry, _heightMin, _heightMax) ); // 高度
_calcCmd(vecOP);
}
3. 足端位置计算（转动时的脚位调整）
文件: /home/wd/micu_unitreedog_base/src/unitree_guide/unitree_guide/src/Gait/FeetEndCal.cpp
关键代码 (第27-45行)：
cpp
Vec3 FeetEndCal::calFootPos(int legID, Vec2 vxyGoalGlobal, float dYawGoal, float phase){
// ... 计算下一步位置 ...
_nextYaw = _dYaw*(1-phase)*_Tswing + _dYaw*_Tstance/2 + _kyaw*(dYawGoal - _dYaw);
// 根据转动角度调整脚的位置
_nextStep(0) += _feetRadius(legID) * cos(_yaw + _feetInitAngle(legID) + _nextYaw);
_nextStep(1) += _feetRadius(legID) * sin(_yaw + _feetInitAngle(legID) + _nextYaw);
_footPos = _est->getPosition() + _nextStep;
_footPos(2) = 0.0;
return _footPos;
}
4. 无线手柄输入（转动指令来源）
文件: /home/wd/micu_unitreedog_base/src/unitree_guide/unitree_guide/src/interface/WirelessHandle.cpp
关键代码 (第57-61行)：
cpp
userValue.L2 = killZeroOffset(_keyData.L2, 0.08);
userValue.lx = killZeroOffset(_keyData.lx, 0.08); // 左摇杆X轴
userValue.ly = killZeroOffset(_keyData.ly, 0.08); // 左摇杆Y轴
userValue.rx = killZeroOffset(_keyData.rx, 0.08); // 右摇杆X轴 -> 控制转动
userValue.ry = killZeroOffset(_keyData.ry, 0.08); // 右摇杆Y轴
转动控制流程总结
plainText
无线手柄右摇杆X轴 (rx)
│
▼
WirelessHandle::receiveHandle()
│
▼
lowState.userValue.rx
│
▼
┌───────────────────────────────────────┐
│ 状态判断 │
└───────────────────────────────────────┘
│
├── FREESTAND ──► State_FreeStand::run()
│ │
│ ▼
│ 调整身体姿态（偏航角）
│
└── TROTTING ──► State_Trotting::getUserCmd()
│
▼
State_Trotting::calcCmd()
│
▼
GaitGenerator::setGait()
│
▼
FeetEndCal::calFootPos()
│
▼
计算各脚的目标位置
关键变量说明
| 变量名 | 含义 | 范围 |
|---|---|---|
_userValue.rx |
手柄右摇杆X轴输入 | [-1, 1] |
_dYawCmd |
目标偏航角速度 | 受 _wyawLim 限制 |
_yawCmd |
累积的目标偏航角 | 弧度 |
_wCmdGlobal(2) |
全局坐标系下的Z轴角速度 | 弧度/秒 |
总结
机器狗的左右转动控制主要在 State_Trotting.cpp 中实现：
- 输入层：手柄右摇杆X轴 (
rx) 通过WirelessHandle读取 - 命令层：
getUserCmd()将手柄输入转换为角速度指令_dYawCmd - 计算层：
calcCmd()累积计算目标偏航角，并传递给步态生成器 - 执行层：
GaitGenerator和FeetEndCal根据转动指令调整各脚的轨迹
站立状态下的转动控制在 State_FreeStand.cpp 中实现，通过调整身体姿态实现原地转动。
WaveGenerator 的工作机制详解
一、Vec4(bias) 的核心作用
1. 参数定义
cpp
WaveGenerator(0.2, 0.8, Vec4(0, 0, 0, 0))
// ↑
// bias = [腿1, 腿2, 腿3, 腿4] 的相位偏置
2. 相位计算原理（WaveGenerator.cpp:107）
cpp
_normalT(i) = fmod(_passT + _period - _period * _bias(i), _period) / _period;
公式解读：
_passT：从步态开始到现在的时间_period * _bias(i)：第i条腿的相位偏移量_normalT(i)：归一化到 [0,1) 的相位值
3. 支撑/摆动判定（WaveGenerator.cpp:108-117）
cpp
if (_normalT(i) < _stRatio){ // 0.8 以内 → 支撑相
contact(i) = 1; // 腿着地
phase(i) = _normalT(i) / _stRatio; // 相位归一化到 [0,1]
} else { // 0.8 以外 → 摆动相
contact(i) = 0; // 腿抬起
phase(i) = (_normalT(i) - _stRatio) / (1 - _stRatio); // 相位归一化到 [0,1]
}
二、不同步态模式的对比
1. Pronk（四条腿同步）
cpp
Vec4(0, 0, 0, 0)
plainText
时间轴 →
腿1: ████████░░░░ (█=支撑 0.8, ░=摆动 0.2)
腿2: ████████░░░░ 完全同步
腿3: ████████░░░░
腿4: ████████░░░░
相位关系：0 ─ 0 ─ 0 ─ 0
2. Trot（对角小跑）
cpp
Vec4(0, 0.5, 0.5, 0)
plainText
时间轴 →
腿1: ████████░░░░
腿2: ░░░░████████ 延迟0.5个周期
腿3: ░░░░████████ 延迟0.5个周期
腿4: ████████░░░░
相位关系：0 ─ 0.5 ─ 0.5 ─ 0
↖───────┘↖───────┘
对角腿同步
3. Crawl（爬行）
cpp
Vec4(0, 0.25, 0.5, 0.75)
plainText
时间轴 →
腿1: ████████░░░░
腿2: ░░████████░░ 延迟0.25
腿3: ░░░░████████ 延迟0.5
腿4: ░░░░░░░░████ 延迟0.75
相位关系：0 ─ 0.25 ─ 0.5 ─ 0.75
依次延迟，轮流支撑
三、WaveGenerator 的职责边界
WaveGenerator 只负责「时间节律控制」
plainText
WaveGenerator 输出：
├── contact[4] → 每条腿是否着地（1=着地，0=抬起）
└── phase[4] → 每条腿当前阶段（0~1，表示在支撑/摆动中的位置）
WaveGenerator 不负责：
├── 腿要抬多高
├── 腿要迈多远
├── 关节角度是多少
后续模块的职责
plainText
WaveGenerator
│
├─► contact → GaitGenerator → 判断腿是支撑还是摆动
│
└─► phase → GaitGenerator → 插值计算腿的位置（摆线轨迹）
│
▼
FeetEndCal → 计算足端落点（考虑速度、转动）
│
▼
unitreeRobot::getQ() → 逆运动学求解关节角度
四、左右转动时的协作流程
完整数据流
plainText
手柄输入 (rx → dYawCmd)
│
▼
State_Trotting::calcCmd()
│
├─► _yawCmd → 目标偏航角累积
├─► _Rd = rotz(_yawCmd) → 旋转矩阵
└─► _wCmdGlobal(2) = _dYawCmd → 角速度指令
│
▼
GaitGenerator::setGait(vxy, wz, height)
│
▼
GaitGenerator::run()
│
├─► contact[i]==1 → 支撑腿：保持当前位置
│
└─► contact[i]==0 → 摆动腿
│
▼
FeetEndCal::calFootPos()
│
├─► 计算落点（考虑转动）
└─► 返回目标位置
│
▼
摆线插值（使用 phase[i]）
│
▼
unitreeRobot::getQ() → 关节角度
五、关键区别：时间节律 vs 运动学
| 模块 | 职责 | 输出 |
|---|---|---|
| WaveGenerator | 时间节律控制 | contact, phase |
| GaitGenerator | 轨迹生成 | 足端位置、速度 |
| FeetEndCal | 落点计算 | 考虑速度/转动的目标点 |
| unitreeRobot | 逆运动学 | 关节角度 |
总结
- Vec4(bias) 确实约定了四条腿的运动顺序和时间节点：
- 通过相位偏置决定哪条腿先动、哪条腿后动
- 不同的 bias 向量对应不同的步态模式
- WaveGenerator 不直接涉及逆运动学：
- 它只输出「时间信号」（contact 和 phase）
- 逆运动学是在
unitreeRobot::getQ()中完成的 - 转动时的落点调整是在
FeetEndCal中根据速度指令计算的
- 协作关系：
plainText
步态模式(bias) → 时间节律(contact/phase) → 轨迹生成(落点+摆线) → 逆运动学(关节角度)
WaveGenerator 负责「时间节律控制」，而左右转动/混合移动的调整发生在「轨迹生成」和「逆运动学」阶段。
速度指令到逆运动学的完整转换流程
一、整体转换架构
plainText
用户手柄输入
│
▼
速度指令 (vx, vy, wz)
│
▼
┌──────────────────────────────────────────┐
│ 1. 坐标变换：身体坐标系 → 全局坐标系 │
└──────────────────────────────────────────┘
│
▼
┌──────────────────────────────────────────┐
│ 2. 落点计算：根据速度计算足端目标位置 │
└──────────────────────────────────────────┘
│
▼
┌──────────────────────────────────────────┐
│ 3. 轨迹生成：摆线插值生成连续轨迹 │
└──────────────────────────────────────────┘
│
▼
┌──────────────────────────────────────────┐
│ 4. 逆运动学：足端位置 → 关节角度 │
└──────────────────────────────────────────┘
│
▼
关节角度指令 (q1, q2, q3) × 4条腿
二、详细转换流程
1. 用户输入到速度指令
文件: State_Trotting.cpp (第136-146行)
cpp
void State_Trotting::getUserCmd(){
/* 前进/后退 */
_vCmdBody(0) = invNormalize(_userValue.ly, _vxLim(0), _vxLim(1));
/* 左右平移 */
_vCmdBody(1) = -invNormalize(_userValue.lx, _vyLim(0), _vyLim(1));
/* 左右转动 */
_dYawCmd = -invNormalize(_userValue.rx, _wyawLim(0), _wyawLim(1));
}
输入输出：
| 手柄输入 | 输出指令 | 物理含义 |
|---|---|---|
ly (左摇杆Y) |
_vCmdBody(0) |
前进/后退速度 |
lx (左摇杆X) |
_vCmdBody(1) |
左右平移速度 |
rx (右摇杆X) |
_dYawCmd |
偏航角速度 |
2. 坐标变换与目标计算
文件: State_Trotting.cpp (第148-165行)
cpp
void State_Trotting::calcCmd(){
/* 身体坐标系 → 全局坐标系 */
_vCmdGlobal = _B2G_RotMat * _vCmdBody;
/* 累积计算目标偏航角 */
_yawCmd = _yawCmd + _dYawCmd * _ctrlComp->dt;
_Rd = rotz(_yawCmd);
_wCmdGlobal(2) = _dYawCmd;
}
关键变量：
_B2G_RotMat：身体坐标系到全局坐标系的旋转矩阵_vCmdGlobal：全局坐标系下的速度指令_Rd：期望的身体姿态旋转矩阵
3. 步态生成与落点计算
文件: FeetEndCal.cpp (第27-45行)
cpp
Vec3 FeetEndCal::calFootPos(int legID, Vec2 vxyGoalGlobal, float dYawGoal, float phase){
// 当前身体状态
_bodyVelGlobal = _est->getVelocity();
_bodyWGlobal = _lowState->getGyroGlobal();
// 计算下一步位移（前馈+反馈）
_nextStep(0) = _bodyVelGlobal(0)*(1-phase)*_Tswing
+ _bodyVelGlobal(0)*_Tstance/2
+ _kx*(_bodyVelGlobal(0) - vxyGoalGlobal(0));
_nextStep(1) = _bodyVelGlobal(1)*(1-phase)*_Tswing
+ _bodyVelGlobal(1)*_Tstance/2
+ _ky*(_bodyVelGlobal(1) - vxyGoalGlobal(1));
// 转动补偿
_nextYaw = _dYaw*(1-phase)*_Tswing + _dYaw*_Tstance/2 + _kyaw*(dYawGoal - _dYaw);
_nextStep(0) += _feetRadius(legID) * cos(_yaw + _feetInitAngle(legID) + _nextYaw);
_nextStep(1) += _feetRadius(legID) * sin(_yaw + _feetInitAngle(legID) + _nextYaw);
// 最终落点 = 身体位置 + 相对位移
_footPos = _est->getPosition() + _nextStep;
_footPos(2) = 0.0;
return _footPos;
}
核心公式：
plainText
落点位置 = 身体当前位置 + 速度×时间 + 转动补偿
4. 轨迹插值
文件: GaitGenerator.cpp (第53-61行)
cpp
Vec3 GaitGenerator::getFootPos(int i){
Vec3 footPos;
// 摆线插值
footPos(0) = cycloidXYPosition(_startP.col(i)(0), _endP.col(i)(0), (*_phase)(i));
footPos(1) = cycloidXYPosition(_startP.col(i)(1), _endP.col(i)(1), (*_phase)(i));
footPos(2) = cycloidZPosition(_startP.col(i)(2), _gaitHeight, (*_phase)(i));
return footPos;
}
摆线插值公式：
cpp
// XY平面插值
float cycloidXYPosition(float start, float end, float phase){
float phasePI = 2 * M_PI * phase;
return (end - start)*(phasePI - sin(phasePI))/(2*M_PI) + start;
}
// Z轴插值（抬腿）
float cycloidZPosition(float start, float h, float phase){
float phasePI = 2 * M_PI * phase;
return h*(1 - cos(phasePI))/2 + start;
}
5. 逆运动学求解（核心）
文件: unitreeLeg.cpp (第64-100行)
腿部结构模型
plainText
髋关节(abad) q1
│
▼
大腿(hip) q2
│
▼
小腿(knee) q3
│
▼
足底
逆运动学算法
cpp
Vec3 QuadrupedLeg::calcQ(Vec3 pEe, FrameType frame){
// 1. 坐标系转换：BODY → HIP
Vec3 pEe2H = pEe - _pHip2B;
float px = pEe2H(0); // X方向（前进）
float py = pEe2H(1); // Y方向（侧移）
float pz = pEe2H(2); // Z方向（高度）
// 2. 求解髋关节角度 q1
float L = sqrt(pow(py,2) + pow(pz,2) - pow(l1,2));
q1 = atan2(pz*l1 + py*L, py*l1 - pz*L);
// 3. 求解膝关节角度 q3
float b = sqrt(pow(c, 2) - pow(a, 2)); // 肩到脚的距离
float temp = (pow(b3z, 2) + pow(b4z, 2) - pow(b, 2)) / (2*fabs(b3z*b4z));
q3 = acos(temp);
q3 = -(M_PI - q3);
// 4. 求解大腿角度 q2
a1 = py*sin(q1) - pz*cos(q1);
a2 = px;
m1 = b4z*sin(q3);
m2 = b3z + b4z*cos(q3);
q2 = atan2(m1*a1 + m2*a2, m1*a2 - m2*a1);
return Vec3(q1, q2, q3);
}
三条腿的几何关系
plainText
/|
/ | l3 (小腿)
/ |
/ |
/ |
l2 / |
(大腿)/ |
└──────┘
b
几何求解步骤：
- q1（髋关节外展角）：通过投影到YZ平面，用反正切求解
- q3（膝关节角）：利用余弦定理求解
- q2（大腿角）：根据已求角度，通过正切求解
6. 速度级逆运动学（可选）
文件: unitreeLeg.cpp (第103-111行)
cpp
Vec3 QuadrupedLeg::calcQd(Vec3 q, Vec3 vEe){
// 雅可比矩阵逆解
return calcJaco(q).inverse() * vEe;
}
雅可比矩阵（unitreeLeg.cpp 第119-147行）描述关节角速度与末端速度的关系：
plainText
v_ee = J(q) * q_dot
q_dot = J(q)^(-1) * v_ee
三、完整数据流总结
plainText
手柄输入 (ly, lx, rx)
│
▼
State_Trotting::getUserCmd()
│
▼
速度指令 (_vCmdBody, _dYawCmd)
│
▼
State_Trotting::calcCmd()
│
▼
全局速度 (_vCmdGlobal, _wCmdGlobal)
│
▼
GaitGenerator::setGait()
│
▼
FeetEndCal::calFootPos()
│
▼
足端目标位置 (_posFeetGlobalGoal)
│
▼
GaitGenerator::run() → 摆线插值
│
▼
平滑轨迹 (feetPos, feetVel)
│
▼
unitreeLeg::calcQ()
│
▼
关节角度 (q1, q2, q3) × 4条腿
四、关键模块职责
| 模块 | 职责 | 输入 | 输出 |
|---|---|---|---|
| State_Trotting | 速度指令生成与坐标变换 | 手柄输入 | 全局速度指令 |
| FeetEndCal | 足端落点计算 | 速度、角速度 | 落点位置 |
| GaitGenerator | 轨迹插值 | 起点、终点、相位 | 平滑轨迹 |
| QuadrupedLeg | 逆运动学求解 | 足端位置 | 关节角度 |
五、数学本质
逆运动学的核心是几何求解：
- 问题定义：已知足端在空间中的位置 (x, y, z)，求三个关节角度 (q1, q2, q3)
- 求解策略：采用解析解法（封闭解）
- 将三维问题分解为平面问题
- 利用三角函数和几何关系直接求解
- 优势：
- 计算速度快（适合实时控制）
- 解的唯一性有保证（在工作空间内）
- 局限性：
- 仅适用于特定的腿部结构（3自由度串联臂）
- 存在奇异位形（如完全伸直或折叠）
总结
速度指令到逆运动学的转换分为四个层次：
- 指令层：手柄输入 → 速度指令（坐标无关）
- 轨迹层：速度指令 → 足端轨迹（全局坐标系）
- 位置层：轨迹点 → 足端位置（身体坐标系）
- 关节层：足端位置 → 关节角度（逆运动学）
核心数学工具是几何解析法，通过三角函数和余弦定理直接求解三条关节角度，确保实时性和稳定性。