[发明专利]一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法

说明书

本发明公开了一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法。方法步骤包括：预设无人船航行的初始路径；控制无人船按照初始路径航行，获取无人船的实时运动状态；根据无人船的实时运动状态，判断无人船真实的航行路径是否偏离初始路径，若航行路径偏离初始路径，则获取初始路径上的目标点；根据目标点获取无人船的偏航距离和偏航角；根据无人船的实时运动状态、偏航距离和偏航角，校正无人船的实时运动状态。本发明方法可以兼顾航向误差和轨迹误差，相比于双闭环控制舵向误差和轨迹误差，具备更好地轨迹流畅性和舵向稳定性；在不同速度和不同轨迹曲率下预瞄点的位置可以自适应调节以兼顾轨迹跟踪精度和航行稳定性。

技术领域

本发明涉及了一种轨迹跟踪方法，具体涉及一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法。

背景技术

船舶的轨迹跟踪是指自动驾驶车辆或船只等载体从某一处出发，在规定的时间内通过轨迹跟踪系统的控制到达原本设定的位置上。高性能的轨迹跟踪算法是水面无人船所迫切需要的。

目前，基于预瞄点的轨迹跟踪控制方法被普遍应用，根据预瞄点计算跟踪方案。但预瞄点距离多为人工预设，将会带来一系列可能的问题，主要表现为大曲率弯道前后出现超前转向与滞后转向的现象、复杂道路中的跟踪精度不高、初始偏差大时控制效果差、与预设路径偏差不大时过于频繁进行调整等问题。

发明内容

为了解决背景技术中存在的问题，本发明所提供一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，能根据无人船与路径的情况，自适应地决策控制方案，改变预瞄点距离大小，以充分利用路径信息提高跟踪精度。

本发明采用的技术方案是：

本发明方法包括如下步骤：

步骤100，预设无人船航行的初始路径；

步骤200，控制无人船按照初始路径航行，获取无人船的实时运动状态；

步骤300，在理想情况下，无人船应当尽可能跟随初始路径航行，在受到海面环境干扰的情况下，无人船会偏离初始路径航行；此时根据无人船的实时运动状态，判断无人船真实的航行路径是否偏离初始路径，若航行路径偏离初始路径，则获取初始路径上的目标点；

步骤400，根据目标点获取无人船的偏航距离和偏航角；

步骤500，根据无人船的实时运动状态、偏航距离和偏航角，校正无人船的实时运动状态。

所述的步骤100中，预设的无人船航行的初始路径即无人船的待跟踪路径，通常为一系列路径点或函数式构成的曲线路径，以初始路径中第一个路径点作为坐标原点，纬线为x轴，经线为y轴，初始路径的路径点表示为：

P(i)＝[px(i),py(i)]

其中，P(i)为初始路径中第i个路径点，px(i)为第i个路径点的x轴坐标，py(i)为第i个路径点的y轴坐标。

所述的步骤200中，无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t)，y(t)]，其中，x(t)为无人船在当前时刻t的x轴坐标，y(t)为无人船在当前时刻t的y轴坐标。

所述的步骤300中，判断无人船真实的航行路径是否偏离初始路径：

若无人船真实的航行路径不偏离初始路径，则继续控制无人船按照初始路径航行；

若无人船真实的航行路径偏离初始路径，即航行路径相对初始路径产生横向偏差，则进行以下判断：

若无人船真实的航行路径是第一次偏离初始路径，则遍历初始路径中所有的路径点，向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点；