一种目标定位误差最小的水面自主航行器最优路径规划方法技术

本发明专利技术提出了一种目标定位误差最小的水面自主航行器最优路径规划方法，包含：S1、构建包含运动模型及观测模型的水面自主航行器的系统模型；S2、推导得到水面自主航行器相对目标之间的最优几何关系；S3、针对1个或2个水面自主航行器通过栅格化区域方式，或针对3个水面自主航行器通过定位技术获取目标先验位置信息；S4、利用蒙特卡洛策略粒子化非线性非高斯测距噪声并构建目标函数闭环表达式；S5、结合非线性动态规划算法对目标函数进行求解获取最优路径。其优点是：解决了目标位置先验信息未知且存在非线性非高斯噪声情况下，无法得到目标函数的闭环表达式，进而导致无法求解定位误差最小的水面航行器最优路径规划问题。位误差最小的水面航行器最优路径规划问题。位误差最小的水面航行器最优路径规划问题。

【技术实现步骤摘要】
而言，使得费雪信息矩阵行列式最大时的角度为π/2，即α
12
＝α
21
＝π/2。
[0029]证明1：令其中τ＝(I
η
)
t
·
θ2，且存在常数γ>0使得γ2＝ζγ1。在 每一时刻，其对应函数可表示为
[0030][0031]为使目标定位误差最小，需要最大化式(6)的行列式，其相当于最小化式(7)， 即
[0032][0033]通过变换，可进一步得到
[0034][0035]显而易见，当时，式(8)能取到最小值。因 此，对于而言，则对于2个ASVs来说，当他们 角度为π/2时，能取到最优的几何关系，即证毕。
[0036]S23、针对3个ASVs场景，推导得到其相对目标最优几何关系角度包括：1)2个ASVs相对目标角度为0或π，另1个ASVs相对于这2个ASVs角度为π/2时；2)3个ASVs 相对目标角度均为2π/3；3)2个ASVs相对目标角度均为π/3，另1个ASVs相对目标角度 为2π/3。推导过程如下：
[0037]设α
12
，α
12
及α
23
为对应第i个ASVs与第l个ASVs之间的角度关系，其中 i,l∈{1,2,3}，τ＝(I
η
)
t
θ2，γ
il
＝γ
i
·
γ
l
且α
12
＝α
21
，α
13
＝α
31
，α
23
＝α
32
。则 第i个ASVs与第l个ASVs之间的角度关系可表示为
[0038][0039]若γ
l
>∑
1≤i≤3,i≠l
γ
i
，意味着某一个ASVs靠近目标，则
[0040][0041]式(10)可进一步表示为
t时刻对应的费雪信息矩阵为
[0062][0063]由于存在异常值，式(17)往往得不到其闭环表达式。为此，本专利技术利用蒙特卡 洛策略，粒子化非线性非高斯噪声，则式(17)可表示为
[0064][0065]其中，
[0066][0067]表示对应一阶梯度；式(20)为利用蒙特卡洛策略粒子化非线性非高斯 噪声操作。
[0068]S42、对于ASVs来说，为使目标定位误差最小，则需要最大化费雪信息矩阵的行 列式，即
[0069][0070]其中，Ξ＝ξ2[(I
η
)
t
]2θ4。
[0071]对式(21)进行对数操作，则目标函数可表示为
[0072][0073]从式(22)可以看出，若以角速度为变量，对于ASVs的最优路径，相应需要求解 的每一时刻参数则是对应的角速度，即
[0074][0075]其中，表示对应1至M时间段内第i个ASVs的最优角速度；c表示对 应约束。
[0076]S43、当仅有1个ASVs时，考虑不确定区域中网格所有交点为k，则需要最大化 不确定区域中最坏的情况，结合网格中所有的交点集合，根据式(23)的优化方程，则仅有 1个ASVs的目标函数可表示为
[0077][0078]S44、当有2个ASVs时，同样地考虑不确定区域中网格所有交点为k，则需要最 大化不确定区域中最坏的情况，结合网格中所有的交点集合，根据式(23)的优化方程，则 有2个ASVs的目标函数可表示为
[0079][0080]S45、当有3个ASVs时，对应目标函数为
[0081][0082]步骤S5结合非线性动态规划算法对目标函数进行求解获取最优路径，具体包含：
[0083]S51、根据S43、S44及S45得到的目标函数，利用Matlab非线性动态规划优化工 具箱求解t时刻的角速度，随后使ASVs根据该角速度运动至下一时刻转向点，如此往复进而 求得规定时间内ASVs的最优路径规划方案。
[0084]本专利技术与现有技术相比具有以下优点：该方法适应于解决了目标位置先验信息未 知且存在非线性非高斯噪声情况下，无法得到目标函数的闭环表达式，进而导致无法求解定 位误差最小的水面航行器最优路径规划问题。
附图说明
[0085]图1为一种目标定位误差最小的水面自主航行器最优路径规划方法流程图
[0086]图2为均匀分布下不同污染因子的异常值影响示意图。
[0087]图3为本专利技术栅格化不确定区域示意图
[0088]图4为相对于目标的ASVs几何关系示意图
[0089]图5本专利技术针对1个ASVs目标定位的最优路径获取算法伪代码
[0090]图6本专利技术针对2个ASVs目标定位的最优路径获取算法伪代码
[0091]图7为本专利技术定位算法伪代码
[0092]图8为本专利技术针对3个ASVs目标定位的最优路径获取流程图
[0093]图9为本专利技术针对1个ASVs场景实验参数。
[0094]图10为本专利技术针对2个ASVs场景实验参数。
[0095]图11为本专利技术针对3个ASVs场景实验参数
[0096]图12为本专利技术不同污染因子下1个ASV最优路径图
[0097]图13为本专利技术污染因子为0.4时2个ASVs最优路径图
[0098]图14为本专利技术污染因子为0.4时3个ASVs最优路径(动态目标)
[0099]图15为本专利技术污染因子为0.4时3个ASVs普通S型路径(动态目标)
[0100]图16为本专利技术最优路径与普通S型路径定位效果对比图
具体实施方式
[0101]以下结合附图，通过详细说明一个较佳的具体实施案例，对本专利技术做进一步阐述。
[0102]图1为本专利技术提出一种目标定位误差最小的水面自主航行器最优路径规划方法流 程图，具体包含：
[0103]S1、构建水面自主航行器的系统模型(运动模型及观测模型)，即系统模型包含运 动模型及观测模型；
[0104]S2、推导得到水面自主航行器相对目标之间的最优几何关系；
[0105]S3、通过栅格化区域方式(针对1个或2个水面自主航行器)或定位技术(针对3 个水面自主航行器)获取目标先验位置信息；
[0106]S4、利用蒙特卡洛策略粒子化非线性非高斯测距噪声并构建目标函数闭环表达式；
[0107]S5、结合非线性动态规划算法对目标函数进行求解获取最优路径。
[0108]本实施案例中，所述的步骤S1具体包含：
[0109]S11、考虑目标水深一直情况，将3D场景映射至2D解决，假设坐标参考系为东
‑ꢀ
北
‑
下的大地坐标参考系，目标在t时刻位置为第i个ASVs在t时刻的位置为 其中T表示转置。若ASVs能够感知其方位角φ(t)，考虑其输入 速度v(t)和角速度r
i
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种目标定位误差最小的水面自主航行器最优路径规划方法，其特征在于，包含以下步骤：S1、构建包含运动模型及观测模型的水面自主航行器的系统模型；S2、推导得到水面自主航行器相对目标之间的最优几何关系；S3、针对1个或2个水面自主航行器通过栅格化区域方式，或针对3个水面自主航行器通过定位技术获取目标先验位置信息；S4、利用蒙特卡洛策略粒子化非线性非高斯测距噪声并构建目标函数闭环表达式；S5、结合非线性动态规划算法对目标函数进行求解获取最优路径。2.如权利要求1所述的一种目标定位误差最小的水面自主航行器最优路径规划方法，其特征在于，步骤S1所述构建包含运动模型及观测模型的水面自主航行器的系统模型，具体包含：S11、考虑目标水深已知情况，将3D场景映射至2D解决，假设坐标参考系为东
‑
北
‑
下的大地坐标参考系，目标在t时刻位置为第i个水面自主航行器(ASVs)在t时刻的位置为其中T表示转置；若ASVs能够感知其方位角φ(t)，考虑其输入速度v(t)和角速度r
i
(t)，则对应ASVs的状态可表示为：定义两个正交单位向量，即则在不考虑流速情况下的ASVs运动模型为：其中，和分别表示上一时刻和此刻第i个ASVs的位置；φ
i
(t
‑
1)和φ
i
(t)分别表示上一时刻和此刻第i个ASVs的方位角信息；当角速度不为0时，表明在相应时间段内，ASVs在做曲线运动，反之当角速度为0时，意味着ASVs在做近似直线的运动；S12、ASVs配有能够接收信号强度的传感器，通过获取其信号强度值来计算其与目标的位置，通过路径损耗模型进一步构造系统的观测模型，即：其中，表示第i个ASVs在t时刻收到目标的信号功率值；P0表示目标发射的功率值(考虑每一时刻发射功率值均相等)；表示路径损耗因子；表示在t时刻第i个ASVs与
目标的距离；d0表示参考距离，通常为1m；表示对应的高斯噪声(考虑每一时刻噪声均相等，即)，其均值为零方差为σ；引入噪声污染因子β，其取值范围为[0,1)，则对应非高斯噪声的概率分布p(η)可表示为：p(η)＝(1
‑
β)Ω(η；0,σ2)+βΦ(η), (5)其中，Ω(η；0,σ2)表示高斯概率分布；Φ(η)表示为非高斯的异常噪声的概率分布。3.如权利要求2所述的一种目标定位误差最小的水面自主航行器最优路径规划方法，其特征在于，步骤S2所述推导得到水面自主航行器相对目标之间的最优几何关系，具体包含：S21、最优几何关系当且仅当ASVs数量超过2个时才存在；S22、针对2个ASVs场景，推导得到其相对目标角度为π2时是最优几何关系；推论1：令α
12
表示为ASVs 1和ASVs 2间的夹角，且α
12
＝α
21
，对于任意固定的而言，使得费雪信息矩阵行列式最大时的角度为π/2，即α
12
＝α
21
＝π/2；证明1：令其中且存在常数γ＞0使得γ2＝ζγ1，在每一时刻，其对应函数可表示为：为使目标定位误差最小，需要最大化式(6)的行列式，其相当于最小化式(7)，即：通过变换，可进一步得到：显而易见，当时，式(8)能取到最小值，因此，对于而言，则对于2个ASVs来说，当他们角度为π/2时，能取到最优的几何关系，即S23、针对3个ASVs场景，推导得到其相对目标最优几何关系角度包括：1)2个ASVs相对目标角度为0或π，另1个ASVs相对于这2个ASVs角度为π/2时；2)3个ASVs相对目标角度均为2π/3；3)2个ASVs相对目标角度均为π/3，另1个ASVs相对目标角度为2π/3，推导过程如下：设α
12
，α
12
及α
23
为对应第i个ASVs与第l个ASVs之间的角度关系，其中i,l∈{1,2,3...

【专利技术属性】
技术研发人员：梅骁峻，韩德志，吴中岱，王骏翔，郭磊，胡蓉，韩冰，徐一言，杨珉，朱宇，
申请(专利权)人：上海海事大学中远海运科技股份有限公司，
类型：发明
国别省市：