基于缆绳动态约束的USV-ROV多源融合相对定位方法及系统

摘要: 本发明涉及海洋机器人领域,尤其涉及一种基于缆绳动态约束的usv-rov多源融合相对定位方法及系统。背景技术:1、水面无人艇(unmannedsurfaceveh...
基于缆绳动态约束的USV-ROV多源融合相对定位方法及系统

本发明涉及海洋机器人领域,尤其涉及一种基于缆绳动态约束的usv-rov多源融合相对定位方法及系统。

背景技术:

1、水面无人艇(unmanned surface vehicle,usv)指能够在水面自主航行、执行巡检、监测、避障等任务的自主平台。水下遥控机器人(remotely operated vehicle,rov),指跨域协同系统的水下作业平台,搭载imu与深度传感器,通过脐带缆与usv连接并执行水下探查或采集任务。水面无人艇与水下机器人组成的跨域协同系统,广泛应用于大坝巡检、水质监测与立体探测。由于卫星导航信号无法穿透水体,获取水面与水下平台之间的相对位姿,是实现伴随跟随、自主回收与多机协同作业的核心前提。水下协同导航按是否依赖声学基阵,分为有声学辅助定位与无声学自主定位两类。前者依赖超短基线或短基线等水声设备,后者则尝试利用连接水面与水下的系缆(脐带缆)进行几何测距,以摆脱对水声硬件的刚性依赖,成为轻量化无人系统现场快速部署的重要研究方向。

2、现有的一种主流无声学定位方案,利用usv与rov之间的系缆物理信息构建几何测距模型。其基本思路是:在usv端通过卫星导航信号获取自身绝对位置,并借助拉力传感器、编码器或角度传感器测量系缆的放出长度、俯仰角和方位角,再假设系缆在两端之间保持完全拉直状态,即采用理想直线基线模型,通过三角几何关系直接解算rov相对于usv的水平跨距与深度差,从而获得水下平台的位置估计。该方法计算资源需求低,易于实时实现,且无需额外布设水声基阵,在静态或低流速作业场景中具备一定的工程实用性。

3、然而,该直线假设方案存在三个关键缺陷:其一,实际系缆受自身水中有效湿重及流场水动力阻力作用,必然呈现弯曲下垂形态,直接采用直线逆解会严重高估水平跨距,引入数米级系统常值偏差;其二,在近垂直下潜或大曲率机动时,几何方程对传感器噪声极度敏感,微小测量波动即可引发解算位置剧烈震荡;其三,后续多源融合滤波器的观测噪声协方差通常固定不变,缺乏对几何形态畸变与新息异常的自适应调节能力,导致突发扰动下轨迹抖动甚至发散,难以满足高动态、高鲁棒性的协同作业需求。

技术实现思路

1、针对上述现有技术的不足,本发明提供一种基于缆绳动态约束与多源自适应融合的usv-rov协同定位方法及系统。本发明旨在完全免除昂贵且易受干扰的水声定位设备,将系缆由传统的物理干扰源正向转化为“高精度几何观测源”;通过构建动态参数抛物线修正模型消除缆绳下垂引起的系统性偏差;并利用基于“几何偏差”与“新息二范数”的双重自适应协方差在线调节机制抑制数值奇异与瞬时突发扰动,实现在有限传感条件下高精度、高鲁棒的水下相对位姿估计。本发明显著提升了机器人系统在复杂水域环境下的相对定位精度、作业稳定性与导航控制的可靠性。

2、为达到上述目的,本发明所采用的技术方案是:

3、第一方面,基于缆绳动态约束的usv-rov多源融合相对定位方法,包括以下步骤:

4、s1、在水面无人艇端配置双天线测向实时动态载波相位差分模组与高频主惯性测量单元,利用高频主惯性测量单元进行状态一步预测,利用低频测向实时动态载波相位差分模组进行状态校正;融合两者的输出,生成连续且无漂移的水面无人艇全局位姿参考向量。

5、s2、通过设置于出缆口下方的随动机构及内置随动imu,与水面无人艇主惯性测量单元进行相对姿态差分解算,实时获取消除船体摇晃干扰的出缆方位角;采集绞车编码器反馈的放缆长度及水下遥控机器人深度计计算所得的垂向深度差;通过构建含动态等效参数的抛物线几何约束方程在线求解真实等效水平跨距,结合出缆方位角、垂向深度差及水面无人艇全局位姿参考向量,构造水下遥控机器人在全局参考坐标系下的三维位置观测向量。

6、s3、基于水面无人艇全局位姿参考向量与水下遥控机器人在全局导航坐标系下的三维观测向量,在扩展卡尔曼滤波框架下引入包含“几何偏差通道加权”与“新息二范数检验”的双重自适应观测噪声协方差在线调节策略,生成水下遥控机器人最优状态估计向量和全局位置分量。

7、作为优选,s1包括:

8、s11、使用高频惯性测量单元获取高频imu数据,包括水面无人艇在全局参考坐标系中的平面位置坐标和航向角和在载体坐标系下的纵向速度、横向速度与偏航角速度;根据高频imu数据和过程噪声向量,通过运动学递推生成状态预测向量;

9、s12、使用实时动态载波相位差分模组获得低频测向rtk数据,包括水下遥控机器人各时刻的平面位置坐标与航向角;根据低频测向rtk数据和从状态预测向量中提取绝对位置与航向角的观测矩阵,通过横轴墨卡托投影生成线性观测向量;

10、s13、在水面无人艇全局参考坐标系下,采用扩展卡尔曼滤波对状态预测向量与线性观测向量进行异步融合,生成水面无人艇全局位姿参考向量。

11、作为优选,s2包括:

12、s21、在全局参考坐标系下描述水面无人艇出缆点位置与水下遥控机器人观测点位置,并获取放缆长度和垂向深度差、定义水平跨距;

13、s22、获取缆绳水平方向张力分量,结合单位长度有效湿重,定义特征参数;基于特征参数,以悬链线最低点为局部坐标原点建立坐标系,得到标准悬链线方程;通过在最低点做二阶泰勒展开得到抛物线近似方程;

14、s23、基于抛物线近似方程构建含动态等效参数的抛物线几何约束方程;通过数值迭代法对抛物线几何约束方程进行在线求解得到真实等效水平跨距;结合出缆方位角、水面无人艇全局位姿参考向量以及垂向深度差,构造水下遥控机器人在全局参考坐标系下的三维位置观测向量。

15、作为优选,s23包括:

16、将入水点处缆绳切向张力看作由绞车决定的标称量,构建标称几何参数:

17、

18、式中,为电机设定输出力矩,为绞盘等效半径,为综合传动效率系数,为单位长度有效湿重,为出缆点处的标称缆绳切向张力;考虑水下遥控机器人下潜深度增加时入水角减小的几何投影关系,定义入水角几何估计值满足,得到动态等效参数为:

19、

20、其中,为垂向深度差,为放缆长度,将坐标系改写为以水面无人艇出缆点为坐标原点、深度方向向下为正的垂直平面坐标系,结合端点约束与,得到抛物线轨迹方程为:

21、

22、其中,为水平跨距。

23、作为优选,s23还包括:

24、对抛物线轨迹方程求导得切线斜率:

25、

26、放缆长度与水平跨距满足隐式核心几何约束方程:

27、

28、在每个采样周期内,利用牛顿迭代法对上述隐式方程进行在线数值求解,实时解算出真实等效水平跨距。

29、作为优选,s23还包括:

30、将出缆方位角与水面无人艇全局位姿参考向量中的水面无人艇全局航向角进行求和,得到缆绳绝对方位角;结合水面无人艇当前全局坐标,构造水下遥控机器人在全局导航坐标系下的三维观测向量:

31、。

32、作为优选,s3包括:

33、s31、定义水下遥控机器人的滤波状态向量,并进行状态预测和状态观测;

34、s32、设计几何偏差与新息二范数结合的自适应观测噪声协方差在线调节策略,动态更新观测噪声协方差矩阵;

35、s33、基于自适应修正后的观测噪声协方差矩阵,计算卡尔曼增益矩阵;通过卡尔曼增益矩阵完成水下遥控机器人滤波状态向量及预测协方差矩阵的闭环更新;持续输出校正后的水下遥控机器人最优状态估计向量,提取其中的全局位置分量。

36、作为优选,s31包括:

37、滤波状态向量包括在全局导航坐标系下的三维位置、航向角、在体固坐标系下的纵向速度、横向速度及垂向平动速度以及偏航角速度;结合水下遥控机器人端高频imu采样的偏航角速度,利用离散运动学模型进行状态预测;对状态转移函数关于上一时刻状态估计值求一阶偏导,导出状态转移雅可比矩阵,完成预测协方差矩阵的离散递推;

38、提取抛物线轨迹方程的平面位置观测,并结合独立深度传感器提供的垂向深度测量值,构建水下遥控机器人的三维观测向量和线性观测方程。

39、作为优选,s32包括:

40、利用理想完全拉直假设与抛物线轨迹方程计算值构造几何偏差指标:

41、

42、其中,为时刻由绞车编码器采集获得的放缆长度测量值,为时刻由水下遥控机器人深度计与水面无人艇水面参考基准导出的两端垂向深度差测量值,为时刻基于动态参数抛物线隐式几何约束方程在线数值解算出的真实等效水平跨距;基于几何偏差量,对平面位置观测通道的方差进行在线加权调控:

43、

44、式中,为标称平面位置观测方差,为几何映射加权系数;保持深度通道方差不变,形成阶段性观测噪声协方差矩阵:

45、

46、计算平面位置新息向量:

47、

48、其中,与为时刻由动态参数抛物线缆绳几何模型导出的水下遥控机器人平面位置观测分量,与为时刻基于离散运动学模型计算出的水下遥控机器人平面位置一步预测分量;取其二范数作为判别量;根据当前平面噪声水平,设定自适应判定门限:

49、

50、式中,为门限调整系数;在更新步骤中,执行如下分段调控逻辑:

51、当时,表示观测正常,保持当前观测噪声协方差矩阵执行常规更新;

52、当时,通过放大系数降低异常观测在卡尔曼校正中的权重,并生成自适应修正后的观测噪声协方差矩阵:

53、

54、

55、基于缆绳动态约束的usv-rov多源融合相对定位系统,包括:水面无人艇、缆绳管理系统及水下遥控机器人;

56、在水面无人艇端配置双天线测向实时动态载波相位差分模组与高频惯性测量单元,分别用于实现状态预测和状态校正;融合实时动态载波相位差分模组与高频惯性测量单元的输出,生成连续且无漂移的水面无人艇全局位姿参考向量;

57、缆绳管理系统包括缆长测量单元和出缆方位角测量单元,分别用于采集放缆长度和出缆方位角;使用深度计采集相对水面的垂向深度差;通过引入动态等效参数,建立水下遥控机器人相对水面无人艇的空间位置关系,构造水下遥控机器人在全局参考坐标系下的三维位置观测向量;

58、基于水面无人艇全局位姿参考向量与水下遥控机器人在全局导航坐标系下的三维观测向量,在卡尔曼滤波框架下引入自适应观测噪声协方差在线调节策略,生成水下遥控机器人最优状态估计向量和全局位置分量;

59、所述基于缆绳动态约束的usv-rov多源融合相对定位系统用于实现如第一方面所述的基于缆绳动态约束的usv-rov多源融合相对定位方法及其步骤。

60、与现有技术相比,本发明具有以下有益效果:

61、1.免除水声基阵依赖,显著降低成本与环境敏感度

62、与传统的rov声学定位或水面阵列系统相比,本发明无需依赖昂贵且部署繁琐的水听器阵列与水声信标。通过将系缆从传统的物理干扰源正向转化为“高信度空间几何约束源”,有效解决了受限水域下声波多径反射、边界强干扰以及纯imu惯导推算位置随时间指数级发散的难题,在大幅降低系统硬件成本与部署复杂度的前提下,实现了无水声辅助条件下水下三维相对位姿的连续、平滑与无漂移估计。

63、2.引入动态抛物线几何物理校正,彻底消除悬垂系统性偏差

64、与将系缆简化为“理想直线”的传统测距模型相比,本发明构建了基于准静态抛物线与力矩-入水角投影的动态参数修正模型()。该模型实时响应缆绳张力变化与下潜深度引起的入水角改变,精确反演系缆在流场与自重作用下的真实三维空间弯曲形态。通过数值迭代解算真实水平跨距,彻底消除了直线模型导致的常值米级位置估计偏差,大幅提升了水平跨距与三维位置的几何解算精度。

65、3.具备双重自适应调节机制,极大地提高了滤波鲁棒性与导航稳定性

66、与采用固定观测噪声协方差(如常规ekf)或单一权重切换的定位算法相比,本发明提出了结合“几何偏差通道加权”与“新息二范数检验”的双重自适应协方差在线调节策略。在rov大机动或近垂直极度敏感工况下,算法能够实时感知几何模型的数值不确定性与突发野值扰动,动态调控平面位置观测在卡尔曼校正中的权重。在不中断滤波更新的前提下,有效抑制了几何奇异与突发测量噪声对状态估计的冲击,显著提升了机器人系统在复杂水域下的定位鲁棒性与导航稳定性。

67、4.贯通“感知-校正-自适应-闭环”完整技术链条,实现高鲁棒无水声相对定位

68、本发明构建了“水面异步全局基准构建—基于随动imu姿态差分与缆长构建相对位姿向量—动态参数抛物线物理校正解算—基于几何偏差解耦与新息二范数的双重自适应协方差调控—无水声依赖的水下相对位姿闭环输出”的完整技术链条。各环节环环相扣,不仅从硬件层面消除了船体晃动带来的方位测量扰动,更从物理建模与自适应估计层面根治了悬垂系统性偏差与几何奇异数值震荡问题,在有限传感条件下实现了无水声依赖的水下高精度、连续、无漂移相对位姿闭环估计,显著提升了异构无人系统在复杂受限水域中的作业稳定性与导航可靠性。