[发明专利]一种基于行为的多水下机器人路径规划方法

权利要求书

1.一种基于行为的多水下机器人路径规划方法，其特征在于：

(1)定义3个基本行为用来约束AUV的运动，保证在安全的前提下，AUV的航行距离最短，并且实现与其它AUV的协同；

(2)行为目标函数与基本行为对应，是基本行为的具体数学化，定义在AUV的决策空间中，并要保证函数的收敛性；

(3)全局目标函数考虑到了3个基本行为的综合影响，利用带有惯性权重的粒子群优化算法来求解不同时刻全局目标函数的最优解，不同时刻全局目标函数的最优解组成AUV的路径轨迹；

所述3个基本行为的目标函数具体为：

路径规划的环境设置为二维平面，u为t时刻AUV的速度大小，θ为当前的艏向角，(x_t,y_t)为该时刻的位置坐标，Δt为时间间隔，AUV的决策输出包括速度和艏向角，假设(f_x,f_y)为下一时刻AUV的位置坐标，得到：

f_x＝x_t+u·Δt·cosθ

f_y＝y_t+u·Δt·sinθ

A、节能目标函数f₁(θ,u,t)为：

其中：θ为AUV当前艏向角，(x_s,y_s)为目标点的位置坐标，d₁为下一时刻AUV与目标点间的欧氏距离，s₁和s₂是放缩系数；B、协同目标函数f₂(θ,u,t)为：

f_bx＝x_b+u_b·Δt·cosθ_b,f_by＝y_b+u_b·Δt·cosθ_b

其中：(f_bx,f_by)为与该AUV距离最近的AUV下一时刻的位置，d₂为下一时刻的两个AUV的欧氏距离，[d_min,d_max]为d₂的期望取值范围，s₁和s₂是放缩系数，M₂为设定的常值；(x_b,y_b)为与该AUV距离最近的AUV在t时刻的位置，u_b为与该AUV距离最近的AUV在t时刻的速度大小，θ_b为与该AUV距离最近的AUV在t时刻的艏向角，

C、安全行为目标函数f₃(θ,u,t)为：

其中：(x_o,y_o)为障碍物点的位置坐标，d₃为AUV与障碍物下一时刻的欧氏距离，d_s为距离d₃的安全阈值，M₃为设定的常值，s₁和s₂是放缩系数。