Chassis::ControllerMaster

ChassisControllerpackagereadmepackage

1. 目录
2. ChassisController
3. Chassis::ControllerMaster

cpkg add Chassis::ControllerMaster

文档

来源

Master

Master 是“下位机自己负责底盘控制”的模式。上层给它的是目标速度或目标位姿，它自己维护轨迹曲线、误差闭环以及最终下发给 Motion 的车体速度。

如果你的上位机只下发目标点、阶段目标或人工速度指令，而不想直接维护每个离散轨迹点，通常应选这个模式。

控制模式

Master 内部有三个控制状态：

• Stopped：底盘锁定在当前位置，参考速度为零
• Velocity：直接按给定速度控制
• Posture：先做 S 曲线规划，再做误差闭环跟踪

切换方式由公开接口决定：

• stop() 进入 Stopped
• setVelocityInWorld() / setVelocityInBody() 进入 Velocity
• setTargetPostureInWorld() / setTargetPostureInBody() / setTargetPostureRelativeTo() 进入 Posture

目标接口的语义

Master 的目标设置不是零散函数堆出来的，而是明确分成两组，共 7 种组合：

• Posture 目标 3 种
• Velocity 目标 4 种

这套设计的核心思路是：

• 对于 Posture，区别在于“目标是相对于哪里定义的”
• 对于 Velocity，区别在于“你手里的速度目标是在哪个坐标系表达的”以及“控制器后续要保持哪个坐标系不变”

位姿目标

Master 支持三种位姿目标表达，它们分别对应不同的动作语义：

接口	目标基准	典型用途
setTargetPostureInWorld()	世界坐标系绝对目标	地图里已有明确目标点，直接去某个绝对位置
setTargetPostureInBody()	当前车体坐标系	简单相对动作，例如“向前 0.5m”“原地左转 90 度”
setTargetPostureRelativeTo()	某个给定世界基准位姿	某个动作需要固定参考起点，例如“以上台阶起点为基准执行一段相对动作”

补充说明：

• setTargetPostureInBody() 的“相对”是相对于“调用这一刻的自己”。
• setTargetPostureRelativeTo() 的“相对”是相对于“调用者显式给出的某个基准点”，它不一定是当前车体位置。
• 三者最终都会转换到世界坐标系下做轨迹规划，因此“世界坐标系由谁定义”仍然取决于所连接的 Loc 后端。

速度目标

速度目标共有 4 种组合。这里要把两个维度分开看：

1. 你拿到的目标速度本来是按世界系表达，还是按车体系表达
2. 控制器后续要保持“车体系速度不变”，还是保持“世界系速度不变”

对应组合如下：

接口	输入表达坐标系	后续保持不变的参考系	平移和旋转同时存在时的典型表现	典型用途
setVelocityInBody(body_velocity, false)	车体系	车体系	轨迹通常是圆弧	最普通的底盘遥控或车体系速度控制
setVelocityInBody(body_velocity, true)	车体系	世界系	边平移边自旋	想用“当前车头方向”给一个速度，再把这个方向锁成世界系方向
setVelocityInWorld(world_velocity, false)	世界系	车体系	轨迹通常是圆弧	上层当前只拿得到世界系速度，但只想在设置瞬间换算一次
setVelocityInWorld(world_velocity, true)	世界系	世界系	边平移边自旋	希望速度始终相对于世界不变，例如一边平移一边自旋

进一步解释：

• “车体系速度不变”表示底盘会一直保持同一个车体坐标系速度参考。如果同时有平移和旋转，从世界上看轨迹通常会是圆弧。
• “世界系速度不变”表示底盘会在每个周期重新把世界系速度投影到当前车体系，因此表现往往是边平移边自旋。
• 这也是 target_in_world 的真实含义：它不是在重复声明输入参数属于哪个坐标系，而是在声明“后续保持哪个参考系不变”。

如果只是遥控或普通速度指令，通常使用 setVelocityInBody(body_velocity, false)；如果希望机器人边平移边自旋，但平移方向始终锁在世界系，则通常使用 setVelocityInWorld(world_velocity, true)。

曲线衔接

位姿控制时，TrajectoryLinkMode 决定新的目标点如何与已有轨迹衔接：

• CurrentState：从当前估计的位姿和速度重新起一条新曲线
• PreviousCurve：从上一条曲线在当前时刻的状态继续衔接

如果上一控制状态不是 Posture，PreviousCurve 不会生效，最终仍会按当前状态重新起轨迹。

周期调用分工

这个控制器不是单函数 update()，而是故意拆成三个入口，方便接入项目按不同频率调度：

入口	当前源码注释建议	作用
profileUpdate(dt)	约 100Hz	推进 S 曲线轨迹，得到当前位置参考和前馈速度
errorUpdate()	约 200~500Hz	计算当前位置误差并更新普通 PD 输出
controllerUpdate()	控制快环	在纯速度模式下把参考速度真正下发给 Motion

几个实际接入上的要点：

• 如果当前处于 Posture 模式，profileUpdate() 和 errorUpdate() 会直接触发底盘速度下发。
• 如果当前处于 Velocity 模式，controllerUpdate() 才是负责持续下发速度的入口。
• stop() 不只是把速度清零，还会把当前位置记录为新的锁定参考点。

规划与限幅

Master::Config 主要包含：

• posture_error_pd_cfg：世界坐标系下 vx / vy / wz 的普通 PD 误差闭环参数
• limit：三轴各自的 S 曲线速度、加速度等限制
• tracking_threshold：轨迹跟踪和完成判定允许的位姿误差阈值，默认 x/y = 0.01m、yaw = 0.5deg
• auto_nearest_yaw_target：设置位姿目标时，是否把目标 yaw 转换为相对本次规划起点最近的等效连续角，默认开启

普通 PD 的 D 项来自位置误差的周期差分；profileUpdate() 计算出的 v_ref_curr_ 仍作为前馈速度叠加到 PD 输出上，但不再作为 速度误差阻尼项传入误差闭环。

位姿轨迹规划在三轴上独立生成，再用总时间最长的轴作为整体持续时间。这样可以保证 x、y、yaw 三条曲线同时结束。 Master 内部仍然使用连续 yaw 角；最短弧处理只发生在设置新目标、生成 yaw 轴 S 曲线之前。例如本次规划起点 yaw 为 179deg，目标传入 -179deg 时，会先转换成等效的 181deg，因此 yaw 轴只规划约 +2deg。如果需要保留调用方传入的原始 yaw 终点，可将 auto_nearest_yaw_target 设为 false。

isTrajectoryTrackingWithinThreshold() 可实时判断当前位置与曲线当前目标的误差是否进入 tracking_threshold； isTrajectoryFinished() 会在此基础上额外要求规划时间已经结束。

使用边界

• Master 假设 Loc 能持续提供稳定的世界坐标系状态，因此如果定位后端尚未建立可靠世界系，上层应先处理初始化问题。
• 该控制器不负责任务创建和定时调度，调用频率由接入工程保证。
• 该控制器不做全局路径规划，只做单段目标或轨迹衔接级别的局部规划。

与主 README 的边界

主 README 负责讲“为什么业务层应直接持有 Controller”。本文件负责讲 Master 具体有哪些控制模式、它们分别该在什么周期入口里运行，以及 target_in_world 这样的专项语义。