CN109765914A - 一种基于滑窗粒子群的水面无人艇避碰方法 - Google Patents

Abstract

本发明涉及海事智能交通技术无人艇避碰领域，具体涉及一种基于滑窗粒子群的水面无人艇避碰方法。步骤一：建立水面无人艇的运动模型，步骤二：获取障碍物的运动模型，步骤三：根据算法进行避障规划；本发明采取粒子群算法和滑动窗口法进行应对处理，最终通过两种算法的相结合确保水面无人艇对已知和未知动态障碍物的良好规避。

Description

一种基于滑窗粒子群的水面无人艇避碰方法

技术领域

本发明涉及海事智能交通技术无人艇避碰领域，具体涉及一种基于滑窗粒子群的水面无人艇避碰方法。

背景技术

无人艇(USV)是一种集自主规划，自主航行，自主完成环境感知，目标探测等功能为一体的小型水面运动平台，已成为探索海洋资源必不可少的设备。世界各国围绕海洋权益的争夺日益升温，海洋已成为国际战略竞争的热点。近海作战成为未来海战的主流模式以及人们对战争中实现有生力量零伤亡的要求日益迫切，无人水面艇这一新概念的作战平台，必将获得突破性的发展。

目前我国对水面无人艇并没有过多的关注,尤其是水面无人艇的核心技术，自动避碰技术未能得到较好地解决。

水面无人艇的未知局部危险的避碰，是在动态障碍物的信息不可以预测到的情况下进行的，这时水面无人艇仅可以通过相关的设备来时时获取周围海洋环境的动态信息，来进行突发障碍物的规避。避碰的过程也是参靠了现有的预测理论与相关的滑动优化原理，而不采用只进行一次的全局优化得出的结果，而是通过不断进行的局部优化来获得的。

发明内容

本发明的目的在于提供一种基于滑窗粒子群的水面无人艇避碰方法，以使水面无人艇可有效避开未知危险。

本发明实施例提供一种基于滑窗粒子群的水面无人艇避碰方法，包括：

步骤一：建立水面无人艇的运动模型：根据水面无人艇的位置参考系统和传感器参考系统分别测得水面无人艇的位置和姿态以及障碍物的位置，将所述数据进行滤波、融合、时空对准处理，得到水面无人艇和障碍物的精确位置及姿态信息，建立水面无人艇的运动模型；

步骤二：获取障碍物的运动模型：根据水面无人艇的传感器所获取的信息，将所述信息进行处理、解算，得到障碍物的速度、相对水面无人艇的速度、障碍物的位置、相对水面无人艇的位置、相对水面的方向、相对水面无人艇的方向和交叉角，获取障碍物的运动模型；

步骤三：根据算法进行避障规划：根据上述的水面无人艇的运动模型以及障碍物的运动模型，采用粒子群算法进行规划，并用滑动窗口算法进行优化处理，完成避碰；

所述步骤一，包括：

建立水面无人艇的运动模型：根据水面无人艇的位置参考系统和传感器参考系统分别测得水面无人艇的位置和姿态以及障碍物的位置，将所述数据进行滤波、融合、时空对准处理，得到水面无人艇和障碍物的精确位置及姿态信息，建立水面无人艇的运动模型；

其中，所述水面无人艇的运动模型具体表示为为：

y＝η+C_wξ+υ

定义系统状态为：x＝[ξ^T η^T ν^T b^T]^T，系统输入为：u＝τ_control+τ_wind，系统噪声为：则有：

y＝Hx+υ

其中，R(ψ)为旋转矩阵，ψ为无人艇艏向角，R^-1(ψ)＝R^T(ψ)，M为质量矩阵，环境偏项b的值通过观测器估计，代表水面无人艇所受低频干扰力，相当于在水面无人艇航速航向控制中加入积分项，D为阻尼矩阵，其中X_u，Y_v,Y_r，N_v，N_r，I_z为水动力参数，m为刚体质量，x_G为刚体重心在x方向的坐标，τ_control为水面无人艇的推力，τ_wind为作用在水面无人艇上的风力，τ_waves为作用在水面无人艇上的波浪作用力，η＝[n e ψ]，n为无人艇北向坐标，e为无人艇东向坐标，ψ为无人艇艏向角，

所述步骤二，包括：

