03.2｜位置 P、速度 PI：为什么两个稳定环接起来仍须证明

输入是位置与速度反馈、位置参考及可选速度/加速度前馈；输出先是导航系加速度，再变为机体执行命令。先去掉滤波、延迟和饱和，推导串级闭环；随后逐一把这些实际因素放回来。这样能分清“线性模型稳定”与“全部工程实现稳定”各自需要什么条件。

符号	含义与维度	单位／坐标约定
$p,p_d,v,v_d$	一轴位置与参考、速度与参考；标量	m、m、m/s、m/s；同一NED轴
$k_p,k_v,k_i$	位置P、速度P、速度I增益	1/s、1/s、1/s²
$\xi,u,a$	裸速度误差积分、控制加速度、阻尼率	m、 $\mathrm{m/s^2}$ 、1/s
$s,z,T$	连续特征变量、离散特征变量、采样周期	1/s、无量纲、s
$G_{xy},q_{xy}$	水平效能矩阵2×2、归一化命令2×1	矩阵元素 $\mathrm{m/s^2}$ ；命令无量纲；机体FRD

需要补的数学只读这些： 状态空间与特征值。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

位置环为什么先给速度，速度环为什么再给加速度

位置差 $1\,\mathrm m$ 只告诉我们“还没到”，没有告诉我们“已经以多快速度冲过去”。外环把距离变成希望的接近速度，内环再比较真实速度决定推力方向。艇还没到目标但已冲得太快时，内环可能先制动；这并不是两个环互相矛盾，而是分别回答距离和动量问题。

状态反馈与极点：把“以后会怎样”算出来

状态反馈用当前位置、速度等状态决定输入。在线性模型 $\dot x=Ax+Bu$ 中选择 $u=-Kx$ ，只是把输入代回去，便得到 $\dot x=(A-BK)x$ 。这里 $x$ 是相对平衡点的状态，各分量可以有不同单位； $K$ 的每列负责把对应状态变为输入单位。负号是这个反馈约定的一部分，不能与另一页的 $u=Kx$ 约定混用。

“极点”描述闭环的基本增长／衰减模式。先看只有一个速度状态的例子： $\dot v=-\lambda v$ ，其中 $\lambda>0$ 、单位 $\mathrm{s^{-1}}$ ，解为 $v(t)=v(0)e^{-\lambda t}$ 。所以极点 $-\lambda$ 越靠左，理想模型衰减越快；若符号变成正，偏差就指数增大。多维矩阵的特征值把同一问题推广到多个耦合方向。离散系统每步乘倍率，稳定条件变成倍率模小于一，而不是“数值为负”。

不要把快当成免费。 很快的极点通常要求更大控制输入；执行器上限、采样延迟和滤波滞后可能使理想快速模型不再成立。下面的串级三阶式保留位置、速度、积分三个状态，正是为了检查它们耦合后的实际模式，不能把两个单环稳定结论直接相加。

微练习： 每拍误差变成上一拍的 $-1.1$ 倍，看起来一直在反向纠错，为什么仍不稳定？

概念答案：反向不代表幅度下降

符号交替，但绝对值每拍乘 $1.1$ ，所以振幅增长；离散极点为 $-1.1$ ，在单位圆外。先在PID练习守住单拍状态顺序，再做二维效能逆解与限幅理解“算出的加速度”与“可执行命令”的差别。

消去内部状态，得到真正的三阶多项式

\begin{aligned}\dot p&=v,&\dot v&=-av+u,\\v_d&=k_p(p_d-p),&\dot\xi&=v_d-v,\\u&=k_v(v_d-v)+k_i\xi.\end{aligned}\tag{3.6}

