申请/专利权人：南京安透可智能系统有限公司

申请日：2022-12-16

公开（公告）日：2024-04-02

公开（公告）号：CN116295511B

主分类号：G01C25/00

分类号：G01C25/00

优先权：

专利状态码：有效-授权

法律状态：2024.04.02#授权;2023.07.11#实质审查的生效;2023.06.23#公开

摘要：本发明涉及一种用于管道潜航机器人的鲁棒初始对准方法及系统，包括以下步骤：获取潜航机器人的移动方向；基于潜航机器人的移动方向计算潜航机器人的光流速度并估计出潜航机器人在机体坐标系下的潜航运行速度；基于潜航机器人的光流速度和潜航运行速度计算潜航机器人的初始姿态角；获得实时姿态角信息；将实时姿态角信息反馈到初始姿态角计算中，得到精准初始姿态角信息。本发明采用鲁棒初始对准方法，结合运动图像、加速度和角速度量以及实时速度信息，获得潜航机器人精准的初始姿态信息，降低管道污水环境对初始对准的影响，实现捷联惯导的动基座精确初始对准。

主权项：1.一种用于管道潜航机器人的鲁棒初始对准方法，其特征在于：潜航机器人前方搭载有摄像头，摄像头的镜头一直维持在潜航机器人的运动方向；潜航机器人设有主控板；潜航机器人搭载内部集成了三轴加速度计和三轴陀螺仪的惯性测量单元IMU，用来测量潜航机器人的加速度和角速度信息；潜航机器人搭载DVL传感器，基于多普勒效应测量潜航机器人的实时速度；包括以下步骤：步骤一、获取潜航机器人的移动方向；主控板通过摄像头模组采集外部世界的运动图像数据，结合运动场和光流场的关系，采用运动主方向原理，确定潜航机器人的移动方向；步骤二、基于潜航机器人的移动方向计算潜航机器人的光流速度并估计出潜航机器人在机体坐标系下的潜航运行速度；分析运动图像在不同时刻的帧数据，基于Harris角点检测算法提取运动图像的角点信息，据此构造运动图像金字塔模型，并利用Lucas-Kanade光流法计算潜航机器人的光流速度，并通过坐标映射的方式估计出潜航机器人在机体坐标系下的潜航运行速度；步骤三、基于潜航机器人的潜航运行速度计算潜航机器人的初始姿态角；构造惯性导航粗对准方程，在进行数值离散化后，结合获得的潜航运行速度以及惯性测量单元IMU输出的加速度和角速度物理量，利用基于四元数的初始对准QMethod方法计算潜航机器人的初始姿态角；步骤四、获得实时姿态角信息；结合DVL传感器输出的潜航机器人机体坐标系下的潜航运行速度，推导非线性卡尔曼滤波状态方程和观测方程，利用卡尔曼滤波器估计实时姿态误差和陀螺仪零偏，获得实时姿态角精确信息；步骤五、将步骤四的实时姿态角信息反馈到步骤三中，得到精准初始姿态角信息；步骤三中，构造惯性导航粗对准方程的具体步骤如下：结合姿态分解技术将惯性导航系统微分方程重写为： 式中，t表示时间，·表示状态的导数，表示导航系的旋转变化矩阵，表示初始姿态变换矩阵，表示载体坐标系的旋转变化矩阵，fb表示比力，表示地球自转速度在导航坐标系下的投影，表示导航坐标系相对地球坐标系的偏转在导航坐标系下的投影，vn表示潜航机器人在地理坐标系下的速度，gn是重力加速度；在方程1两端同时左乘 整理方程2可得 表示导航系随着时间变化的姿态变化矩阵，τ为积分变量，式3、4、5为获得的惯性导航粗对准方程，其中αtm和βtm均是在时间间隔[tm，t]，tm∈[0，t]上使用广义速度积分公式构造的矢量观测值；步骤三中，计算潜航机器人的初始姿态角的方法为：将式4和5离散化，结合分部积分，离散化后的形式如下 式中，tj＝jTs，t＝tk＝kTs，Ts表示采样时间，表示经卡尔曼滤波器修正后的变换矩阵，表示从tj到τ时刻之间姿态的变化； 其中从符号m开始求和计算，k表示时刻，表示导航系从t0到tm时刻的变化量，表示导航系从t0到tk时刻的变化量；基于方程3、6、7，将摄像头通过光流算法计算的速度惯性系统输出的角速度和加速度矢量代入，运用QMethod方法得到存在误差的初始姿态矩阵步骤四中，获得实时姿态角精确信息的方法为：构建如下形式的状态方程： 利用DVL传感器的输出，构建出如下观测方程：Zk＝HkXk9式中： 表示陀螺仪测量的角速度，ηgu代表过程噪声变量，Xk表示k时刻的状态；表示姿态的变化量，表示初始姿态矩阵；结合方程8～11并执行卡尔曼滤波器，估计出姿态误差φ以及陀螺仪零偏εb，卡尔曼滤波器的执行流程包括时间更新和量测更新两个阶段：时间更新： 量测更新： Pk＝E-KkHkPkk-116式中表示k时刻的最优状态，Pk表示k时刻的状态协方差矩阵，Φkk-1表示一步预测矩阵，表示k-1时刻的状态值，表示一步预测状态，Pk-1表示k-1时刻的状态协方差矩阵，Pkk-1表示一步预测协方差矩阵，Qk-1表示k-1时刻的过程噪声协方差矩阵，Γk-1表示过程噪声转移矩阵；Hk表示量测噪声矩阵，Rk表示k时刻量测噪声矩阵，Zk表示k时刻的量测值，E表示单位矩阵，Kk表示计算的卡尔曼滤波器系数。

全文数据：

权利要求：

百度查询： 南京安透可智能系统有限公司 一种用于管道潜航机器人的鲁棒初始对准方法及系统

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD