基于人体运动模型辅助的穿戴式行人导航定位方法和设备与流程
本发明涉及基于人体运动模型辅助的穿戴式行人导航定位方法和设备，属于个人移动定位技术领域。
背景技术：
行人定位导航作为近些年来民用导航技术发展的主要领域之一，正逐渐被重视和研究。近年来，个人定位设备逐步在民用化市场得到普及，而大多数民用级便携式设备采用的是gps卫星定位与无线通信技术相结合的定位原理，但是在大型城市、商场、隧道、丛林和山谷等卫星导航信号易受干扰或屏蔽的应用环境中，该类型设备的导航定位功能将受到严重影响。在某些特定行业中，如消防员在执行救援任务时，可能出现匍匐、跳跃、跑步等多种复杂运动模态，所携带的导航定位设备容易失效或者定位错误，而导致搜救任务失败，严重时甚至将危及使用人员的人身安全。针对无法使用卫星定位和通信定位产品的使用环境，国外厂商对我国技术垄断，产品价格昂贵。因此，迫切需要研究适应大型城市、商场、隧道、丛林和山谷等复杂应用环境下的针对人体多运动模态的行人导航定位技术，以适应相应的需求。
现有行人导航研究方向主要可分为以下两类：一是基于各类无线网络的源头定位，然而这类方法依赖额外设备(如wifi、蓝牙、uwb等)，定位精度受环境影响较大，易受干扰；二是基于惯性传感器的行人导航定位，以惯性器件为核心，具有短时高精度和高稳定性的特点，是一种完全自主的导航系统，然而该方案在长时间下误差会累积而迅速发散。目前，国外个人定位系统主要采用了微惯性/卫星导航实现组合定位，其对卫星导航依赖性较强；美国在基于人体运动学辅助的行人定位算法方面取得了较好的研究进展，已经完成了原型样机的研制，不过尚没有向民用领域进行大规模的推广应用。国内也有多家单位和研究所针对个人微惯性定位算法进行了研究，在系统构造以及传感器误差修正等方面开展了富有成效的工作，但总体上的研究水平与国外还具有一定的差距，在基于人体运动学辅助的行人定位算法研究方面尚处于起步阶段。其只能实现正常行走模态下的行人定位导航技术，而且多采用了额外设备辅助惯导的方式，既极大增加了成本，又约束了其适用范围，因此，迫切需要研究一种基于人体运动学建模辅助的定位技术，基于运动学特征建立人体运动模型，实时判别人体运动模式，实现在没有卫星导航与无线通信信号下人体多模态的自主实时移动定位。
对于单一自主的惯性导航，低成本的mems捷联解算高度误差较大，而且发散速度较快，必须要经过有效的修正，才能确保高度计算精度实际可用。捷联解算的航向误差也较大，容易发生波动，导致导航结果与实际路线不符，必须要经过有效的修正，才能提高导航结果的精度。行人在实际运动过程中存在多种运动模态，如快速行走、跑步、上下楼梯、跳跃等，同时，不同的人在运动过程中步态间也存在较大差异，步态的检测与分辨困难；而现有的国内外研究主要集中在行人的正常行走模态的辨识，对于快速行走、跑步、上下楼梯、跳跃等还鲜有提及。
技术实现要素：
为了解决上述存在的问题，本发明公开了基于人体运动模型辅助的穿戴式行人导航定位方法和设备，提供在gps信号以及无线通讯信号失效时行人的连续导航定位方法，该方法能够在行人正常步行，快速行走，慢跑，上下楼梯等运动模态下实现姿态，速度和位置解算，满足复杂应用环境下行人的高精度实时导航定位要求，其具体技术方案如下：
基于人体运动模型辅助的穿戴式行人导航定位方法，其特征在于包括以下操作步骤:
步骤1：分析人体运动时的步态以及不同运动模态下的惯性传感器输出，所述步态为人体运动时足部与地面的接触情况，所述不同运动模态包括步行、跑步、上下楼梯；
步骤2：建立基于人体运动学模型辅助的零速修正判别模型，所述基于人体运动学模型辅助的零速修正判别模型利用加速度计和陀螺仪的输出判断当前时刻是否为足部与地面的接触时刻；
步骤3：在步骤1和2的基础上，建立基于地磁辅助捷联惯导解算航向角误差模型，利用磁传感器输出信息计算当地磁航向角；
步骤4：在步骤1和2的基础上，建立基于气压高度计辅助捷联惯导解算高度误差模型，利用气压计输出信息计算当地海拔高度。
所述步骤1中人体运动步态以及不同运动模态具有以下特征：
在正常行走时，行人的双脚交替运动，分为四个阶段：抬脚、跨步、落地和支撑，两只脚交替运动，分别处于不同的时刻，因此，将传感器件固定于其中一只脚上，对其进行运动分析，此时，人体足部不能被视为一个质点，在判断零速时刻时，应对落地、支撑阶段作进一步的分析；
当以脚尖上一点为质点时，在落脚阶段，脚后跟先与地面接触，接着以脚后跟为支点整个脚底部逐渐放平直至完全接触地面，即脚尖与地面接触，然后，以脚尖为支点，整个脚底部逐渐抬升直至完全离开地面，即脚尖与地面分离，脚尖上质点与地面的实际接触时间为一个步态期间零速度的时间；
行人在快速行走时，步态周期将比正常行走的周期短，但是两者足部的运动过程是相似的，在这两种运动模态下，足部大约有一半的时间是处于抬脚、跨步阶段，而相应地，另一只脚同时处于落地和支撑阶段，在大步行走时，足部运动过程仍然是与正常行走时相似，步态周期更长，这两种运动模态下，足部运动的周期性和对称性并未发生改变；
在跑步运动模态中，则不存在这种对称性，当一只脚的后跟落地时，另一条腿可能还在空中，甚至可能出现两条腿同时处于空中的状态，但是，跑步运动模态下一只脚落地支撑阶段的零速时刻与正常行走时是类似的，但其足部与地面的接触时间更短些；
在上下楼梯运动模态中，在抬脚阶段增加了高度信息的变化，这并不影响零速时刻的判断，落地支撑阶段，足部的零速时刻与正常行走是类似的，但是，与正常行走模态不同的是，落地阶段不再是脚后跟先接触地面，而是足部的前端先落地，然后以其为支点，脚底部逐渐放平直至完全与地面接触，在实际情况中，行人的脚底部可能并不是完全地与地面接触，通常接触部分只有脚掌的前大半部分，此时，脚后跟是悬空的状态；
通过对不同运动模态下imu惯性传感器件三轴数据信息的采集，能够得出，在不同的运动模态下，陀螺仪和加速度计的三轴输出信息与正常行走时相似，仍然具有周期性，行人在快速行走或跑步较剧烈运动模态下，imu惯性传感器件的输出变化也更快，此时，imu惯性传感器件的输出或用于零速判定的条件会失效；在匀速行走等正常模态下，imu惯性传感器件的输出较为平缓；在上下楼梯运动模态下，能够看出，imu惯性传感器件的输出更加平缓，为了保持平衡，人体在足部落地阶段需要停留更长的时间。
所述步骤2中建立基于人体运动学模型辅助的零速修正判别模型的算法为：
在人体行走过程中，足部运动包含抬脚、跨步、落地、支撑四个阶段，在落地支撑阶段存在一段时间足部的速度为零，需要对陀螺仪和加速度计的输出信息进行判断，当满足条件时，即在算法上将速度置零，当检测到足部为零速度时，采用加速度计的测量值重新求解姿态角，以抑制惯导误差的发散：
其中，θ、γ依次是俯仰角、横滚角，
所述对陀螺仪和加速度计的输出信息进行判断用的判断方法是：以陀螺仪输出为主判断条件、加速度计输出为辅判断条件的滑动窗口法，滑动窗口的宽度设为n，n的大小根据不同的步态情况设置为不同的值，先设置如下参数：
stdy＝std(datay,0,1)
stdz＝std(dataz,0,1)
ωxωyωz依次是陀螺仪x、y、z三轴输出的极大值和极小值之差，a是加速度计x、y、z三轴输出值平方和开方后的最大值，stdy，stdz依次是加速度计y、z轴方差，axi,ayi,azi分别为加速度计x轴，y轴，z轴滑动窗口内从0到n时刻的输出值；
为判断当前时刻是否为零速时刻，取当前时刻后的一小段时间，分别利用加速度计信息和陀螺仪信息来判断输出数据是否满足阈值设置，在不同模态下设置相应的参数，加速度计三轴模值以及方差输出满足条件时，人为地将速度置零；陀螺仪的任一轴满足条件时，速度置零的同时更新姿态角信息，并对误差进行观测和补偿，以获得更加精确的姿态和位置信息。
所述步骤3中的建立基于地磁辅助捷联惯导解算航向角误差模型的方法为：
将磁传感器获取的当地三轴磁数据投影到水平面上，将磁信息从机体系转换到导航系下，
其中，θ、γ依次是俯仰角、横滚角，
然后根据投影后水平方向的磁传感器数据计算磁航向角，三轴磁信息的正负不同，所对应的计算方式也不同：
ψe为待求解的航向角信息；为提高磁航向的精度，事先对磁传感器进行椭圆标定：首先使用传统最小二乘椭圆拟合算法求解磁传感器数据的初始最佳拟合椭圆参数，随后由初始参数通过实时递推能够获得实时的最佳拟合椭圆参数，椭圆方程为：
ax2+by2+cz2+2dxy+2exz+2fyz+2gx+2hy+2iz＝1，
方程中a,b,c,d,e,f,g,h,i均为椭圆拟合的参数，x,y,z分别对应所拟合三维椭球体的三轴，接着对后续测得的磁传感器数据(hx,hy)进行坐标平移、旋转和拉伸，将其映射为正圆坐标(h'x,h'y)；最后通过补偿的磁传感器数据，计算求得磁航向角
所述步骤4中的建立基于气压高度计辅助捷联惯导解算高度误差模型的方法为：
利用大气压随水平高度升高按指数律递减的关系，通过感受行人导航定位设备所处位置的大气压的大小，推算所处位置的海拔高度，气压高度测量模型为：
气压高度(m):
上式中，hpb为所求气压高度，t0为传感器所处环境温度，psb为传感器所处环境气压值，ps0为海平面气压高度值，τ,r均为气压高度求解参数。
一种实现基于人体运动模型辅助的穿戴式行人导航定位方法的设备，该设备能够实现基于人体运动模型辅助的穿戴式行人导航定位方法，该设备内置有imu惯性传感器件、姿态解算模块、蓝牙模块和按钮。
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
本发明在分析人体运动时的步态以及不同运动模态下的惯性传感器输出的基础上，提出零速修正判别算法辅助捷联惯导解算以修正捷联解算的速度位置等信息随时间发散的问题；同时建立地磁航向辅助模型以及气压高度辅助模型以修正航向角和高度解算误差。
本发明很好地解决纯惯性解算下速度位置的发散问题，提高无gps和无线通信信号下行人导航定位的精度和可靠性。
附图说明
图1是本发明个人导航定位算法解算流程图，
图2是本发明人体正常行走模态下陀螺仪和加速度计三轴输出示意图，
图3是本发明人体跑步模态下陀螺仪和加速度计三轴输出示意图，
图4是本发明人体上下楼梯模态下陀螺仪和加速度计三轴输出示意图，
图5是本发明根据加速度计和陀螺仪信息判断出的零速区间示意图，
图6是本发明三轴磁传感器地磁场测量原理图，
图7是本发明纯惯性高度通道示意图，
图8是本发明二阶气压阻尼算法示意图，
图9是本发明行人实际行走路线图，
图10是图9行人二维轨迹相对位置仿真图，
图11是图9行人三维轨迹相对位置仿真图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明。应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
基于人体运动模型辅助的穿戴式行人导航定位方法，包括以下操作步骤:
步骤1：分析人体运动时的步态以及不同运动模态下的惯性传感器输出，所述步态为人体运动时足部与地面的接触情况，所述不同运动模态包括步行、跑步、上下楼梯；
步骤2：建立基于人体运动学模型辅助的零速修正判别模型，所述基于人体运动学模型辅助的零速修正判别模型利用加速度计和陀螺仪的输出判断当前时刻是否为足部与地面的接触时刻；
步骤3：在步骤1和2的基础上，建立基于地磁辅助捷联惯导解算航向角误差模型，利用磁传感器输出信息计算当地磁航向角；
步骤4：在步骤1和2的基础上，建立基于气压高度计辅助捷联惯导解算高度误差模型，利用气压计输出信息计算当地海拔高度。
所述步骤1中人体运动步态以及不同运动模态具有以下特征：
在正常行走时，行人的双脚交替运动，分为四个阶段：抬脚、跨步、落地和支撑，两只脚交替运动，分别处于不同的时刻，因此，将传感器件固定于其中一只脚上，对其进行运动分析，此时，人体足部不能被视为一个质点，在判断零速时刻时，应对落地、支撑阶段作进一步的分析；
当以脚尖上一点为质点时，在落脚阶段，脚后跟先与地面接触，接着以脚后跟为支点整个脚底部逐渐放平直至完全接触地面，即脚尖与地面接触，然后，以脚尖为支点，整个脚底部逐渐抬升直至完全离开地面，即脚尖与地面分离，脚尖上质点与地面的实际接触时间为一个步态期间零速度的时间；
行人在快速行走时，步态周期将比正常行走的周期短，但是两者足部的运动过程是相似的，在这两种运动模态下，足部大约有一半的时间是处于抬脚、跨步阶段，而相应地，另一只脚同时处于落地和支撑阶段，在大步行走时，足部运动过程仍然是与正常行走时相似，步态周期更长，这两种运动模态下，足部运动的周期性和对称性并未发生改变；
在跑步运动模态中，则不存在这种对称性，当一只脚的后跟落地时，另一条腿可能还在空中，甚至可能出现两条腿同时处于空中的状态，但是，跑步运动模态下一只脚落地支撑阶段的零速时刻与正常行走时是类似的，但其足部与地面的接触时间更短些；
在上下楼梯运动模态中，在抬脚阶段增加了高度信息的变化，这并不影响零速时刻的判断，落地支撑阶段，足部的零速时刻与正常行走是类似的，但是，与正常行走模态不同的是，落地阶段不再是脚后跟先接触地面，而是足部的前端先落地，然后以其为支点，脚底部逐渐放平直至完全与地面接触，在实际情况中，行人的脚底部可能并不是完全地与地面接触，通常接触部分只有脚掌的前大半部分，此时，脚后跟是悬空的状态；
通过对不同运动模态下imu惯性传感器件三轴数据信息的采集，能够得出，在不同的运动模态下，陀螺仪和加速度计的三轴输出信息与正常行走时相似，仍然具有周期性，行人在快速行走或跑步较剧烈运动模态下，imu惯性传感器件的输出变化也更快，此时，imu惯性传感器件的输出或用于零速判定的条件可能会失效；在匀速行走等正常模态下，imu惯性传感器件的输出较为平缓；在上下楼梯运动模态下，能够看出，imu惯性传感器件的输出更加平缓，为了保持平衡，人体在足部落地阶段需要停留更长的时间。
根据权利要求2所述的基于人体运动模型辅助的穿戴式行人导航定位方法，其特征在于所述步骤2中建立基于人体运动学模型辅助的零速修正判别模型的算法为：
在人体行走过程中，足部运动包含抬脚、跨步、落地、支撑四个阶段，在落地支撑阶段存在一段时间足部的速度为零，需要对陀螺仪和加速度计的输出信息进行判断，当满足条件时，即在算法上将速度置零，当检测到足部为零速度时，采用加速度计的测量值重新求解姿态角，以抑制惯导误差的发散：
其中，θ、γ分别是俯仰角、横滚角，
所述对陀螺仪和加速度计的输出信息进行判断用的判断方法是：以陀螺仪输出为主判断条件、加速度计输出为辅判断条件的滑动窗口法，滑动窗口的宽度设为n，n的大小根据不同的步态情况设置为不同的值，先设置如下参数：
stdy＝std(datay,0,1)
stdz＝std(dataz,0,1)
ωxωyωz分别是陀螺仪x、y、z三轴输出的极大值和极小值之差，a是加速度计x、y、z三轴输出值平方和开方后的最大值，stdy，stdz分别是加速度计y、z轴方差，
为判断当前时刻是否为零速时刻，取当前时刻后的一小段时间，分别利用加速度计信息和陀螺仪信息来判断输出数据是否满足阈值设置，在不同模态下设置相应的参数，加速度计三轴模值以及方差输出满足条件时，人为地将速度置零；陀螺仪的任一轴满足条件时，速度置零的同时更新姿态角信息，并对误差进行观测和补偿，以获得更加精确的姿态和位置信息。
所述步骤3中的建立基于地磁辅助捷联惯导解算航向角误差模型的方法为：将磁传感器获取的当地三轴磁数据投影到水平面上，将磁信息从机体系转换到导
航系下，
其中，θ、γ分别是俯仰角、横滚角，
然后根据投影后水平方向的磁传感器数据计算磁航向角，三轴磁信息的正负不同，所对应的计算方式也不同：
为提高磁航向的精度，事先对磁传感器进行椭圆标定：首先使用传统最小二乘椭圆拟合算法求解磁传感器数据的初始最佳拟合椭圆参数，随后由初始参数通过实时递推能够获得实时的最佳拟合椭圆参数，椭圆方程为：
ax2+by2+cz2+2dxy+2exz+2fyz+2gx+2hy+2iz＝1，
接着对后续测得的磁传感器数据(hx,hy)进行坐标平移、旋转和拉伸，将其映射为正圆坐标(h'x,h'y)；最后通过补偿的磁传感器数据，计算求得磁航向角
所述步骤4中的建立基于气压高度计辅助捷联惯导解算高度误差模型的方法为：
利用大气压随水平高度升高按指数律递减的关系，通过感受行人导航定位设备所处位置的大气压的大小，推算所处位置的海拔高度，气压高度测量模型为：
气压高度(m):
本发明设备在实际导航定位过程中，用胶带将本设备固定于人体足部脚背位置。本设备内置了imu惯性传感器件，姿态解算模块，蓝牙模块和按钮。
如图1所示，本发明复杂应用环境下基于人体运动模型辅助的可穿戴式行人导航定位算法解算流程有以下步骤：
步骤1，惯性测量单元获取行人运动时的加速度和角加速度信息，经过误差补偿，陀螺仪数据求解四元数并进行规范化，由四元数得到姿态变换矩阵并将获取的加速度计信息进行比力转换，由姿态转换矩阵求解姿态角，由四元数方程和上一时刻的导航信息求解速度和位置信息并输出；
步骤2，在姿态解算期间，通过加速度计和陀螺仪的输出信息判断当前时刻是否处于零速时刻，若满足加速度计或陀螺仪判定条件，则对应的标志位将被置位，若在一段时间内对应的标志位连续有效，则将当前时刻视为零速时刻，将速度置零的同时，利用加速度计信息修正姿态角。
本发明的具体实施方式如下：
1、分析人体运动时的步态以及不同运动模态下的惯性传感器输出
人体运动过程中，双脚交替运动，以其中一只为例，足部落地阶段可分为四个阶段：抬脚、跨步、落地、支撑。将惯性传感器件置于人体足部，通过分析惯性传感器输出信息可得，如图2所示，足部完全与地面接触的时间约为0.2-0.4秒之间，这个时间段内应对其进行零速修正。
人体在跑步过程中，可能会出现双脚同时离地的情况，此时无法进行零速修正。分析跑步模态下的惯性传感器输出信息，如图3所示，加速度计和陀螺仪的信息变化更剧烈，周期变化较短。但是，跑步模态下一只脚落地支撑阶段的零速时刻与正常行走时是类似的，足部完全与地面接触的时间变短，但这时仍能够进行零速修正。
人体在上下楼梯上足部的运动与正常行走时类似，分析该模态下的惯性传感器输出信息，如图4所示，可看出加速度计和陀螺仪的信息变化较为缓慢，周期变短，为了保持平衡，人体在足部落地阶段需要停留更长的时间。这一阶段也可利用零速进行修正。
2、建立基于人体运动学模型辅助的零速修正判别模型
基于人体运动学分析，根据加速度计和陀螺仪的输出特性设置零速判定条件，如图5所示，由于运动过程中imu惯性传感器件的输出与静止时刻imu惯性传感器件的输出在数值上有较大的差异，为了显示的方便，将运动过程中imu惯性传感器件的输出置为零，即输出为零的时间段是人体的运动过程，而输出不为零的时间段为算法判定为零速的时刻。可知单条件判断可能存在误判情况，导致零速判断不准确，而联合判断则具有较高的准确性。因此，在行人导航定位方法中，我们使用加速度计和陀螺仪联合判断条件来判断当前时刻是否是零速时刻。
3、建立基于地磁辅助捷联惯导解算航向角误差模型
地球是一个大磁体，地球本身以及周围空间存在的磁场叫“地球磁场”，简称“地磁场”，其主要部分是一个偶极场。地面任何一点的磁场的磁感应强度矢量具有一定的大小和方向。如图6所示，由于地磁的南北极与地理的南北极不完全重合，存在磁偏角。磁偏角即是当地磁场强度矢量的水平投影与正北方向之间的夹角，即磁子午线与地理子午线之间的夹角。
磁传感器通过测量地球磁场的三维投影来确定载体的航向信息，为了减少外界干扰对磁强计的精度影响，因此事先要对其进行标定。目前最常用的就是最小二乘椭圆拟合法，该方法在校准过程具有较高的抗突变干扰能力，可在不增加成本的情况下实现高精度的对准。
4、建立基于气压高度计辅助捷联惯导解算高度误差模型
如图7所示为纯惯性高度通道模型框架，垂直加速度计测得比力后，经过误差补偿，两次积分得到高度信息。由于捷联惯导高度通道误差较大且发散较快，需要利用外部信息进行高度方向误差修正以满足所需精度。如图8所示为利用大气传感器进行高度辅助的二阶气压阻尼算法模型框架，大气传感器利用大气压随高度升高按指数律递减的关系推算所处位置的海拔高度。其中，
图10给出了采用本行人导航定位设备进行定位的二维效果图，图9是本发明行人实际行走路线图，测试的地点为学院楼的走廊，测试人员将该设备固定于足部，按照一定的路线开始运动，无论是方形路线还是直线路线，运动轨迹与实际路线基本吻合。图11为采用本行人导航定位设备进行定位的的三维效果图，测试地点为学院楼，行走路线为学院走廊l形→下楼→走廊l形→上楼，形成闭合曲线，高度方向未发散，高度方向变化与楼层高度基本吻合，导航效果较好。由试验可知，该行人导航定位设备满足复杂情况下行人导航定位精度，适用于工程实践当中。
本发明方案所公开的技术手段不仅限于上述技术手段所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全能够在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
- 还没有人留言评论。精彩留言会获得点赞！