功率控制下的力控底盘

Introduction

在港科的开源中, 利用RLS算法自适应系统辨识的方法让人眼前一亮，然而在功率分配上仍然是老一套——先用PID算出各电机的原始输出电流，然后直接对电流乘上一个系数。只不过进行了简单的优化，定义了Error值的置信度，以提高分配的效率。
本文尝试将功率控制与力矩前馈相结合，设计在数学上更优的功率分配方法。

控制框架

• 能量控制
  这部分基本采用港科的思路。电能的来源除了裁判系统的Chassis输出还有超级电容储存的电能。在这一层，根据反馈的底盘缓冲能量，超级电容剩余能量，Chassis功率上限，可以通过简单的PID控制器(通常PD即可)给出底盘的期望功率，并交给功率规划层分配
  Input: 期望超电能量，反馈超电能量，底盘缓冲能量，裁判系统要求功率
  Output: 期望功率
• 功率规划
  在港科思路的基础上，直接对期望速度进行判断，对目标速度进行限制使得目标速度下正常运行是不会超功率的。这样避免了电机速度环一直在追不合理的目标速度值的情况。在确定下来合理的目标速度后，对PID想要的加速度进行功率限幅，使得底盘能够在不超功率的情况下以最快响应到目标速度。
  Input: 期望速度, 反馈速度, 系统参数, 期望功率, 反馈功率, 重力分量, PID输出力矩
  Output: 目标速度, 输出力矩
• 电机输出
  Input: 目标速度, 输出力矩, 反馈转速
  Output: PID输出力矩(回送功率规划层), 输出电流

理论推导

能量控制

赛场中有能量的存储机制:超级电容和裁判系统的缓冲能量。它们允许了我们更加灵活地安排底盘功率。在情况危急时，大量输出能量用于应对需要大功率的情况(e.g.交火时自旋，快速行进，上坡)
我们可以用能量环PID来管理底盘能量。当有一个期望的能量水平，PID输出的就是充放电的功率。由此得到期望功率

符号约定: 时, 超电放电, 底盘可用功率高于裁判系统上限; 反之, 让出部分功率给超电充电
缓冲能量低于阈值时应直接压低, 避免扣血

功率规划

电机功率模型

参考港科的电机功率模型

若对所有电机分开建模，则总功率为

其中

在原开源中，港科选择了对整个底盘的功率输出进行建模。
对于参数k1,k2, 其短时间内也不会突然有大幅度改变，所以其更新频率完全不用过高
这里每个电机都有独立的功率反馈(见下文辨识参数)，因此可以对每个电机分别建模，不会提高多少运算量的同时还提高系统辨识效果
若电调不会把制动能量回馈到母线，则求和前应对每个电机取，这一点可以用单电机的功率反馈验证

FF-RLS 系统辨识

最小二乘法

对于线性模型

其中

• 为系统输出
• 为系统输入(行向量)
• 为系统参数，也就是**权矢量**
• 为零均值噪声

记为输入的第k条数据，为输入数据的维度
将前k条数据堆叠起来，记

为辨识，要使残差平方和最小:

由于方程个数远多于未知数，一般没有精确解。对上式求导并令其为0，最小二乘法等价于要求解正规方程

可求得解为

递归最小二乘法

由于在实际中, k是随时间增大的。假如观测频率为100Hz，那么k就会在1s内增加100。这样的计算量非常大
若能够找到最小二乘法的递推形式解，那么最小二乘法就能够被用来处理流数据
详细推导过程->递推最小二乘法（Recursive Least Squares，RLS）
总之，结论是存在这样的递推形式解，且解为:

矩阵的逆求解困难，利用Woodbury恒等式

得

遗忘递归最小二乘

在上面的更新公式中加入遗忘因子:

取1就是普通RLS，越小则旧数据的权重越低

底盘静止或匀速时，几乎不变(激励不足)，遗忘因子会让不断放大(协方差发散)，一旦有扰动参数就会剧烈跳变。实际使用时:

• 激励不足(如、都很小)时暂停更新
• 给的对角元设上限
• 把辨识结果投影回物理可行域，例如保证

辨识参数

电机参数辨识

每个电机有独立的功率检测器，提供相对误差为1%以内的真实功率反馈
电机会反馈其当前的转速，力矩电流
对每个电机，把已知的机械功率移到等号左边，就得到了标准的线性模型:

传动效率无法在这里辨识：电功率只与电机侧的、有关，描述的是减速箱之后的损耗，根本不出现在里。
因此下文把轮端力矩定义为，轮端转速(为减速比，不含)，的影响交给底盘参数吸收(要求各轮组相近)。
这样定义下，功率模型改用轮端量时只需把换算为，换算为(下文仍记为)

底盘参数辨识

底盘有参数共11个参数要辨识(动力学方程见下节)
底盘合力由轮端力矩合成得到，而动力学方程对每个参数都是线性的，因此可以按轴拆成三个RLS:

在x、y两路中各估计一次，取加权平均即可

正好是加速度计在x轴测得的比力，可以直接使用，不必先解算姿态、也不必对速度差分(IMU不在底盘中心时需减去自旋带来的向心和切向加速度) 由陀螺仪角速度差分得到，噪声较大，需要先低通滤波

动力学解算

先在底盘坐标系下对整个底盘做受力分析:

其中为重力加速度在底盘坐标系下的分量取反(上坡时沿前进方向为正)
把与速度有关的项合并，记

其中，表示逐元素带符号平方，为逐元素符号函数，于是

牵连项来自在旋转的底盘坐标系中描述运动：小陀螺同时平移时，底盘系下的速度方向一直在转，需要额外的力来维持，不可忽略
阻尼必须用带符号的和：若写成或常数，反向运动时阻尼方向错误，静止时还会输出恒定力矩导致溜车
实际实现中可用代替，避免零速附近来回抖动

由力合成等式

求解力矩合成矩阵的伪逆矩阵，可得力矩分配:

矩阵为动力学分配矩阵

为动力轮半径，为动力轮触地点到底盘中心点的距离

这里的对应X型(45°)布置的全向轮；麦克纳姆轮的力臂不同，需要重新推导，后文流程不变

上坡的情况

针对上坡的情况，由于重心在底面的投影偏移出几何中心，由pitch和roll上合力偶为0，所以每个轮子的支持力不再均匀。
又因为最大静摩擦力正比于支持力，所以支持力更小的轮组更有可能出现打滑。
为防止此原因造成的打滑，可以给支持力更大的轮组分配更大的力矩。
设正定权重矩阵，此时动力学分配矩阵改为:

加权伪逆实际上是在求解

越大，越小。

平地上取有额外的好处：的零空间为，沿调整力矩既不改变底盘合力，也不改变机械功率()，只改变铜损。因此得到的就是功率最优的力矩分配
坡上可取(为第i个轮组的支持力)，同时兼顾铜损和防打滑

打滑的情况

若轮组打滑，则该轮组输出的力矩不再贡献入合力矩。例如当2号轮组打滑，则更新

此时动力学分配矩阵也要更新:

注：此处与文末「可改进点 - 打滑时的力矩分配」矛盾。打滑轮组仍有滑动摩擦力，不应把整列置零；且列置零后对应行为零，该轮组最终输出恒为0。改进做法是把该轮组的力矩上限降为滑动摩擦水平后继续参与分配

运动学解算

由几何关系，每个轮组的转速由底盘速度唯一确定(逆解):

矩阵为运动学雅可比矩阵。对比可以发现，这正是虚功原理的结论：底盘合力的功率等于各轮组功率之和，

反过来，由四个轮组的反馈转速求底盘速度(正解)，求的伪逆即可:

打滑的情况

打滑轮组的转速不再反映底盘运动。例如当2号轮组打滑，估计底盘速度时应删去2号轮组，用剩下3个轮组解出:

不能直接把对应的列置零：剩下三列的系数是按四轮平均设计的，置零后即使没有打滑，估计值也会偏离真实值

不需要更新：打滑轮组仍按给定目标转速，才能尽快恢复抓地

注：此处与文末「可改进点 - 打滑时的力矩分配」矛盾。按动力学部分的处理，打滑轮组的输出力矩恒为0，目标转速实际不起作用；且恢复抓地时轮速应匹配真实地面速度，而非

打滑检测：4个轮组只有3个自由度，存在1维冗余。不打滑时，于是；若显著大于0，说明有轮组打滑，再结合陀螺仪测得的判断具体是哪一个

注：此处与文末「可改进点 - 打滑检测与定位」矛盾。对称布局下，四个轮速加陀螺仪只能确定打滑的是对角的1、3号还是2、4号，需要再借助力矩方向区分；多个轮组同时滞空时此检测失效，见「可改进点 - 滞空与落地」

功率优化方向分析

力矩组成分析

由上可以得出

不难发现，电机的输出力矩有两个组成部分:

