CN113532477B - 一种骑行码表设备及骑行码表初始姿态自动校准方法 - Google Patents

Abstract

本发明属于人力骑行码表便携设备技术领域，具体公开一种骑行码表设备及骑行码表初始姿态自动校准方法，包括获取GPS测量的经纬度数值以及IMU测量的加速度和角增量数值；将其分别与预设GPS和IMU状态参数相比较，不断解算满足GPS状态为可用或IMU状态为静置时的航偏角、速度矢量、横滚角和俯仰角；当GPS状态为可用且IMU状态为静置时，将解算得到的航偏角、速度矢量、横滚角和俯仰角作为码表初始姿态输出，完成自动对准。通过在惯导静置状态解算得到初始俯仰角和横滚角，再利用GPS反算得到航向和速度，在解算完成俯仰角和横滚角的时间到GPS反算航向的时间内，俯仰角和横滚角通过实时采集的IMU数据进行更新，从而确保初始状态解算数值的准确性。

Description

一种骑行码表设备及骑行码表初始姿态自动校准方法

技术领域

本发明属于人力骑行码表便携设备技术领域，具体地说涉及一种骑行码表设备及骑行码表初始姿态自动校准方法。

背景技术

现有GPS定位受到的局限性较大，信号容易受到树木、楼宇等地物的遮挡，同时多径效应也时有发生，因此码表利用GPS定位时存在定位结果可信度和鲁棒性低的问题；在GPS码表结合惯导系统后，惯导系统虽然可以与GPS构成多传感器定位系统，但是惯导系统需要较为精确的初始姿态(经纬度位置、三轴姿态角、导航坐标系下的三维速度)，由于码表为自由拆卸设备(与固定位置的车载导航系统不同)，每次安装码表的位置不同会导致初始姿态数据的不同，因而在码表每次开启导航时，难以在短时间内准确的标定码表的初始姿态，通常需要借助外部API接口或者更多的传感设备，设备配置成本较高。

因此，现有技术还有待于进一步发展和改进。

发明内容

针对现有技术的种种不足，为了解决上述问题，现提出一种骑行码表设备及骑行码表初始姿态自动校准方法。本发明提供如下技术方案：

一种骑行码表初始姿态自动校准方法，包括：

获取GPS测量的经纬度数值以及IMU测量的加速度和角增量数值；

将经纬度数值和加速度、角增量数值分别与预设GPS和IMU状态参数相比较，不断解算满足GPS状态为可用或IMU状态为静置时的航偏角速度矢量横滚角θ和俯仰角γ；

当GPS状态为可用且IMU状态为静置时，将解算得到的航偏角速度矢量横滚角θ_i和俯仰角γ_i作为码表初始姿态输出，完成自动对准。

进一步的，航偏角的计算方法包括：当GPS状态为可用时，获取经纬坐标并将其转为空间大地坐标，在空间大地坐标内不断滑动窗口，选取满足精度要求的位置序列反算航偏角。

进一步的，在空间大地坐标内不断滑动窗口，选取满足精度要求的位置序列反算航偏角的方法包括：基于转换的空间大地坐标，获取预设窗口范围内连续历元的平面坐标时间序列，基于平面坐标时间序列内相邻两坐标进行航向反算得到方向序列和对应的均方误差，若均方误差小于预设精度阈值，则基于方向序列进行航偏角反算；若均方误差大于预设精度阈值，则预设窗口前进一个步长单位，重新获取新的平面坐标时间序列进行反算。

进一步的，选取满足精度要求的位置序列反算航偏角的方法包括：

获取位置序列z_i＝{(x,y,z)_m|m＝i,i+1,...,i+n}，其中，i为位置序列对应的起始时间历元，(x,y,z)对应的坐标系是东北天地理坐标系；

将位置序列坐标投影到平面上得到平面坐标集合

pos_i＝{(x,y)m|m＝i,i+1,...,i+n}；

将平面坐标集合整理得到方程组：

基于最小二乘法解算得到[bias k]＝y^T·x·(x^T·x)^-1，其中，k为东北天地理坐标系下窗口内骑行路径的方向斜率；

基于反正切函数求解航偏角

