阅读说明：本技术 一种基于前端融合的航向角计算方法 (Course angle calculation method based on front-end fusion ) 是由 黄小春 李瑞峰 常骐川 霍光磊 于 2020-06-29 设计创作，主要内容包括：本发明机器人航向角计算方法领域,具体是公开一种基于前端融合的航向角计算方法,包括惯导模块、前端融合模块和定位算法模块,惯导模块输出原始IMU数据,定位算法模块计算处理得出位姿数据,前端融合模块包括卡尔曼滤波单元和运动模型计算单元,前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块,其航向角计算方法以定位算法模块输出的位姿数据作为观测实现航向角增量卡尔曼滤波的方法；还有采用运动模型进行更新的航向角输出方法,该方法能够实现为定位算法提供一个较准确的航向角估计,可及时修正定位算法在配准时偶发的位置丢失问题,提升定位的可靠性和稳定性。(The invention relates to the field of a robot course angle calculation method, in particular to a course angle calculation method based on front-end fusion, which comprises an inertial navigation module, a front-end fusion module and a positioning algorithm module, wherein the inertial navigation module outputs original IMU data, the positioning algorithm module calculates and processes the original IMU data to obtain pose data, the front-end fusion module comprises a Kalman filtering unit and a motion model calculation unit, the front-end fusion module receives the pose data and the original IMU data to process the pose data to obtain fused IMU data comprising course angles, angular velocities and linear accelerations, and outputs the fused IMU data to the positioning algorithm module, and the course angle calculation method of the invention adopts the pose data output by the positioning algorithm module as a method for observing and realizing course angle increment Kalman filtering; and a course angle output method for updating by adopting the motion model can provide a more accurate course angle estimation for the positioning algorithm, can correct the problem of accidental position loss of the positioning algorithm during registration in time, and improves the reliability and stability of positioning.)

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

技术领域

本发明涉及机器人航向角计算方法领域。

背景技术

现有技术中在机器人通常是获取激光雷达的激光数据和惯导模块的IMU数据来进行定位计算，如图1所示，其中惯导模块是机器人导航技术中经常需要使用的模块，其包括有陀螺仪，加速度计可为定位算法提供角速度、线加速度等数据，还可包括有磁力计，使用带磁力计的惯导模块，可提供航向角信息，通过坐标变换后，直接作为定位算法估计位姿的朝向，这种方式可以为定位算法提供较为准确的估计位姿，减少定位算法的迭代次数与运算量，但磁力计的使用会受到场景干扰，特别是受到比较严重的干扰场景下，会导致惯导模块航向角不准确；也有采用不带磁力计的惯导模块，没有航向角的数据，则利用角速度进行积分，得到角度增量，结合上一时刻位姿，通过运动模型计算得到当前时刻的估计位姿，但是这种方式容易因角速度积分的累计误差，导致精度不足。

综上，现有技术方案，或依赖于惯导模块是否支持航向角输出；或容易因角速度积分的累计误差，导致精度不足，无法为定位算法提供较精确的估计位姿，严重的可能导致定位丢失。

发明内容

本发明的目的在于提供一种基于前端融合的航向角计算方法，该方法能够实现为定位算法提供一个较准确的航向角估计，可及时修正定位算法在配准时偶发的位置丢失问题，提升定位的可靠性和稳定性。

为实现上述目的，本发明的技术方案是：一种机器人基于前端融合的航向角计算方法，包括惯导模块、前端融合模块和定位算法模块，所述惯导模块输出原始IMU数据，所述定位算法模块计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据处理得出所述位姿数据；所述航向角的计算方法步骤如下；

1)、初始，进行参数初始化，原始IMU数据包括角速度和线加速度输出至运动模型单元，经运动模型计算单元计算处理得到航向角增量预测值；

2)、首次，前端融合模块首次从定位算法模块中得到位姿数据为首次位姿数据，记录首次位姿数据中的航向角作为基准航向角；