• 维持当前运动状态的必要力矩

  当底盘加速度为0，，此时有。
  也就是说，如果轮组提供不了，底盘是无法维持其运动状态的，会因为无法克服重力或者无法克服阻力而减速。

• 改变当前运动状态的动作力矩

  该项主要由底盘加速度决定，另外两项矩阵均为系统参量
  加速度越高，底盘响应目标速度的速度就越快。代价是消耗的功率也越大。由于在RM中，超功率的惩罚比响应速度偏慢的后果严重的多，因此优先保证不超功率，其次尽可能找到最大的加速度，以求更快响应

功率组成分析

前文已有电机功率模型

将代入，得到

可见，总功率同样可以分为两部分:

• 必要功率, 这部分由底盘实际运动速率矩阵决定,当加速度为0,，因此这就是底盘要维持当前运动状态的功率
• 动作功率, 这部分功率和加速度有关。因此可以用规划加速度

功率分配策略

当我们收到决策层发来的底盘运动期望速度 (e.g. 哨兵导航发给下位机的速度指令， 操作手给机器人发送的运动指令)，能够根据必要功率公式计算出先验必要功率

此时我们比较能量控制层提供的期望总功率 :

• : 期望速度不合理，照这么下去要超功率
  • 允许提高 (e.g.开启超级电容，哨兵切换移动姿态): 重新回到上一步判断
  • 不能够提高 : 限制期望速度大小，使得传给电机输出层的满足(方法见下文梯度下降)
• : 期望速度合理, 直接采用

据此，我们根据先验必要功率确定了底盘目标速度
而我们还有由轮组反馈转速推算出的后验必要功率，因为

• 底盘反馈速度已知(打滑时按上文删去打滑轮组)
• 后验必要力矩已知

结论是：在轮组反馈转速给定的情况下，先验动作功率只由目标加速度决定

所以我们能够借助来确定一个最优的目标加速度 (如果有解)

以上都基于模型。为了抵消模型误差，可以用功率反馈做闭环修正：记为与模型预测功率之差的低通滤波值，规划时用代替

梯度下降法限制

先验必要功率已知，可求其偏导数

梯度下降法获得合适的
由于功率限制不应该改变原有的平动速度方向，因此求得的偏导数也不能直接使用，要限制梯度下降的自由度

取而不是作为自由度，是为了让两个分量的单位一致(都是m/s)，否则归一化后的下降方向会取决于单位的选取

此时梯度下降有两个自由度: 平动速度和自旋速度。平动速度和自旋速度并没有必要保持成比例关系，利用梯度下降，可以优先削减更消耗功率的速度方向，提高功率分配效率
梯度下降的过程中，循环进行小步下降，直到或者下降步数超出预设步数(防卡死)

保障目标速度总还有小份能量用于加速

每一步下降需要确定下降方向和下降步长

下降过程中需要注意 不能变号，，不能超过物理极限
纯自旋()时无定义，此时只沿一个自由度下降
梯度下降会优先削减最耗功率的速度分量，通常是自旋。若希望保住小陀螺，可以给加权，降低方向的下降速度

取最优加速度

对的规划力图让底盘在短暂的未来不超功率，对的规划则让底盘不在当下超功率

有了规划过的目标速度，各轮组的速度环PID跟踪，计算出力矩输出
然而，如果PID给出的力矩输出过大，同样会导致超功率。先把PID输出换算成它想让底盘产生的加速度

前馈已经抵消了重力和阻尼，所以只负责改变运动状态。将它合成为底盘合力，再除以惯性矩阵即得加速度。在零空间里的分量(四个轮组相互对抗)对加速度没有贡献，只消耗功率，在这一步被自然丢弃

为了底盘的运动尽可能不违背PID输出，规划出的应该和同向(若，直接取)，记

则

代入,得到

由于正定且，有，是开口向上的二次函数。记动作功率的预算为

的取值方案

取值原则：在不超功率的前提下尽量贴近PID的要求；如果怎么取都会超功率，就取功率最小的点(优先保证不超功率)

不等式的解集为区间(时无解):

由此得到逐步的取值方案:

1. : 取
2. : PID输出不超功率，取
3. 且 且 : 取

   最常见的情况：加速时功率不够，取不超功率的最大加速度

4. 且 且 : 取

   只在已经超功率()、且沿减速能回收功率()时出现。此时要比PID要求的刹得更狠一点，才能把功率拉回预算内

