CN110954102B - 用于机器人定位的磁力计辅助惯性导航系统及方法 - Google Patents

Abstract

本申请提供一种提供用于机器人定位的磁力计辅助惯性导航系统的方法，包括以下步骤：构建三轴加速度计、三轴陀螺仪以及三轴磁力计构成的导航系统；采集加速度计、陀螺仪以及磁力计输出的数据，透过加速度计解算姿态，通过磁力计解算航向；解算速度并对速度量进行误差补偿；解算位置并对位置进行误差补偿；利用扩展卡尔曼滤波对以上信息进行处理后输出最终的位置信息。由此，利用本申请的技术方案能够准确反映移动机器人姿态和位置信息。

Description

用于机器人定位的磁力计辅助惯性导航系统及方法

技术领域

本申请涉及MEMS传感技术领域，尤其涉及用于机器人定位的磁力计辅助惯性导航系统及方法。

背景技术

移动机器人在军事、工业、农业、商业、交通、物流等领域有着广泛的应用前景。移动机器人的核心问题在于移动机器人的自主导航能力，基于MEMS传感器的惯性导航系统是近年来惯性导航领域的研究热点和未来主要研究方向之一，也是自主导航进而实现智能自主控制的一项关键技术。

但是，由于MEMS惯性传感器精度较低，存在许多误差，长时间工作会造成误差累积，由其构成的惯导系统无法长时间独立运作。磁力计的特点与MEMS惯性传感器相反，短时间精度低于MEMS传感器，但其不随着时间累积误差。

发明内容

本申请提供一种基于用于机器人定位的磁力计辅助惯性导航系统的方法，能够稳定准确的得到移动机器人的位置信息。

根据本发明的一方面，提供用于机器人定位的磁力计辅助惯性导航系统的方法，包括以下步骤：

构建三轴加速度计、三轴陀螺仪以及三轴磁力计构成的导航系统；

采集加速度计、陀螺仪磁力计输出的数据，透过加速度计解算姿态，通过磁力计解算航向，进而获得初始姿态角；

对速度量进行误差补偿；

对位置进行误差补偿；

设计扩展卡尔曼滤波对以上信息进行处理后输出最终的位置信息。

在一些实施方式中，透过加速度计解算姿态包括：

根据加速度计输出的数据，确定初始俯仰角α和初始横滚角β：

其中，f_xf_yf_z分别为加速度计X，Y，Z轴的初始测量值。

在一些实施方式中，通过磁力计解算航向包括：通过旋转矩阵将磁力计三轴分量投影到水平面上，

则，

M_nx＝M_bx cosα+M_by sinαsinβ-M_bz sinαcosβ (4)

M_ny＝M_by cosβ+M_bz cosβ (5)

其中M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影，

为由倾角补偿解算出来的方位角。

在一些实施方式中，对速度量进行误差补偿包括以下步骤：

速度的比力方程为：

其中，f为加速度计的测量值，

为哥氏加速度，

是载体保持在地球表面运动引起的对地向心加速度，g为重力加速度；

对上式

积分得到t_m时刻在导航坐标系中的载体速度：

经过化简得到：

设

从而得到载体速度

其中，Δθ为陀螺仪输出值积分得到，表示采样时间内的角度变化，ΔV_g是在t[m-1，m]时间段内由有害加速度补偿量，ΔV_sfm是由比力引起的速度补偿量。

在一些实施方式中，

其为加速度积分得到的速度变化量；

其为陀螺仪的输出值；

其为加速度计的输出值；

由上面推导可以将比力引起的速度补偿写成下式：

从而得到：

旋转效应补偿项：

划桨效应补偿项：

在一些实施方式中，对位置进行误差补偿包括以下步骤：

选择旋转椭球体模型中的WGS84标准作为数学模型，其中，R_e和R_p为椭球体的半长轴和半短轴，f为椭球体的扁率，R_M为子午圈曲率半径，R_N为卵西圈曲率半径，g(L)为载体所在位置的重力加速度，L为纬度；

V_E、V_N、V_U、分别为运载体在大地坐标系中东、北、天方向的速度，由磁力计解算的的航向角度将坐标系转换至导航坐标系，加速计与陀螺仪的输出解算东北天三方向的速度，具体公式如下：

将速度转换成角速度，经过积分后得到经度和纬度，λ为经度、L为纬度：

在一些实施方式中，设计扩展卡尔曼的步骤包括：

