01.4｜从六轴需求到八台推进器：几何、伪逆与故障约束

控制器要求的是合力和合力矩，执行器接收的却是各台推进器的命令。本课将这两种空间连接起来。输入是机体系六维广义力需求与推进器状态；输出是满足推力和变化率约束的各台推力，以及实际可实现广义力。先从单台推进器的方向和位置写一列，再由最小范数问题推导伪逆，最后在同一目标函数中加入饱和与单台故障。

开始前：先分配两台，再看八台

先读 00.1 杆臂叉力与 00.3 秩、逆和零空间。本课的未知数是每台推进器的推力，不是机器人的速度。第一遍先做两台同向推进器的合力分配，确认“满足需求”与“在多个解中选一个”是两个不同问题，再读六轴约束。

符号	含义与维度	单位／坐标约定
$d_i^b,r_i^b,f_i$	第 $i$ 台推力单位方向、力臂、带符号推力	无量纲、m、N；方向与杆臂 $b$
$B_\tau,f,\tau_d$	$6\times8$ 几何矩阵、 $8\times1$ 推力、 $6\times1$ 期望广义力	上3行方向无量纲，下3行力臂m
$W_f,Q_\tau,R_f,S_f$	努力权重、跟踪权重、推力正则、变化平滑权重	含逆尺度单位，使代价无量纲；在归一化变量下可取无量纲权重
$f_{min},f_{max},\dot f_{max},\alpha_i$	推力上下限、推力变化率限、效能比例	N、N、 $\mathrm{N/s}$ 、无量纲
$e_\tau=\tau_d-B_\tau f$	分配残差	前三项N，后三项N·m； $b$

几何矩阵每一列都可以用尺子和方向箭头解释

假设固定方向推进器、推力作用线已知，忽略螺旋桨反扭矩；若反扭矩重要，应把它加到力矩列。第 $i$ 台产生方向乘推力的力，并对参考点产生杆臂叉该力的力矩。提取共同的标量推力，便得到六维列向量。八台相加不是新的物理定律，只是线性叠加。杆臂参考点应与动力学惯性和控制需求的力矩参考点一致。

\begin{aligned}F_i^b&=d_i^bf_i,\quad N_i^b=r_i^b\times d_i^bf_i,\\b_i&=\begin{bmatrix}d_i^b\\{[r_i^b]_\times d_i^b}\end{bmatrix},\quad B_\tau=[b_1\ b_2\ \cdots\ b_8],\\\tau&=\sum_{i=1}^8b_if_i=B_\tau f.\end{aligned}

(1.23)

式 $(1.23)$ 用一个推进器同时解释力与力矩

$d_i$ 必须是单位向量， $f_i$ 才能表示实际牛顿推力；若方向列没有归一化，则比例会混进推力标定。 $r_i$ 是从控制力矩参考点到推进器作用点的位置。叉乘只取垂直于推力作用线的力臂，因此沿推力方向移动推进器不会改变该列力矩。

例如 $r=(0.2,0.25,0)^T$ 与 $r=(0,0.25,0)^T$ 对前向推力给出的偏航力矩相同，都是 $-0.25f$ 。代码组装时上三行直接复制方向，下三行调用同一个叉乘函数；不要独立手填六个符号。艇体参考点改变后，所有力矩列都需要一致地换参考点，而不是只改惯性矩阵。

一台位于 $(0,0.25,0)$ m且向前推的推进器，其列为 $(1,0,0,0,0,-0.25)^T$ 。因此施加4 N前推会产生−1 N·m偏航力矩。不要用“左电机”“右电机”的名称猜偏航符号，直接算叉乘。若推进器与机体轴成45度，应填单位方向的正余弦，而不是同时把两个方向系数都填一再忘记推力尺度。

先做两台的题：为什么约束还不够？

两台同向推进器满足 $f_1+f_2=10\ \mathrm N$ ，有无限多组解。若还想让总平方推力最小，则把 $f_2=10-f_1$ 代入目标：

J(f_1)=\tfrac12\big[f_1^2+(10-f_1)^2\big], \qquad J'(f_1)=2f_1-10.

这里数字按牛顿代入。令导数为零，得 $f_1=f_2=5\ \mathrm N$ 。这不是新的受力定律，而是在满足受力定律的许多解中，用一个偏好选择答案。若第一台上限为三牛顿，最优可行解变成 $(3,7)\ \mathrm N$ ；把原答案 $(5,5)$ 各自裁剪只会得到 $(3,5)$ ，合力少两牛顿。这就是本课需要约束优化而非简单限幅的最小例子。