获取障碍物的运动模型：根据水面无人艇的传感器所获取的信息，将所述信息进行处理、解算，得到障碍物的速度、相对水面无人艇的速度、障碍物的位置、相对水面无人艇的位置、相对水面的方向、相对水面无人艇的方向和交叉角，获取障碍物的运动模型；

其中，所述处理解算的具体方法为：

将避碰阶段由远及近划分为五个：自由行动阶段、碰撞危险阶段、紧迫局面阶段、紧迫危险阶段与发生碰撞；对不同避碰阶段，采取不同标准的避碰行动策略，建立碰撞危险度CRI评价模型，取CRI的值为0～1,利用水面无人艇的传感器所得数据评价发生碰撞的概率大小，CRI越大表示发生碰撞的可能性越大，按照CRI的取值，实施不同的避碰措施；

已知水面无人艇的位置点为S_o(x_o,v_o)，障碍物的位置点为S_T(x_T,y_T)，水面无人艇的航行速度为v_o(vx_o,vy_o)，障碍物的运动速度为v_T(vx_T,vy_T)，则：

水面无人艇的运动速度大小为：水面无人艇的航向为： 其中，

障碍物的运动速度大小为：障碍物的航向为：

水面无人艇与障碍物的相对运动速度分量表示为：相对运动速度大小为：相对运动速度方向为：

水面无人艇与障碍物的相对距离为：

障碍物相对于水面无人艇的方向为：

水面无人艇相对于障碍物的方向为：

障碍物的相对位置为：

水面无人艇航行方向和障碍物运动方向的交叉角为：

水面无人艇与障碍物最近的相遇距离为：其中，如果水面无人艇在障碍物的后方，则DCPA_T是正值；如果水面无人艇在障碍物的前方，则DCPA_T是负值；

水面无人艇到达最近会遇点的时间为：其中，如果障碍物还没有经过二者最近相遇位置点时，TCPA_T是正值；如果障碍物运动航迹已经超出了二者最近的相遇位置点时，TCPA_T是负值；

所述步骤三，包括：

根据算法进行避障规划：根据上述的水面无人艇的运动模型以及障碍物的运动模型，采用粒子群算法进行规划，并用滑动窗口算法进行优化处理，完成避碰；

其中，所述粒子群算法具体描述为：

假设种群规模为N，在迭代时刻t，每个粒子在D维空间中的坐标位置可以表示为： 粒子的速度表示为：坐标位置和速度在t+1时刻，按照下述方式进行调整：

粒子的速度更新公式包括三部分：第一部分是表示粒子先前的速度，具有自身开拓、扩大搜索空间、探索新的搜索区域的趋势，；第二部分是表示粒子i所经历的最优位置(pbest)；第三部分是表示种群中最好的粒子位置(gbest)；

其中，所述滑动窗口算法具体描述为：

滑窗算法在传统粒子群算法的基础上，重新设计步骤如下：

(a)初始化，随机产生粒子的位置和速度；(b)评价种群，计算每个粒子的适应度函数值；(c)更新(a)中比较粒子的适应度值；(d)在一定适应度范围内判断最优值是否改变，若没有变化，则通过启发式函数min f(P)＝g(P)+h(P)确定子目标；(e)局部信息素和全局信息素更新；(f)输出无人艇避碰的路径；

采用滑动窗口模块进行动态障碍物规避的过程中，在每次滑动窗口中周围海洋环境信息更新完成后生成局部子目标P_win(t)，通过启发式函数min f(P)＝g(P)+h(P)确定子目标，g(P)为水面无人艇由当前点运行到P位置的代价，而P点的位置以及当前的环境信息决定了g(P)值的确定情况，h(P)是P位置到终止点的代价，其值通过估价P位置到终止位置的间距而获得，子目标P_win(t)采用最小函数值的窗口边界点P：

本发明的有益效果在于：

1.本发明根据实际检测海洋环境情况，从已知动态移动物体信息和未知动态移动物体两种情况考虑，采取粒子群算法和滑动窗口法进行应对处理，最终通过两种算法的相结合确保水面无人艇对已知和未知动态障碍物的良好规避。

附图说明

图1为本发明中水面无人艇与障碍物的相对运动示意图；

图2为本发明中水面无人艇与障碍物的相对运动分解图；

