08 · 深入｜交互矩阵、跟随与环绕

从像点导数推到控制律

对静止场景点，相机运动 ξ_C=[v_x,v_y,v_z,ω_x,ω_y,ω_z]ᵀ 使点的相对运动满足 Ṗ=−v−ω×P。对 x=X/Z、y=Y/Z 求导并收集速度项，就得到点特征交互矩阵：

$${\text{L}}_{\text{xy}}\text{=}\left[\begin{array}{cccccc}\frac{\text{−1}}{\text{Z}}& \text{0}& \frac{\text{x}}{\text{Z}}& \text{xy}& \text{−(1+}{\text{x}}^{\text{2}}\text{)}& \text{y}\\ \text{0}& \frac{\text{−1}}{\text{Z}}& \frac{\text{y}}{\text{Z}}& \text{1+}{\text{y}}^{\text{2}}& \text{−xy}& \text{−x}\end{array}\right]$$

$$\text{e = s−s*， ṡ = L\_s ξ\_C， ξ\_C = −λ L̂\_s† e}$$

当模型和可控特征条件合适时，这个速度让图像误差趋向减小。直觉检查：目标很远时 1/Z 很小，平移对像点影响弱，而转动影响依然明显。一个点只有两个标量约束；单凭中心位置不能决定六自由度运动，通常要使用多个特征或约束只控制部分自由度。

相机速度还要转到机体原点

相机系采用右、下、前，正向安装到 FRD 机体系时，R_BC=[[0,0,1],[1,0,0],[0,1,0]]。若相机离机体原点有杆臂 r_BC，则 v_B=R_BC v_C−ω_B×r_BC，ω_B=R_BCω_C。只旋转线速度而忽略参考点差异，会在转动时产生误差。

跟随消除误差，环绕还要创造切向运动

有目标中心 c 与机器人位置 p 时，水平面令 q=p−c、ρ=‖q‖、n=q/ρ，J 为平面旋转 90° 的矩阵，可设计：

$$\text{v\_d = −k\_r(ρ−R)n + v\_t Jn + v̂\_target}$$

径向项把距离拉回半径 R，切向项让机器人不停留而沿圆周运动，目标速度项补偿目标本身的移动。算例：ρ=2.4 m、R=2 m、k_r=0.5 s⁻¹，径向速度为向内 0.2 m/s；若切向速度 0.3 m/s，最终理想角速度约为 0.15 rad/s。艏向可以单独朝向目标，使相机持续可见。

多个目标不要靠相加争抢控制权

深度保持希望升沉为零，图像竖直居中可能又要求升沉；导航希望向前，视觉可能要求转向。先决定硬约束与优先级，再生成一个最终参考。可把任务写成带速度限制的加权最小二乘，或采用明确的模式切换；姿态、深度和可见性约束应与任务目标共同考虑。伪逆零空间可承载次任务，但只有主任务剩余的自由度可以使用。

小实验：只用一个像点同时控制六自由度，观察解的不唯一；再固定深度与横滚／俯仰，只控制横移、前进和偏航，理解任务降维的价值。学习交互矩阵时参考 Chaumette 的作者教学材料：https://www.gdr-robotique.org/cours_de_robotique/online/Chaumette_Visual-Servoing/Chaumette_Visual-Servoing.pdf 。