进一步的，基于获取的平面坐标集合计算速度标量其中，n为窗口长度，(x y)₁为窗口内第一个平面坐标，(x y)_n为窗口内最后一个平面坐标。

进一步的，横滚角和俯仰角的计算方法包括：当IMU状态为静置时，加速度计测量的是重力在载体坐标系下的投影，即重力和加速度计的比力关系为：

其中，f_ib ^b为比力输出值，f_ib ^b＝[f_ibx ^b,f_iby ^b,f_ibz ^b]表示加速度计x、y、z三轴的输出，为重力加速度，进而推导出横滚角θ和俯仰角γ的解算公式为：

进一步的，实时获取IMU测量的加速度和角增量数值，利用四阶龙格库塔法动态更新姿态角的解算公式，具体方法如下：

设置初始四元数，其中，为初始航偏角，θ₀为初始横滚角，γ₀为初始俯仰角，q为四元数向量；

进而得到四阶龙格库塔更新四元数

通过数据采样对上式相关元素进行解算：其中，h为数据采样间隔，q_(t)、q_(t+h)分别为t、t+h时刻的四元数，分别表示陀螺仪的x、y、z三轴采集到的的实时角增量；

对更新后的四元数进行归一化得到

解算出姿态角

进一步的，基于获取的速度标量和姿态角解算三轴速度矢量具体方法包括：

基于姿态角求解姿态阵

基于姿态阵和速度标量求解速度矢量

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现一种骑行码表初始姿态自动校准方法。

一种骑行码表设备，包括：传感器、GPS、处理器及存储器，所述传感器与处理器通过通讯协议信号连接，所述GPS与处理器通过串口信号连接，所述存储器与处理器通过SPI信号连接；

所述存储器用于存储计算机程序，所述处理器用于执行所述存储器存储的计算机程序，以使所述处理器执行一种骑行码表初始姿态自动校准方法。

有益效果：

1、通过在惯性测量单元静置状态解算得到初始俯仰角和横滚角，再利用GPS反算得到航向和速度后，便可以得到码表初始位置所有的初始状态，无需借助外部API接口或者更多的传感设备，成本低，且能在磁信号的情况下，实现自动对用户骑行初始状态的解算；

2、利用GPS平均误差较小且成本低，平面单点定位可达到米级的特点，获取经纬度信息转换成平面坐标，从而反算航偏角；

3、通过对时变步长的窗口进行数据特征提取，动态监测是否执行解算过程，该特征检测为短时域特种功能检测，利用的数据是窗口长度5秒内的时间序列，与传统解算方法相比，用时更短；

4、通过GPS可以得到绝对位置，而惯导系统可以解算相对位移，利用松组合的方法可以实现基于GPS/IMU的组合导航系统；

5、实时判断接收到的数据的可用性，自动计算数据是否在系统解算的阈值之内，如果未达到解算阈值，则关闭IMU解算相对位移的线程，只通过GPS进行导航应用，直到数据达到解算要求，才进行初始姿态解算，进而利用GPS和惯性设备的数据，进行融合导航定位。

附图说明

图1是本发明具体实施例中一种骑行码表初始姿态自动校准方法结构示意图；

图2是本发明具体实施例中一种骑行码表初始姿态自动校准方法流程示意图；

图3是本发明具体实施例中GPS反算航向方法流程示意图；

图4是本发明具体实施例中平面化坐标转换方式示意图；

图5是本发明具体实施例中满足精度要求的位置序列选取方法流程示意图；

图6是本发明具体实施例中一种骑行码表设备模块结构示意图。

具体实施方式

为了使本领域的人员更好地理解本发明的技术方案，下面结合本发明的附图，对本发明的技术方案进行清楚、完整的描述，基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的其它类同实施例，都应当属于本申请保护的范围。此外，以下实施例中提到的方向用词，例如“上”“下”“左”“右”等仅是参考附图的方向，因此，使用的方向用词是用来说明而非限制本发明创造。

如图1-2所示，一种骑行码表初始姿态自动校准方法，包括：

S100、获取GPS测量的经纬度数值以及IMU测量的加速度和角增量数值；

S200、将经纬度数值和加速度、角增量数值分别与预设GPS和IMU状态参数相比较，不断解算满足GPS状态为可用或IMU状态为静置时的航偏角速度矢量横滚角θ和俯仰角γ；

