基于多波束测深仪的水下移动障碍物感知方法技术

一种基于多波束测深仪的水下移动障碍物感知方法，经多波束测深仪采样得到不同波束方向上AUV与移动障碍物之间的距离、移动障碍物方位以及AUV水下定位信息，通过坐标变换得到移动障碍物的边界点集合位置坐标后，通过最小圆覆盖算法根据边界点集合位置坐标得到移动障碍物的估计外形，即避碰圆的几何模型；将边界点集合位置坐标的几何中心作为移动障碍物运动质心，通过卡尔曼滤波器实时估计得到移动障碍物的估计位置和运动速度；最后将估计位置和运动速度与几何模型合成得到估计避碰速度圆模型，实现水下移动障碍物感知。本发明专利技术能够有效降低AUV动态避障过程的感知层运算负载和估计误差，且保证AUV动态避障安全裕度，具有快速、精确、安全的特点，并通过仿真验证该方法的快速性和有效性。快速性和有效性。快速性和有效性。

【技术实现步骤摘要】
基于多波束测深仪的水下移动障碍物感知方法


[0001]本专利技术涉及的是一种水下机器人领域的技术，具体是一种基于多波束测深仪的水下移动障碍物感知方法。

技术介绍

[0002]水下机器人(Autonomous Underwater Vehicle，AUV)在高速状态下通过对移动障碍物的快速感知技术保证AUV实时安全避障，但现有的单波束测深仪一般只能采集到固定方位的障碍物信息，尽管借助机械扫描仪等辅助设备，能够采集到更多方位的信息，但扫描采样产生的时间成本与大型高速AUV动态避障的实时性需求冲突。而多波束测深仪在一次采样后便可得到探测范围内的障碍物信息，因此其更加适用于大型高速AUV的动态避障。

技术实现思路