六轴八台对应六个方程、八个未知数。只有几何矩阵行秩为六时，任意六维需求才在无约束模型中可实现；八台并不保证六个独立方向。秩为六仍有两维零空间自由度，随后用权重和约束选择具体推力。

无约束伪逆是一个优化问题的答案

当 $B$ 满行秩且推进器多于需求维度时，同一个广义力有多个解。最小二乘与最小范数不是同一句话：这里先要求精确满足需求，再在所有可行解中最小化推力平方。写拉格朗日函数，对推力求导得到推力与乘子的关系；代回等式约束求乘子，便得到伪逆。

\begin{aligned}\min_f\ &\tfrac12f^Tf\quad\text{s.t. }B_\tau f=\tau_d,\\\mathcal L&=\tfrac12f^Tf+\lambda^T(\tau_d-B_\tau f),\\\nabla_f\mathcal L&=f-B_\tau^T\lambda=0,\\B_\tau B_\tau^T\lambda&=\tau_d,\qquad f^*=B_\tau^T(B_\tau B_\tau^T)^{-1}\tau_d=B_\tau^\dagger\tau_d.\end{aligned}

(1.24)

式 $(1.24)$ 的乘子不是新的推进器状态

$\lambda$ 是六维拉格朗日乘子，仅用于把等式约束带进优化。对 $f$ 求导时， $\lambda^T Bf$ 的导数是 $B^T\lambda$ ，这解释了为什么转置出现在解的最前面。满行秩使六阶矩阵 $BB^T$ 可逆；若丢失某个受控方向，就应使用SVD或降维求最小二乘，而不是继续套这个逆。

设三台完全同向、同位置的理想推进器只负责一个合力， $B=(1,1,1)$ ，要求六牛顿，则 $BB^T=3$ ，每台分到两牛顿。所有总和六牛顿的组合都可行，但平方和最小的一定均分。这个一维例子就是六乘八伪逆的同一个优化逻辑。

零空间中的推力组合不改变合力，可以用于减小某台负载或满足附加目标。若不同推进器的代价不同，将推力平方替换为正定加权平方；对 $f$ 求导变成 $W_ff$ ，随后步骤完全一样。代价较高的推进器倾向于少出力。平方推力是方便的代理目标，不等于精确电功率模型。

\begin{aligned}f&=B_\tau^\dagger\tau_d+(I-B_\tau^\dagger B_\tau)z,\quad z\in\mathbb R^8,\\f_W^*&=W_f^{-1}B_\tau^T(B_\tau W_f^{-1}B_\tau^T)^{-1}\tau_d.\end{aligned}

(1.25)

式 $(1.25)$ 解释冗余，而不把“多两台”当作自动容错

$z$ 是任意八维向量，投影算子 $I-B^\dagger B$ 只保留不会改变任务输出的分量。用 $B$ 左乘这一项得到零，因而它叫零空间运动。对上一例三台同向推进器，增加 $(1,-1,0)^T$ 不改变总推力，却改变单台负载。带限幅时可以利用这种自由度，但不代表一定找得到满足所有边界的组合。

加权解来自 $W_ff-B^T\lambda=0$ 。权重正定才可求逆，而且不同推力变量应在同一标定尺度下比较。两台同向推进器要合计六牛顿、代价权重分别为一和二，最优推力是四牛顿和两牛顿；代价高的一台较少出力。

手算二维教学分配器：两台推进器沿前轴，分别位于左右0.25 m，需求只保留前向力和偏航力矩。要求8 N前推且零偏航时，两台各4 N。若要求8 N且1 N·m正偏航，则联立和为8、差乘0.25为1，得到左台6 N、右台2 N。这个两推进器例子虽没有冗余，却能清楚看到几何、符号和饱和的冲突。

\begin{bmatrix}F_x\\N_z\end{bmatrix}=\underbrace{\begin{bmatrix}1&1\\0.25&-0.25\end{bmatrix}}_{B_2}\begin{bmatrix}f_L\\f_R\end{bmatrix},\qquad\begin{bmatrix}8\\1\end{bmatrix}=B_2\begin{bmatrix}6\\2\end{bmatrix}.

(1.26)

式 $(1.26)$ 的两行具有不同量纲

第一行是牛顿，第二行是牛顿米， $0.25$ 的单位是米。把第二行除以力臂，得到两台推力之差；再与第一行相加减，得到 $f_L=(F_x+N_z/0.25)/2$ 、 $f_R=(F_x-N_z/0.25)/2$ 。这也说明很短的力臂需要很大的差动推力才能制造同样力矩。