S300、当GPS状态为可用且IMU状态为静置时，将解算得到的航偏角速度矢量横滚角θ_i和俯仰角γ_i作为码表初始姿态输出，完成自动对准。

惯性传感器只能得到载体的相对变化，如果想获得载体的绝对位置，则需要给定载体的初始经纬信息、方向角信息以及速度信息，再通过迭代解算出位置。通过在惯性测量单元静置状态解算得到初始俯仰角和横滚角，再利用GPS反算得到航向和速度后，便可以得到所有的初始状态，在解算完成俯仰角和横滚角的时间到GPS反算航向的时间内，俯仰角和横滚角通过实时采集的IMU数据进行更新，从而确保初始状态解算数值的准确性。

进一步的，如图3所示，航偏角的计算方法包括：当GPS状态为可用时，获取经纬坐标并将其转为空间大地坐标，在空间大地坐标内不断滑动窗口，选取满足精度要求的位置序列反算航偏角。

该部分为骑行码表设备中特征数据检测模块的主要处理方法，该模块接收到的数据为经过坐标转换后的空间大地坐标(x,y,z)，主要功能为检测坐标序列内的数据是否符合解算要求，符合要求的才进行航向反算，不符合要求则继续迭代，直到选取到符合要求的数据序列。

将经纬坐标转为空间大地坐标，即将(经度，纬度，高度)的坐标表示形式转化为(x，y，z)的形式，如图4所示，由于进行特征检测方向斜率解算航向需要在欧几里得空间内进行，所以先要将球面空间的坐标进行平面化，通过高斯克吕格投影的方法实现坐标转换，其主要步骤包括：

输入：经度lon,纬度lat

iPI＝1/π

Zonewide＝3 or 6(应用于骑行差别不大)

a＝6378137(地球长半轴)

f＝1/298.257223563(扁率)

分带解算

projNo＝floor(lon/Zonewide)，floor(·)为向下取整

中央子午线：

longitude0＝longitude0*iPI

latitude0＝0

角度转弧度

longitude1＝lon*iPI

latitude1＝lat*iPI

中间变量解算

e2＝2*f-f²

ee＝e2*(1-e2)

T＝tan(latitudel1)²

C＝ee*cos(latitudel1)²

A＝(longitudel1-longitudel0)*cos(latitudel1)

坐标转换

该部分为骑行码表设备中坐标系转换模块的主要处理方法，通过转换将球面空间的坐标进行平面化，从而便于数据的计算使用。

进一步的，如图5所示，在空间大地坐标内不断滑动窗口，选取满足精度要求的位置序列反算航偏角的方法包括：基于转换的空间大地坐标，获取预设窗口范围内连续历元的平面坐标时间序列，基于平面坐标时间序列内相邻两坐标进行航向反算得到方向序列和对应的均方误差，若均方误差小于预设精度阈值，则基于方向序列进行航偏角反算；若均方误差大于预设精度阈值，则预设窗口前进一个步长单位，重新获取新的平面坐标时间序列进行反算。

具体来讲，通过坐标转换，可以得到五秒内的经纬数据转换成大地坐标的时间序列：

(L B)→(x y)

得到平面坐标时间序列如下：

p＝{(x y)_i|i＝1,2,3,4,5}

对向量p内的相邻两坐标进行航向反算，便可得到方向序列，并求解均方误差，因为该数值反映的是数据的收敛情况，此处设置的收敛值为2°，也就是均方误差小于该值便可以航偏角反算：

θ＝{θ_i|i＝1,2,3,4}

如果std<2(该值的设定需要结合具体产品的经度设定，精度不同的话，需要根据具体情况调参)，则进行航偏角反算，为了满足快速达解算的需求，此处的窗口长度设定为5，即批处理5s内的数据(可以理解该延迟只会影响前五秒，不会影响后续的解算)；步长则是窗口移动的速度，这与数据的更新速度有关，由于本系统数据更新频率为50Hz，所以步长为步/0.02s；因为本系统不进行数据反向解算，窗口移动方向是数据更新的时间序列。

进一步的，选取满足精度要求的位置序列反算航偏角的方法包括：