3)、再次，前端融合模块再次从定位算法模块中得到位姿数据为再次位姿数据，将再次位姿数据中的航向角与基准航向角做差，得到再次位姿数据中航向角与首次位姿数据中的航向角的角度差为航向角增量观测值，卡尔曼滤波单元获取所述航向角增量观测值与所述航向角增量预测值进行滤波处理得到当前时刻航向角增量估计值和当前时刻航向角增量协方差；之后把当前时刻航向角增量估计值与基准航向角相加得到当前航向角，再将当前航向角填充到原始IMU 数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块；在上述过程中，由于位姿数据输出频率小于原始IMU数据输出的频率，在没有定位算法模块的位姿数据输出给前端融合模块的时刻，即只有IMU 数据输入的时刻，卡尔曼滤波单元没有进行滤波处理，原始IMU数据直接通过运动模型计算单元计算得出航向角增量预测值，并作为当前时刻航向角增量估计值，再将该航向角估计值与基准航向角相加得到当前航向角，填充到原始IMU 数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块。

系统IMU本身精度的噪声协方差记为Cov_{imu_base}，当前时刻航向角增量协方差记为Cov_t，通过相邻两个时刻的位姿数据中的航向角计算得到的一个动态矩阵为观测噪声协方差记为Cov_ob，所述航向角增量预测值记为所述航向角增量估计值记为均是一个如

的三维向量，由于向量中三个角度独立，又角速度为角度对时间一阶导数，故状态转移矩阵记为A和观测矩阵记为H均为单位矩阵I，即

所述航向角增量观测值记为z_t，z_t也是一个三维向量；

根据计算得到的

为当前时刻的航向角增量预测值，其中为上一时刻航向角增量的估计值，B为控制矩阵u为三轴角速度

根据

计算得到的为当前时刻航向角增量估计值；根据Cov_t＝(I-K*H)*(A*Cov_t-1*A^T+Cov_{imu_base})，计算得到的Cov_t为更新的当前时刻航向角增量协方差，

其中K为增益系数，表示为：

K＝(A*Cov_t-1*A^T+Cov_{imu_base})*H^T*(H*(A*Cov_t-1*A^T+Cov_{imu_base})* H^T+Cov_ob)^-1，

即K＝(Cov_t-1+Cov_{imu_base})*(Cov_t-1+Cov_{imu_base}+Cov_ob)^-1；

在只有原始IMU数据输入的时刻，根据计算更新当前时刻的航向角增量预测值，并将其作为当前时刻航向角增量的估计值，再与基准航向相加，得到当前时刻的航向角，填充至IMU数据结构中，输出给定位模块。

通过采用上述技术方案，本发明的有益效果是：本发明的航向角计算方法以定位算法模块输出的位姿数据作为观测实现航向角增量卡尔曼滤波的方法；还有定位算法模块与惯导模块输出数据频率较大情况下，采用运动模型进行更新的航向角输出方法，本发明的发明发放特别适用于移动机器人，可以不依赖于通过带磁力计输出航向角的惯导模块，采用以定位算法模块的末端输出作为反馈，与惯导模块的原始IMU数据进行融合，能有效消除累计误差，输出较稳定平滑的航向角，为定位算法模块提供较稳定可靠的航向角，并且能获得一个反应迅速，抗干扰性强的航向角数据，在角度变化较大的场景下能够有效加强定位算法模块的稳定性与准确性，从而实现本发明的目的。

附图说明

图1是现有技术中定位算法得出位姿的示意结构框图；

图2是本发明涉及的定位算法得出位姿的示意结构框图；

图3是本发明涉及的一种基于前端融合的航向角计算方法的原理结构框图。

具体实施方式

为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。

一种基于前端融合的航向角计算方法，如图2所示，包括惯导模块、前端融合模块和定位算法模块，图中还包括有激光雷达模块，给定位算法模块提供激光数据以供定法算法模块计算处理得出位姿数据，所述惯导模块输出原始IMU 数据，本实施例中，所述惯导模块中为不包含有磁力计，即惯导模块不支持输出航向角，但其还是能够输出角速度和线加速度，所述定位算法模块用于计算处理得出位姿数据，所述前端融合模块包括卡尔曼滤波单元和运动模型计算单元，所述前端融合模块接收位姿数据和原始IMU数据处理得到包括航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，所述定位算法模块接收融合IMU数据和激光数据处理得出所述位姿数据，通过上述内容以及结合从图1 和图2对比可明显看出本发明的技术方案与现有技术方案的不同之处，本发明的结构中将位姿数据不断反馈到前端融合模块进行计算处理并与原始IMU数据填充融合形成包含时刻更新的航向角的IMU数据输出给定位算法，该方法既能够避免使用磁力计受到场景干扰，特别是受到比较严重的干扰场景下会导致惯导模块输出的航向角不准确的问题，还能够避免采用角速度积分的累计误差导致精度不足的问题，实现本发明的目的效果。