三条状态方程的物理任务。 $p$ 记录艇在哪里， $v$ 记录运动快慢， $\xi$ 记录速度欠账；所以状态是三维而非二维。 $\xi=\int e_v\,dt$ 的单位为米，乘 $k_i$ 后回到加速度。位置参考暂取常数，是为了研究平衡点附近的自主动态；参考变化会增加已知强迫项，但不改变此线性结构的齐次特征式。若取 $p_d=0$ 、 $p=0.2\,\mathrm m$ 、 $v=0$ 、 $k_p=1\,\mathrm{s^{-1}}$ ，外环先给出 $v_d=-0.2\,\mathrm{m/s}$ ，不是直接给负推力；内环才把这条速度任务转成加速度。

位置多了一个积分，速度 PI 又引入一个积分状态，所以整个系统是三阶，不能只分析速度内环的二阶分母。常值目标的平衡点平移到原点，令位置目标为零，则速度目标为负的位置反馈。对速度方程再求一次导数，是为了消去积分状态；然后依次以位置导数替换速度。

\begin{aligned}\ddot v&=-a\dot v+k_v(-k_pv-\dot v)+k_i(-k_pp-v),\\p^{(3)}+(a+k_v)\ddot p+(k_vk_p+k_i)\dot p+k_ik_pp&=0,\\\chi(s)&=s^3+A s^2+B s+C,\\A&=a+k_v,\quad B=k_vk_p+k_i,\quad C=k_ik_p.\end{aligned}\tag{3.7}

逐项消元，防止丢掉一个耦合项。 先代入 $v_d=-k_pp$ ，得到 $\dot v=-k_vk_pp-(a+k_v)v+k_i\xi$ 。对整式求导后，第一项变为 $-k_vk_pv$ ，第二项变为 $-(a+k_v)\dot v$ ，第三项必须使用 $\dot\xi=-k_pp-v$ ，所以同时贡献位置项 $-k_ik_pp$ 和速度项 $-k_iv$ 。把它们移到左边才形成 $B=k_vk_p+k_i$ 。 $s$ 代表指数模态 $e^{st}$ 的增长率：因为其一、二、三阶导数分别乘 $s,s^2,s^3$ ，代入微分方程并除去非零模态幅度后才得到多项式。特征式不是另一条经验公式，而是同一运动方程的模态表达。

第一行中的最后一项来自积分状态的导数，不是凭经验加出来的“位置积分”。逐项单位都为 $\mathrm{m/s^3}$ ；多项式的 $A,B,C$ 分别为 1/s、1/s²、1/s³。Routh 表把根在左半平面的条件化成首列同号：首两行交错放系数，第三行由二阶行列式消元得到。

\begin{array}{c|cc}s^3&1&B\\s^2&A&C\\s^1&(AB-C)/A&0\\s^0&C&0\end{array}\qquad A>0,\ B>0,\ C>0,\ AB>C.\tag{3.8}

Routh 第三行从哪里来。 首行来自三阶与一阶系数 $(1,B)$ ，次行来自二阶与常数项 $(A,C)$ 。消元时先形成 $AB-C$ 再除以首项 $A$ ，因此第三行首项为 $(AB-C)/A$ 。对于首项为正的三阶实系数多项式，没有零首项的情形下，首列没有符号变化等价于所有根在左半平面。若 $AB=C$ ，则多项式可写为 $(s+A)(s^2+B)$ ，出现纯虚根，不能把等号算成渐近稳定。实际设计应给严格不等式留裕量，因为阻尼识别、采样与滤波都会移动根。

当所有物理阻尼和增益都正时，前三个条件成立；但第四个仍要代入，不能因两个局部环“看起来好”就忽略。展开可见 $AB-C=ak_vk_p+ak_i+k_v^2k_p+k_vk_i-k_ik_p$ ：过强的位置环会通过最后一项破坏裕度。取 $a=0.1$ 、 $k_v=0.2$ 、 $k_i=1$ 、 $k_p=2$ ，则 $A=0.3$ 、 $B=1.4$ 、 $C=2$ ， $AB=0.42$ 小于 2，正增益也会不稳定。