获取位置序列z_i＝{(x,y,z)_m|m＝i,i+1,...,i+n}，其中，i为位置序列对应的起始时间历元，(x,y,z)对应的坐标系是东北天地理坐标系；

将位置序列坐标投影到平面上得到平面坐标集合

pos_i＝{(x,y)m|m＝i,i+1,...,i+n}；

将平面坐标集合整理得到方程组：

基于最小二乘法解算得到[bias k]＝y^T·x·(x^T·x)^-1，其中，k为东北天地理坐标系下窗口内骑行路径的方向斜率；

基于反正切函数求解航偏角

该部分为骑行码表设备中方向斜率解算航偏角模块的主要处理方法，特征数据检测模块在完成特征数据检测后，会得到符合解算要求的一段坐标序列，该段坐标序列作为方向斜率解算航偏角模块的输入，并利用最小二乘反算该序列的空间方向斜率，再反算三角函数，得到初始航偏角。

进一步的，基于获取的平面坐标集合计算速度标量其中，n为窗口长度，(x y)₁为窗口内第一个平面坐标，(x y)_n为窗口内最后一个平面坐标。因为本系统GPS更新频率为1Hz，所以窗口长度刚好与采样间隔时间在数值上一致，故不需要再乘以比例放缩系数。

进一步的，横滚角和俯仰角的计算方法包括：当IMU状态为静置时，加速度计测量的是重力在载体坐标系下的投影，即重力和加速度计的比力关系为：

由于MEMS传感器的器件误差较大，可以不考虑重力在北方向和东方向的投影，即不考虑重力的垂线偏差，其中，f_ib ^b为比力输出值，f_ib ^b＝[f_ibx ^b,f_iby ^b,f_ibz ^b]表示加速度计x、y、z三轴的输出，为重力加速度，进而推导出横滚角θ和俯仰角γ的解算公式为：

由于自行车导航是在地表，也就是海拔高度没有达到航天航空领域的水平，海拔对重力的倾斜影响不显著，因而可以忽略重力的垂线偏差对初始姿态角影响。

进一步的，实时获取IMU测量的加速度和角增量数值，利用四阶龙格库塔法动态更新姿态角的解算公式，具体方法如下：

设置初始四元数，其中，为初始航偏角，θ₀为初始横滚角，γ₀为初始俯仰角，q为四元数向量；

进而得到四阶龙格库塔更新四元数

通过数据采样对上式相关元素进行解算：其中，h为数据采样间隔，q_(t)、q_(t+h)分别为t、t+h时刻的四元数，分别表示陀螺仪的x、y、z三轴采集到的的实时角增量；

对更新后的四元数进行归一化得到

解算出姿态角

进一步的，基于获取的速度标量和姿态角解算三轴速度矢量具体方法包括：

基于姿态角求解姿态阵

基于姿态阵和速度标量求解速度矢量

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现一种骑行码表初始姿态自动校准方法。

如图6所示，一种骑行码表设备，包括：传感器、GPS、处理器及存储器，所述传感器与处理器通过通讯协议信号连接，所述GPS与处理器通过串口信号连接，所述存储器与处理器通过SPI信号连接；

所述存储器用于存储计算机程序，所述处理器用于执行所述存储器存储的计算机程序，以使所述处理器执行一种骑行码表初始姿态自动校准方法。

进一步的，一种骑行码表设备，包括GPS反算航向模块和惯性传感器解算初始姿态角模块，所述GPS反算航向模块和惯性传感器解算初始姿态角模块以松组合方式进行融合导航定位。GPS可以得到绝对位置，而惯导系统可以解算相对位移，利用松组合的方法可以实现基于GPS/IMU的组合导航系统。GPS瞬时稳定性低，误差大，离散度大，但是平均误差较小，成本低，该过程利用了GPS定位的平均定位误差较小的特点，利用短时域窗口内的连续定位数据进行位置拟合。

进一步的，GPS反算航向模块包括坐标系转换模块、特征数据检测模块和方向斜率解算航偏角模块。

相关专业词汇解释：

IMU：惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。

松组合：松组合导航系统是一种相对简单的组合方式，在该方式下，GPS与SINS(捷联惯导系统)各自独立工作，最终将两者数据融合，用于修正SINS系统的相关参数，最终给出较好的导航估计结果。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

