04.4 环绕不是横移：径向收敛、切向运动与视线前馈

零基础入口：绕圈不是“朝目标前进再加一点转弯”

先固定目标中心，画一根从目标指向机器人的箭头。这是径向，沿它走会改变距离；与它垂直的方向是切向，沿它的瞬时速度用来绕行。把箭头长度除掉得到单位方向，是为了让后面乘上的速度真正具有 m/s 单位，不被当前距离再放大一次。两个方向正交，所以一个负责调距离，一个负责推进绕行，相互职责清楚。

例如机器人位于目标正北 $2$ m，径向朝北、正切向朝东。若距离太大，径向命令应朝南；若要开始绕行，另加向东的切向命令。这里“向东”是世界方向，机头若正朝南，它在机体上反而对应左移；旋转到机体系之后才可填前向／横向通道。

角速度描述每秒转过多少角度，不是每秒走多少米。弧度由圆弧长度除以半径定义，因此为了把切速换成轨道角速度，使用 $\dot\theta=v_t/\rho$ ： $v_t$ 是切向速度（m/s）， $\rho$ 是当前半径（m），结果是 rad/s。取 $v_t=0.2$ m/s、 $\rho=2$ m，得到 $0.1$ rad/s，约 $5.73^\circ$ /s；同一切速在 $4$ m 半径上只转 $0.05$ rad/s。不要把 $0.1$ rad/s 当成 $0.1^\circ$ /s。

保持面向目标需要机头跟随不断旋转的视线，故先给出相同角速度的前馈；再根据实际艏向偏差加反馈。前馈预告“理想情况下目标视线本来就在转”，反馈纠正“实际没跟上多少”。只把图像中心误差压到零，并不会自动生成切向运动；机器人完全可能原地停住。

本课先讨论连续时间运动学，切向速度在一个瞬间不改半径；计算机每步却沿切线走一小段直线，可能有很小的外漂。缩短步长检查误差是否减小，才能区分离散误差与控制律符号错误。

对应的基础练习： 坐标与杆臂检查世界／机体方向，LOS 练习方向角；目录没有独立圆轨道题，圆轨道半径和角速率由本页折叠 C++ 验证。

保持目标居中会让机器人一直看着目标，却不保证围绕目标运动；单纯侧移会改变视线，却不保证距离恒定。本课把任务拆成三个可独立解释的目标：径向误差归零、切向持续前进、艏向跟上视线。先假设知道米制目标中心，再回到只有检测框的项目策略，辨认哪些几何量可以估计、哪些只能用图像线索替代。

变量与假设

符号	含义	维数 / 单位	坐标或约定
$p,c,q=p-c$	机器人位置、目标中心、相对位置	2×1 / m	NED 的北东平面
$\rho,R_d,n_r,n_t$	当前半径、期望半径、径向单位矢量、切向单位矢量	m；m；2×1；2×1	目标中心向外为径向正
$k_r,v_t,v_T$	径向增益、有符号切速、目标中心速度	1/s；m/s；2×1 m/s	NED
$J,\theta,\psi_d,r_d$	平面正九十度旋转、方位、期望艏向、期望偏航率	2×2；rad；rad；rad/s	从北向东为正
$e_r,e_\psi$	半径误差、当前减期望艏向误差	m；rad	角误差包裹至主值区间
$k_\psi,\tilde v,V_r,a_{\rm circle},T_{\rm orbit}$	艏向误差增益、速度跟踪误差、径向能量、向心加速度、周期	1/s；2×1 m/s；m²；m/s²；s	水平 NED；正增益

假设目标中心可估、相对距离非零、水平速度能跟踪参考，且目标中心速度已知或足够慢。目标速度用大写 T 下标，与上一课水流速度区分。环绕半径必须同时满足视场、障碍物、缆绳与执行能力要求；恒半径的数学圆不自动是环境中的可行轨迹。

径向与切向为何能分开设计