a=1,\quad k_v=2,\quad k_i=1,\quad k_p=1\ \Longrightarrow\ \chi(s)=s^3+3s^2+3s+1=(s+1)^3.\tag{3.9}

把极点核对变成手写测试。 从 $(s+1)^2=s^2+2s+1$ 出发再乘 $(s+1)$ ，二次项系数来自 $s^2+2s^2$ ，一次项来自 $2s+s$ ，所以正好是 $(1,3,3,1)$ 。该核对同时检查了位置反馈负号、积分状态单位及三阶项归一化。若数值求根得到三个略有差别的复数，不必立刻判错：三重根对浮点系数扰动特别敏感；更可靠的单元测试是直接比较系数和特征矩阵多项式。

这个手算例的三个连续极点都在 −1 $\mathrm{s^{-1}}$ ，Routh 余量为 $9-1=8\,\mathrm{s^{-3}}$ 。重复极点对参数扰动敏感，但足以核对系数。若只保留速度内环，其特征式是 $s^2+3s+1$ ，极点并不是 −1；串级后的极点必须重新求。

采样与一拍延迟：用最简单子问题看清危险

为了隔离延迟作用，暂时只保留速度比例反馈 $u=-kv$ ，并对一阶对象做零阶保持精确离散。一个周期内输入常数，把微分方程乘积分因子后积分，得到 $\alpha$ 为自然衰减、 $\beta$ 为输入累计作用。这里 $\alpha$ 与上一课滤波 $\alpha$ 不是同一参数。

\begin{aligned}v_{k+1}&=\alpha v_k+\beta u_k,\quad\alpha=e^{-aT},\quad\beta=\frac{1-e^{-aT}}a,\\u_k=-kv_k&\Rightarrow z=\alpha-\beta k,\\u_k=-kv_{k-1}&\Rightarrow z^2-\alpha z+\beta k=0.\end{aligned}\tag{3.10}

从积分因子到离散极点。 在 $[kT,(k+1)T)$ 内固定输入为 $u_k$ ，解 $\dot v+av=u_k$ 得到 $v((k+1)T)=e^{-aT}v(kT)+u_k\int_0^Te^{-a(T-\sigma)}d\sigma$ 。积分等于 $(1-e^{-aT})/a$ ，单位为秒；当 $a\to0$ 时取极限 $\beta=T$ ，程序不能直接除零。没有延迟时代入 $u_k=-kv_k$ ，只剩一个状态。延迟一拍时必须把 $v_{k-1}$ 作为第二个状态；将 $v_k=z^k$ 代入递推式，除以 $z^{k-1}$ 得到二次特征式。此处 $k$ 在控制增益中表示正比例系数，而时间下标也常用 $k$ ；手写程序建议增益命名 gain，下标命名 step。

离散稳定要求所有极点模小于一，不能把连续域的负实部判断照搬。 $a=1\,\mathrm{s^{-1}}$ 、 $T=0.1\,\mathrm s$ 、 $k=12\,\mathrm{s^{-1}}$ ，无延迟极点约 −0.2371，仍稳定；加入一拍延迟后复共轭根的乘积为 $\beta k\approx1.14195$ ，其模等于平方根约 1.0686，因而发散。这个例子没有说某项目存在一拍延迟，而是说明提高采样率、减少队列滞后与调低增益为何属于同一个设计问题。

前馈不是另一种积分，它提前支付模型所需输入

\begin{aligned}v_d&=\dot p_d+k_p(p_d-p),\\\dot v_d&=\ddot p_d+k_p(\dot p_d-v),\\u&=a v_d+\dot v_d+k_v(v_d-v)+k_i\xi,\qquad\dot\xi=v_d-v,\\\dot e_v&=-(a+k_v)e_v-k_i\xi.\end{aligned}\tag{3.11}