以上已将本发明做一详细说明，以上所述，仅为本发明之较佳实施例而已，当不能限定本发明实施范围，即凡依本申请范围所作均等变化与修饰，皆应仍属本发明涵盖范围内。

Claims (8)

1.一种骑行码表初始姿态自动校准方法，其特征在于，包括：

获取GPS测量的经纬度数值以及IMU测量的加速度和角增量数值；

将经纬度数值和加速度、角增量数值分别与预设GPS和IMU状态参数相比较，不断解算满足GPS状态为可用或IMU状态为静置时的航偏角速度矢量横滚角θ和俯仰角γ，其中，航偏角的计算方法包括：当GPS状态为可用时，获取经纬坐标并将其转为空间大地坐标；基于转换的空间大地坐标，获取预设窗口范围内连续历元的平面坐标时间序列，基于平面坐标时间序列内相邻两坐标进行航向反算得到方向序列和对应的均方误差，若均方误差小于预设精度阈值，则基于方向序列进行航偏角反算；若均方误差大于预设精度阈值，则预设窗口前进一个步长单位，重新获取新的平面坐标时间序列进行反算；

当GPS状态为可用且IMU状态为静置时，将解算得到的航偏角速度矢量横滚角θ_i和俯仰角γ_i作为码表初始姿态输出，完成自动对准。

2.根据权利要求1所述的一种骑行码表初始姿态自动校准方法，其特征在于，选取满足精度要求的位置序列反算航偏角的方法包括：

获取位置序列z_i＝{(x,y,z)_m|m＝i,i+1,...,i+n}，其中，i为位置序列对应的起始时间历元，(x,y,z)对应的坐标系是东北天地理坐标系；

将位置序列坐标投影到平面上得到平面坐标集合

pos_i＝{(x,y)m|m＝i,i+1,...,i+n}；

将平面坐标集合整理得到方程组：

基于最小二乘法解算得到[biask]＝y^T·x·(x^T·x)^-1，其中，k为东北天地理坐标系下窗口内骑行路径的方向斜率；

基于反正切函数求解航偏角

3.根据权利要求2所述的一种骑行码表初始姿态自动校准方法，其特征在于，基于获取的平面坐标集合计算速度标量其中，n为窗口长度，(x y)₁为窗口内第一个平面坐标，(x y)_n为窗口内最后一个平面坐标。

4.根据权利要求3所述的一种骑行码表初始姿态自动校准方法，其特征在于，横滚角和俯仰角的计算方法包括：当IMU状态为静置时，加速度计测量的是重力在载体坐标系下的投影，即重力和加速度计的比力关系为：

其中，f_ib ^b为比力输出值，表示加速度计x、y、z三轴的输出，为重力加速度，进而推导出横滚角θ和俯仰角γ的解算公式为：

5.根据权利要求4所述的一种骑行码表初始姿态自动校准方法，其特征在于，实时获取IMU测量的加速度和角增量数值，利用四阶龙格库塔法动态更新姿态角的解算公式，具体方法如下：

设置初始四元数，其中，为初始航偏角，θ₀为初始横滚角，γ₀为初始俯仰角，q为四元数向量；

进而得到四阶龙格库塔更新四元数

通过数据采样对上式相关元素进行解算：其中，h为数据采样间隔，q_(t)、q_(t+h)分别为t、t+h时刻的四元数，分别表示陀螺仪的x、y、z三轴采集到的的实时角增量；

对更新后的四元数进行归一化得到

解算出姿态角

6.根据权利要求5所述的一种骑行码表初始姿态自动校准方法，其特征在于，基于获取的速度标量和姿态角解算三轴速度矢量具体方法包括：

基于姿态角求解姿态阵

基于姿态阵和速度标量求解速度矢量

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：该程序被处理器执行时实现权利要求1至6中任一项所述的方法。

8.一种骑行码表设备，其特征在于，包括：传感器、GPS、处理器及存储器，所述传感器与处理器通过通讯协议信号连接，所述GPS与处理器通过串口信号连接，所述存储器与处理器通过SPI信号连接；

所述存储器用于存储计算机程序，所述处理器用于执行所述存储器存储的计算机程序，以使所述处理器执行如权利要求1至6中任一项所述的方法。