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

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


背景技术：

2.船舶的轨迹跟踪是指自动驾驶车辆或船只等载体从某一处出发，在规定的时间内通过轨迹跟踪系统的控制到达原本设定的位置上。高性能的轨迹跟踪算法是水面无人船所迫切需要的。
3.目前，基于预瞄点的轨迹跟踪控制方法被普遍应用，根据预瞄点计算跟踪方案。但预瞄点距离多为人工预设，将会带来一系列可能的问题，主要表现为大曲率弯道前后出现超前转向与滞后转向的现象、复杂道路中的跟踪精度不高、初始偏差大时控制效果差、与预设路径偏差不大时过于频繁进行调整等问题。


技术实现要素：

4.为了解决背景技术中存在的问题，本发明所提供一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，能根据无人船与路径的情况，自适应地决策控制方案，改变预瞄点距离大小，以充分利用路径信息提高跟踪精度。
5.本发明采用的技术方案是：
6.本发明方法包括如下步骤：
7.步骤100，预设无人船航行的初始路径；
8.步骤200，控制无人船按照初始路径航行，获取无人船的实时运动状态；
9.步骤300，在理想情况下，无人船应当尽可能跟随初始路径航行，在受到海面环境干扰的情况下，无人船会偏离初始路径航行；此时根据无人船的实时运动状态，判断无人船真实的航行路径是否偏离初始路径，若航行路径偏离初始路径，则获取初始路径上的目标点；
10.步骤400，根据目标点获取无人船的偏航距离和偏航角；
11.步骤500，根据无人船的实时运动状态、偏航距离和偏航角，校正无人船的实时运动状态。
12.所述的步骤100中，预设的无人船航行的初始路径即无人船的待跟踪路径，通常为一系列路径点或函数式构成的曲线路径，以初始路径中第一个路径点作为坐标原点，纬线为x轴，经线为y轴，初始路径的路径点表示为：
13.p(i)＝[px(i),py(i)]
[0014]
其中，p(i)为初始路径中第i个路径点，px(i)为第i个路径点的x轴坐标，py(i)为第i个路径点的y轴坐标。
[0015]
所述的步骤200中，无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t)，y(t)]，其中，x(t)为无人船在当前时刻t的x轴坐标，y(t)为无
人船在当前时刻t的y轴坐标。
[0016]
所述的步骤300中，判断无人船真实的航行路径是否偏离初始路径：
[0017]
若无人船真实的航行路径不偏离初始路径，则继续控制无人船按照初始路径航行；
[0018]
若无人船真实的航行路径偏离初始路径，即航行路径相对初始路径产生横向偏差，则进行以下判断：
[0019]
若无人船真实的航行路径是第一次偏离初始路径，则遍历初始路径中所有的路径点，向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点；
[0020]
若无人船真实的航行路径不是第一次偏离初始路径，则从无人船上一次偏离初始路径获取的目标点p(m)开始，遍历无人船前方的初始路径中间的所有路径点，获取距离无人船的当前位置最近的一个路径点作为当前的目标点，其中，p(m)为初始路径中第m个路径点，v(t)为当前无人船的航行速度，δt为由控制无人船航行时发出的控制信号的控制频率的单位时间，s为初始路径的路径点间的平均距离。
[0021]
所述的步骤400中，将目标点在初始路径上的切线作为投影线，获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离d，获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。
[0022]
所述的步骤500中，预设投影阈值λ和航向角阈值用于判断无人船真实的航行路径与预设路径的区别；
[0023]
航向角的调整：
[0024]
根据当前无人船的偏航距离d与偏航角α，进行以下判断，调整当前无人船的航向角：
[0025]
若d≤λ或时，则不对当前无人船的航向角进行调整；
[0026]
若d》λ且时，则对当前无人船的航向角进行调整，使得当前无人船的航向朝向目标点；
[0027]
航行速度的调整：
[0028]
根据当前无人船的偏航距离d与偏航角α，同时进行以下判断，调整当前无人船的航行速度：
[0029]
若d≤λ且时，或者d≤λ且时，则在当前无人船前方的初始路径中，从目标点开始，以相同距离d1取两个初始路径上的点作为预瞄点，d1＝v(t)*δt，v(t)为当前无人船的航行速度，δt为由控制无人船航行时发出的控制信号的控制频率的单位时间；并进行以下判断：
[0030]
若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均小于航向角阈值则控制当前无人船进行加速；
[0031]
若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均不小于航向角阈值或仅有其中一个航向角不小于航向角阈值则保持当前无人船的航行速度v(t)；
[0032]
若d》λ时，则在当前无人船前方的初始路径中，从目标点开始，按照以下公式以距
离dj取第j个预瞄点：
[0033][0034]
其中，κj为第j个预瞄点在初始路径中的曲率，j＝1、2、3；
[0035]
共取三个预瞄点p1、p2和p3，计算预瞄点距离d，d＝d1 d2 d3；记当前目标点为p0，计算无人船的航向角分别与向量p0p1、p0p2和p0p3的夹角α1、α2和α3，再按照以下公式计算当前无人船的航向转角为：
[0036][0037]
其中，t0为预设跟随时间常数，根据发出控制信号的控制设备的实际情况调整；
[0038]
根据航向转角进行以下判断：
[0039]
若时，则控制当前无人船进行减速，航向转角越大减速的幅度越大；
[0040]
若时，则保持当前无人船的航行速度v(t)。
[0041]
本发明的有益效果是：
[0042]
1、跟随预瞄点的轨迹控制方式可以兼顾航向误差和轨迹误差，相比于双闭环控制舵向误差和轨迹误差，具备更好地轨迹流畅性和舵向稳定性。
[0043]
2、不同速度和不同轨迹曲率下预瞄点的位置可以自适应调节以兼顾轨迹跟踪精度和航行稳定性。
附图说明
[0044]
图1为自适应预瞄点和投影定位的轨迹跟踪方法流程图；
[0045]
图2为本发明投影长度示意图。
具体实施方式
[0046]
下面结合附图及具体实施例对本发明作进一步详细说明。
[0047]
如图1所示，本发明方法包括如下步骤：
[0048]
步骤100，预设无人船航行的初始路径。
[0049]
步骤100中，预设的无人船航行的初始路径即无人船的待跟踪路径，通常为一系列路径点或函数式构成的曲线路径，如图2所示，以初始路径中第一个路径点作为坐标原点，纬线为x轴，经线为y轴，初始路径的路径点表示为：
[0050]
p(i)＝[px(i),py(i)]
[0051]
其中，p(i)为初始路径中第i个路径点，px(i)为第i个路径点的x轴坐标，py(i)为第i个路径点的y轴坐标。
[0052]
步骤200，控制无人船按照初始路径航行，获取无人船的实时运动状态。
[0053]
步骤200中，无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t)，y(t)]，其中，x(t)为无人船在当前时刻t的x轴坐标，y(t)为无人船在当前时刻t的y轴坐标。
[0054]
步骤300，在理想情况下，无人船应当尽可能跟随初始路径航行，在受到海面环境干扰的情况下，无人船会偏离初始路径航行；此时根据无人船的实时运动状态，判断无人船真实的航行路径是否偏离初始路径，步骤300中，判断无人船真实的航行路径是否偏离初始路径：
[0055]
若无人船真实的航行路径不偏离初始路径，则继续控制无人船按照初始路径航行；
[0056]
若无人船真实的航行路径偏离初始路径，即航行路径相对初始路径产生横向偏差，则进行以下判断：
[0057]
若无人船真实的航行路径是第一次偏离初始路径，则遍历初始路径中所有的路径点，向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点；
[0058]
若无人船真实的航行路径不是第一次偏离初始路径，则从无人船上一次偏离初始路径获取的目标点p(m)开始，遍历无人船前方的初始路径中间的所有路径点，获取距离无人船的当前位置最近的一个路径点作为当前的目标点，其中，p(m)为初始路径中第m个路径点，v(t)为当前无人船的航行速度，δt为由控制无人船航行时发出的控制信号的控制频率的单位时间，s为初始路径的路径点间的平均距离。
[0059]
若航行路径偏离初始路径，则获取初始路径上的目标点。
[0060]
步骤400，根据目标点获取无人船的偏航距离和偏航角。
[0061]
如图2所示，步骤400中，将目标点在初始路径上的切线作为投影线，获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离d，获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。
[0062]
步骤500，根据无人船的实时运动状态、偏航距离和偏航角，校正无人船的实时运动状态。
[0063]
步骤500中，预设投影阈值λ和航向角阈值用于判断无人船真实的航行路径与预设路径的区别；
[0064]
航向角的调整：
[0065]
根据当前无人船的偏航距离d与偏航角α，进行以下判断，调整当前无人船的航向角：
[0066]
若d≤λ或时，则不对当前无人船的航向角进行调整；
[0067]
若d》λ且时，则对当前无人船的航向角进行调整，使得当前无人船的航向朝向目标点；
[0068]
航行速度的调整：
[0069]
根据当前无人船的偏航距离d与偏航角α，同时进行以下判断，调整当前无人船的航行速度：
[0070]
若d≤λ且时，或者d≤λ且时，则在当前无人船前方的初始路径中，从目标点开始，以相同距离d1取两个初始路径上的点作为预瞄点，d1＝v(t)*δt，v(t)为当前无人船的航行速度，δt为由控制无人船航行时发出的控制信号的控制频率的单位时间；并
进行以下判断：
[0071]
若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均小于航向角阈值则控制当前无人船进行加速；
[0072]
若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均不小于航向角阈值或仅有其中一个航向角不小于航向角阈值则保持当前无人船的航行速度v(t)；
[0073]
若d》λ时，则在当前无人船前方的初始路径中，从目标点开始，按照以下公式以距离dj取第j个预瞄点：
[0074][0075]
其中，kj为第j个预瞄点在初始路径中的曲率，j＝1、2、3；
[0076]
共取三个预瞄点p1、p2和p3，计算预瞄点距离d，d＝d1 d2 d3；记当前目标点为p0，计算无人船的航向角分别与向量p0p1、p0p2和p0p3的夹角α1、α2和α3，再按照以下公式计算当前无人船的航向转角为：
[0077][0078]
其中，t0为预设跟随时间常数，根据发出控制信号的控制设备的实际情况调整；
[0079]
根据航向转角进行以下判断：
[0080]
若时，则控制当前无人船进行减速，航向转角越大减速的幅度越大；
[0081]
若时，则保持当前无人船的航行速度v(t)。