前馈项逐个抵消什么。 阻尼项 $av_d$ 支付维持目标速度所需的力， $\dot v_d$ 支付目标加速所需的惯性；它们都为 $\mathrm{m/s^2}$ 。令 $e_v=v_d-v$ ，则 $\dot e_v=\dot v_d+av-u$ ，把控制律完整代入后，两个 $\dot v_d$ 相消， $av-av_d=-ae_v$ ，比例项留下 $-k_ve_v$ ，积分项留下 $-k_i\xi$ 。因此式末尾没有神秘地丢失参考，而是参考已由准确模型前馈解释。若阻尼估计错了，抵消不完整，积分或扰动估计还要负责剩余项；不能因加入前馈就删除稳态偏差分析。

最后一行把真实速度方程代入误差导数，阻尼前馈与参考加速度正好消去已知的目标运动。这个理想结构让内环误差与外环误差形成三角级联；它和前面“不含模型前馈”的三阶系统不是相同控制律，不能混用特征多项式。实际参考应经过速度、加速度限制；若直接对带跳变的速度目标差分，前馈会要求不可达的冲击。项目中的 FF 是接口层增益，不能仅凭名称就认定它精确实现了上述模型抵消。

从导航加速度到机体水平命令，逐个单位过关

\begin{aligned}a^b&=(R_b^n)^\top a^n,\quad G_{xy}=\begin{bmatrix}5.33&-0.83\\1.39&3.21\end{bmatrix},\\a^b_{xy}&=G_{xy}q_{xy},\quad \det G_{xy}=18.263,\\q_{xy}&=\frac1{18.263}\begin{bmatrix}3.21&0.83\\-1.39&5.33\end{bmatrix}\begin{bmatrix}1\\0.5\end{bmatrix}=\begin{bmatrix}0.19848875\\0.06981328\end{bmatrix}.\end{aligned}\tag{3.12}

为什么先旋转再解线性方程。 $a^n$ 是导航北东下三维需求，推进器却固定在机体前右下方向，必须先用 $(R_b^n)^\top$ 取得同一向量的机体系坐标。 $G_{xy}$ 的两行分别是机体前、右加速度，两列分别是 forward、lateral 命令作用。逆矩阵由交换对角、取负非对角再除行列式得到；例如第一命令分子为 $3.21\times1+0.83\times0.5=3.625$ 。若行列式接近零，两个输入产生近乎相同的加速度方向，求逆会放大测量误差；此时应报告不可可靠分配，而不是输出巨大命令后静默裁剪。 R 是 3×3 无量纲旋转矩阵，先用完整三维姿态旋转，再取机体前右分量； $G_{xy}$ 是水平经验效能，不是惯性矩阵，也不是六轴推进器几何矩阵。矩阵的列表示分别给单位 forward/lateral 命令时的加速度响应。行列式单位为 $\mathrm{m^2/s^4}$ ，逆矩阵单位为 $\mathrm{s^2/m}$ ，因此所得命令无量纲。把结果乘回 G 得到 (1,0.5) $\mathrm{m/s^2}$ ，交叉项被同时补偿。

q_a=\frac{q_c}{\max(1,\|q_c\|_\infty)},\qquad a^b_{xy,a}=G_{xy}q_a.\tag{3.13}

公共缩放与实现残差。 $\|q_c\|_\infty$ 是两个命令绝对值的最大值。除数至少为一，需求可达时完全不改变它；需求越界时使最大绝对分量恰好到一。这个除数无量纲，所以单位不变。用 $q_c=(1.2,0.6)^T$ 代入，实际命令为 $(1,0.5)^T$ ；要求加速度为 $(5.898,3.594)^T$ ，实际加速度为 $(4.915,2.995)^T\,\mathrm{m/s^2}$ ，两者相差 $(-0.983,-0.599)^T$ 。这条残差若要送回导航系积分器，还必须先补零组成三维向量并按所选模型旋转回导航系。

公共缩放把 (1.2,0.6) 变为 (1,0.5)，保持命令比例；逐轴裁剪会改变方向。由于 G 线性，这也保持局部加速度方向。随后混控仍可能饱和，最终可达值要在执行层追踪。姿态水平、航向向东时，北向目标对应机体左向，不能把导航北分量直接塞进 forward。

