学习笔记 / 水下机器人 GNC

C++ 练习

把推导写成程序，再用多组输入检查边界和符号。

1. 选择题目，在页面编辑 C++17 模板。
2. 点击“编译并运行全部测试”，直接查看逐例结果。
3. 先独立完成，再展开参考实现对照。代码只在浏览器运行。

• 练习 01

  偏航坐标变换与刚体杆臂速度

  坐标系 · 刚体运动 · 26 组测试

  给定右手系机体系位置 $\mathbf r$、速度 $\mathbf v$、角速度 $\boldsymbol\omega$ 和杆臂 $\boldsymbol\ell$，计算 $$\mathbf p=R_z(\psi)(\mathbf r+\boldsymbol\ell),\qquad\mathbf u=R_z(\psi)(\mathbf v+\boldsymbol\omega\times\boldsymbol\ell).$$ $R_z$ 是主动旋转，正角从 $+x$ 向 $+y$；不是 NED/ENU 自动转换。

• 练习 02

  PID：导数低通与条件积分抗饱和

  控制 · 离散滤波 · 27 组测试

  初始 $I=D=e_{\rm prev}=0$。每步先计算 $$D\leftarrow\frac{\tau}{\tau+\Delta t}D+\frac{e-e_{\rm prev}}{\tau+\Delta t},\quad I_{\rm try}=I+K_i e\Delta t,\quad u_{\rm try}=K_p e+I_{\rm try}+K_dD.$$ 若 $u_{\rm try}>L$ 且 $K_i e>0$，或 $u_{\rm try}<-L$ 且 $K_i e<0$，则拒绝本次积分，否则 $I\leftarrow I_{\rm try}$。用更新后的积分算 $$u=\operatorname{clamp}(K_p e+I+K_dD,-L,L),$$ 最后 $e_{\rm prev}\leftarrow e$。

• 练习 03

  2×2 控制分配与等比例限幅

  控制分配 · 线性代数 · 28 组测试

  求解 $A\mathbf u=\mathbf b$。若 $|\det A|<10^{-10}$，输出 SINGULAR；否则按 $$s=\min\left(1,\frac{L}{\max(|u_0|,|u_1|)}\right)$$ 对两轴共同缩放，零向量时 $s=1$。不是逐轴裁剪，也不是约束最优分配。

• 练习 04

  标量卡尔曼滤波与 Joseph 协方差

  状态估计 · Kalman · 26 组测试

  随机游走模型，每步先预测，再校正：$$P^-=P+Q,\quad K=\frac{P^-}{P^-+R},\quad x\leftarrow x+K(z-x),\quad P\leftarrow(1-K)^2P^-+K^2R.$$ 最后一个式子是标量 Joseph 协方差更新。

• 练习 05

  二维匀速 Kalman：完整协方差传播

  状态估计 · 矩阵 · 25 组测试

  状态为位置和速度 $\mathbf x=[x,v]^T$，初值 $[0,0]^T$，$P=I_2$。$$F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix},\quad Q=q\begin{bmatrix}\Delta t^4/4&\Delta t^3/2\\\Delta t^3/2&\Delta t^2\end{bmatrix},\quad H=\begin{bmatrix}1&0\end{bmatrix},\quad R=r.$$ 每步先预测再位置观测更新；用 Joseph 形式更新协方差。 本题假设每个采样区间内加速度为独立随机常量 $a_k$，均值为零，方差 $q=\operatorname{Var}(a_k)$，单位为 $\mathrm{m^2/s^4}$；因此 $Q=qGG^T$，$G=[\Delta t^2/2,\Delta t]^T$。观测噪声参数 $r$ 是位置测量方差，单位为 $\mathrm{m^2}$。这不是连续白加速度噪声谱密度模型：若采用谱密度 $q_c$（单位 $\mathrm{m^2/s^3}$），应使用另一离散化 $Q_c=q_c\begin{bmatrix}\Delta t^3/3&\Delta t^2/2\\\Delta t^2/2&\Delta t\end{bmatrix}$，不可混用两种参数。

• 练习 06

  延迟观测的时间缓存插值

  时间同步 · 缓存 · 25 组测试

  对严格递增时间戳缓存做分段线性插值：$$x(t)=x_i+\frac{t-t_i}{t_{i+1}-t_i}(x_{i+1}-x_i).$$ 端点可查询，超出范围输出 OUT，禁止外推。本题只做观测对齐，不等同于完整乱序 Kalman 回滚重放。

• 练习 07

  三维 A*：26 邻域与严格禁止穿角

  路径规划 · A* · 34 组测试

  求体素栅格起点至终点的最短路径长度。26 邻域边长为 $\sqrt{d_x^2+d_y^2+d_z^2}$。一次移动的每个非空轴子集所对应的中间体素必须都空闲（面、棱、体对角均禁止穿角）。使用欧氏启发式 $h(\mathbf p)=\lVert\mathbf p-\mathbf g\rVert_2$ 实现 A*；只输出长度，不限定路径。离散栅格不代表真实载具安全走廊。

• 练习 08

  二维直线路径 LOS 制导

  制导 · 几何 · 32 组测试

  无限直线路径 $A\to B$：$$\chi=\operatorname{atan2}(B_y-A_y,B_x-A_x),\quad e=-(x-A_x)\sin\chi+(y-A_y)\cos\chi,$$ $$\psi=\operatorname{wrap}_{[-\pi,\pi]}\left(\chi-\operatorname{atan2}(e,\Delta)\right).$$ 若 $\lVert B-A\rVert_2<10^{-10}$，输出 DEGENERATE。不是航点切换、避障或海流补偿。 判题对首个输出角度使用环绕角误差 $|\operatorname{atan2}(\sin(\psi-\psi^*),\cos(\psi-\psi^*))|$，因此 $-\pi$ 与 $+\pi$ 等价；仍要求输出在 $[-\pi,\pi]$ 内（容差允许边界舍入）。第二个输出横向误差按普通浮点规则比较。

• 练习 09

  CRC-16/CCITT-FALSE 字节帧校验

  驱动基础 · 通信 · 27 组测试

  计算 CRC-16/CCITT-FALSE：poly=0x1021，init=0xFFFF，refin=false，refout=false，xorout=0。空负载结果为 $65535$，ASCII 123456789 的标准检查值为 0x29B1，即 $10673$。