基于天牛须搜索优化的无人帆船路径跟踪制导方法

(ψd)表示避碰代价函数；表示随速度变化的危险度函数；表示第i个障碍物的速度；表示第i个障碍物的最小会遇距离；
16.步骤22：构建路径跟踪代价函数；
17.路径跟踪代价函数需要分情况确定，当帆船位于参考路径右侧时，构造代价函数如下式所示：
[0018][0019]
式中：f2(ψd)表示路径跟踪代价函数；k2表示一个正的常数；ψ
pf
表示路径跟踪的参考航向角；β表示帆船到参考路径的垂线距离与帆船和目标点间距离的夹角；g2表示横向误差影响函数；ye表示横向误差；
[0020]
当帆船位于参考路径左侧时，构造代价函数如下式所示：
[0021][0022]
步骤23：构建速度损失代价函数；
[0023]
为保证无人帆船前进速度的准确性，构造速度损失的代价函数如下式所示：
[0024][0025]
式中：u1表示帆船速度；表示帆船速度u1的最大值；k3表示调优参数；f3(ψd)表示速度损失代价函数；
[0026]
步骤24：构建航程损失代价函数；
[0027]
如果当前计算的时间为t，上一时刻为t-1，则构造的航程损失代价函数为：
[0028]
f4(ψd)＝k4|ψd(t)-ψ(t-1)|
[0029]
式中：k4为一个正的调优参数；f4(ψd)表示航程损失代价函数；ψd(t)表示时间t时刻的帆船航行时的参考航向角；ψ(t-1)表示时间t-1时刻的帆船航行时的航向角；
[0030]
步骤25：构建无人帆船路径跟踪制导的代价函数；
[0031]
获取步骤21、步骤22、步骤23和步骤24的结果，获得无人帆船路径跟踪制导的代价函数的具体形式由下式所示；
[0032]
f(ψd)＝f1(ψd)+f2(ψd)+f3(ψd)+f4(ψd)
[0033]
式中：f(ψd)表示无人帆船路径跟踪制导的代价函数；
[0034]
步骤3：求解无人帆船路径跟踪制导的代价函数；
[0035]
获取步骤2得到的帆船路径跟踪制导的代价函数，通过改进的天牛须搜索算法实现优化，为提高天牛须搜索性能建立如下搜索状态的迭代表达式：
[0036]
[0037]
式中：x
t+1
表示t+1时刻搜索状态；x
t
表示t时刻搜索状态；δ
t
表示搜索状态参数；sign表示符号函数；f(x
l
)表示左须对应的帆船路径跟踪制导的代价函数；f(xr)表示右须对应的帆船路径跟踪制导的代价函数；
[0038]
通过上述迭代函数，使无人帆船路径跟踪制导的代价函数f(ψd)达到最小值，进而确定最优的参考航向角ψd，通过参考航向角控制帆船的航行姿态，实现帆船路径跟踪制导。
[0039]
可优选的是，所述步骤1中的航行数据具体包括：参考航向角为ψd，帆船航向角为ψ，帆船速度方向与选定障碍物之间的夹角为ψ1，障碍物速度方向与帆船的夹角ψ2，帆船的速度为u1，障碍物速度为u2,帆船到参考路径的垂线距离与帆船和目标点间距离的夹角为β，障碍物的纵坐标y0，障碍物的横坐标x0，帆船与障碍物之间初始的距离d0。
[0040]
可优选的是，所述步骤21中的避碰代价函数具体获得方法如下：
[0041]
以无人帆船所在的平面位置为圆心，设置一个圆形的探测区域用以表示雷达和声纳的探测能力，而且只对探测区域内的障碍物进行处理；其中ψ1表示朝向选定障碍物(x0,y0)的夹角，具体表示为：
[0042][0043]
式中：ψ1表示帆船速度方向与选定障碍物的夹角；y0表示选定障碍物的纵坐标；y表示帆船纵坐标；x0表示选定障碍物的横坐标；x表示帆船横坐标；
[0044]
帆船与障碍物之间的距离d通过下式确定：
[0045][0046]
式中：d表示随时间变化的帆船与障碍物之间的距离；d0表示帆船与障碍物之间初始的距离；a1表示会遇时间参数；a2表示会遇距离参数；t
cpa
表示最小会遇时间；
[0047]
其中，会遇时间参数和会遇距离参数的计算表达式如下式所示：
[0048][0049]
式中：u1表示帆船速度；u2表示障碍物速度；ψ2表示障碍物速度方向与帆船的夹角；
[0050]
进一步，确定最小会遇时间和最小会遇距离的计算表达式如下式所示：
[0051][0052]
式中：d
cpa
表示最小会遇距离；
[0053]
检测区域内的所有障碍物均应通过以上两式进行评估，当t
cpa
≤0成立时，其对应障碍物首先被排除，仅针对t
cpa
＞0成立时的障碍物进行考虑；
[0054]
其中，随速度变化的危险度函数如下式所示：
[0055]
[0056]
式中：表示随速度变化的危险度函数；表示第i个障碍物的速度；k1表示一个比例系数；表示速度的阈值。
[0057]
可优选的是，所述步骤22中的路径跟踪代价函数具体获得过程如下所示：
[0058]
前进方向目标点p的相对坐标为(x
p
,y
p
)，则路径跟踪的参考航向角可以表示为：
[0059][0060]
式中：ψ
pf
表示路径跟踪的参考航向角；y
p
表示目标点纵坐标；x
p
表示目标点横坐标；
[0061]
当ψd在参考路径的垂线距离与帆船和目标点间距离的夹角β区域内时，实现路径跟踪，而如果位于180
°‑
β的区域时，针对障碍物和帆船的不同位置，需要分情况考虑，构造不同的代价函数；
[0062]
其中，横向误差影响函数如下式所示；
[0063][0064]
式中：g2表示横向误差影响函数；表示一个正的阈值参数。
[0065]
可优选的是，所述步骤3中的改进的天牛须搜索算法优化过程如下所示：
[0066]
首先，分别确定当前时刻的左须位置和右须位置，其具体表达式如下所示：
[0067][0068]
式中：x
l
表示左须位置；xr表示右须位置；x
t
表示t时刻搜索状态；d
t
表示当前搜索长度；表示一个任意方向的单位向量；
[0069]
其中，搜索状态参数的计算表达式如下式所示：
[0070]
δ
t
＝c0d
t
；0＜c0＜1
[0071]
式中：c0表示搜索状态参数缩减系数；
[0072]
接着，搜索长度迭代计算表达式如下式所示：
[0073]dt+1
＝c1d
t
；0＜c1＜1
[0074]
式中：d
t+1
表示t+1时刻的搜索长度；c1表示搜索长度缩减系数；
[0075]
并且，当d
t+1
小于预先设定的一个搜索范围d
min
时，搜索完成，输出；
[0076]
x1＝ψ(t-1)
[0077]
式中：x1表示初始搜索状态；
[0078]
使帆船路径跟踪制导的代价函数f(ψd)在t时刻达到最小值，输出对应的参考航向角ψd。
[0079]
与现有技术相比，本发明的有益效果在于：
[0080]
(1)本发明通过天牛须搜索方法与约束优化相结合，将制导问题转化为天牛须优化问题，对天牛须搜索算法进行改进来提高算法的性能，保证实际航向角对参考航向角的收敛，提出了一种完全表达式化描述且易于编程实现的无人帆船制导方法。
[0081]
(2)本发明适用于无人驾驶帆船的路径跟踪与避碰混合制导，能够应对更复杂的海洋环境，大幅提高无人帆船航行的安全性。
附图说明
[0082]
图1为本发明实施例的基于天牛须搜索优化的无人帆船路径跟踪制导方法的流程图；
[0083]
图2是本发明实施例的无人帆船路径跟踪和避碰制导算法的原理说明图；
[0084]
图3是本发明实施例的无人帆船速度极坐标图；
[0085]
图4-图9分别是本发明实施例的基于天牛须搜索优化的无人帆船路径跟踪、避碰制导方法下的实船轨迹图；
[0086]
图10是本发明实施例的制导方法的无人帆船速度变化曲线图。
具体实施方式
[0087]
以下，参照附图对本发明的实施方式进行说明，如图1所示为基于天牛须搜索优化的无人帆船路径跟踪制导或者导航方法的流程图。
[0088]
s1：采集帆船航行数据参数；
[0089]
通过声呐、雷达、传感器等设备采集帆船航行过程中的航行数据；所述的航行数据具体包括：参考航向角为ψd，帆船航向角为ψ，帆船速度方向与选定障碍物之间的夹角为ψ1,障碍物速度方向与帆船的夹角ψ2，帆船的速度为u1，障碍物速度为u2,帆船到参考路径的垂线距离与帆船和目标点间距离的夹角为β，障碍物的纵坐标y0，障碍物的横坐标x0，帆船与障碍物之间初始的距离d0。如表1所示为无人帆船航行过程中的基本参数。
[0090]
表1无人帆船的基本参数
[0091][0092][0093]
s2：构建帆船路径跟踪制导的代价函数；
[0094]
经过对无人帆船路径跟踪制导的研究确定动静障碍物、速度、航程等因素会对帆船的航行状态产生影响；因此，无人帆船路径跟踪制导的代价函数由避碰代价函数、路径跟
踪代价函数、速度损失代价函数、航程损失代价函数共同组成。
[0095]
s21：确定避碰代价函数；
[0096]
在本发明中，发明人以无人帆船的平面位置为圆心，设置一个圆形的探测区域，表示雷达、声纳等设备的探测能力如图2所示描绘出了探测设备探测区域的示意图，只有探测区域内的障碍才会被处理。其中ψ1表示朝向选定障碍物(x0,y0)的夹角，具体表示为：
[0097][0098]
式中：ψ1表示帆船速度方向与选定障碍物的夹角；y0表示选定障碍物的纵坐标；y表示帆船纵坐标；x0表示选定障碍物的横坐标；x表示帆船横坐标；sign表示符号函数；
[0099]
帆船与障碍物之间的距离d可通过下式确定：
[0100][0101]
式中：d表示随时间变化的帆船与障碍物之间的距离；d0表示帆船与障碍物之间初始的距离；a1表示会遇时间参数；a2表示会遇距离参数；t
cpa
表示最小会遇时间；
[0102]
其中，会遇时间参数和会遇距离参数的计算表达式如下式所示：
[0103][0104]
式中：u1表示帆船速度；u2表示障碍物速度；ψ2表示障碍物速度方向与帆船的夹角；
[0105]
进一步，确定最小会遇时间和最小会遇距离的计算表达式如下式所示：
[0106][0107]
式中：d
cpa
表示最小会遇距离；
[0108]
检测区域内的所有障碍物均应通过以上两式进行评估，t
cpa
≤0的障碍物首先被排除，仅对t
cpa
＞0的障碍物进行考虑。
[0109]
避碰代价函数的计算表达式如下式所示：
[0110][0111]
式中：i表示障碍物编号；n表示障碍物总个数；表示随速度变化的危险度函数；表示第i个障碍物的速度；表示第i个障碍物的最小会遇距离；
[0112]
其中，随速度变化的危险度函数如下式所示：
[0113][0114]
式中：k1表示一个比例系数；表示障碍物速度的阈值；
[0115]
s22：确定路径跟踪代价函数；
[0116]
路径跟踪作为无人帆船根本的导航任务，在本发明中采用los制导算法，其基本制导框架是以如图2所示的探测区域的示意图进行的。前进方向目标点p的相对坐标为(x
p
,y
p
)，则路径跟踪的参考航向角可以表示为：
[0117][0118]
式中：ψ
pf
表示路径跟踪的参考航向角；y
p
表示目标点纵坐标；x
p
表示目标点横坐标；
[0119]
当ψd在参考路径的垂线距离与帆船和目标点间距离的夹角β区域内时，可以实现路径跟踪，而如果位于180
°‑
β的区域时，不能很好的实现路径跟踪任务，因此针对障碍物和帆船的不同位置，需要分情况考虑，构造不同的代价函数；
[0120]
当帆船位于参考路径右侧时，构造代价函数如下式所示：
[0121][0122]
式中：k2表示一个正的常数；β表示帆船到参考路径的垂线距离与帆船和目标点间距离的夹角；g2表示横向误差影响函数；ye表示横向误差；
[0123]
当帆船位于参考路径左侧时，代价函数如下式所示：
[0124][0125]
其中，横向误差影响函数如下式所示；
[0126][0127]
式中：表示一个正的阈值参数；
[0128]
s23：确定速度损失代价函数；
[0129]
在稳定的风况中，帆船前进速度与α夹角之间的关系可以用速度极坐标图来描述，如图3所示为本发明实施例的无人帆船速度极坐标图；对于风帆船航行状况的不确定性，发明人还要考虑速度损失问题；为保证帆船前进速度的存在性，构造速度损失的代价函数如下式所示：
[0130][0131]
式中：表示u1的最大值；k3表示调优参数；
[0132]
s24：确定航程损失代价函数；
[0133]
帆船转角过大会造成速度损失以及控制效果下降，整个航行过程也有一定的损失。关于参考航向角的连续计算比较复杂，因此采用离散计算方法。当前计算的时间为t，上
一时刻为t-1，构造的航程损失代价函数为：
[0134]
f4(ψd)＝k4|ψd(t)-ψ(t-1)|
[0135]
式中：k4为一个正的调优参数。
[0136]
s25：确定帆船路径跟踪制导的代价函数；
[0137]
结合步骤s21、s22、s23和s24的计算结果确定帆船路径跟踪制导的代价函数如下所示；
[0138]
f(ψd)＝f1(ψd)+f2(ψd)+f3(ψd)+f4(ψd)
[0139]
式中：ψd表示参考航向角；f(ψd)表示帆船路径跟踪制导的代价函数；f1(ψd)表示避碰代价函数；f2(ψd)表示路径跟踪代价函数；f3(ψd)表示速度损失代价函数；f4(ψd)表示航程损失代价函数；
[0140]
s3：求解帆船路径跟踪制导的代价函数；
[0141]
获取步骤s25得到的帆船路径跟踪制导的代价函数，通过改进的天牛须搜索算法，
[0142]
可优选的是，所述步骤3中的改进的天牛须搜索算法具体优化过程如下所示：
[0143]
将制导问题转化为天牛须优化问题，首先，分别定义当前时刻的左右须搜索状态，其具体表达式如下所示：
[0144][0145]
式中：x
l
表示左须位置；xr表示右须位置；x
t
表示t时刻搜索状态；d
t
表示当前搜索长度；表示一个任意方向的单位向量；
[0146]
为提高天牛须搜索性能建立如下搜索状态的迭代表达式：
[0147][0148]
式中：x
t+1
表示t+1时刻搜索状态；δ
t
表示搜索状态参数；f(x
l
)表示左须对应的帆船路径跟踪制导的代价函数；f(xr)表示右须对应的帆船路径跟踪制导的代价函数；
[0149]
其中，搜索状态参数的计算表达式如下式所示：
[0150]
δ
t
＝c0d
t
；0＜c0＜1
[0151]
式中：c0表示搜索状态参数缩减系数；
[0152]
进一步，搜索长度迭代计算表达式如下式所示：
[0153]dt+1
＝c1d
t
；0＜c1＜1
[0154]
式中：d
t+1
表示t+1时刻的搜索长度；c1表示搜索长度缩减系数；
[0155]
当d
t+1
小于预先设定的一个搜索范围d
min
时，搜索完成，输出。
[0156]
x1＝ψ(t-1)
[0157]
式中：x1表示初始搜索状态；
[0158]
使帆船路径跟踪制导的代价函数f(ψd)在t时刻达到最小值，输出对应的参考航向角ψd；通过参考航向角控制帆船的航行姿态，达到帆船路径跟踪制导的目的。
[0159]
如表2所示为本方法与基于视线制导的路径跟踪控制方法计算结果的对比，通过对比可以看出本方法在收敛时间、计算时间和迭代次数方面的优越性。
[0160]
表2本方法与基于视线制导方法的对比分析
[0161][0162][0163]
如图4-9所示为步骤s3对本发明制导算法进行matlab仿真测试的结果。分别为6个时间点下实船的轨迹图。其中，图4为帆船45s时的航行轨迹，在留有安全距离的同时，帆船成功的躲避了静态障碍物。图5为帆船60s时的航行轨迹，帆船成功躲避左侧来向障碍物。图6为帆船90s时的航行轨迹，帆船在完成躲避动态障碍物后成功收敛到参考路径。图7为帆船135s时的航行轨迹，由图可知，帆船成功躲避参考路径上的障碍物。图8为帆船170s时的航行轨迹，帆船完成障碍物的避碰任务，成功收敛到参考路径上。图9为帆船完成整个航线的轨迹图，由图可知，无人帆船成功的完成动、静障碍物的避碰任务，实现路径跟踪。其结果表明该制导和控制策略的有效性，保证了实船在避碰避障时有效的收敛到参考路径，帆船成功的绕过障碍物，避碰避障结束后很快回归到路径跟踪任务，证明了基于天牛须优化制导算法的有效性；图10描述了无人帆船的速度变化曲线。
[0164]
综上，对本案例的结果进行分析证明了基于天牛须搜索优化的无人帆船路径跟踪制导方法具有很好的应用效果。
[0165]
(1)本发明通过天牛须搜索方法与约束优化相结合，将无人帆船制导问题转化为天牛须优化问题，对天牛须搜索算法进行改进来提高算法的性能，保证实际航向角对参考航向角的收敛，提出了一种易于编程实现的无人帆船制导方法。
[0166]
(2)本发明实施例中静态障碍物信息不需要提前获取，只需要对动态障碍物的速度进行初始值为零的赋值，通过实施例的进一步分析，表明本方法能够很大程度上减少计算量，提高算法的运算性能。
[0167]
(3)本发明适用于无人驾驶帆船的路径跟踪与避碰混合制导，通过实施例的进一步分析，证明本方法能够应对更复杂的海洋环境，大幅提高无人帆船路径制导的准确性和航行的安全性。
[0168]
以上所述的实施例仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。