第 45 卷第 3 期 2018 年 6 月 应 用 科 技 Applied Science and Technology Vol.45 No.3 Jun. 2018 DOI: 10.11991/yykj.201706005 网络出版地址：http://kns.cnki.net/kcms/detail/23.1191.U.20170727.0716.002.html 基于LOS法的自航模航迹跟踪控制算法实现 韩鹏1，刘志林1，周泽才2，唐昊1，班良1，郝留磊2 1. 哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001 2. 哈尔滨工程大学 船舶工程学院，黑龙江 哈尔滨 150001 摘 要：为解决实验室环境下的小型自航模能快速、准确、稳定地实现航迹跟踪问题，利用视线法(line of sight, LOS)将自航 模的位置跟踪问题转化为自航模的航向控制问题，通过PID控制器，使自航模的航向角收敛于期望航向角，从而使自航模 不断驶向期望航向点。通过设置不同的期望航向点，实现自航模航迹跟踪控制。之后通过MATLAB对其进行仿真研究， 验证了其可行性。最后将其用于实验室环境下的自航模航迹跟踪，得到了良好的效果，进一步验证了其工程实用性。 关键词：自航模；航迹跟踪；航向控制；视线法；MATLAB仿真；PID控制器；期望航向点 中图分类号：TP273 文献标志码：A 文章编号：1009−671X(2018)03−0066−05 Path tracking control algorithm based on LOS method for surface self- propulsion vessel HAN Peng1, LIU Zhilin1, ZHOU Zecai2, TANG Hao1, BAN Liang1, HAO Liulei2 1. College of Automation, Harbin Engineering University, Harbin 150001, China 2. College of Shipbuilding Engineering, Harbin Engineering University, Harbin 150001, China Abstract: Aiming at the small self-propelled model in the laboratory environment, the paper discussed how to realize the path tracking quickly, accurately and stably. The problem of path tracking of the self-propelled model was transformed into the heading control problem of the self-propelled model by the Line of Sight algorithm. Via the PID controller, the heading angle of the self-propelled model converged to the desired course angle, so that the self-propelled model continuously headed towards the desired course point. By setting different expected heading points, the path tracking control of the self-propelled model was realized. Subsequently, through the MATLAB simulation, the feasibility was verified. Finally, the algorithm was used for the path tracking of the self-propelled flight in the laboratory environment, a good effect was obtained and the engineering practicability was further verified. Keywords: surface self-propulsion vessel; path tracking; heading control; LOS algorithm; MATLAB simulation; PID controller; expected heading point 船舶的航迹跟踪控制，是近年来船舶自动控 制领域涌现出来的一个热门课题。目前，国外对 无人艇航迹跟踪的研究中有Do等[1-3]根据反步法 (backstepping)以 及 输 出 重 定 义 等 方 法 得 到 了 全 局、一致并以指数趋近稳定的反馈控制律和船舶 的航迹状态；国内对于船舶的自主航迹跟踪研究 也较多，具有代表性有李铁山等[4]利用自适应积 分反演技术、耗散理论以及输入输出线性化等方 收稿日期：2017−06−07. 网络出版日期：2017−07−27. 基金项目：国家自然科学基金项目(51379044). 作者简介：韩鹏(1997−)，男，本科生； 刘志林(1977−)，男，副教授，博士. 通信作者：韩鹏，E-mail：1210161598@qq.com. 法对船舶的直线航迹跟踪控制系统设计了非线性 控制器。这类控制器一般对被控模型的参数有较 高的要求，而在实际的控制应用中，很难准确地 辨识出模型的各项参数，导致这一方法在很多应 用中无法具体实施。 本文采用的视线(line-of-sight, LOS)算法是自 航模航迹跟踪控制的理论基础，该算法通过简单 的推导得出自航模跟踪期望航向点所需要的航向 角，结合PID控制器，消除航向角偏差，使自航模 的当前航向角收敛于期望航向角，以便自航模能 够航行在期望航迹线上并且不断向目标航向点靠 近，最终行驶到目标航向点，完成对目标点的跟 踪控制[5]。 

