[发明专利]一种考虑混合多目标避障的水面无人艇路径跟踪制导方法

权利要求书

1.一种考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于包括以下步骤：

S1：设定航路点信息W₁,W₂,…,W_n，分别建立导引虚拟小船和动态虚拟小船的运动数学模型；其中，所述导引虚拟小船和动态虚拟小船的运动数学模型的表达式为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标，分别为距离的一阶导数，ψ为艏向角，u为前进速度，r为转艏角速度；

S2：导引虚拟小船根据制导算法规划出参考路径，并始终沿参考路径行驶，启动路径跟踪制导模式，动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角，并向实船发出制导命令，以引导实船进行路径跟踪，实船不断获取动态虚拟小船的导引变量；其中，所述动态虚拟小船的导引变量包括艏向角ψ_d和前进速度u_d，艏向角ψ_d的计算公式为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标；

前进速度u_d的计算公式为：

其中，k_d为用于调整收敛速度快慢的设定参数，l_dg为动态虚拟小船到导引虚拟小船的距离，u_g为导引虚拟小船的前进速度，ψ_g为导引虚拟小船的艏向角，ψ_d为动态虚拟小船的艏向角，l_dbset为实船到动态虚拟小船的距离的上限设定，l_db为实船到动态虚拟小船的距离；

S3：在采样时间点进行采样，判断动态虚拟小船是否进入障碍物探测环，如进入障碍物探测环，进入障碍物排序过程，执行S4；如未进入障碍物探测环，保持路径跟踪制导模式，执行S6；

S4：进入障碍物排序过程：对避障物的优先级进行排序，得到障碍物的优先级序列，选取最高优先级障碍物作为当前避障目标，判断当前避障目标是否满足避障操纵条件，若当前避障目标满足避障操纵条件，则进入对当前障碍物的避障制导模式，即执行S5；若当前避障目标不满足避障操纵条件，选取次优先级的障碍物作为当前避障目标；若所有障碍物都不满足避障操纵条件，则进入路径跟踪模式，执行S6；

S5：进入避障制导模式：在每一采样时间点，判断当前障碍物的避障操纵条件是否满足，如满足避障操纵条件，则启动对当前障碍物的避障制导模式，获取动态虚拟小船的导引变量，引导实船；如不满足避障操纵条件，则从障碍物优先级序列中去除当前障碍物，并执行S4；其中，S5所述的避障制导模式中，动态虚拟小船的速度u_d调整为避障速度，艏向角按照下列方法选取：

S51：如当前避障目标为静态障碍物，则

其中，ψ_do为动态虚拟小船在避障制导模式中的艏向角，φ为静态障碍物相对动态虚拟小船的真方位角，e为动态虚拟小船到该静态障碍物的安全极限环的距离，Δ为设定的前向距离，λ＝±1决定动态虚拟小船避障时环绕静态障碍物的方向，+1为顺时针方向环绕静态障碍物的安全极限环，-1为逆时针方向环绕静态障碍物的安全极限环；

S52：如当前避障目标为动态障碍物，则

其中，φ为动态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，e为动态虚拟小船到该动态障碍物的安全极限环的距离，λ＝±1，+1为顺时针环绕动态障碍物的安全极限环，-1为逆时针环绕动态障碍物的安全极限环；补偿参数k用于补偿会遇船舶运动对艏向角造成的影响，其定义为：

其中，b＝-2eV₀²，c＝-(Δ²+e²)V₀²且u_do≥u_c≥V₀；；

u_c为当前动态障碍物的速度；u_do表示动态虚拟小船的避障速度；

S6：根据动态虚拟小船的导引变量和控制策略，实时调整实船的执行器输入，控制实船跟踪动态虚拟小船；

S7：测量实船位置，判断是否到达终点，如果是，结束船舶航行，如果否，则执行S2。