\begin{aligned}\rho&=\|q\|,\quad n_r=q/\rho,\quad J=\begin{bmatrix}0&-1\\1&0\end{bmatrix},\quad n_t=Jn_r,\\e_r&=\rho-R_d,\qquad v_d=v_T-k_re_r n_r+v_tn_t,\\\dot\rho&=\frac{q^T\dot q}{\rho}=n_r^T(v_d-v_T)=-k_re_r,\\e_r(t)&=e_r(0)e^{-k_rt},\qquad\dot V_r=-k_re_r^2,\quad V_r=\tfrac12e_r^2.\end{aligned}

(4.18)

第一行建立极坐标的两条正交单位轴。对距离平方开方求导得到径向速度只等于相对速度在径向的投影；切向项与径向正交，所以在理想连续模型中不改变半径。剩下的就是一阶线性误差方程，可直接积分得到指数解。全程保持正距离是必要条件；若目标中心重合，应退出这套除以半径的控制律，先建立一个可定义的接近方向。

\begin{aligned}\dot\theta&=\frac{q_N\dot q_E-q_E\dot q_N}{\rho^2}=\frac{v_t}{\rho},\\\psi_d&=\operatorname{atan2}(-q_E,-q_N)=\theta+\pi\pmod{2\pi},\\r_d&=\frac{v_t}{\rho}-k_\psi\operatorname{wrap}(\psi-\psi_d),\quad k_\psi>0.\end{aligned}

(4.19)

角度求导的分子是二维叉积，径向速度在其中抵消，只留下切向速度乘半径，因此视线角速度等于切速除半径。机头朝向目标与向外径向相差半周，导数却相同，所以偏航前馈与轨道角速度同号。余下比例反馈消除艏向误差。这里 kψ 单位为 1/s；如果偏航角速率内环理想，包裹边界之外的局部角误差导数就是负增益乘误差。

检验符号：机器人位于目标正北，径向指北，正切速指东，绕行方位角从北向东增加；朝向目标原本指南，随着东移会转向西南，NED 偏航同样增加。机器人此时机头朝南，它的机体右侧是西，故“正切向向东”对应负的 body sway。不能因为协议字段名是 lateral，就不经姿态转换直接填正切速。

从笛卡尔微分推到视线前馈

径向速度公式可以直接从 $\rho^2=q_N^2+q_E^2$ 推出：两边对时间求导，得 $2\rho\dot\rho=2q_N\dot q_N+2q_E\dot q_E$ ；除以正的 $2\rho$ ，便是单位径向向量与相对速度的内积。这里减去目标速度不可省略，因为 $\dot q=\dot p-\dot c$ 。若目标向北移动而机器人保持世界坐标不动，相对位置仍在变。

角度从 $\theta=\operatorname{atan2}(q_E,q_N)$ 出发。微分分子为什么交叉？在 $q_N\ne0$ 的局部区域，可先对 $\arctan(q_E/q_N)$ 用链式法则，再把分母整理为 $q_N^2+q_E^2$ ；atan2 负责把同一导数推广到正确象限。代入径向与切向两项后：

\begin{aligned} q_N\dot q_E-q_E\dot q_N &=-k_re_r(q_Nn_{r,E}-q_En_{r,N})\\ &\quad+v_t(q_Nn_{t,E}-q_En_{t,N})\\ &=0+v_t\rho. \end{aligned}

第一项为零是径向向量与自身平行；第二项等于半径，是因为切向与径向正交且单位长度。再除以 $\rho^2$ ，才得到 $\dot\theta=v_t/\rho$ 。这条前馈需要的是当前半径，而不是期望半径：尚在远处接近轨道时，用 $R_d$ 代替 $\rho$ 会使转艏过快。

角度反馈使用 $\operatorname{wrap}$ 是为了取最短转角。例如当前 $179^\circ$ 、期望 $-179^\circ$ ，直接相减得到 $358^\circ$ ，但真正的短角误差是 $-2^\circ$ 。在不跨包裹边界的局部区域，若实际偏航率能跟踪 $r_d$ ，则 $\dot e_\psi=r_d-\dot\psi_d=-k_\psi e_\psi$ ；若漏掉前馈，则持续环绕时会留下约为 $-\dot\psi_d/k_\psi$ 的角误差。前馈负责已知几何变化，反馈负责实际残差，两者职责不同。