第 3 期 韩鹏，等：基于 LOS 法的自航模航迹跟踪控制算法实现 ·67· LOS算法最大的优点在于其不依赖于被控对 象的模型，即可以在模型参数不确定的情况下， 或者外界扰动对船模影响较大的环境中设计控制 器，完成对目标模型的控制。其次，LOS算法设 计简便，抗干扰能力强，控制效果出色也是其被 使用的主要因素。因此，对LOS算法的进一步研 究和改进对无人艇航迹跟踪控制技术的应用和发 展具有重要意义。 1 LOS算法介绍 视线法的导航原理体现在它对舵手操舵和船 舶运动的直观理解上[6-8]。该原理认为：如果使被 控船舶的航向保持对准视线角(LOS角)，那么经 过 适 当 的 控 制 ， 就 能 使 被 控 船 舶 到 达 期 望 的 位 置，达到航迹跟踪的效果[8]。而且LOS算法能将 传统的控制量从3个自由度的船舶位置 和航 向角 减少到2个自由度的船舶航向角 和航行 速度 ，这种特性对于欠驱动船舶的控制尤为重要。 1.1 航向角的求取 由于LOS理论认为只要使被控船舶的航向 保 持 对准LOS角 ， 被 控 船 舶 就 能 达 到 期 望 的 位 置 ， 所 以 如 何 获得LOS角 便 成 了 研 究 的 重 点 所 在。如图1所示，假设航迹跟踪当前的期望位置 是 被控船舶的当前位置记为 ， ，并假设 和 可以进行测量[9]。则LOS角可以通过下面公式 ，上一次的期望位置是 计算得到： 式中 ，为LOS角。 算法通过引入一个可视距离 来协助完成，原理如 图2所示[8]。 图2 LOS航迹跟踪导航方式示意 由图2可知，期望航向点 与被 控 船 舶 当 前 位 置 在 期 望 航 迹 上 的 投 影 点 相 距 。 为 船 舶 长 度 ，Δ称 为 被 控船舶的可视距离。此时，LOS控制算法可以写成： ， 这 里 式中： 表示被控船舶的横向跟随误差， 是大地 坐标系正北方向与期望航迹线 的夹角。本 文假设航向角偏差 ，当 趋于零的时候， 自 航 模 即 向 目 标 航 向 点 行 驶 ， 最 终 到 达 期 望 航 迹，完成路径跟踪。最后通过简单的PID控制，可 以有效地使 趋向于零，当前航向角 便跟踪上了 期望航向角 ，通过连续的控制，自航模可以沿期 望航迹行驶。 船 舶 经 控 制 算 法 跟 踪 当 前 一 个 期 望 航 向 点 ，当船舶进入该期望航向点一定范 围时，则需要自动放弃跟踪该期望航向点，而转 入跟踪下一个期望航向点，即令 。这样就 需要一个控制算法来对期望航向点进行判别和切 换，本文采用如下算法。 假设存在一个以船舶所跟踪的当前期望航向 为圆心，以 为半径的圆，若某一时刻， 点 被控船舶的当前位置 满足如下条件： 图1 LOS航迹跟踪控制算法原理 由于LOS法控制原理易于理解、控制效果较 其他控制方法更为优越、适用性强，所以LOS法 在船舶的航迹跟踪控制中得到了广泛的应用。下 面 将 分 别对LOS法的2种 导 航 方 式 作 出 分 析 。 1.2 LOS法导航方式分析 LOS控制算法可以用于直线航迹跟踪和曲线 航迹跟踪，是一种较好的航迹跟踪控制算法。该 则需要将跟踪的当前期望航向点转换成下一个期 望航向点。在对 进行计算时也要切换到下一 个去期望航向点，利用下一个期望航向点的数据 进行计算。 1.3 航向角映射原理 上述LOS航迹跟踪控制算法在理论上是非 常 有 效 的 。 但 是 由 于 这 种 算 法 的 控 制 区 间 在 上 ， 使 得 其 在 和 这 两 个 节 点 是 不 连 续 ，当船舶航向角继 ，这时算 的。如船舶当前航向角 续顺时针变化，那么下一刻很可能 (x;y) vPk+1=[xk+1;yk+1]TPk=[xk;yk]TP=[x;y]TPk+1Pk LOS=arctan(yk+1yxk+1x) LOS2⟨;⟩R YXψPk+1Pk+2PkΨLOS∆东eUxΔ北(xLOS, yLOS)Pk+1αkαfβΨαxb(x, y)PLOS=[xLOS;yLOS]T∆=nLppn=25Lppϕ=k+arctan(e∆)ekPkPk+1 e=ϕ e e ϕPk+1=[xk+1;yk+1]Tk=k+1(xk+1;yk+1)R0P=[x;y]T(xk+1x)2+(yk+1y)2⩽R20 LOS⟨;⟩ =179◦ =179◦