确定系统模型：

其中：x(k)表示k时刻非线性系统的状态矢量；z(k)表示k时刻非线性系统的观测矢量；Φ为状态转移矩阵，非线性函数h代表了状态矢量x(k)与观测矢量z(k)之间的关系，称为系统转移矩阵，w(k-1)为状态矢量的噪声，v(k)为观测矢量的噪声，扩展卡尔曼滤波的核心公式可以分成两类，时间更新与观测更新。

在一些实施方式中，时间更新：

第一步骤，由状态转移矩阵可得状态量的更新方程：

x(k，k-1)＝Φx(k-1，k-1)

第二步骤，计算误差协方差：

P(k，k-1)＝ΦP(k，k-1)Φ^T+Q(k)

系统的状态矢量为：

x(k)＝[f_xyz，ω_xyz，m_xyz，v，λ₁，L₁]^T

f_xyz为三轴加速度计的测量值，ω_xyz为三轴陀螺仪的测量值，m_xyz为三轴磁力计的测量值。v为经过旋转效应以及划桨效应补偿后得到的速度量，λ₁，L₁为经过解算后的位置信息，分代表经度与纬度；

观测更新：

由系统转移矩阵可得观测量的更新方程：

z(k，k-1)＝h([x(k，k-1)]

计算卡尔曼增益矩阵：

K(k)＝P(k，k-1)H^T(HP(k，k-1)H^T+R(k))^-1

k时刻状态量估计：

更新误差协方差矩阵：

P(k)＝(I-K(k)H)P(k，k-1)

系统观测量为：z(k)＝[a_c，a_y，λ₂，L₂，h]^T

a_c载体运动时的向心加速度，由速度量与陀螺仪Z轴测量值所求得的加速度，a_c＝vω_z，a_y为经过两效应补偿后求得的速度量微分后求得的加速度，λ₂，L₂是利用a_c，a_y替换原有的加速度计测量值f_x，f_y而求得新的经度与纬度，h为透过加速度计、陀螺仪、磁力计融合解算得到的航向角；

Φ也称作雅克比矩阵，在将Φ带入滤波器的核心公式中计算；状态噪声协方差Q和观测噪声协方差R均是可以不断调整的参数；根据所需的滤波强度可以通过增加阈值判断的方式进行自适应调整；

根据本申请的另一个方面，还提供了应用前述方法的惯性系统，包括加速度计、磁力计、陀螺仪、处理器和滤波器，所述加速度计、磁力计和陀螺仪的数据输出端与所述处理器的输入端相连，所述处理器的数据输出端与所述滤波器的输入端相连。

本申请技术方案与现有技术相比，具有以下有益效果：

本发明为一种能够准确反映移动机器人姿态和位置信息的基于磁力计辅助惯性导航系统的方法。该方法利用磁力计其长时间工作下具有不随着时间累积误差的特性，来弥补IMU组成的惯性导航系统误差随时间累计的缺点，提高INS惯性导航系统的定位精度；并且可以由磁力计来计算出真北，并能补偿动态航向角，解决INS惯性导航系统的动态航向角长时间使用严重漂移的问题。本技术方案利用三轴加速度计和三轴陀螺仪和三轴磁力计构成的导航系统，透过分析，分别对加速度、速度、位置、姿态等信息建立误差模型与误差补偿，设计状态方程与观测方程，透过扩展卡尔曼滤波对其进行融合，算法稳定性较强，能够输出更为准确的位置信息。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本技术方案一实施方式的方法流程图；

图2是本技术方案一实施方式的系统结构示意图；

图3是本技术方案一实施方式的载体的实验环境示意图；

图4是本技术方案一实施方式的航向角的比较图；

图5为本技术方案一实施方式的速度的比较图；

图6为本技术方案一实施方式的位置的比较图。

具体实施方式

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

实施例1

用于机器人定位的磁力计辅助惯性导航系统的方法，包括以下步骤：

S1：构建三轴加速度计、三轴陀螺仪以及三轴磁力计构成的导航系统；

S2：采集加速度计、陀螺仪磁力计输出的数据，透过加速度计解算姿态，通过磁力计解算航向；

S3：解算速度并对速度量进行误差补偿；

S4：解算位置并对位置进行误差补偿；

S5：利用扩展卡尔曼滤波对以上信息进行处理后输出最终的位置信息。

在一些实施方式中，透过加速度计解算姿态包括：

根据加速度计输出的数据，确定初始俯仰角α和初始横滚角β：

其中，f_xf_yf_z分别为加速度计X，Y，Z轴的初始测量值。

在一些实施方式中，通过磁力计解算航向包括：通过旋转矩阵将磁力计三轴分量投影到水平面上，

则，

M_nx＝M_bx cosα+M_by sinαsinβ-M_bz sinαcosβ (4)

M_ny＝M_by cosβ+M_bz cosβ (5)

其中，M_bx、M_by和M_bz为磁力计的输出数据，M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影，

为由倾角补偿解算出来的方位角。

由于使用了正切函数求解方位角，其有效角度范围是0到90°的范围内，即只在第一象限内有效，为了在一周范围内求解正确方位角，需要对方位角设定特定规则，从而把有效角度范围扩展到360°度。

M_nx＝0 and M_ny＜0：

M_nx＝0 and M_ny＞0：

M_nx＞0 and M_ny＞0：

M_nx＞0 and M_ny＜0：

在一些实施方式中，解算速度并对速度量进行误差补偿包括以下步骤：

速度的比力方程为：

其中，

为旋转矩阵，

是在n系(导航坐标系)中地球自转的角速度，

是在n系(为加速度计的输出测量值，

为哥氏加速度，

是载体保持在地球表面运动引起的对地向心加速度，g为重力加速度；而其中的旋转矩阵

其中，Θ俯仰角φ为横滚角ψ为航向角。

对上式

积分得到t_m时刻在导航坐标系中的载体速度：

经过化简得到：

设

从而得到载体速度

其中，Δθ为陀螺仪输出值积分得到，表示采样时间内的角度变化，ΔV_g是在t[m-1，m]时间段内由有害加速度补偿量，ΔV_sfm是由比力引起的速度补偿量。

在一些实施方式中，，

其为加速度积分得到的速度变化量；

其为陀螺仪的输出值；

其为加速度计的输出值；

由上面推导可以将比力引起的速度补偿写成下式：

从而得到：

旋转效应补偿项：

划桨效应补偿项：

在一些实施方式中，对位置进行误差补偿包括以下步骤：

选择旋转椭球体模型中的WGS84标准作为数学模型，其中，R_e和R_p为椭球体的半长轴和半短轴，f为椭球体的扁率，R_M为子午圈曲率半径，R_N为卵西圈曲率半径，g(L)为载体所在位置的重力加速度，L为纬度；WGS84参数为：

R_e＝6378137；R_p＝6356752；

V_E、V_N、V_U、分别为运载体在大地坐标系中东、北、天方向的速度，由磁力计解算的的航向角度将坐标系转换至导航坐标系，加速计与陀螺仪的输出解算东北天三方向的速度，具体公式如下：

将速度转换成角速度，经过积分后得到经度和纬度，λ为经度、L为纬度：

在一些实施方式中，设计扩展卡尔曼的步骤包括：

确定系统模型：

其中：x(k)表示k时刻非线性系统的状态矢量；z(k)表示k时刻非线性系统的观测矢量；Φ为状态转移矩阵，非线性函数h代表了状态矢量x(k)与观测矢量z(k)之间的关系，称为系统转移矩阵，w(k-1)为状态矢量的噪声，v(k)为观测矢量的噪声；

时间更新：

第一步骤，由状态转移矩阵可得状态量的更新方程：

x(k，k-1)＝Φx(k-1，k-1)

第二步骤，计算误差协方差：

P(k，k-1)＝ΦP(k，k-1)Φ^T+Q(k)

系统的状态矢量为：

x(k)＝[f_xyz，ω_xyz，m_xyz，v，λ₁，L₁]^T

f_xyz为三轴加速度计的测量值，ω_xyz为三轴陀螺仪的测量值，m_xyz为三轴磁力计的测量值。v为经过旋转效应以及划桨效应补偿后得到的速度量，λ₁，L₁为经过解算后的位置信息，分代表经度与纬度；

观测更新：

由系统转移矩阵可得观测量的更新方程：

z(k，k-1)＝h([x(k，k-1)]；

第一步骤，计算卡尔曼增益矩阵：

K(k)＝P(k，k-1)H^T(HP(k，k-1)HT+R(k))^-1

第二步骤，k时刻状态量估计：

x(k)＝x(k，k-1)+K(k)(z(k)-Hx(k，k-1))

第三步骤，更新误差协方差矩阵，再将其代回时间更新

P(k)＝(I-K(k)H)P(k，k-1)

系统观测量为：z(k)＝[a_c，a_y，λ₂，L₂，h]^T

a_c载体运动时的向心加速度，由速度量与陀螺仪Z轴测量值所求得的加速度，a_c＝vω_z，a_y为经过两效应补偿后求得的速度量微分后求得的加速度，λ₂，L₂是利用a_c，a_y替换原有的加速度计测量值f_x，f_y而求得新的纬度与经度，h为透过加速度计、陀陀螺仪、磁力计融合解算得到的航向角；

Φ也称作雅克比矩阵，在将Φ带入滤波器的核心公式中计算；状态噪声协方差Q和观测噪声协方差R均是可以不断调整的参数；根据所需的滤波强度可以通过增加阈值判断的方式进行自适应调整；

实施例2：

如图2所示用于移动机器人定位的惯性导航系统，包括加速度计1、磁力计2、陀螺仪3、处理器4和滤波器5，其中，加速度计1、磁力计2和陀螺仪3的数据输出端与处理器4的输入端相连，处理器4的数据输出端与滤波器5的输入端相连。处理器4用于获取加速度计1、磁力计2和陀螺仪3测量数据并进行处理，将处理后的结果发送至滤波器5进行飞行姿态的估计。

实施例3

本次实验的载体为Arduino car，将传感器安装在载体上，使用的传感器为BEWISSENSNG的HEC-395，是一个9轴传感器，包含了3轴加速度计，3轴磁力计，3轴陀螺仪。输出频率设定为20Hz。本次实验以GPS为标准量，将实验结果与GPS输出量做比较，GPS提供航向角、速度与位置等信息，GPS的输出频率为20Hz。

实验的环境如图3所示，载体绕建筑物一圈，方形的运动路径同时具有直线与转弯。图4、图5、图6为实验结果与标准量的比较图。

图4为航向角的比较图，透过本申请技术方法解算出来的航向角大致与GPS给的航向角重合，图5为速度的比较图，单位为公尺/秒，趋势与GPS输出的速度量大致相等。图6为位置的比较图，趋势与GPS输出的速度量大致相等。

综合以上，本发明为一种能够准确反映移动机器人姿态和位置信息的基于磁力记辅助惯性导航系统的算法。该算法利用磁力计其长时间工作下具有不随着时间累积误差的特性，来弥补IMU组成的惯性导航系统误差随时间累计的缺点，提高INS惯性导航系统的定位精度；并且可以由磁力计来计算出真北，并能补偿动态航向角，解决INS惯性导航系统的动态航向角长时间使用严重漂移的问题。本技术方案利用三轴加速度计和三轴陀螺仪和三轴磁力计构成的导航系统，透过分析，分别对加速度、速度、位置、姿态等信息建立误差模型与误差补偿，设计状态方程与观测方程，透过扩展卡尔曼滤波对其进行融合，算法稳定性较强，能够输出更为准确的位置信息。

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

Claims (7)

1.用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，包括以下步骤：

构建三轴加速度计、三轴陀螺仪以及三轴磁力计构成的导航系统；

采集加速度计、陀螺仪以及磁力计输出的数据，透过加速度计解算姿态，通过磁力计解算航向；

解算速度并对速度量进行误差补偿；

解算位置并对位置进行误差补偿；

利用扩展卡尔曼滤波对以上信息进行处理后输出最终的位置信息；

利用扩展卡尔曼进行滤波处理的步骤包括：

设计扩展卡尔曼的系统模型：

其中：x(k)表示k时刻非线性系统的状态矢量；z(k)表示k时刻非线性系统的观测矢量；Φ为状态转移矩阵，非线性函数h代表了状态矢量x(k)与观测矢量z(k)之间的关系，称为系统转移矩阵，w(k-1)为状态矢量的噪声，v(k)为观测矢量的噪声；扩展卡尔曼滤波的核心公式可以分成两类，时间更新与观测更新；

时间更新：

第一步骤，由状态转移矩阵可得状态量的更新方程：

x(k，k-1)＝Φx(k-1，k-1)

第二步骤，计算误差协方差：

P(k，k-1)＝ΦP(k，k-1)Φ^T+Q(k)

系统的状态矢量为：

x(k)＝[f_xyz，ω_xyz，m_xyz，v，λ₁，L₁]^T

f_xyz为三轴加速度计的测量值，ω_xyz为三轴陀螺仪的测量值，m_xyz为三轴磁力计的测量值， v为经过旋转效应以及划桨效应补偿后得到的速度量，λ₁，L₁为经过解算后的位置信息，分代表经度与纬度；

观测更新：

由系统转移矩阵可得观测量的更新方程：

z(k，k-1)＝h([x(k，k-1)]

第一步骤，计算卡尔曼增益矩阵：

K(k)＝P(k，k-1)H^T(HP(k，k-1)H^T+R(k))^-1

第二步骤，计算k时刻状态量的估计值：

x(k)＝x(k，k-1)+K(k)(z(k)-Hx(k，k-1))

第三步骤，更新误差协方差矩阵，再将其代回时间更新：

P(k)＝(I-K(k)H)P(k，k-1)

系统观测量为：z(k)＝[a_c，a_y，λ₂，L₂，h]^T

a_c载体运动时的向心加速度，由速度量与陀螺仪Z轴测量值所求得的加速度，a_c＝vω_z，a_y为经过两效应补偿后求得的速度量微分后求得的加速度，λ₂，L₂是利用a_c，a_y替换原有的加速度计测量值f_x，f_y而求得新的经度与纬度，h为透过加速度计、陀螺仪、磁力计融合解算得到的航向角；

Φ也称作雅克比矩阵，在将Φ代入滤波器的核心公式中计算；状态噪声协方差Q和观测噪声协方差R均是可以不断调整的参数；根据所需的滤波强度通过增加阈值判断的方式进行自适应调整；

2.根据权利要求1所述的用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，所述透过加速度计解算姿态包括：

根据加速度计输出的数据，确定初始俯仰角α和初始横滚角β：

其中，f_xf_yf_z分别为加速度计X，Y，Z轴的初始测量值。

3.根据权利要求2所述的用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，通过磁力计解算航向包括：通过旋转矩阵将磁力计三轴分量投影到水平面上，

则，

M_nx＝M_bx cosα+M_by sinαsinβ-M_bz sinαcosβ；

M_ny＝M_by cosβ+M_bz cosβ；

其中，M_bx、M_by和M_bz为磁力计的输出数据，M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影，

为由倾角补偿解算出来的方位角；

由此，初始姿态角为：

其中，α为俯仰角、β为横滚角和

为航向角。

4.根据权利要求1所述的用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，所述解算速度并对速度量进行误差补偿包括以下步骤：

速度的比力方程为：

其中，

为旋转矩阵，

是在导航坐标系中地球自转的角速度，

是在导航坐标中物体的运动相对于地球的角速度，f为加速度计的测量值，

为哥氏加速度，

是载体保持在地球表面运动引起的对地向心加速度，g为重力加速度；

对上式积分得到t_m时刻在导航坐标系中的载体速度：

经过化简得到：

设

从而得到载体速度

其中，Δθ为陀螺仪输出值积分得到，表示采样时间内的角度变，ΔV_g是在t[m-1，m]时间段内由有害加速度补偿量，ΔV_sfm是由比力引起的速度补偿量。

5.根据权利要求4所述的用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，

简化

由上面推导可以将比力引起的速度补偿写成下式：

从而得到：

旋转效应补偿项：

划桨效应补偿项：

6.根据权利要求3所述的用于机器人定位的磁力计辅助惯性导航系统的方法，其特征在于，所述对位置进行误差补偿包括以下步骤：

选择旋转椭球体模型中的WGS84标准作为数学模型，其中，R_e和R_p为椭球体的半长轴和半短轴，f为椭球体的扁率，R_M为子午圈曲率半径，R_N为卵西圈曲率半径，g(L)为载体所在位置的重力加速度，L为纬度；

V_E、V_N、V_U、分别为运载体在大地坐标系中东、北、天方向的速度，由磁力计解算的的航向角度将坐标系转换至导航坐标系，加速计与陀螺仪的输出解算东北天三方向的速度，具体公式如下：

将速度转换成角速度，经过积分后得到经度和纬度，λ为经度、L为纬度：

7.一种应用权利要求1-6任一项所述方法的惯性系统，其特征在于：

包括加速度计(1)、磁力计(2)、陀螺仪(3)、处理器(4)和滤波器(5)，

所述加速度计(1)、磁力计(2)和陀螺仪(3)的数据输出端与所述处理器(4)的输入端相连，

所述处理器(4)的数据输出端与所述滤波器(5)的输入端相连。

Images