全国大学生智能车竞赛第21届单车定向平衡控制算法详解
平衡控制算法:
单车的物理模型可以看成一个倒立摆，往哪边倾舵机朝哪边倒。舵机的转动幅度与速度成负相关，如果出现调了参数单车依旧不能跑的情况，大概率不是参数的问题，应该怀疑控制逻辑。
如果只考虑平衡控制，速度无需闭环也可。但强烈建议闭环以让单车增强不同环境下的适应性，否则会出现在某种场地能跑，但换个场地，由于摩擦系数或者电池电压等外部因素干扰，导致单车出现原来的平衡参数不适配甚至倒地的情况。
常见的平衡控制方案分两种，第一种就是常见的三环的串级pid，第二种是PD控制器 + 几何模型映射的平衡方案
一.PD控制器 + 几何模型映射 的平衡方案
核心假设 ：
1. 单车视为刚体，后轮驱动，前轮转向
2. 忽略轮胎变形和地面摩擦力
3. 小角度近似：sin(θ) ≈ θ, tan(θ) ≈ θ
物理意义：利用静力学平衡，通过前后轮载荷比例估算重心位置。
单位意义:
轴距 L 含义:前后轮中心距
重心距后轮 Lm 含义:重心到后轮轴心距离
总质量 M 含义：整车质量
前轮载荷 Ff 含义:前轮静态载荷
balance_data.com_from_rear_m = balance_data.wheelbase_m * (front_load_g / total_mass_g);
平衡控制原理
核心思想 ：通过调整前轮舵机角，产生向心力使车身保持平衡
┌─────────────────────────────────────────────────────────────────────┐
│ 平衡控制架构 │
├─────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────┐ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ IMU传感器│ ──► │ 姿态解算 │ ──► │ PD控制 │ ──► │ 几何映射 │ │
│ │ (陀螺仪) │ │ (EKF) │ │ │ │(舵机角度) │ │
│ └──────────┘ └──────────┘ └──────────┘ └──────────┘ │
│ │ │ │ │ │
│ │ roll(deg) │ d_angle │ ax_cmd │ │
│ │ roll_rate │ roll_rate │ │ │
│ ▼ ▼ ▼ ▼ │
│ ┌──────────────────────────────────────────────────────────────┐ │
│ │ 物理模型 │ │
│ │ 重心位置(Lm) → 轴距(L) → 速度(V) → 舵机角(δ) │ │
│ └──────────────────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────┘物理方程 ：
1. 横向加速度需求 ：
a_x = KP × θ_error - KD × θ_dot- θ_error：目标倾角与实际倾角的偏差
- θ_dot：倾角角速度（阻尼项）
几何映射关系 ：
δ = arctan( L/Lm × tan( arcsin( -a_x × Lm / V² ) ) )其中：
- δ：舵机角度（前轮转向角）
- V：前进速度
- L：轴距
- Lm：重心距后轮
姿态解算:
从EKF姿态解算模块获取当前roll角和roll角速度后计算相对倾倒角
balance_data.angle_car_deg = balance_data.roll - balance_data.angle_core_deg;
balance_data.d_angle_deg = balance_data.balance_angle_deg - balance_data.angle_car_deg;物理意义 ：
- angle_core_deg ：重心偏置角（用于抵消机械安装误差,单车有时持续偏向一边的原因之一，需要根据实际情况矫正）
- angle_car_deg ：相对重心的实际倾倒角
- d_angle_deg ：角度误差（目标倾角 - 实际倾角）
PD控制器计算
balance_data.ax_cmd = balance_data.kp * balance_data.d_angle_deg - balance_data.kd * balance_data.roll_rate;核心控制律 ：
- P项 ： KP × d_angle_deg —— 比例控制，根据角度偏差产生恢复力
- D项 ： -KD × roll_rate —— 微分控制，阻尼项，抑制角速度，防止过冲
几何映射
第一步 ：计算重心处的等效转向角（弧度）
temp = arcsin( -a_x × Lm / V² )负号表示：当a_x为正时（需要向右的横向加速度），前轮应向左转。
第二步 ：几何变换到前轮实际转向角
servo_deg = arctan( L/Lm × tan(temp) ) × RAD_TO_DEG限幅保护
建议给舵机设置一个最大限幅对舵机进行保护，一般在30度左右，根据实际进行设定
舵机随速度自适应
temp = -balance_data.ax_cmd / (v * v) * lm;物理意义 ：所需舵机角度与速度平方成反比。
- 高速时 ：所需舵角小，系统更稳定
- 低速时 ：所需舵角大，系统更灵活但易抖
控制流程图
┌─────────────────────────────────────────────────────────────────┐
│ balance_tick() 执行流程 │
├─────────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────────┐ │
│ │ 获取姿态数据 │ │
│ │ roll, roll_rate │ │
│ └────────┬────────┘ │
│ ▼ │
│ ┌─────────────────┐ │
│ │ 检查输出使能 │──── 未使能 ────► 舵机回中 ──► return │
│ └────────┬────────┘ │
│ ▼ 使能 │
│ ┌─────────────────┐ │
│ │ 计算相对倾倒角 │ │
│ │ angle_car = roll│ │
│ │ - core│ │
│ └────────┬────────┘ │
│ ▼ │
│ ┌─────────────────┐ │
│ │ PD控制器 │ │
│ │ ax_cmd = KP×dθ │ │
│ │ - KD×dθ/dt│ │
│ └────────┬────────┘ │
│ ▼ │
│ ┌─────────────────┐ │
│ │ 几何映射 │ │
│ │ temp = -ax×Lm/V²│ │
│ │ servo = arctan( │ │
│ │ L/Lm×tan( │ │
│ │ arcsin(temp)│ │
│ │ )) │ │
│ └────────┬────────┘ │
│ ▼ │
│ ┌─────────────────┐ │
│ │ 限幅保护 │ │
│ │ [-40°, 40°] │ │
│ └────────┬────────┘ │
│ ▼ │
│ ┌─────────────────┐ │
│ │ 输出到舵机 │ │
│ │ duoji_set_angle │ │
│ └─────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────┘平衡层总结
- 数学模型 ：基于刚体动力学和几何关系，通过重心位置、轴距、速度等参数建立精确映射
- 控制算法 ：经典PD控制器，P项提供恢复力，D项提供阻尼
- 几何映射 ：使用反正切-反正弦复合函数实现从横向加速度到舵机角度的精确转换
- 速度自适应 ：通过1/V²项实现不同速度下的自动调整
- 保护机制 ：多重限幅和除零保护确保系统安全
此外说一些小思路
1，在单车到达某个设定的roll再启动整个系统。好处是可以是让单车上电前倒地，扶起来后直接开跑，对发车稳定性与陀螺仪的稳定性有可观的帮助
2，通过实时改变重心偏执角可以将车固定在某个角度前进（车身正立不倾斜）
电机速度斜坡控制
核心代码
static uint8 enable_last = 0u;
static float speed_ref_ramp_mps_last = 0.0f;
if ((enable_last == 0u) && (motor_speed_enable != 0u))
{
speed_ref_ramp_mps_last = speed_ref_cmd_mps; // 启动时直接设为目标速度
}
enable_last = motor_speed_enable;
speed_ref_ramp_mps = speed_ref_ramp_mps_last;
if (speed_ref_ramp_mps < (speed_ref_cmd_mps - motor_speed_cfg.speed_ref_ramp_up_mps_per_tick))
{
speed_ref_ramp_mps = speed_ref_ramp_mps + motor_speed_cfg.speed_ref_ramp_up_mps_per_tick;
}
else if (speed_ref_ramp_mps > (speed_ref_cmd_mps + motor_speed_cfg.speed_ref_ramp_dn_mps_per_tick))
{
speed_ref_ramp_mps = speed_ref_ramp_mps - motor_speed_cfg.speed_ref_ramp_dn_mps_per_tick;
}
else
{
speed_ref_ramp_mps = speed_ref_cmd_mps;
}
speed_ref_ramp_mps_last = speed_ref_ramp_mps;┌──────────────────────────────────────────────────────┐
│ 速度斜坡控制 │
├──────────────────────────────────────────────────────┤
│ │
│ 1. 启动检测：enable_last=0 → enable=1 │
│ 直接将斜坡值设为目标速度（快速启动） │
│ │
│ 2. 上升斜坡： │
│ if (ramp < target - step) │
│ ramp += step │
│ │
│ 3. 下降斜坡： │
│ if (ramp > target + step) │
│ ramp -= step │
│ │
│ 4. 到达目标： │
│ else │
│ ramp = target │
│ │
└──────────────────────────────────────────────────────┘速度自适应整体架构
┌─────────────────────────────────────────────────────────────────────┐
│ 速度自适应架构 │
├─────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ 速度设定 │ ───► │ 速度斜坡 │ ───► │ 速度闭环PID │ │
│ │ (0.6m/s) │ │ (平滑过渡) │ │ (占空比输出) │ │
│ └──────────────┘ └──────────────┘ └──────┬───────┘ │
│ │ │
│ ▼ │
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ 轮速反馈 │ ◄─── │ 电机驱动 │ ◄─── │ 占空比下发 │ │
│ │ (RPM) │ │ (BLDC) │ │ │ │
│ └──────┬──────┘ └──────────────┘ └──────────────┘ │
│ │ │
│ ▼ │
│ ┌──────────────┐ │
│ │ 速度计算 │ │
│ │ (m/s) │ ─────────────────────────────────────────────┐ │
│ └──────┬──────┘ │ │
│ │ │ │
│ ▼ │ │
│ ┌──────────────┐ │ │
│ │ 平衡环速度 │ ◄───────────────────────────────────────────┘ │
│ │ 自适应 │ │
│ │ (1/V² 映射) │ │
│ └──────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────┘电机层总结:
1. 平衡环层面 ：通过 1/V² 因子实现速度自适应，高速时更稳定，低速时更灵敏
2. 电机控制层面 ：通过斜坡控制实现平滑加速/减速，避免速度突变
（其实速度闭环没什么难度，通俗一点讲就是通过读取当前速度和目标速度的差距来对当前进行速度加减，最终使速度稳定在某个区间）
GPS部分
常见的导航分两种，一种以GPS追点为主，一种以陀螺仪建立坐标系为主。
在这里先介绍流程图
┌─────────────────────────────────────────────────────────────────┐
│ GPS导航系统流程 │
├─────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ 航点采集 │───>│ Flash存储 │───>│ 航点读取 │ │
│ └──────────────┘ └──────────────┘ └──────────────┘ │
│ │ │
│ v │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ nav_gps_start() │ │
│ │ ┌─────────┐ ┌─────────┐ ┌─────────┐ │
│ │ │ 检查GPS │───>│构建路径 │───>│初始化KF │ │ │
│ │ └─────────┘ └─────────┘ └─────────┘ │ │
│ └─────────────────────────────────────────────────────────┘ │
│ │ │
│ v │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ nav_gps_tick() │ │
│ │ │ │
│ │ GPS数据 ──> 坐标转换 ──> KF融合 ──> 路段选择 │ │
│ │ │ │ │
│ │ v │ │
│ │ ┌───────────────────────────┐ │ │
│ │ │ Stanley控制器 │ │ │
│ │ │ - 航向误差 │ │ │
│ │ │ - 横向偏差 │ │ │
│ │ │ - 积分补偿 │ │ │
│ │ │ - 阻尼项 │ │ │
│ │ └───────────────────────────┘ │ │
│ │ │ │ │
│ │ v │ │
│ │ ┌───────────────────────────┐ │ │
│ │ │ 线路锁定 │ │ │
│ │ └───────────────────────────┘ │ │
│ │ │ │ │
│ │ v │ │
│ │ ┌───────────────────────────┐ │ │
│ │ │ 输出限幅 + 平滑处理 │ │ │
│ │ └───────────────────────────┘ │ │
│ │ │ │ │
│ │ v │ │
│ │ balance_set_nav_roll() │ │
│ └─────────────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────┘打点使用PSO精准打点算法
算法原理
PSO通过模拟鸟群觅食行为，在搜索空间中找到使目标函数最小的位置（这样打点比传统打点更精确，也更有鲁棒性）：
// 代价函数：Huber损失（抗噪声）
static float gps_pso_cost(const float *sample_x_m, const float *sample_y_m,
uint8 sample_count, float center_x_m, float center_y_m)
{
float total_cost = 0.0f;
for (i = 0u; i < sample_count; i++)
{
float dx = sample_x_m[i] - center_x_m;
float dy = sample_y_m[i] - center_y_m;
float distance_m = sqrtf(dx * dx + dy * dy);
// Huber损失：对野值更鲁棒
if (distance_m <= GPS_PSO_HUBER_M) // 0.45m
{
total_cost += distance_m * distance_m; // 平方损失
}
else
{
// 线性损失（避免野值影响过大）
total_cost += GPS_PSO_HUBER_M * (2.0f * distance_m - GPS_PSO_HUBER_M);
}
}
return total_cost;
}PSO迭代更新公式
// 速度更新
velocity_x[i] = inertia * velocity_x[i]
+ c1 * r1 * (best_particle_x[i] - particle_x[i]) // 个体最优
+ c2 * r2 * (global_best_x - particle_x[i]); // 全局最优
// 位置更新
particle_x[i] = particle_x[i] + velocity_x[i];
// 参数配置
inertia = 0.72 - 0.02 * iter; // 惯性权重（递减）
c1 = 1.45; // 个体学习因子
c2 = 1.55; // 社会学习因子关于点的利用有两种方式，第一种是线性存点后期拟合，第二种就是常规的打点。一般将数据存在Flash中。
Flash存储结构
// 存储格式
flash_data[0] = FLASH_DATA_MAGIC (0x47505331) // 魔数"GPS1"
flash_data[1] = point_num // 点数量
flash_data[2..3] = 第一个点的 lat, lon
flash_data[4..5] = 第二个点的 lat, lon
...
// 坐标编码（放大1e7倍存储）
latitude_scaled = (uint32)(capture_latitude * 10000000.0); // 精度7位小数
longitude_scaled = (uint32)(capture_longitude * 10000000.0);值得一提的是依靠陀螺仪为主的方案适应性会更强，在此基础上融合GPS收益会更大。可以把陀螺仪当成一张甩出去的网，GPS当作固定这张网的钉子。但也有缺点，如果GPS点一开始就飘，不仅不会起到纠正作用，反而会适得其反。常见的应对策略除了传统的过滤异常数据，分配权重，设置最大纠偏限幅，还有就是通过按键更改点，这样看他往哪边偏就手动纠正即可。
最后，我想对未来刚刚接触这个新手小伙伴说，选对方向远比盲目努力更重要，慢慢走，把身体养好，多学多看多问，一条路走不通往往不是因为你不够努力，而是需要新的思考方向。
这里有一份参考的代码入口，它只能帮你更好起步，并且提供一些奇特的思路，但无法帮你完成比赛：【免费】26届单车定向参考代码资源-CSDN下载
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
10
0- 0
已为社区贡献1条内容
所有评论(0)