一种基于前端融合的航向角计算方法技术

本发明专利技术机器人航向角计算方法领域，具体是公开一种基于前端融合的航向角计算方法，包括惯导模块、前端融合模块和定位算法模块，惯导模块输出原始IMU数据，定位算法模块计算处理得出位姿数据，前端融合模块包括卡尔曼滤波单元和运动模型计算单元，前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，其航向角计算方法以定位算法模块输出的位姿数据作为观测实现航向角增量卡尔曼滤波的方法；还有采用运动模型进行更新的航向角输出方法，该方法能够实现为定位算法提供一个较准确的航向角估计，可及时修正定位算法在配准时偶发的位置丢失问题，提升定位的可靠性和稳定性。

【技术实现步骤摘要】
一种基于前端融合的航向角计算方法
本专利技术涉及机器人航向角计算方法领域。
技术介绍
现有技术中在机器人通常是获取激光雷达的激光数据和惯导模块的IMU数据来进行定位计算，如图1所示，其中惯导模块是机器人导航技术中经常需要使用的模块，其包括有陀螺仪，加速度计可为定位算法提供角速度、线加速度等数据，还可包括有磁力计，使用带磁力计的惯导模块，可提供航向角信息，通过坐标变换后，直接作为定位算法估计位姿的朝向，这种方式可以为定位算法提供较为准确的估计位姿，减少定位算法的迭代次数与运算量，但磁力计的使用会受到场景干扰，特别是受到比较严重的干扰场景下，会导致惯导模块航向角不准确；也有采用不带磁力计的惯导模块，没有航向角的数据，则利用角速度进行积分，得到角度增量，结合上一时刻位姿，通过运动模型计算得到当前时刻的估计位姿，但是这种方式容易因角速度积分的累计误差，导致精度不足。综上，现有技术方案，或依赖于惯导模块是否支持航向角输出；或容易因角速度积分的累计误差，导致精度不足，无法为定位算法提供较精确的估计位姿，严重的可能导致定位丢失。
技术实现思路
本专利技术的目的在于提供一种基于前端融合的航向角计算方法，该方法能够实现为定位算法提供一个较准确的航向角估计，可及时修正定位算法在配准时偶发的位置丢失问题，提升定位的可靠性和稳定性。为实现上述目的，本专利技术的技术方案是：一种机器人基于前端融合的航向角计算方法，包括惯导模块、前端融合模块和定位算法模块，所述惯导模块输出原始IMU数据，所述定位算法模块计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据处理得出所述位姿数据；所述航向角的计算方法步骤如下；1)、初始，进行参数初始化，原始IMU数据包括角速度和线加速度输出至运动模型单元，经运动模型计算单元计算处理得到航向角增量预测值；2)、首次，前端融合模块首次从定位算法模块中得到位姿数据为首次位姿数据，记录首次位姿数据中的航向角作为基准航向角；3)、再次，前端融合模块再次从定位算法模块中得到位姿数据为再次位姿数据，将再次位姿数据中的航向角与基准航向角做差，得到再次位姿数据中航向角与首次位姿数据中的航向角的角度差为航向角增量观测值，卡尔曼滤波单元获取所述航向角增量观测值与所述航向角增量预测值进行滤波处理得到当前时刻航向角增量估计值和当前时刻航向角增量协方差；之后把当前时刻航向角增量估计值与基准航向角相加得到当前航向角，再将当前航向角填充到原始IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块；在上述过程中，由于位姿数据输出频率小于原始IMU数据输出的频率，在没有定位算法模块的位姿数据输出给前端融合模块的时刻，即只有IMU数据输入的时刻，卡尔曼滤波单元没有进行滤波处理，原始IMU数据直接通过运动模型计算单元计算得出航向角增量预测值，并作为当前时刻航向角增量估计值，再将该航向角估计值与基准航向角相加得到当前航向角，填充到原始IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块。系统IMU本身精度的噪声协方差记为Covimu_base，当前时刻航向角增量协方差记为Covt，通过相邻两个时刻的位姿数据中的航向角计算得到的一个动态矩阵为观测噪声协方差记为Covob，所述航向角增量预测值记为所述航向角增量估计值记为均是一个如的三维向量，由于向量中三个角度独立，又角速度为角度对时间一阶导数，故状态转移矩阵记为A和观测矩阵记为H均为单位矩阵I，即所述航向角增量观测值记为zt，zt也是一个三维向量；根据计算得到的为当前时刻的航向角增量预测值，其中为上一时刻航向角增量的估计值，B为控制矩阵u为三轴角速度根据计算得到的为当前时刻航向角增量估计值；根据Covt＝(I-K*H)*(A*Covt-1*AT+Covimu_base)，计算得到的Covt为更新的当前时刻航向角增量协方差，其中K为增益系数，表示为：K＝(A*Covt-1*AT+Covimu_base)*HT*(H*(A*Covt-1*AT+Covimu_base)*HT+Covob)-1，即K＝(Covt-1+Covimu_base)*(Covt-1+Covimu_base+Covob)-1；在只有原始IMU数据输入的时刻，根据计算更新当前时刻的航向角增量预测值，并将其作为当前时刻航向角增量的估计值，再与基准航向相加，得到当前时刻的航向角，填充至IMU数据结构中，输出给定位模块。通过采用上述技术方案，本专利技术的有益效果是：本专利技术的航向角计算方法以定位算法模块输出的位姿数据作为观测实现航向角增量卡尔曼滤波的方法；还有定位算法模块与惯导模块输出数据频率较大情况下，采用运动模型进行更新的航向角输出方法，本专利技术的专利技术发放特别适用于移动机器人，可以不依赖于通过带磁力计输出航向角的惯导模块，采用以定位算法模块的末端输出作为反馈，与惯导模块的原始IMU数据进行融合，能有效消除累计误差，输出较稳定平滑的航向角，为定位算法模块提供较稳定可靠的航向角，并且能获得一个反应迅速，抗干扰性强的航向角数据，在角度变化较大的场景下能够有效加强定位算法模块的稳定性与准确性，从而实现本专利技术的目的。附图说明图1是现有技术中定位算法得出位姿的示意结构框图；图2是本专利技术涉及的定位算法得出位姿的示意结构框图；图3是本专利技术涉及的一种基于前端融合的航向角计算方法的原理结构框图。具体实施方式为了进一步解释本专利技术的技术方案，下面通过具体实施例来对本专利技术进行详细阐述。一种基于前端融合的航向角计算方法，如图2所示，包括惯导模块、前端融合模块和定位算法模块，图中还包括有激光雷达模块，给定位算法模块提供激光数据以供定法算法模块计算处理得出位姿数据，所述惯导模块输出原始IMU数据，本实施例中，所述惯导模块中为不包含有磁力计，即惯导模块不支持输出航向角，但其还是能够输出角速度和线加速度，所述定位算法模块用于计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据和激光数据处理得出所述位姿数据，通过上述内容以及结合从图1和图2对比可明显看出本专利技术的技术方案与现有技术方案的不同之处，本专利技术的结构中将位姿数据不断反馈到前端融合模块进行计算处理并与原始IMU数据填充融合形成包含时刻更新的航向角的IMU数据输出给定位算法，该方法既能够避免使用磁力计受到场景干扰，特别是受到比较严重的干扰场景下会导致惯导模块输出的航向角不准确的问题，还能够避免采用角速度积分的累计误差导致精度不足的问题，实现本专利技术的目的效果。具体的，如本文档来自技高网...