·68· 应 用 科 技 第 45 卷 法将会认为船舶做了急速的旋转，从而得到不可 预料的结果。这种情况的出现在船舶进行航迹跟 踪控制时是非常不利的，它为控制器的设计带来 困难并可能出现在船舶跟踪期望航向点时，控制 器发生误命令，使得船舶向相反方向绕大圈来跟 踪期望航向点，这些都会给实际船舶控制时带来 极大的影响[10]。 为解决这一问题，我们需要对问题的原因作 进 一 步 分 析 。 船 舶 的 控 制 器 与 航 向 角 偏 差 有 关，通过分析发现：当 时，控制器将发出 正 确 的 指 令 指 导 船 模 跟 踪 目 标 航 向 点 ； 当 时，控制器将发出相反的指令从而船模 实际航向角可能会从反方向向期望航向角收敛。 到 所 以 我 们 只 要 一 个 使 的映射即可解决上述问题。具体映射 方法参照图3[12]。 角，该舵角会控制自航模偏向期望航向点航行， 此时自航模的当前航向角就趋近于自航模期望航 向角， 减小。通过不断控制，只要 存在，那么 便会出现一个控制舵角使 减小，直到 趋于零， 即自航模达到了期望航迹线[13-14]。 3 MATLAB仿真结果及分析 本次MATLAB仿 真 试 验 中 ， 主 要为2个 转 角，第一个是90°的小转角，第二个是 90°的较 大 转 角 。 仿 真 航 迹 图 如图5所 示 ， 自 航 模 从 点 (1 800，1 200)处开始跟踪期望航迹。期间航向角 速度、航向角及控制舵角变化曲线分别如图6~8 所示。 图3 航向角偏差的映射说明 2 基于视线法的PID控制器设计 为了使基于LOS控制算法获得的航向角偏 差 趋近于零，即使得自航模当前航向角跟踪上 期望航向角，本文需要采用一种控制方法来控制 ，使之趋近于零，实现航迹跟踪目的。PID控制 图如图4所示。 图4 PID控制示意图 从图4中可以看出，通过设定合理的PID控 制器的参数，便可得到稳定收敛的控制规律，使 趋于零[14]。PID控制器的控制规律为 由于舵角 与当前航向角 成比例变化，航向 角偏差为 由式(1)、(2)可以看出，如果某一时刻，系统 存在一个较大的 ，经PID控制便会产生一个舵 图5 期望航迹与仿真航迹比较 图6 航向角速度变化曲线 图7 航向角变化曲线 e e2(;) e<(;) e2(2;)[(;2) e2(;)ψe ∈<−2π, −π>ψe=ψe+2πψe=ψe−2πψe= ψeψeψe ∈<π, 2π>ψe ∈<−π, π>ψe e e自航模(xk, yk)LOS算法器ψLOSPID控制器ψeδψnowψnow(x, y)ψe=ψLOS−ψnow e=k1 e+k2 e+k3wt0 edt(1) e= LOS(2) e e e e e e<1.61.82.02.22.42.62.83.03.20.20.40.60.81.01.21.41.6×103Y/m期望航迹仿真航迹X/m050100150200250300−0.0200.020.040.060.080.100.12航向角速度/((°)·s−1)t/s050100150200250300t/s050100150200250300航向角/(°) 

第 3 期 韩鹏，等：基于 LOS 法的自航模航迹跟踪控制算法实现 ·69· 图8 控制舵角变化曲线 从图7可以看出，自航模的期望航向角共进 行了3次变化，第1次和第3次变化时引起少许 超调，而第2次期望航向角变化使得实际航向角 的变化出现较大延迟，这与图5中的航迹变化是 一致的。总体上看，无论 90°还是 90°，实 际航向角均可收敛于期望航向角，因此，LOS算 法可以控制船舶实现不同情况下的路径点跟踪。 4 自航模实验 4.1 自航模系统 本文采用的自航模系统是一个小型的自航模 实船实验平台，利用此平台可以完成一些基本的 自航模实船仿真实验。本实验平台由小型可充气 式水池、自航模、单目视觉摄像头、上位机4部分 组成。 图9 是实验室环境下自航模实船试验仿真平 台中的可充气水池以及小型自航模的实物图。小 型自航模长约为66 cm，宽25 cm，高18 cm，最小 回转半径约为0.6 m，为减少小船横摇，船体整体 为扁平状。水池采用中型充气气垫水池，规格为 350 cm×350 cm。 图10 自航模实物仿真平台结构 4.2 自航模实验分析 在 实 验 过 程 中 ， 保 持 自 航 模 的 航 速 为U = 0.289 1 m/s。在图5所示的航迹图中选取部分点 位，其具体实验数据如表1所示。 表1 自航模航迹跟踪实验数据 点位编号 自航模实时位置 x 坐标/m y 坐标/m 航迹偏差 e/m 航向角偏差 ψ/(°) 偏差数据 1 2 3 4 0.44 0.50 1.45 2.00 1.00 1.40 1.50 0.89 0.17 0.18 0.32 0.12 23.98 19.00 12.34 0 图11是基于LOS算法的自航模曲线航迹跟 踪实验结果。该试验中自航模所要跟踪的航迹线 为锐角，可以将这条曲线航迹看成是2条直线航 迹。如图11所示，自航模首先跟踪第1个期望航 向点(0.8，1.5)，从图中可以看出，此阶段跟踪效果 较好。当LOS法判断为继续跟踪下一个期望航 向点时，自航模迅速转舵，但由于自航模本身的 惯性，此时出现一定的超调，随着LOS算法以及 PID控制器的迅速调节，自航模最终快速的跟踪 期望航迹，并达到第2个路径点(2.3, 0.5)，实现航 迹跟踪控制。 图9 自航模航迹跟踪平台仿真水池 图10是本实物仿真平台的构架图。利用本 平台，可以对不同的航迹跟踪控制算法进行实际 验证，达到实物仿真的目的。本试验中的期望航 迹通过不连续的一组有序的路径点组成。2个相 邻路径点之间可得到一条线段，所有路径点所得 到线段相接，即得到我们的期望路径。 图11 基于LOS算法的曲线航迹跟踪实际航迹 5 结论 1) 本文首先通过MATLAB仿真验证LOS法 的可行性，仿真结果表明，将LOS法应用于小型 050100150200250300−4−202468 舵角/(°)t/s e< e>直流电机航模控制系统盒直流电源电机驱动艇艏艇身微处理器，姿态传感器，串口扩展，电平转换电路板电机驱动器开关电源无线数传模块连接线LED指示灯充气水池工控机无线数传DA板摄像头吊舱式矢量推进2.01.51.00.5000.51.01.5X/mY/m2.02.53.0

