[发明专利]一种路径跟踪避障制导方法

摘要

本发明公开了一种路径跟踪避障制导方法，包括以下步骤：(1)启动制导模式，执行路径规划获取GVS运动状态变量的时间序列信号；(2)执行路径跟踪制导算法，获取DVS的ψ_dp和u_dp；(3)实时监测DVS到障碍物安全圆族边界的最小距离l_min，并根据l_min确定DVS的ψ_d；(4)计算DVS的u_d和r_d；(5)判断DVS与GVS的距离，如果其大于等于阈值距离，则GVS运动状态保持不变；否则，则GVS运动状态信息按照其时间序列的排序下移；(6)将DVS制导信息代入控制回路，并更新下一时间点DVS位置信息；(7)测量实船位置判断是否到达目的地，如果“是”，结束航行；如果“否”，时间更新并进入步骤(2)。本发明实现了船舶行驶过程中制导算法与避障算法的相结合，可以更精确的控制船舶进行制导和避障。

主权项

1.一种路径跟踪避障制导方法，其特征在于包括以下步骤：/n(1)执行路径规划并获取导引虚拟小船的时间序列信号；/n(2)根据动态虚拟小船和导引虚拟小船的位置信息，计算动态虚拟小船的路径跟踪艏向角ψ_dp和路径跟踪前进速度u_dp；/n(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离l_min，并根据实时监测的最小距离l_min确定动态虚拟小船的最终艏向角ψ_d，/n所述根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离l_min确定动态虚拟小船的最终艏向角ψ_d包括：/n1)当动态虚拟小船到障碍物安全圆族边界的最小距离l_min小于等于船舶障碍物探测设备的激活圈半径R_active时，执行避障算法以确定最终艏向角ψ_d：/n避障算法原理为：根据船舶上障碍物探测设备的作用半径确定避障算法的探测圈半径R_test和激活圈半径R_active，当障碍物进入探测圈时，在探测到的障碍物的边缘生成半径为R_s的安全圆族，此时若安全圆族部分或全部进入激活圈，从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψ_dp开始，向两侧搜索能够避免与圆族相交的最近的艏向角和/n按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψ_d：/n /n其中i＝1,2，ε是自定义的权重值，ψ_d(j-1)代表上一个采样时间点的最终艏向角；/n2)当动态虚拟小船到障碍物安全圆族边界的最小距离l_min大于船舶中障碍物探测设备的激活圈半径R_active时，动态虚拟小船的最终艏向角ψ_d＝ψ_dp；/n(4)计算动态虚拟小船的最终前进速度u_d和转艏角速度r_d；/n(5)判断动态虚拟小船与导引虚拟小船间的距离l_dr与设定的阈值距离l_drset的大小，如果动态虚拟小船与导引虚拟小船的距离l_dr大于等于阈值距离l_drset，则导引虚拟小船下一时间点的运动状态保持当前状态不变；如果动态虚拟小船与导引虚拟小船的距离l_dr小于阈值距离l_drset，则导引虚拟小船下一时间点运动状态信息按照其时间序列的排序下移；/n(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息；/n(7)测量实船位置，判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入步骤(2)。/n