5. 其余情况(或) : 不存在不超功率的解，取功率最小点
6. 摩擦限幅: 若不全为0，再取

   打滑时功率被白白浪费，加速度也不会兑现，所以摩擦限幅优先级最高。为轮组与地面的摩擦系数

   注：此处与文末「可改进点 - 逐轮摩擦约束」矛盾。整车限幅未计入必要力矩，也无法反映单个轮组先打滑；且在功率选值之后再截断，时可能重新超功率。应改为逐轮总力矩约束，并与功率约束先求交集

最终。对应的代码:

// A, b: 动作功率的二次函数系数; P_bud = P_exp - P_n_check; rho_pid = ||a_PID||
// rho_fric: 摩擦限幅, 纯自旋时传入 INFINITY
float select_rho(float A, float b, float P_bud, float rho_pid, float rho_fric)
{
    float rho;
    if (rho_pid <= 0.0f) {
        rho = 0.0f;
    } else if (A * rho_pid * rho_pid + b * rho_pid <= P_bud) {
        rho = rho_pid;                              // 不超功率, PID原样输出
    } else {
        rho = fmaxf(0.0f, -b / (2.0f * A));         // 默认: 无可行解, 取功率最小点
        float delta = b * b + 4.0f * A * P_bud;
        if (delta >= 0.0f) {
            float rho1 = (-b - sqrtf(delta)) / (2.0f * A);
            float rho2 = (-b + sqrtf(delta)) / (2.0f * A);
            if (rho2 >= 0.0f && rho_pid > rho2)
                rho = rho2;                         // 加速时功率不够
            else if (rho1 > 0.0f && rho_pid < rho1)
                rho = rho1;                         // 已超功率, 需要多刹一点
        }
    }
    return fminf(rho, rho_fric);                    // 摩擦限幅
}

注：代码最后一行fminf(rho, rho_fric)与文末「可改进点 - 逐轮摩擦约束」矛盾，摩擦约束应与功率约束先求交集，而不是最后截断

抗积分饱和：当(输出被限幅)时，PID的积分项应停止累加(条件积分)，否则限幅解除后积分项会导致超调

电机输出

由前，我们得到了最终的输出力矩

假设：

• 电机输出电流正比于电机输出力矩
• 电机内置的电流环响应速度非常快，在速度环的一个控制周期内就达到目标电流值
• 轮端力矩按换算，其中为减速比；传动效率已被底盘参数吸收(见底盘参数辨识)

那么电机的输出电流为

可改进点

以下内容尚未并入正文，正文中与之冲突的说法以本节为准

逐轮摩擦约束

正文第6步用限制加速度，存在三个问题:

• 只限制了动作力矩。维持速度、抵抗重力的必要力矩同样要靠摩擦力提供，上坡时留给加速的余量更少
• 打滑是单个轮组先打滑。坡上支持力最小的轮组最先达到极限，整车的反映不出来
• 全向轮只在驱动方向上靠摩擦传力，应逐轮限制；纯自旋同样需要摩擦力，不能不限幅

改为对每个轮组的总输出力矩限幅:

对是线性的，每个轮组给出一个区间(的轮组不受影响，跳过):

四个区间取交集，记为摩擦可行区间

与功率约束的组合

时不单调，正文先按功率选、再截断到摩擦上限，可能重新超功率。例如，功率可行区间为；时按功率选出，若摩擦上限为1，截断后，，又超功率了

因此应先求交集:

• 非空: 取中离最近的点
• 为空: 摩擦约束优先(打滑时功率白白浪费，加速度也不会兑现)，在中取功率最小的点，即把截断到该区间内
• 摩擦可行区间也为空: 连必要力矩都提供不了(如坡太陡)，进入降级处理，降低并报警

打滑力矩上限的获取

实际需要的不是本身，而是，直接测量更可靠:

1. 离线实测: 底盘顶住墙壁，逐渐增大某个电机的电流，记录轮组开始空转瞬间的电流，得。该值按的名义单位测得，已包含。必须在比赛场地材质上测量，并乘以安全系数(如0.8)
2. 支持力修正: 平地上各轮组支持力近似相等，直接使用；坡上按缩放，由pitch、roll和重心高度算出
3. 在线修正: 检测到某轮组打滑时，以当时的作为的观测值低通更新。打滑时只往小调，长时间不打滑再缓慢恢复

   滞空与落地期间必须冻结在线修正，否则空转会被误判为摩擦力趋近于0(见下文)

打滑检测与定位

正文中”再结合陀螺仪测得的判断具体是哪一个”的说法有误。定义两个残差:

若只有第j个轮组打滑，转速偏离无滑动值，则:

打滑轮组
1号
2号
3号
4号