【技术保护点】
1.一种基于前端融合的航向角计算方法，其特征在于，包括惯导模块、前端融合模块和定位算法模块，所述惯导模块输出原始IMU数据，所述定位算法模块计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据处理得出所述位姿数据；所述航向角的计算方法步骤如下；/n1)、初始，进行参数初始化，原始IMU数据包括角速度和线加速度输出至运动模型单元，经运动模型计算单元计算处理得到航向角增量预测值；/n2)、首次，前端融合模块首次从定位算法模块中得到位姿数据为首次位姿数据，记录首次位姿数据中的航向角作为基准航向角；/n3)、再次，前端融合模块再次从定位算法模块中得到位姿数据为再次位姿数据，将再次位姿数据中的航向角与基准航向角做差，得到再次位姿数据中航向角与首次位姿数据中的航向角的角度差为航向角增量观测值，卡尔曼滤波单元获取所述航向角增量观测值与所述航向角增量预测值进行滤波处理得到当前时刻航向角增量估计值和当前时刻航向角增量协方差；之后当前时刻航向角增量估计值与基准航向角相加得到当前航向角，再将当前航向角填充到原始IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块；在上述过程中，由于位姿数据输出频率小于原始IMU数据输出的频率，在没有定位算法模块的位姿数据输出给前端融合模块的时刻，即只有IMU数据输入的时刻，卡尔曼滤波单元没有进行滤波处理，原始IMU数据直接通过运动模型计算单元计算得出航向角增量预测值，并作为当前时刻航向角增量估计值，再将该航向角估计值与基准航向角相加得到当前航向角，填充到原始IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块。/n...

【技术特征摘要】
1.一种基于前端融合的航向角计算方法，其特征在于，包括惯导模块、前端融合模块和定位算法模块，所述惯导模块输出原始IMU数据，所述定位算法模块计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据处理得出所述位姿数据；所述航向角的计算方法步骤如下；
1)、初始，进行参数初始化，原始IMU数据包括角速度和线加速度输出至运动模型单元，经运动模型计算单元计算处理得到航向角增量预测值；
2)、首次，前端融合模块首次从定位算法模块中得到位姿数据为首次位姿数据，记录首次位姿数据中的航向角作为基准航向角；
3)、再次，前端融合模块再次从定位算法模块中得到位姿数据为再次位姿数据，将再次位姿数据中的航向角与基准航向角做差，得到再次位姿数据中航向角与首次位姿数据中的航向角的角度差为航向角增量观测值，卡尔曼滤波单元获取所述航向角增量观测值与所述航向角增量预测值进行滤波处理得到当前时刻航向角增量估计值和当前时刻航向角增量协方差；之后当前时刻航向角增量估计值与基准航向角相加得到当前航向角，再将当前航向角填充到原始IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块；在上述过程中，由于位姿数据输出频率小于原始IMU数据输出的频率，在没有定位算法模块的位姿数据输出给前端融合模块的时刻，即只有IMU数据输入的时刻，卡尔曼滤波单元没有进行滤波处理，原始IMU数据直接通过运动模型计算单元计算得...

【专利技术属性】
技术研发人员：黄小春，李瑞峰，常骐川，霍光磊，
申请(专利权)人：福建泉州哈工大工程技术研究院，
类型：发明
国别省市：福建;35