CN114252073A - 一种机器人姿态数据融合方法 - Google Patents

Abstract

本发明公开了一种机器人姿态数据融合方法，该方法利用陀螺仪输出的角加速度积分获得姿态角四元数，在世界坐标系下将三轴线加速度与加速度真值[0,0,1]^T对比得到Δqacc，来修正滚转角和俯仰角，根据磁力计输出得到Δq_mag，来修正偏航角，然后对姿态进行修正。采用本发明数据融合所得到的绘制地图边缘线明显且墙体平滑，直角拐角处也很清晰。

Description

一种机器人姿态数据融合方法

技术领域

本发明属于机器人技术领域，特别涉及一种机器人姿态数据融合方法。

背景技术

移动机器人的SLAM(同步定位与建图，simultaneous localization andmapping)是自动导航的关键技术之一。随着人工智能和硬件计算能力的不断提高，SLAM技术越来越多样化。基于二维雷达的SLAM一直是一种在研究过程中的热点，将面临相对复杂的环境问题和疑难问题。Gmapping是一种常见的基于滤波的开源SLAM算法框架。该算法可以实时绘制室内地图，在创建小场景地图时进度很高，但高度依赖里程计数据。如果里程计只使用从运动传感器获得的数据来估计物体的位置，定位会出现偏差。在实际过程中，也存在着车轮打滑和累积误差。准确的里程计数据对整个***是非常重要的，如果单单使用编码器得到里程计会出现一定的偏差。

IMU为惯性测量单元，是一种高灵敏度传感器，由于制造过程中的物理因素，IMU惯性测量单元的实际坐标轴与理想坐标轴之间会有一定的偏差，从而导致IMU噪声、标度误差和轴偏差。因此，需要对收集的数据进行校准，以弥补这些误差。在IMU标定数据的数据融合过程中，加速度计和磁强计的高频噪声和陀螺仪的低频噪声会导致姿态解算的较大误差。因此需要选择合适的数据融合方法。

发明内容

本发明针对上述现有技术的存在的问题，提供一种机器人姿态数据融合方法。

本发明通过以下技术手段实现解决上述技术问题的：

加速度计对加速度比较敏感，取瞬时值计算倾角误差比较大；加速度计测倾角，其动态相应较慢，在高频时信号不可用，所以可通过低通抑制高频。陀螺仪积分得到的角度不受加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大，陀螺仪响应快，积分后可测倾角。

本发明的使用陀螺仪输出的角加速度，积分获得角度，然后通过加速度计的输出，修正该角度的滚转角和俯仰角；最后通过磁力计修正偏航角。

具体如下步骤：

1)离散***下的第K时刻的姿态角四元数采用下式计算获得：

G代表世界坐标系，L代表当前坐标系，

表示k时刻时，四元数q的导数。

2)采用下式将三轴线加速度转换至世界坐标系：

表示

反变换四元数。

3)将得到的世界坐标系下加速度的估算^Gg_p和实际的重力加速度作对比，采用下式计算出

的误差四元数：

经计算，

4)采用下式计算Δq_mag：

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T

经计算，

5)、采用下式进行姿态修正

本发明的有益效果为：采用本发明数据融合所得到的绘制地图边缘线明显且墙体平滑，直角拐角处也很清晰。

附图说明

图1为本发明方法的流程图；

图2为未采用本发明方法所得到的绘制地图；

图3为采用本发明方法所得到的绘制地图；

图4为本发明机器人姿态数据融合方法的应用场景。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

如图4所示，为本发明机器人姿态数据融合方法的应用场景，IMU姿态传感器位于消毒机器人中轴线上，与激光雷达同轴，机器人采用轮式驱动，里程计位于与轮毂电机相连接的DS伺服驱动器中，轮毂电机分布于中轴两侧，关于中轴对称。

SLAM中的旋转矩阵R可以用四元数q来表示，q具有一个实部和三个虚部，写作q＝[q0,q1,q2,q3]^T或q＝[w,x,y,z]^T，q0为实部，为标量，[q1,q2,q3]^T是虚部，为矢量。

图1为该方法的流程图，该方法的具体步骤包括：

使用四元数q来表示旋转，离散***下的第K时刻的姿态角四元数可通过下式(1)计算获得：

式(1)中，G代表世界坐标系，L代表当前坐标系；

乘以任意一个处于世界坐标系中的向量，可将该向量从世界坐标系旋转到当前坐标系；

表示k时刻时，四元数q的导数；

四元数导数计算公式(2)为：

式(2)中，圈乘的计算公式(3)为：

使用前向差分计算式(1)中

将圈乘化为普通的点乘运算：

使用通过角速度计算得到的

将三轴线加速度转换到世界坐标系下，利用转换后的世界坐标系下的加速度真值[0,0,1]^T(重力加速度)对比，得到Δqacc，由于利用三轴线加速度数据，只能计算出滚转角及俯仰角，因此Δqacc只包含滚转角和俯仰角的修正。利用陀螺仪得到的

将^La转换到世界坐标系下：

式(8)中，

表示

反变换四元数。

将得到的世界坐标系下加速度的估算^Gg_p，和实际的重力加速度作对比，计算出

的误差四元数：

R(q)表示四元数q对应的旋转矩阵，具体公式如下：

整理得到：

令Δqacc＝0，最后解得：

叠加后最终可得：

由于加速度计只能修正滚转角和偏航角，采用磁力计修正偏航角，磁力计的输出可以理解为磁线与机体坐标系三轴夹角的表彰或者投影。

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T......(14)；

联立式(14)和(15)，解得：

式(16)中Γ为伽马函数；

修正加速度后的姿态，加入偏航角修正：

在Turtlebot仿真实验环境中进行仿真实验，其中图2对应为未采用本发明数据融合所得到的绘制地图，图3对应为采用本发明数据融合所得到的绘制地图。由仿真图可以清晰的看出，数据融合前的地图存在着墙体识别的偏差已经一些边缘线的不清晰，尤其在直角拐角的位置存在模糊情况。而经过数据融合后的地图边缘线明显且墙体平滑，直角拐角处也很清晰。

要说明的是，在本文中，如若存在第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种机器人姿态数据融合方法，其特征在于，包括如下步骤：

步骤1)、利用陀螺仪输出的角加速度积分获得姿态角四元数；

步骤2)、在世界坐标系下将三轴线加速度与加速度真值[0,0,1]^T对比得到Δqacc，来修正滚转角和俯仰角；

步骤3)、根据磁力计输出得到Δq_mag，来修正偏航角；

步骤4)、采用下式进行姿态修正

2.根据权利要求1所述的一种机器人姿态数据融合方法，其特征在于，步骤1)中离散***下的第K时刻的姿态角四元数采用下式计算获得：

G代表世界坐标系，L代表当前坐标系，

表示k时刻时，四元数q的导数。

3.根据权利要求1所述的一种机器人姿态数据融合方法，其特征在于，步骤2)中采用下式将三轴线加速度转换至世界坐标系：

表示

反变换四元数。

4.根据权利要求3所述的一种机器人姿态数据融合方法，其特征在于，将得到的世界坐标系下加速度的估算^Gg_p和实际的重力加速度作对比，采用下式计算出

的误差四元数：

经计算，

5.根据权利要求1所述的一种机器人姿态数据融合方法，其特征在于，步骤3)中，采用下式计算Δq_mag：

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T

经计算，

Images