零偏航时两台相等，正偏航时左台较大，负偏航时右台较大。用这三个工况做符号测试，再进入QP求解；否则优化器可能精确地解决了一个左右符号写反的问题。

为什么求完逆再裁剪通常不对

假设每台最多5 N，上例左台6 N不可行。直接裁剪为 $(5,2)$ 会同时改变合力和力矩，得到7 N与0.75 N·m。若更重视前向力，可以选择 $(5,3)$ 准确达到8 N但只有0.5 N·m；若更重视偏航，可以选择 $(5,1)$ 准确达到1 N·m但前向只有6 N。饱和时没有一个无条件“正确”的裁剪方法，必须明确任务优先级。由于牛顿和牛顿米不能直接相加，先用各轴允许误差或标称尺度归一化，再指定权重。

\begin{aligned}S_\tau&=\operatorname{diag}(1/F_0,1/F_0,1/F_0,1/N_0,1/N_0,1/N_0),\\Q_\tau&=S_\tau^TW_\tau S_\tau,\quad W_\tau\succeq0,\\\min_f\ J&=\tfrac12(B_\tau f-\tau_d)^TQ_\tau(B_\tau f-\tau_d)+\tfrac12 f^TR_ff\\&\quad+\tfrac12(f-f_{prev})^TS_f(f-f_{prev}),\\\text{s.t. }l_i&\leq f_i\leq h_i,\\l_i&=\max(f_{min,i},f_{prev,i}-\dot f_{max,i}\Delta t),\\h_i&=\min(f_{max,i},f_{prev,i}+\dot f_{max,i}\Delta t).\end{aligned}

(1.27)

式 $(1.27)$ 每个约束负责一个不同现实限制

$F_0$ 和 $N_0$ 分别是允许或标称力、力矩尺度； $W_\tau$ 是无量纲任务偏好。这样 $Q_\tau=S_\tau^TW_\tau S_\tau$ 包含逆尺度单位，力和力矩残差才可相加。 $R_f$ 抑制总推力努力， $S_f$ 抑制与上一拍的差，二者也含相应逆推力尺度。

物理极限与变化率极限取交集才是当前一拍的边界。若上一拍推力四牛顿，最大变化率十牛顿每秒、采样间隔零点一秒，变化率允许范围是三到五牛顿；即使电机物理上允许负五到正五，也不能一拍跳到负五。故障强制归零属于另一种状态切换，不能同时要求失效推进器保持上一拍推力。

$F_0$ 与 $N_0$ 分别是力和力矩尺度， $R$ 与 $S$ 也要包含推力尺度才能使目标无量纲。第一项允许无法实现时有残差，第二项防止不必要的大推力，第三项偏好平滑；硬变化率边界则真正禁止过快变化。展开平方项，将与 $f$ 无关的常数丢掉，得到标准凸二次规划。正定 $R$ 或充分的信息使 $H$ 正定，解唯一；即使需求不可达，只要推力边界一致，软跟踪问题仍有可行解。

\begin{aligned}J(f)&=\tfrac12 f^THf+c^Tf+\mathrm{const},\\H&=B_\tau^TQ_\tau B_\tau+R_f+S_f,\\c&=-B_\tau^TQ_\tau\tau_d-S_ff_{prev},\\\nabla J&=Hf+c.\end{aligned}

(1.28)

式 $(1.28)$ 展开平方时为什么没有漏掉一个二

对称矩阵 $Q$ 下，二次跟踪项展开为 $\tfrac12f^TB^TQBf-f^TB^TQ\tau_d+\tfrac12\tau_d^TQ\tau_d$ 。两个交叉项相同，而外面的二分之一恰好抵消，所以线性项只有一个负的 $B^TQ\tau_d$ 。平滑项同理给出 $S_f$ 与 $-S_ff_{prev}$ 。与决策变量无关的常数可以删掉，因为它不改变最优点。

在边界内，最优条件是 $Hf+c=0$ ；在上界处，沿可行方向再降低目标已经不可能，普通梯度未必为零。投影梯度迭代把梯度步裁回可行盒子，因此用相邻投影点之差判收敛，而不能要求所有梯度元素都为零。

单台故障：改变可达集合，不只是少一份总推力

确认某台推进器完全失效后，将该台变量上下限都置零，或删除该列并重求分配；部分效能下降可用0到1之间的比例乘该列，但要说明变量是标称推力还是实际推力。删列后先检查秩，再检查带限幅的可达性：满行秩只表示无限推力假设下能产生任意方向，不代表当前边界下能产生任意大小。