数值例与有限执行能力

取期望半径 2 m、当前机器人相对位置 (2.5,0) m，径向增益 0.4/s，切速 0.2 m/s，目标静止。径向命令为向南 0.2 m/s，切向命令为向东 0.2 m/s，总速约 0.28284 m/s。初始偏航前馈为 0.08 rad/s，约 4.58°/s；收敛到 2 m 圆周后为 0.1 rad/s，约 5.73°/s。连续模型在 5 s 后的半径误差约为 0.06767 m。

\begin{aligned}\|v_d-v_T\|^2&=k_r^2e_r^2+v_t^2,\\a_{\rm circle}&=v_t^2/R_d,\qquad T_{\rm orbit}=2\pi R_d/|v_t|,\\\dot e_r&=-k_re_r+n_r^T\tilde v,\quad\tilde v=v_{\rm actual}-v_d.\end{aligned}

(4.20)

正交性让总速平方等于两个分量平方和。因此接近轨道时径向纠偏占用速度预算，应降低切速而非任意截断径向项；绕行稳定以后才恢复任务切速。例中的圆周向心加速度为 0.02 m/s²，一圈约 62.83 s。最后一式把速度跟踪误差重新放回模型：径向扰动若持续为常值，稳态半径误差约为扰动除以径向增益。增益越大需要越大瞬时速度，不能仅凭理想指数公式无限增益。

若对整个相对速度同比缩放且比例始终为正，径向误差仍朝零，但收敛变慢；若逐轴裁剪，径向和切向的正交分解被破坏，应重新计算实际径向速度。离散欧拉积分还有一个细节：纯切向一步是沿切线走直线，半径会略增，其量级与步长平方有关，不能把离散轨迹的微小外漂误认为连续证明符号错误。

先手写，再展开参考实现

将本课公式写成一个 C++17 单文件程序。先列输入单位与退化分支，再写核心循环；不要先看答案再照抄。参考程序只依赖标准库，可用 g++ -std=c++17 -O2 lesson.cpp -o lesson && ./lesson 离线运行，不访问机器人。断言是对上文手算结果的检查，不是用程序输出代替推导。

参考解答（展开查看）

// 教学模型：目标中心已知，机器人理想跟踪水平速度。
#include <array>
#include <cmath>
#include <iostream>
#include <cassert>
using V=std::array<double,2>;
struct Ref{V velocity;double yaw,yawRate;};
Ref orbit(V p,V center,V targetVelocity,double radius,double kr,double vt){
    V q{p[0]-center[0],p[1]-center[1]};double rho=std::hypot(q[0],q[1]);
    assert(rho>.1 && radius>0 && kr>0);
    V nr{q[0]/rho,q[1]/rho},nt{-nr[1],nr[0]};
    V v{targetVelocity[0]-kr*(rho-radius)*nr[0]+vt*nt[0],
        targetVelocity[1]-kr*(rho-radius)*nr[1]+vt*nt[1]};
    return {v,std::atan2(-q[1],-q[0]),vt/rho};
}
int main(){
    V p{2.5,0},center{};double dt=.001,kr=.4,radius=2,vt=.2;
    auto first=orbit(p,center,{},radius,kr,vt);
    assert(std::abs(first.velocity[0]+.2)<1e-12);
    assert(std::abs(first.velocity[1]-.2)<1e-12);
    assert(std::abs(first.yawRate-.08)<1e-12);
    for(int i=0;i<5000;++i){auto r=orbit(p,center,{},radius,kr,vt);
        p[0]+=dt*r.velocity[0];p[1]+=dt*r.velocity[1];
    }
    double actual=std::hypot(p[0],p[1])-radius,exact=.5*std::exp(-kr*5);
    assert(std::abs(actual-exact)<1e-5);
    std::cout.precision(12);std::cout<<actual<<' '<<exact<<'\n';
}