具体的，如图3所示的，所述航向角的计算方法步骤如下；

1)、初始，进行参数初始化，初始化系统中惯导模块本身的噪声协方差记为Cov_{imu_base}，其一般位置为固定值，通常为使用模块中提供的参数值，初始化当前时刻航向角增量协方差记为Cov_t，其是一个变量，在每个时刻的计算中进行迭代更新，初始化观测噪声协方差记为Cov_ob，观测噪声协方差是通过相邻两个时刻的位姿数据中的航向角计算得到的一个动态矩阵；原始IMU数据包括角速度和线加速度输出至运动模型单元，经运动模型计算单元计算处理得到航向角增量预测值记为通过卡尔曼滤波单元卡尔曼滤波得出的航向角增量估计值记为均是一个如

的三维向量，由于向量中三个角度独立，又角速度为角度对时间的一阶导数，故状态转移矩阵记为A和观测矩阵记为H均为单位矩阵I，即航向角增量观测值记为z_t，z_t也是一个三维向量；

2)、首次，前端融合模块首次从定位算法模块中得到位姿数据为首次位姿数据，记录首次位姿数据中的航向角作为基准航向角；

3)、再次，前端融合模块再次从定位算法模块中得到位姿数据为再次位姿数据，将再次位姿数据中的航向角与基准航向角做差，得到再次位姿数据中航向角与首次位姿数据中的航向角的角度差为航向角增量观测值z_t，卡尔曼滤波单元获取所述航向角增量观测值z_t与所述航向角增量预测值进行滤波处理得到当前时刻航向角增量估计值和当前时刻航向角增量协方差Cov_t；在该步骤中的计算公式如下：

根据

计算得到的为当前时刻航向角增量估计值；根据Cov_t＝(I-K*H)*(A*Cov_t-1*A^T+Cov_{imu_base})，计算得到的Cov_t为更新的当前时刻航向角增量协方差，

其中K为增益系数，表示为：

K＝(A*Cov_t-1*A^T+Cov_{imu_base})*H^T*(H*(A*Cov_t-1*A^T+Cov_{imu_base})* H^T+Cov_ob)^-1，

即K＝(Cov_t-1+Cov_{imu_base})*(Cov_t-1+Cov_{imu_base}+Cov_ob)^-1；

上述计算公式内容是卡尔曼滤波单元进行卡尔曼滤波的过程，在有位姿数据输入的时候，以定位算法模块输出的位姿数据的航向角与基准航向做差后的值作为观测值，以运动模型计算的航向角增量作为预测值，两者进行数据融合，可的到当前时刻航向角增量的最优估计值。

之后把当前时刻航向角增量估计值

与基准航向角相加得到当前时刻航向角，然后填充到原始IMU数据结构中，输出给定位算法模块。

4)在只有原始IMU数据输入的时刻，根据

计算更新当前时刻的航向角增量预测值，直接作为当前时刻航向角增量估计值并将该估计值与基准航向相加得到航向角，填充至IMU数据结构中，，这就得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块。

在上述过程中，由于位姿数据输出频率小于原始IMU数据输出的频率，在没有定位算法模块的位姿数据输入给前端融合模块的时刻，卡尔曼滤波单元没有进行滤波处理，只将原始IMU数据通过运动模型计算单元计算得的出航向角增量预测值作为当前时刻航向角增量估计值

再与基准航向角相加得到当前时刻航向角，填充到IMU数据结构中，得到包括有航向角、角速度和线加速度的融合IMU数据输出给定位算法模块，也就是在没有观测值的时候，只进行预测过程，不进行更新过程。在间隔几次的无位姿数据输出给前端融合模块后再次有位姿数据的反馈处理融合，如此循环，航向角增量在前端融合模块中不断的迭代更新，从而达到优化提供有较好稳定性、准确性和抗干扰性的航向角

上述实施例和图式并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。