源码阅读与手写实验

沿 AC_PosControl_Sub 的 update_xy_controller_poshold_impl 看位置误差如何生成速度目标，再看 accel_target_to_xy_thrust 如何完成旋转、单位变换与效能补偿。PosHold、Guided 非零 XY 速度目标、CIRCLE 直接平移输入的语义不同；先辨别输入物理量，再比较增益。垂直链 control_depth → AC_PosControl::update_z_controller 有自己的位置、速度、加速度层级，不能绕过它直接把“深度误差”交给同一水平矩阵。最后 AP_Motors6DOF 负责多轴争用、归一化与执行器输出。

理论 NED 米值 (1.2,−0.4,2.0) 转为控制接口 NEU 厘米值 (120,−40,−200)。水平加速度在进入 G 前除以一百；垂直还要检查正号。手写练习：位置误差 20 cm、位置增益 0.5 $\mathrm{s^{-1}}$ 、速度前馈 5 cm/s、实测速度 8 cm/s，速度 P 增益 3 $\mathrm{s^{-1}}$ 。

参考解答（展开查看）

依次为 $15\,\mathrm{cm/s}$ 、 $7\,\mathrm{cm/s}$ 、 $21\,\mathrm{cm/s^2}=0.21\,\mathrm{m/s^2}$ 。

练习二：独立展开三阶特征式，并用三阶矩阵的行列式复核。

参考解答（展开查看）

// 教学实现：标准库复数极点与一般2×2逆解。
#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>
#include <complex>
#include <iomanip>
#include <iostream>
#include <stdexcept>
std::array<double,2> allocate(double a,double b,double c,double d,
                              double ax,double ay) {
    const double scale=std::max({std::abs(a),std::abs(b),std::abs(c),std::abs(d)});
    if(scale==0) throw std::runtime_error("zero effectiveness");
    // 先缩放矩阵，防止不同物理尺度下固定det门限失效。
    a/=scale;b/=scale;c/=scale;d/=scale;ax/=scale;ay/=scale;
    const double det=a*d-b*c;
    if(std::abs(det)<1e-12) throw std::runtime_error("singular effectiveness");
    double qx=(d*ax-b*ay)/det, qy=(-c*ax+a*ay)/det;
    const double s=std::max({1.0,std::abs(qx),std::abs(qy)});
    return {qx/s,qy/s};
}
int main(){
    const double a=1,T=.1,gain=12;
    const double alpha=std::exp(-a*T),beta=-std::expm1(-a*T)/a;
    const auto disc=std::sqrt(std::complex<double>(alpha*alpha-4*beta*gain,0));
    const auto z=(alpha+disc)/2.0;
    std::cout<<std::setprecision(12)<<"no delay "<<alpha-beta*gain
             <<" delayed "<<z<<" modulus "<<std::abs(z)<<'\n';
    auto q=allocate(5.33,-.83,1.39,3.21,1,.5);
    assert(std::abs(5.33*q[0]-.83*q[1]-1)<1e-12);
    assert(std::abs(1.39*q[0]+3.21*q[1]-.5)<1e-12);
    std::cout<<q[0]<<' '<<q[1]<<'\n';
    q=allocate(1,0,0,1,1.2,.6);
    assert(std::abs(q[0]-1)<1e-12 && std::abs(q[1]-.5)<1e-12);
    bool rejected=false;
    try { allocate(1,2,2,4,1,1); } catch(const std::runtime_error&) { rejected=true; }
    assert(rejected);
}

精读路线：回到 TB03-S1 的运动学变换，解释为何水平控制也需要完整姿态；把该模型的阻尼项与本课的 a 对齐，再在项目函数边界逐一写下 m、cm、NED、NEU、FRD。

用状态矩阵复核串级推导，而不是依赖一种代数路线