\begin{aligned}B_{eff}&=B_\tau\operatorname{diag}(\alpha_1,\ldots,\alpha_8),\quad0\leq\alpha_i\leq1,\\\mathcal A&=\{B_{eff}f\mid l\leq f\leq h\},\qquad e_\tau=\tau_d-B_{eff}f^*.\end{aligned}

(1.29)

式 $(1.29)$ 中的效能比例放在哪一层

若 $f_i$ 表示按健康推力曲线预测的标称推力， $\alpha_i f_i$ 才是故障后的实际推力，因此比例乘在几何矩阵列上。若变量已经是测得的实际推力，就不应再次乘效能比例，否则同一个损失被计算两遍。完全失效时将该台命令约束为零，避免优化器在零效能列上输出无意义的大命令。

可达集合 $\mathcal A$ 是推力盒子经过线性映射后的凸集合。删去一列可能降低维数，也可能只缩小集合而保持维数；这两种故障不能用一个“剩余总推力百分比”概括。将实际输出 $B_{eff}f^*$ 与需求逐轴比较，才知道此刻牺牲的是前进、深度还是姿态。

二维例中右台故障后，只剩 $(1,0.25)$ 这个方向：每1 N前推必然伴随0.25 N·m正偏航，不能再独立控制两轴。取前向尺度1 N、偏航尺度0.25 N·m，目标 $(8,0)$ 归一化为 $(8,0)$ ，可用列变成 $(1,1)$ 。最小化 $\tfrac12[(f-8)^2+f^2]$ ，求导得 $2f-8=0$ ，因此最优推力为 $4\ \mathrm N$ ；实现 $(4,1)$ 而不是 $(8,0)$ 。这不是求解器失败，而是可控方向已改变。上层应据残差降低任务要求；积分器也应得到实际实现量，否则继续积累无法兑现的需求。

离线手写投影梯度：看见约束如何工作

箱约束二次规划可以用投影梯度做教学实现：先沿负梯度走，再逐元素投影到上下界。取步长为 $H$ 最大特征值的倒数，凸问题的目标不会因步长过大而发散。用投影梯度映射的范数判收敛，而非仅看梯度是否为零：边界最优点的普通梯度本来就可能非零。生产分配器还应评估求解时间、容错与优先级，但这些不改变这里的优化结构。

参考实现：C++17，先独立手写再展开

// 独立教学实现：二维箱约束QP，不是ArduSub源代码，C++17
#include <array>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <iostream>
using V=std::array<double,2>;
V allocate(double Fx,double Nz,double limit,bool rightFailed){
    // B=[[1,1],[.25,-.25]], Q=diag(1,16)
    // 因而H=B^T Q B=2I，c=(-Fx-4Nz,-Fx+4Nz)
    V f{0,0},c{-Fx-4*Nz,-Fx+4*Nz};
    const double step=.5; // 1/lambda_max(H)
    for(int k=0;k<100;++k){
        V next{};
        for(int i=0;i<2;++i){
            double lo=-limit,hi=limit;
            if(i==1 && rightFailed)lo=hi=0;
            next[i]=std::clamp(f[i]-step*(2*f[i]+c[i]),lo,hi);
        }
        double change=std::hypot(next[0]-f[0],next[1]-f[1]);
        f=next;if(change<1e-12)break;
    }
    return f;
}
int main(){
    V healthy=allocate(8,1,5,false),fault=allocate(8,0,5,true);
    assert(std::abs(healthy[0]-5)<1e-12 && std::abs(healthy[1]-2)<1e-12);
    assert(std::abs(fault[0]-4)<1e-12 && fault[1]==0);
    double Fx=fault[0]+fault[1],Nz=.25*(fault[0]-fault[1]);
    assert(Fx==4 && Nz==1);
    // 检查健康分配在所有细网格可行点中没有更差的目标
    auto cost=[](double l,double rr){return .5*(std::pow(l+rr-8,2)+16*std::pow(.25*(l-rr)-1,2));};
    for(int i=-100;i<=100;++i)for(int j=-100;j<=100;++j)
        assert(cost(5,2)<=cost(i*.05,j*.05)+1e-12);
    std::cout<<healthy[0]<<" "<<healthy[1]<<" | "<<Fx<<" "<<Nz<<"\n";
}

