无人船 | 图解基于视线引导(LOS)的无人艇制导算法

1 视线引导法介绍

视线引导法（Line of Sight, LOS）作为无人水面艇（USV）自主导航领域的核心技术，通过几何制导与动态控制深度融合的机制，为复杂海洋环境下的高精度路径跟踪提供了鲁棒性解决方案。其核心原理建立在运动学几何投影与闭环反馈控制的双重架构上：系统实时计算无人船当前位置与预设航路点之间的视线角，通过航向控制器动态调整船体艏摇角使其精确对准该视线方向，同时配合自适应航速策略，形成兼具轨迹跟踪精度与运动稳定性的制导体系。

为实现视线角的精确跟踪，LOS制导与航向控制器形成级联控制架构：LOS制导系统首先根据全局路径规划生成的航路点序列构建虚拟参考路径，在每个控制周期内选取距离船体最近的路径点作为当前跟踪目标，通过几何投影建立船体坐标系与惯性坐标系间的空间映射关系，计算得到视线角${\psi }_{los}$，从而生成期望航向指令${\psi }_d$，内环控制层则基于航向偏差$e_{\psi }\bar{p}s{i}_d-\psi$，通过控制算法输出转艏力矩τ_r，驱动推进系统调整船体方向，如下图所示

2 LOS制导原理推导

如图所示为LOS制导几何关系示意图

设上一个路点为${\mathbf{p}}_i$，下一个路点为${\mathbf{p}}_{i+1}$，USV当前位置为$\mathbf{p}={\left[\begin{array}{cc}x& y\end{array}\right]}^T$。以$\mathbf{p}$为圆心作半径为$R_k$的圆交直线段${\mathbf{p}}_i{\mathbf{p}}_{i+1}$于两点，距离下一个路点${\mathbf{p}}_{i+1}$较近的交点为当前跟踪的期望路点${\mathbf{p}}_{\mathrm{los}}$。定义前视距离$\Delta$为USV在当前段路径投影点与LOS制导点${\mathbf{p}}_{\mathrm{los}}$的前向距离，在静态LOS制导律中，$\Delta$或$R_k$为定值(已知$\Delta$或$R_k$中的一项可以求出另一项)，一般设为USV长度$L$的$n$倍，即

$$\Delta =nL$$

由几何关系可得横纵向跟踪误差

$$\{\begin{array}{l}{x}_e=\left(x-x_i\right)\cos {\alpha }_i+\left(y-y_i\right)\sin {\alpha }_i\\ y_e=-\left(x-x_i\right)\sin {\alpha }_i+\left(y-y_i\right)\cos {\alpha }_i\end{array}$$

其中${\alpha }_i=\mathrm{arc}\tan \left(\left(y_{i+1}-y_i\right)/\left(x_{i+1}-x_i\right)\right)$。在USV运动过程中，受到风浪或自身驱动等因素影响将产生横向速度$v$，使船艏不一定沿着航向行驶，艏向和航向的差角称为漂角

$$\beta =\mathrm{arc}\tan \left(v,u\right)$$

则USV当前航向角为$\chi =\psi +\beta$，期望的航向角${\chi }_d={\alpha }_i-\mathrm{arc}\tan \left(y_e/\Delta \right)$，令$\chi ={\chi }_d$可得

$${\psi }_d={\alpha }_i-\mathrm{arc}\tan \left(y_e/\Delta \right)-\beta$$

即控制USV艏向为${\psi }_d$时可使其沿着期望航向运动

3 Lyapunov稳定性分析

下来验证当采用$\psi ={\psi }_d$制导时可使横向误差收敛为零。求横向误差的一阶导数

$${\dot{y}}_e=-\dot{x}\sin {\alpha }_i+\dot{y}\cos {\alpha }_i$$

根据运动学方程得到

$${\dot{y}}_e=-\left(u\cos \psi -v\sin \psi \right)\sin {\alpha }_i+\left(u\sin \psi +v\cos \psi \right)\cos {\alpha }_i$$

根据辅助角公式展开

$${\dot{y}}_e=\sqrt{u^2+v^2}\sin \left(\psi +\beta -{\alpha }_i\right)$$

代入LOS制导律

$$\begin{array}{rl}{\dot{y}}_e& =\sqrt{u^2+v^2}\sin \left(-\mathrm{arc}\tan \left(y_e/\Delta \right)\right)\\ & =-\sqrt{u^2+v^2}\frac{y_e}{\sqrt{{\Delta }^2+y_e^2}}\end{array}$$

设Lyapunov函数

$$V=\frac{y_e^2}{2}$$

则

$$\dot{V}=-\sqrt{u^2+v^2}\frac{y_e^2}{\sqrt{{\Delta }^2+y_e^2}}⩽0$$

根据Lyapunov第二判据可知横向误差在LOS制导律下渐进稳定。

4 LOS制导效果

在下面的演示中，红色的是全局路径点，绿色的是LOS制导生成的模拟轨迹

直行场景

绕行场景

掉头场景

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