取齐次参考，状态顺序固定为 $x_c=(p,v,\xi)^T$ 。把式（3.6）逐行按这三个状态收集系数，可得

\dot x_c=A_cx_c,\qquad A_c=\begin{bmatrix} 0&1&0\\ -k_vk_p&-(a+k_v)&k_i\\ -k_p&-1&0 \end{bmatrix}.

第一行第二列的一表示位置导数就是速度；第二行第一列的负号来自外环“位置偏正就向负方向走”；第三行第一列与第二列共同组成速度误差。状态各分量的单位分别是米、米每秒、米，矩阵元素的单位随行列映射决定，并非整个 $A_c$ 的每个元素都能写作统一单位。手写行列式 $\det(sI-A_c)$ 时沿第一行展开：第一项产生 $s[s(s+a+k_v)+k_i]$ ，第二项产生 $k_vk_ps+k_ik_p$ ，相加便重新得到式（3.7）。两种推导一致，才比较有把握没有在微分时丢掉积分耦合。

“内环快、外环慢”是有用设计方向，但不是独立于动力学的口号。外环越快，要求速度目标随位置误差迅速变化；内环若有测量滤波、执行器迟滞和采样延迟，就可能来不及实现。实践上应先在无位置外环时验证速度阶跃、饱和恢复和噪声响应，再接回位置环并检查完整特征根；不能只沿一条直线试一次悬停就宣称所有方向都稳定。阻尼随速度变化时，还需在多个工作点做同样检查。

算法层面的完整链为：从 EKF 取当前而非陈旧导航状态 → 将目标与反馈统一到同一位置单位 → 计算位置误差 → 形成并限幅速度目标 → 计算速度误差与积分状态 → 加入具有明确语义的前馈 → 形成导航加速度 → 旋转为机体需求 → 用水平效能矩阵求命令 → 进行局部可达缩放 → 送入混控 → 根据实际已知约束处理积分。每个箭头都值得一条带时间戳的日志。Guided 接收外部速度时可能从这条链的中间进入；这就是为什么不能机械套用 PosHold 的全部闭环多项式。

原始混控代码：最后一个约束不在水平矩阵里

下面是 ArduPilot/ArduSub-4.1.1 公开上游、GPL-3.0-or-later 的 AP_Motors6DOF.cpp 第479行，来自 output_armed_stabilizing_vectored 分支；许可证。

_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]), -1.0f, 1.0f);

rpy_out[i] 汇集旋转通道，linear_out[i] 汇集平移通道；先相加再乘电机反向约定，最后裁到归一化区间。由这行可直接看出，水平局部分配可达并不保证加入姿态、深度之后仍然可达。假定某电机平移分量为 $0.8$ 、旋转分量为 $0.5$ 、反向系数为 $1$ ，合成请求 $1.3$ 最终只有 $1$ ，损失 $0.3$ 。不能把该损失直接当成全艇某一轴的加速度误差；必须先通过推进器效能映射，才能回到控制器所用坐标与单位。

不同机架分支的归一化策略不同，例如同文件 output_armed_stabilizing_vectored_6dof 对分组量另有缩放。读原代码时先确定实际机架与分支，再解释局部限幅；本段不把一个分支的这一行泛化为所有机架的完整分配算法。项目报告中的水平 $G_{xy}$ 与上游混控系数属于两层模型：前者是命令到加速度的经验响应，后者是控制通道到电机组合的接口约定。

C++ 手写任务：把线性代数与不可达处理一起实现

打开 C++ 在线练习：二维效能逆解与公共缩放。这个练习不是带多轴约束的 QP：它先求二维逆解，再按最大绝对命令等比例缩放。输入采用一般 $2\times2$ 矩阵，因此不能把本章四个系数写死。先写单元矩阵、交叉耦合矩阵、负需求、需要缩放、奇异矩阵五类测试；再用 $Gq_a$ 验证实际加速度。仅判断返回命令在区间内不足以发现轴顺序错误。