完整算法顺序：用安装几何建立 $B$ ；将控制需求统一为 $b$ 系牛顿和牛顿米；读取故障效能与实际上一拍推力；更新推力及变化率边界；求解QP；用推力曲线逆映射到命令；重新估计实际广义力并输出残差。若推力曲线前进与倒车不对称，应分别标定，不能把带符号牛顿直接当PWM或归一化油门。

项目阅读映射：libraries/AP_Motors/AP_Motors6DOF.cpp 查看各推进器水平、垂直、偏航、横滚与俯仰系数，沿输出路径追踪归一化、反向、限幅、PWM和非零死区；项目报告中的前四台主要负责水平／偏航，后四台主要负责垂直／横滚／俯仰。这里的几何力分配是理解和扩展这些系数的方法，不应把归一化混控表直接宣称为牛顿制 $B$ 。上游 accel_target_to_xy_thrust 的 $G$ 逆映射与下游多轴竞争饱和是两层不同问题。

练习一：用纸笔算 $(6,2)$ 裁剪后的合力力矩，再比较 $(5,3)$ 与 $(5,1)$ 的任务取舍。练习二：保持相同两台几何，把偏航尺度从0.25 N·m改成0.5 N·m，右台故障且目标 $(8,0)$ 时，无约束最优为6.4 N，5 N上限使最优变5 N。练习三：自行构造八台方向和杆臂，打印每列、奇异值及删去各列后的秩；答案要点是以数值秩和限幅残差区分“失去方向”和“仍有方向但能力降低”。最后将实际残差传给一个离线积分器，比较有无反算抗饱和的恢复速度。

TB01-S5 · Thor I. Fossen：Marine Systems Simulator

精读路线：先确认GNC与LIBRARY目录分工，再用本课自行构造的几何模型比较控制需求和执行器输入；伪逆与QP在本文从约束优化完整推导。

原始C++逐行阅读：官方混控表与牛顿制分配不是同一个接口

以下原文来自官方 ArduPilot ArduSub-4.1.1 的 AP_Motors6DOF::output_armed_stabilizing，固定提交 66c674c1927d5d31401e6b4bc48b8eb3ee7e066e，第345—348行。版权归 ArduPilot 项目贡献者；代码依 GNU GPL v3-or-later 提供，许可证。

                linear_out[i] = throttle_thrust * _throttle_factor[i] +
                                forward_thrust * _forward_factor[i] +
                                lateral_thrust * _lateral_factor[i];

先看赋值左边：linear_out[i] 是第 $i$ 台电机的平移混控分量，不是牛顿单位的艇体合力。右边三项分别将垂直、前向、侧向控制通道乘该电机对应的混控系数后相加。相邻代码另算横滚、俯仰、偏航分量，再组合最终输出。因此这段实现从任务通道映射到单台命令；本课 $B_\tau f$ 则从各台实际推力映射到合力与合力矩，方向相反、单位也不同。

把右端当成矩阵的一行，可以看出线性叠加与我们推导的共同点；但要将混控提升为物理分配，还必须知道推进器安装方向、力臂、推力—命令曲线以及限幅策略。举例说某电机的前向系数为一、侧向系数也为一，两个通道同时给零点八时，未限幅和为一点六；是否公共缩放、牺牲某一轴或按优先级限制，要继续读后续代码，不能从这三行推断已实现QP。项目自定义的水平 $G_{xy}$ 系数来自用户报告，不能归到这一上游版本的原文中。

建议实际手写两个不同函数：bodyWrenchFromThrusters 接受牛顿推力并返回牛顿／牛顿米；normalizedMixer 接受归一化控制通道并返回归一化电机命令。先让类型、单位与测试分开，再研究怎样用推力标定把两者连接。这样即使未来改成约束QP，也不必同时推倒传感器、控制器和输出协议。

零基础停步自检

两台同向推进器只要求合力十牛顿，能唯一决定每台推力吗？

展开答案与原因

不能。 $f_1=s,f_2=10-s$ 都满足；必须加入努力偏好、上下限等进一步条件。

01.4｜从六轴需求到八台推进器：几何、伪逆与故障约束

开始前：先分配两台，再看八台

几何矩阵每一列都可以用尺子和方向箭头解释

式(1.23)(1.23)(1.23)用一个推进器同时解释力与力矩

先做两台的题：为什么约束还不够？

无约束伪逆是一个优化问题的答案

式(1.24)(1.24)(1.24)的乘子不是新的推进器状态

式(1.25)(1.25)(1.25)解释冗余，而不把“多两台”当作自动容错

式(1.26)(1.26)(1.26)的两行具有不同量纲