离散实现为何有微小半径偏差，如何理解输出顺序

先看已在圆周上的纯切向一步：当前位置为 $\rho n_r$ ，欧拉更新后为 $\rho n_r+v_t\Delta t\,n_t$ 。正交性给出

\rho_{k+1}=\sqrt{\rho_k^2+(v_t\Delta t)^2} \approx\rho_k+\frac{v_t^2\Delta t^2}{2\rho_k}.

所以连续切向运动不改半径，离散的“直走一小段”却略向外偏。这是弦切几何造成的误差，随步长减小而减弱；径向反馈又会纠正它。示例实际还含径向指数衰减的欧拉误差，两种误差合在一起，所以最后离散结果不必总比解析结果大。比较 $0.001$ s 与 $0.0005$ s 步长，应观察到结果向连续解靠近，而不是要求每步精确落在圆上。

一次更新的完整顺序是：读取同一时刻的位置与目标中心；计算相对位置和半径；检查半径非零；构建径向、切向；由半径误差求径向速度；根据剩余速度预算选择切速；加目标速度前馈；由当前半径求视线角速度；最后输出一致的水平速度与朝向参考。限幅之后若切速被改变，偏航前馈也应使用实际采用的切速，而不能继续用原任务速度。

回到图像环绕与源码

项目 orbit_logic.py 以面积误差产生 surge，以中心误差产生 yaw/vertical，加入主动 lateral 和与侧移相关的 yaw 前馈，再做变化率限制与水平预算。把它逐项标成“距离线索”“居中反馈”“主动环绕”“视线预测”四类，就能解释逻辑而不用把它包装成米制定位器。若面积只提供相对距离，前馈中的半径也只能用标定参考或估计值；半径错一倍，几何偏航前馈便错一倍，中心反馈必须修正剩余误差。

项目 CIRCLE 水平输出是归一化 forward/lateral，数值不等于 m/s；上面的速度矢量场是离线教学模型，不能未经速度反馈或已标定映射就直接放入归一化推力字段。控制分配与饱和决定可实现的水平预算，视觉制导负责提出与该预算相容的运动意图。

手写练习与答案

将切速改为负值，分别预测半径动力学、轨道方向和偏航前馈。
令目标向北以 $0.05$ m/s 移动，比较有无目标速度前馈。
只保持框中心、不给切速，是否能保证环绕？
距离不变但目标真实尺寸增大，面积反馈会采取什么动作？

参考解答（展开查看）

练习一：切速改为负数。半径误差动力学不变，而轨道方向、偏航前馈均反号。练习二：目标以 (0.05,0) m/s 平移，分别加入和删去目标速度前馈。答案：加入时相对动力学保持原式；删去时多一个目标运动引起的周期性径向扰动。练习三：只让中心误差归零而不给切速，答案是可能停在任意方位，无法保证环绕。练习四：固定中心与面积却让真实目标尺寸变大，图像策略会后退；这不是半径控制器的神秘失稳，而是面积距离假设已改变。

视觉环绕怎样落到项目接口

本课米制向量场先生成导航系速度，若实际通道使用机体系，则用当前姿态把它旋转为 FRD；随后才由速度闭环或已明确的执行映射产生 normalized forward/lateral。项目 orbit_logic.py 的面积、中心、侧移、前馈分别对应距离线索、视线误差、轨道推进、视线变化预测；它还在参考层处理变化率与水平预算。OrbitPublisher 再给这些数值附上有效期。遇到 CIRCLE 与 GUIDED 不同接口，必须连同物理量改变映射，不能只改模式名而保留同一组未经解释的数字。

TB04-S6 · Lekkas & Fossen：Line-of-Sight Guidance for Path Following of Marine Vehicles

作者上传的 2013 章节。用其路径切向、航迹和艏向区分作为几何背景；本课圆轨道向量场与收敛式由极坐标运动学独立推导。