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

权利要求书

1.一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，其特征在于：

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

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

步骤300，根据无人船的实时运动状态，判断无人船真实的航行路径是否偏离初始路径，若航行路径偏离初始路径，则获取初始路径上的目标点；

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

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

2.根据权利要求1所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，其特征在于：

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

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

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

3.根据权利要求2所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，其特征在于：

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

4.根据权利要求2所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，其特征在于：

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

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

若无人船真实的航行路径偏离初始路径，则进行以下判断：

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

若无人船真实的航行路径不是第一次偏离初始路径，则从无人船上一次偏离初始路径获取的目标点P(m)开始，遍历无人船前方的初始路径中间的所有路径点，获取距离无人船的当前位置最近的一个路径点作为当前的目标点，其中，P(m)为初始路径中第m个路径点，v(t)为当前无人船的航行速度，Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间，s为初始路径的路径点间的平均距离。

5.根据权利要求3所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法，其特征在于：

所述的步骤400中，将目标点在初始路径上的切线作为投影线，获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离D，获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。

6.根据权利要求1所述的一种基于自适应预瞄点和投影位的无人船轨迹跟踪方法，其特征在于：

所述的步骤500中，预设投影阈值λ和航向角阈值

航向角的调整：

根据当前无人船的偏航距离D与偏航角α，进行以下判断，调整当前无人船的航向角：

若D≤λ或时，则不对当前无人船的航向角进行调整；

若Dλ且时，则对当前无人船的航向角进行调整，使得当前无人船的航向朝向目标点；

航行速度的调整：

根据当前无人船的偏航距离D与偏航角α，同时进行以下判断，调整当前无人船的航行速度：

若D≤λ且时，或者D≤λ且时，则在当前无人船前方的初始路径中，从目标点开始，以相同距离d₁取两个初始路径上的点作为预瞄点，d₁＝v(t)*Δt，v(t)为当前无人船的航行速度，Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间；并进行以下判断：

若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均小于航向角阈值则控制当前无人船进行加速；

若无人船从当前位置沿直线航行到该两个预瞄点时的航向角均不小于航向角阈值或仅有其中一个航向角不小于航向角阈值则保持当前无人船的航行速度v(t)；

若Dλ时，则在当前无人船前方的初始路径中，从目标点开始，按照以下公式以距离d_j取第j个预瞄点：

其中，κ_j为第j个预瞄点在初始路径中的曲率，j＝1、2、3；

共取三个预瞄点P1、P2和P3，计算预瞄点距离d，d＝d₁+d₂+d₃；记当前目标点为P0，计算无人船的航向角分别与向量P0P1、P0P2和P0P3的夹角α1、α2和α3，再按照以下公式计算当前无人船的航向转角为：

其中，T₀为预设跟随时间常数；

根据航向转角进行以下判断：

若时，则控制当前无人船进行减速；

若时，则保持当前无人船的航行速度v(t)。