·70· 应 用 科 技 第 45 卷 自航模航迹跟踪时有良好的控制效果。 2) 在小转角航迹跟踪过程中，跟踪效果良好， 而大转角跟踪过程初期，会出现的较小的超调， 经 过 控 制 算 法 的 调 节 ， 最 终 实 现 航 迹 跟 踪 的 效 果。在转角过大的情况下，对于航迹跟踪点进行 切换时出现的超调，可能使小船完全偏离航向， 由于控制器的限制，目前本文并未得到有效减少 超调的方法。 3) 在自航模航迹跟踪控制实验中，小船可稳 定快速地实现预定航迹的跟踪，表明LOS法可有 效应用于实际的水面无人艇控制。 与目前普遍采用的仿真研究方式不同，本文 在基于LOS法设计的控制器在MATLAB理论验 证的前提下，又利用实验室自航模实物仿真平台 做了实船试验。结果表明，该算法能够应用于实 际的小型自航模的航迹跟踪，具有一定的工程实 用性。 对于本文的后续研究，可以从控制器的改进 方面入手，可选用较为复杂的滑模控制、自适应 PID等先进控制理论，结合LOS法，得到更好的 实际控制效果。 参考文献： [1] [2] 金平仲. 船舶喷水推进[M]. 北京: 国防工业出版社, 1986: 41–136. DO K D, PAN J. Global waypoint tracking control of underactuated ships relaxed assumptions[C] //Proceedings of the 42nd Conference on Decision and Control. Maui, USA, 2003: 1244−1249. DO K D, JIANG Z P, PAN J. Robust global stabilization of under [3] 本文引用格式： [4] [5] [6] [7] [8] [9] underactuated ships on a linear course: state and output feedback[J]. International journal of control, 2003, 76(1): 1–17. 李铁山. 船舶直线航迹非线性设计方法[D]. 大连: 大连海 事大学, 2005. 于瑞亭. 欠驱动水面船舶的全局镇定控制方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2012. PARK W G, YUN H S, CHUN H H, et al. Numerical flow simulation of flush type intake duct of waterjet[J]. Ocean engineering, 2005, 32(17/18): 2107–2120. PARK W G, JANG J H, CHUN H H, et al. Numerical flow and performance analysis of waterjet propulsion system[J]. Ocean engineering, 2005, 32(14/15): 1740–1761. FOSSEN T I, BREIVIK M, SKJETNE R. Line-of-Sight Path following of underactuated marine craft[J]. IFAC proceedings volumes, 2003, 36(21): 211–216. 班良. 小型自航模控制系统设计与实现[D]. 哈尔滨: 哈尔 滨工程大学, 2014. HARVALD S A. Resistance and propulsion of Ships[M]. New York: John Wiley & Sons, 1983: 57-599. 董飞垚, 雷虎民, 周池军, 等. 导弹鲁棒高阶滑模制导控 制一体化研究[J]. 航空学报, 2013, 25(9): 2212–2218. 杨震, 王岩, 刘志林. 一种欠驱动船舶编队滑模鲁棒控制 方法[J]. 电机与控制学报, 2014, 18(11): 90–96, 105. 朱齐丹, 于瑞亭, 夏桂华, 等. 风浪流干扰及参数不确定 欠驱动船舶航迹跟踪的滑模鲁棒控制[J]. 控制理论与 应用, 2012, 29(7): 959–964. 金小婷, 杜佳璐, 汤占军. 基于Backstepping的非线性船 舶 航 迹 跟 踪 控 制 器 设 计[J]. 科 学 技 术 与 工 程, 2012, 12(13): 3146–3149, 3154. 胡寿松. 自动控制原理简明教程[M]. 2版. 北京: 科学出 版社, 2008. [10] [11] [12] [13] [14] [15] 韩鹏, 刘志林, 周泽才, 等. 基于LOS法的自航模航迹跟踪控制算法实现[J]. 应用科技, 2018, 45(3): 66–70. HAN Peng, LIU Zhilin, ZHOU Zecai, et al. Path tracking control algorithm based on LOS method for surface self-propulsion vessel[J]. Applied science and technology, 2018, 45(3): 66–70.