[发明专利]一种多模式自切换的管线仪自适应精密定位方法

权利要求书

1.一种多模式自切换的管线仪自适应精密定位方法，该方法包括数据采集和数据计算，所述数据采集包括：

步骤1：将惯性测量单元放入管线仪中时，与管道的右向、前向、天向保持对齐；

步骤2：将管线仪放置于发球站内；

步骤3：保持静止3～5分钟，配置初始偏航角与初始位置；

步骤4：待静止初始化阶段结束后，开始牵引运动，并在运动过程中标记多个地面标志点位置信息；

步骤5：待管线仪到达收球筒后，再静置3～5分钟，并测量管线仪的末端偏航角与末端位置；

步骤6：将所采集的惯性测量单元原始数据、里程计原始数据、地面标志点位置以及起末偏航信息融合处理，以获取管线仪高精度运动轨迹；

所述数据计算包括：

步骤1：获取惯性测量单元原始数据、里程计原始数据、地面标志点位置；

步骤2：进行原始数据预处理；

步骤2-1：设置阈值剔除角速率粗差；

步骤2-2：设置阈值剔除比力粗差；

步骤2-3：设置阈值剔除里程计粗差；

步骤2-4：使用有限脉冲响应滤波器对惯性测量单元的数据滤波；

步骤2-5：标注里程计打滑时段；

(1)对里程计的输出速度求导，计算加速度a_ODO；

(2)计算当前惯性测量单元姿态捷联矩阵

其中表示的微分形式，表示载体系相对于地心惯性坐标系的旋转角速率，表示导航系相对于i系的旋转角速率，分别表示的反对称矩阵；

(3)计算惯性测量单元测量的载体加速度a_fsf：

其中f_x,f_y,f_z分别表示加速度计输出的三轴比力信息，为n系至b系的旋转矩阵，g为当地重力加速度；

(4)计算里程计与惯性测量单元导出的加速度差值δ_v：

δ_v＝|a_fsf|-|a_ODO| (3)

(5)给定加速度阈值k_dv以标注里程轮打滑时段；

步骤3：利用机器学习方法识别管线仪在各时刻下的运动模态；

步骤3-1：生成训练样本与测试样本；

步骤3-2：构造多元线性回归分类模型；

步骤3-3：定义损失函数；

步骤3-4：训练模型并获得模型关键参数；

步骤3-5：将测试数据集导入训练模型中获取分类结果；

步骤3-6：按分类结果记录对应的时间区间；

步骤4：设计多模式自切换的卡尔曼滤波器进行精密定位解算；

步骤4-1：搭建状态模型、初始对准、消除常值零偏；

(1)建立15维系统误差状态模型如下：

其中

状态向量X依次包含：平台失准角φ、速度误差δvⁿ、位置误差δp、陀螺仪零偏误差ε^b、加速度计零偏误差其中，位置误差依次包含纬度误差δL、经度误差δλ、高度误差δh，其他元素依次包含相应的x、y、z三轴分量；和分别为陀螺角速率和加速度计比力测量白噪声，w_rG和w_rA表示一阶马尔科夫过程激励白噪声；在状态转移矩阵F中，τ_Gi和τ_Ai分别为陀螺仪与加速度计的相关时间常数，其大小应根据传感器特性测试分析结果而设定，其他符号取值如下：

M_av＝M₂,M_ap＝M₁+M₃

M_vp＝(vⁿ×)(2M₁+M₃)+M₄

其中：

式中表示比力在导航系下的投影；vⁿ表示载体在导航系下的速度，其北向分量与东向分量分别表示为v_N、v_E；L、h表示载体所处的纬度与高程；R_Mh＝R_M+h，表示子午圈主曲率半径与高程之和；R_Nh＝R_N+h，表示卯酉圈主曲率半径与高程之和；ω_ie表示地球自转角速率；g_e表示赤道重力；β,β₁,β₂为地球重力模型中的常数；

(2)初始对准

通过初始静止阶段的加速度计输出数据计算初始俯仰角θ₀与滚转角γ₀：

初始偏航角ψ₀与初始位置由外界测量得到，又知初始状态下三轴速度均为0，则初始对准阶段完成；

(3)采用如下方式消除常值零偏：

D_GYRO＝D′_GYRO-m_GYRO (7)

其中下标GYRO与ACC分别表示陀螺仪与加速度计；D与D'分别表示处理后和处理前的数据；m_GYRO,m_ACC意为这段静止数据的平均值；

步骤4-2：进行当前运动模态的智能判决；

步骤4-3：若当前处于静止状态，则采用零速修正(ZUPT)模式，否则转至步骤4-4；

步骤4-4：若当前处于转弯或过焊接口状态，则采用纯惯性导航力学编排方程进行位置、速度以及姿态解算，否则转至步骤4-5；

步骤4-5：若当前为直线运动状态，且俯仰角小于阈值k_θ，则采用SINS/ODO/AGM/HC/YC/PC的完整信息求解模式，否则转至步骤4-6；

步骤4-6：采用SINS/ODO/AGM/PC/YC模式；

若步骤4-5的判定均不满足，则判定载体做“非水平直线运动”，采用“SINS/ODO/AGM/YC/PC”组合定位，这一步骤与步骤4-5的主要区别在于屏蔽了HC模式；

步骤4-6-1搭建SINS/ODO/AGM/HC滤波模型

(1)若当前时刻同时存在里程计速度观测量、AGM位置观测量，则观测方程为：

若此时恰处在里程计打滑区间内，则应适当放大里程计观测噪声V⁽³⁾；

(2)若当前时刻存在里程计速度观测量、无AGM位置观测量，则观测方程为：

若此时恰处在里程计打滑区间内，则应适当放大里程计观测噪声V⁽³⁾；

(3)若当前时刻无里程计速度观测量、存在AGM位置观测量，则观测方程为：

Z⁽⁴⁾＝[0_3×6 I_3×3 0_3×6]X+V⁽⁴⁾ (11)

(4)若当前时刻里程计速度观测量无效、AGM位置观测量无效，则不使用量测修正；

通过上述步骤对系统状态量进行一步估计，再用平台失准角修正载体姿态，得到管线仪的俯仰角以及偏航角，分别记为：

步骤4-6-2：与步骤4-5-2一致；

步骤4-6-3：与步骤4-5-3一致；

步骤4-7：存储当前解算状态参数及其方差-协方差矩阵；

步骤4-8：提取下一时刻的采样数据，返回步骤4-2，直至所有数据解算完成；

步骤5：引入反向滤波平滑策略，进一步优化运动轨迹；

步骤5-1：反向滤波系统的初始化；

步骤5-2：反向滤波平滑的计算步骤与前向滤波执行逻辑类似，参考步骤4-2至4-8，此处不再赘述；

步骤5-3：将正向与反向计算结果进行加权融合并输出最终优化轨迹。