图3为本发明的水面无人艇规避动态障碍物流程图；

图4为本发明的路径规划算法流程图；

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明做进一步描述：

本发明的技术方案是这样实现的：

步骤1.使用合适的无人艇的运动模型。无人艇上的的测量系统主要由两大部分组成：位置参考系统和传感器参考系统，分别测得航行中无人艇的位置及姿态与障碍物的位置。考虑到不同传感器在时间及步调上的不一致，以及测量误差和各种不确定因素等。将不同传感器的数据进行滤波、融合、时空对准后得到更为精确的位置及姿态信息。

无人艇数学模型为以下形式：

y＝η+C_wξ+υ

定义系统状态为x＝[ξ^T η^T v^T b^T]^T，系统输入为u＝τ_control+τ_wind；系统噪声为则有：

y＝Hx+υ

其中，R(ψ)为旋转矩阵，ψ为无人艇艏向角，R^-1(ψ)＝R^T(ψ)。M为质量矩阵，环境偏项b的值需要观测器估计，代表了无人艇所受到的低频干扰力，因此相当于在无人艇航速航向控制中加入了积分项。D为阻尼矩阵，其中X_u，Y_v,Y_r，N_v，N_r，I_z为水动力参数，m为刚体质量，x_G为刚体重心在x方向的坐标。τ_control为无人艇推力装置产生推力，τ_wind为作用在无人艇上风力，τ_waves为作用在无人艇上的波浪作用力。η＝[n e ψ]，n为无人艇北向坐标，e为无人艇东向坐标，ψ为无人艇艏向角。环境偏项b的值需要观测器估计，代表了无人艇所受到的低频干扰力，因此相当于无人艇动态控制中加入了积分项。

步骤2.获取障碍物的模型，速度，对于无人艇的速度，位置，对于无人艇的位置，对于水面的方向，对于无人艇的方向，交叉角。

依据《1972年国际海上避碰规则》并按照不同的距离，将避碰阶段划分为五个，由远及近的对其进行划分，分别为：自由行动阶段、碰撞危险阶段、紧迫局面阶段、紧迫危险阶段与发生碰撞。对于不同的避碰阶段，采取不同标准的避碰行动策略。在此基础上建立碰撞危险度(Collision Risk Index，简称CRI)的评价模型，取CRI的值为0～1,我们用无人艇上的传感器所测量出来的数据来评价发生碰撞的概率大小，CRI越大表示发生碰撞的可能性越大，对于处于航行状态下的的无人艇应按照CRI的取值，实施不同的避碰措施。

首先将无人艇上传感器所获取的信息进行处理，解算出无人艇与障碍物的航向：

已知水面无人艇的位置点为S_o(x_o,v_o)，障碍物的位置点为S_T(x_T,y_T)，无人艇的航行速度为v_o(v_xo,v_yo)，障碍物的运动速度为v_T(v_xT,v_yT)，则在水面无人艇航行在海洋环境中。

(1)水面无人艇的运动速度

速度的大小：

航向：

其中，

(2)目标船舶的运动速度

速度大小：

航向：

(3)相对运动的速度

相对速度的分量：

相对速度大小：

相对速度方向：

(4)水面无人艇与目标船只的相对距离

(5)目标船只相对于水面无人艇的方向

(6)水面无人艇相对于目标船只的方向

(7)移动船只的相对位置：

(8)移动船只和水面无人艇航行方向的交叉角：

(9)水面无人艇与移动船只最近的相遇距离：

其中，如果水面无人艇在移动船只的后方，则DCPA_T是正值；如果水面无人艇在移动船只的前方，则DCPA_T是负值。

(10)到达最近会遇点的时间

如果移动船只还没有经过两船最近相遇位置点时，TCPA_T是正值；如果移动船只运动航迹已经超出了两船最近的相遇位置点时，TCPA_T是负值。

步骤3.粒子群算法进行规划，并用滑动窗口算法进行优化，完成避碰。

(1)粒子群算法的数学描述如下，假设种群规模为N，在迭代时刻t，每个粒子在D维空间中的坐标位置可以表示为，粒子的速度表示为 坐标位置和速度在t+1时刻，按照下述方式进行调整，

公式可以看出粒子的速度更新公式包括三部分：第一部分是它表示粒子先前的速度，它具有自身开拓、扩大搜索空间、探索新的搜索区域的趋势，这使算法具有全局优化能力，但是在算法的迭代后期它可能影响局部精细搜索；第二部分是表示粒子i所经历的最优位置(pbest)，称作粒子的“自知部分”学习，表示粒子本身的思考，即向自身学习的能力；第三部分是表示种群中最好的粒子位置(gbest)，称作粒子的“社会部分”学习，表示粒子向整个种群学习的能力。

粒子群算法是一种非常好的用于避碰的算法，但是粒子群算法的缺点是容易陷入局部最优，导致收敛精度低和不易收敛。

(2)席裕庚等国内的科研学者在智能机器人他的路径规划中应用了滑动窗口法来解决障碍物规避的问题。这种方法是通过预测控制中的滑动优化原理，利用机器人自身获得的局部环境的实时信息，在不确定的环境下进行动态路径的规划，采用滑动的方式结合反馈的相关信息不断地进行局部规划。在采用滑动窗口模块进行动态障碍物规避的过程中，在每次滑动窗口中周围海洋环境信息更新完成后生成局部子目标P_win(t)，通过启发式函数min f(P)＝g(P)+h(P)确定出子目标，g(P)为水面无人艇由当前点运行到P位置的代价，而P点的位置以及当前的环境信息决定了g(P)值的确定情况，h(P)是P位置到终止点的代价，它的值则是通过估价P位置到终止位置的间距而获得的。子目标P_win(t)采用最小函数值的窗口边界点P：

使用滑窗算法对粒子群算法进行优化。

(3)滑窗算法在传统粒子群算法的基础上，重新设计步骤如下：

步骤1、初始化，随机产生粒子的位置和速度；

步骤2、评价种群，计算每个粒子的适应度函数值；

步骤3、更新步骤1中比较粒子的适应度值；

步骤4、在一定适应度范围内，最优值是否改变，若没有变化，则通过启发式函数min f(P)＝g(P)+h(P)确定出子目标；

步骤5、局部信息素和全局信息素更新；

步骤6、输出无人艇避碰的路径。

Claims (4)

1.一种基于滑窗粒子群的水面无人艇避碰方法，其特征在于，包括：

步骤一：建立水面无人艇的运动模型：根据水面无人艇的位置参考系统和传感器参考系统分别测得水面无人艇的位置和姿态以及障碍物的位置，将所述数据进行滤波、融合、时空对准处理，得到水面无人艇和障碍物的精确位置及姿态信息，建立水面无人艇的运动模型；

步骤二：获取障碍物的运动模型：根据水面无人艇的传感器所获取的信息，将所述信息进行处理、解算，得到障碍物的速度、相对水面无人艇的速度、障碍物的位置、相对水面无人艇的位置、相对水面的方向、相对水面无人艇的方向和交叉角，获取障碍物的运动模型；

步骤三：根据算法进行避障规划：根据上述的水面无人艇的运动模型以及障碍物的运动模型，采用粒子群算法进行规划，并用滑动窗口算法进行优化处理，完成避碰。

2.根据权利要求1所述的一种基于滑窗粒子群的水面无人艇避碰方法，其特征在于，所述步骤一，包括：

建立水面无人艇的运动模型：根据水面无人艇的位置参考系统和传感器参考系统分别测得水面无人艇的位置和姿态以及障碍物的位置，将所述数据进行滤波、融合、时空对准处理，得到水面无人艇和障碍物的精确位置及姿态信息，建立水面无人艇的运动模型；

其中，所述水面无人艇的运动模型具体表示为为：

y＝η+C_wξ+υ

定义系统状态为：x＝[ξ^T η^T ν^T b^T]^T，系统输入为：u＝τ_control+τ_wind，系统噪声为：则有：

y＝Hx+υ

其中，R(ψ)为旋转矩阵，ψ为无人艇艏向角，R^-1(ψ)＝R^T(ψ)，M为质量矩阵，环境偏项b的值通过观测器估计，代表水面无人艇所受低频干扰力，相当于在水面无人艇航速航向控制中加入积分项，D为阻尼矩阵，其中X_u，Y_v,Y_r，N_v，N_r，I_z为水动力参数，m为刚体质量，x_G为刚体重心在x方向的坐标，τ_control为水面无人艇的推力，τ_wind为作用在水面无人艇上的风力，τ_waves为作用在水面无人艇上的波浪作用力，η＝[n e ψ]，n为无人艇北向坐标，e为无人艇东向坐标，ψ为无人艇艏向角，