[0003]本专利技术针对现有反应式的建模方法即利用传感器数据进行实时的障碍物静态建模，不具备准确描述障碍物动态特性的能力的不足，结合大型高速AUV在水下未知环境中动态避障的需求，提出一种基于多波束测深仪的水下移动障碍物感知方法，有效降低AUV动态避障过程的感知层运算负载和估计误差，且保证AUV动态避障安全裕度，具有快速、精确、安全的特点，并通过仿真验证该方法的快速性和有效性。
[0004]本专利技术是通过以下技术方案实现的：
[0005]本专利技术涉及一种基于多波束测深仪的水下移动障碍物感知方法，经多波束测深仪采样得到不同波束方向上AUV与移动障碍物之间的距离、移动障碍物方位以及AUV水下定位信息，通过坐标变换得到移动障碍物的边界点集合位置坐标后，通过最小圆覆盖算法根据边界点集合位置坐标得到移动障碍物的估计外形，即避碰圆的几何模型；将边界点集合位置坐标的几何中心作为移动障碍物运动质心，通过卡尔曼滤波器实时估计得到移动障碍物的估计位置和运动速度；最后将估计位置和运动速度与几何模型合成得到估计避碰速度圆模型，实现水下移动障碍物感知。
[0006]所述的边界点集合位置坐标，通过以下方式得到：以垂直面为例，根据多波束测深仪返回数据{(β1，d1)，
…
，(β
i
，d
i
)，
…
，(β
n
，d
n
)}，计算得到障碍物边界点集合在AUV机体系B下的坐标其中：β
i
为探测角度，d
i
为探测角度对应的AUV与障碍物之间距离，测深仪较AUV机体系B的偏置坐标为(l
x
，l
z
)；根据AUV在惯性系i下的坐标(p
x
，p
z
)，经机体系B到惯性系i的坐标转换矩阵J可得惯性系i下移动障碍物的边界点集合位置坐标其中i＝1，
…
，n。
[0007]所述的最小圆覆盖算法是指：以随机增量的方式，基于递归方法，从一个空集合开始，不断向其中随机添加点；检查该集合是否包含新增点，包含则继续添加，不包含则将新增点作为集合边界点，重新计算集合；包含最后一个新增点后，返回最小覆盖圆。
[0008]所述的通过卡尔曼滤波器实时估计是指：将水下移动障碍物位置、速度作为滤波变量；根据运动状态设计转移矩阵，根据多波束测深仪设计测量矩阵，并考虑状态转移与测
量过程的不确定性；根据过程估计结果与测量结果完成数据融合，得到估计位置和速度。
[0009]本专利技术涉及一种实现上述方法的系统，包括：状态感知单元、外形估计单元、位置速度估计单元以及避碰模型合成单元，其中：状态感知单元根据多波束测深仪返回的角度、距离信息，进行坐标变换处理，得到移动障碍物的边界点集合位置坐标；外形估计单元根据边界点集合位置坐标，基于最小圆覆盖算法，进行外形拟合和模型膨胀处理，得到避碰圆的几何模型；位置速度估计单元根据边界点集合位置坐标，基于卡尔曼滤波器，进行状态过程估计结果与测量结果的数据融合，得到移动障碍物估计位置和速度；避碰模型合成单元根据避碰圆的几何模型与移动障碍物估计位置和速度进行避碰模型合成处理，得到估计避碰速度圆模型。上述单元共同构成AUV动态避障系统感知层，估计避碰速度圆模型后续将由感知层输入至系统运动规划层以完成动态避障。技术效果
[0010]本专利技术仅需多波束测深仪作为AUV水下动态避障的障碍感知设备，将水下移动障碍物外形估计过程与位置速度估计过程分离，基于简捷高效的算法框架，保证实时安全的障碍估计。相比现有技术，在深度信息数据量远小于声学图像的条件下，本专利技术通过感知算法的设计保证安全有效的动态避障感知过程，且计算负载显著降低，快速性、实时性均优于现有方案。
[0011]本专利技术采用分布式的感知算法架构，可在工程实现中分模块并行计算，进一步提升算法实时求解的快速性。将输入的移动障碍物各状态信息解耦，分别考虑其外形、位置和速度信息，实现状态信息各自对应的估计方法及其计算参数的分布式调控，移动障碍物状态估计精度、动态避障感知层安全裕度以及感知技术实时求解速度的平衡，保证复杂障碍物场景下AUV动态避障的安全性与鲁棒性。
附图说明
[0012]图1为本专利技术流程图；
[0013]图2为实施例移动障碍物外形估计示意图；
[0014]图3为实施例移动障碍物位置估计示意图；
[0015]图4为实施例移动障碍物估计速度与真实速度对比图；
[0016]图5为实施例移动障碍物估计速度误差图。
具体实施方式
[0017]如图1所示，为本实施例涉及一种基于多波束测深仪的水下移动障碍物感知方法，包括：
[0018]步骤1、基于AUV和多波束测深仪设备属性的参数初始化：根据多波束测深仪的声纳波束数量、探测角度、探测深度、工作频率以及AUV的初始位置、姿态以及速度信息进行感知算法的参数初始化。
[0019]考虑AUV动态避障的运动规划层设计，AUV会根据移动障碍物状态信息实时调整自身位姿和速度，因此在仿真实施例中提前为AUV设定避障轨迹。本方法核心在于对水下移动障碍物的状态感知，因此重点检验针对障碍物速度大小和方向的估计准确性。
[0020]步骤2、经多波束测深仪采样得到不同波束方向上AUV与移动障碍物之间的距离以
及AUV水下定位信息；根据AUV水下定位信息、经多波束测深仪返回的移动障碍物方位、距离信息，通过坐标变换，计算得到移动障碍物的边界点集合位置坐标{(x1，z1)，
…
，(x
i
，z
i
)，
…
，(x
n
，z
n
)}，具体操作包括：由多波束测深仪返回探测角度及其对应的AUV与障碍物之间距离{(β1，d1)，
…
，(β
i
，d
i
)，
…
，(β
n
，d
n
)}，考虑测深仪较AUV机体系B的偏置坐标为(l
x
，l
z
)，则可以计算得到障碍物边界点集合在AUV机体系B下的坐标根据AUV在惯性系i下的坐标本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多波束测深仪的水下移动障碍物感知方法，其特征在于，经多波束测深仪采样得到不同波束方向上AUV与移动障碍物之间的距离、移动障碍物方位以及AUV水下定位信息，通过坐标变换得到移动障碍物的边界点集合位置坐标后，通过最小圆覆盖算法根据边界点集合位置坐标得到移动障碍物的估计外形，即避碰圆的几何模型；将边界点集合位置坐标的几何中心作为移动障碍物运动质心，通过卡尔曼滤波器实时估计得到移动障碍物的估计位置和运动速度；最后将估计位置和运动速度与几何模型合成得到估计避碰速度圆模型，实现水下移动障碍物感知。2.根据权利要求1所述的基于多波束测深仪的水下移动障碍物感知方法，其特征是，所述的边界点集合位置坐标，通过以下方式得到：以垂直面为例，根据多波束测深仪返回数据{(β1，d1)，
…
，(β
i
，d
i
)，
…
，(β
n
，d
n
)}，计算得到障碍物边界点集合在AUV机体系B下的坐标其中：β
i
为探测角度，d
i
为探测角度对应的AUV与障碍物之间距离，测深仪较AUV机体系B的偏置坐标为(l
x
，l
z
)；根据AUV在惯性系i下的坐标(p
x
，p
z
)，经机体系B到惯性系i的坐标转换矩阵J可得惯性系i下移动障碍物的边界点集合位置坐标其中i＝1，
…
，n。3.根据权利要求1所述的基于多波束测深仪的水下移动障碍物感知方法，其特征是，所述的最小圆覆盖算法是指：以随机增量的方式，基于递归方法，从一个空集合开始，不断向其中随机添加点；检查该集合是否包含新增点，包含则继续添加，不包含则将新增点作为集合边界点，重新计算集合；包含最后一个新增点后，返回最小覆盖圆。4.根据权利要求1所述的基于多波束测深仪的水下移动障碍物感知方法，其特征是，所述的通过卡尔曼滤波器实时估计是指：将水下移动障碍物位置、速度作为滤波变量；根据运动状态设计转移矩阵，根据多波束测深仪设计测量矩阵，并考虑状态转移与测量过程的不确定性；根据过程估计结果与测量结果完成数据融合，得到估计位置和速度。5.根据权利要求4所述的基于多波束测深仪的水下移动障碍物感知方法，其特征是，具体包括：3.1)根据移动障碍物边界点集合位置坐标计算出几何中心，即障碍物的运动质心(x
pc
，z
pc
)，其中：x
pc
为表示点集合x坐标的平均值即点集合几何中心的x坐标；z
pc
为表示点集合z坐标的平均值即点集合几何中心的z坐标，n表示点集合包含的边界点数量；3.2)构建滤波变量状态空间其中：x
k
和z
k
为障碍物在x轴和z轴上的坐标，和为其一阶导数即速度；3.3)在多波束测深仪的每次采样周期内，基于状态转移方程：D
k
＝FD
k
‑1+Aw
k
‑1，由上一时刻的水下移动障碍物状态信息D
k
‑1，计算出障碍物当前时刻的状态D
k
，其中F是匀速运动模型下的状态转移矩阵，A表示过程噪声w的增益矩阵，状态转移过程不确定性的影响由Aw
k
‑1体现；移动障碍物测量结果S
k
由运动质心(x
pc
，z
pc
...

【专利技术属性】
技术研发人员：乔磊，李祥弘，于林，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：