只能区分出是对角的1、3号还是2、4号。原因是对角两轮的平动系数恰好相反：1号轮组打滑、底盘速度为，与3号轮组打滑、底盘速度为，四个轮速和陀螺仪读数完全相同

进一步区分需要用到力矩信息：打滑轮组的转速偏差与其力矩同向，(摩擦力饱和后，多余的力矩只能加速轮组自身的转动惯量；驱动打滑和制动打滑都成立)。由此:

1. 由、检测打滑，并确定是哪一对对角轮组
2. 对角两轮的平动力矩符号相反，有平动加速度时通常只有一个满足
3. 两个都满足(如纯自旋加速)时，取更大的一个
4. 仍无法确定时，不删除任何轮组，退回四轮估计，同时降低这两个轮组的力矩

   误判的代价很大：把1号误判成3号并删去，的估计误差正好是上述对角偏移

打滑时的力矩分配

正文把打滑轮组在中的列置零，有两个问题:

• 打滑不等于接触力为零：轮组仍与地面接触，仍有滑动摩擦力，只是力矩不能再按无滑动关系换算为底盘驱动力
• 列置零后对应行也为零，最终。正文所说”打滑轮组仍按给定目标转速以恢复抓地”在最终控制式中并没有实现

改进：不置零，而是把该轮组的力矩上限降为滑动摩擦水平，继续参与分配，并与上文逐轮摩擦约束统一处理

带上下限的分配不再有闭式解，可以迭代处理：超限的轮组固定在上限，其余轮组重新求加权伪逆分配剩余的合力

恢复抓地时，轮速应匹配真实的地面速度，而不是目标速度。加速过程中二者可能相差很大，追目标速度只会让轮组继续打滑

滞空与落地

底盘从台阶上飞跃落下时，多个轮组会短暂滞空，正文的打滑检测会失效:

• 多个轮组同时”打滑”，超出了单轮打滑的前提
• 四轮全部滞空时，空载的轮组被速度环迅速拉到，此时，也可能接近0，完全检测不到，而轮速推算的实际等于目标速度
• 落地时轮速与地面速度不匹配，产生冲击打滑和功率尖峰
• 滞空时不成立，底盘参数RLS与在线修正都会被污染
• 前馈继续输出，只会让轮组空转

检测

1. IMU失重检测(全部滞空)：自由落体时加速度计比力接近0，例如持续数毫秒；落地时出现明显尖峰(如)。从10cm台阶落下仅约0.14s，只能依靠IMU及时判断。IMU不在底盘中心时需先扣除自旋带来的向心加速度
2. 逐轮负载观测器(部分滞空、一般打滑):

其中为折算到轮端的等效转动惯量，将底盘架空后给阶跃力矩即可辨识。滞空轮组，打滑轮组被限制在滑动摩擦水平，正常轮组接近分配值

负载观测器逐轮独立判断，不依赖运动学冗余，也能解决上文对角轮组无法区分的问题；缺点是噪声大，滤波后延迟较大

处理

状态机：正常 → 滞空(失重，或≥2个轮组) → 落地(尖峰) → 正常(轮速重新与运动学一致)

模块	滞空期间	落地后短时间窗(约50~100ms)
速度估计	不用轮速，保持起跳前速度，用加速度计水平分量外推	、回落后再切回轮速估计
轮组目标转速	，与地面速度匹配，落地时无速度差	同左，再平滑过渡回
前馈、规划	关闭	降低力矩上限后逐步恢复
PID积分	冻结	冻结至时间窗结束
底盘参数RLS、在线修正	冻结	冻结，落地冲击数据同样不可用
电机功率RLS	照常	照常

部分滞空

例如前两个轮组先离开台阶，记:

• 相邻两轮着地(如1、2号)：平动系数、线性无关，两个轮速加陀螺仪恰好3个方程，速度仍可解
• 对角两轮着地(如1、3号)：平动系数、线性相关，沿方向的速度不可观，只能靠IMU外推
• 只剩两个轮组着地时秩为2，无法同时控制三个自由度。这一短暂阶段不做力分配，只保持轮速与地面速度匹配

---

参考:
【RM2024-功率控制算法开源（含舵轮，轮腿，全自动调参）】香港科技大学ENTERPRIZE战队, Wilson (HUANG, Lok Hin), Fallengold (GUO, Zilin)
递推最小二乘法（Recursive Least Squares，RLS）, ggggnui

AIGC:
Claude Opus 5.5, 控制框架改进与模型优化 (约10%)
Deepseek web chat, 偏导数辅助计算验证 (约5%)