3.根据权利要求1所述的一种基于滑窗粒子群的水面无人艇避碰方法，其特征在于：所述步骤二，包括：

获取障碍物的运动模型：根据水面无人艇的传感器所获取的信息，将所述信息进行处理、解算，得到障碍物的速度、相对水面无人艇的速度、障碍物的位置、相对水面无人艇的位置、相对水面的方向、相对水面无人艇的方向和交叉角，获取障碍物的运动模型；

其中，所述处理解算的具体方法为：

将避碰阶段由远及近划分为五个：自由行动阶段、碰撞危险阶段、紧迫局面阶段、紧迫危险阶段与发生碰撞；对不同避碰阶段，采取不同标准的避碰行动策略，建立碰撞危险度CRI评价模型，取CRI的值为0～1,利用水面无人艇的传感器所得数据评价发生碰撞的概率大小，CRI越大表示发生碰撞的可能性越大，按照CRI的取值，实施不同的避碰措施；

已知水面无人艇的位置点为S_o(x_o,v_o)，障碍物的位置点为S_T(x_T,y_T)，水面无人艇的航行速度为v_o(v_xo,v_yo)，障碍物的运动速度为v_T(v_xT,v_yT)，则：

水面无人艇的运动速度大小为：水面无人艇的航向为： 其中，

障碍物的运动速度大小为：障碍物的航向为：

水面无人艇与障碍物的相对运动速度分量表示为：相对运动速度大小为：相对运动速度方向为：

水面无人艇与障碍物的相对距离为：

障碍物相对于水面无人艇的方向为：

水面无人艇相对于障碍物的方向为：

障碍物的相对位置为：

水面无人艇航行方向和障碍物运动方向的交叉角为：

水面无人艇与障碍物最近的相遇距离为：其中，如果水面无人艇在障碍物的后方，则DCPA_T是正值；如果水面无人艇在障碍物的前方，则DCPA_T是负值；

水面无人艇到达最近会遇点的时间为：其中，如果障碍物还没有经过二者最近相遇位置点时，DCPA_T是正值；如果障碍物运动航迹已经超出了二者最近的相遇位置点时，DCPA_T是负值。

4.根据权利要求1所述的一种基于滑窗粒子群的水面无人艇避碰方法，其特征在于：所述步骤三，包括：

根据算法进行避障规划：根据上述的水面无人艇的运动模型以及障碍物的运动模型，采用粒子群算法进行规划，并用滑动窗口算法进行优化处理，完成避碰；

其中，所述粒子群算法具体描述为：

假设种群规模为N，在迭代时刻t，每个粒子在D维空间中的坐标位置可以表示为： 粒子的速度表示为：坐标位置和速度在t+1时刻，按照下述方式进行调整：

粒子的速度更新公式包括三部分：第一部分是表示粒子先前的速度，具有自身开拓、扩大搜索空间、探索新的搜索区域的趋势，；第二部分是表示粒子i所经历的最优位置(pbest)；第三部分是表示种群中最好的粒子位置(gbest)；

其中，所述滑动窗口算法具体描述为：

滑窗算法在传统粒子群算法的基础上，重新设计步骤如下：

(a)初始化，随机产生粒子的位置和速度；(b)评价种群，计算每个粒子的适应度函数值；(c)更新(a)中比较粒子的适应度值；(d)在一定适应度范围内判断最优值是否改变，若没有变化，则通过启发式函数min f(P)＝g(P)+h(P)确定子目标；(e)局部信息素和全局信息素更新；(f)输出无人艇避碰的路径；

采用滑动窗口模块进行动态障碍物规避的过程中，在每次滑动窗口中周围海洋环境信息更新完成后生成局部子目标P_win(t)，通过启发式函数min f(P)＝g(P)+h(P)确定子目标，g(P)为水面无人艇由当前点运行到P位置的代价，而P点的位置以及当前的环境信息决定了g(P)值的确定情况，h(P)是P位置到终止点的代价，其值通过估价P位置到终止位置的间距而获得，子目标P_win(t)采用最小函数值的窗口边界点P：