1.一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,包括以下步骤:
步骤1,在行人左右腿的踝关节处安装测距模块及惯性节点,其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息,测距模块实时采集两个节点间的距离信息;
步骤2,初始阶段,利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息;
步骤3,利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计,区分运动状态与静止状态;
步骤4,对于静止状态的节点通过零速修正减小误差,对于运动状态的节点利用陀螺仪对姿态角信息进行实时更新,得到节点当前姿态信息,并结合参考姿态角信息得到姿态角变化的结果;
步骤5,利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息;
步骤6,根据测距模块提供的两节点间的距离信息以及矢量的角度信息,获取节点的实时位置信息;
步骤7、综合行人单步的运动信息,实现人员定位功能。
2.根据权利要求1所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤1中,在行人左右腿的踝关节处安装测距模块及惯性节点,其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息,测距模块实时采集两个节点间的距离信息,具体包括:通过调整测距模块的功率、选择合适的定向天线及馈线等方式来实现测距模块的近距离高精度测距功能,可以实时测量踝关节节点间的距离信息。
通过惯性节点实时采集节点处的运动信息。
3.根据权利要求1所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤2中,初始阶段,利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息;在初始阶段,人体的左足与右足均处于支撑阶段,此时,踝关节节点处于静止状态,因此,可以通过加速度计与磁力计计算该状态下的姿态角信息,作为踝关节节点的初始姿态角即参考姿态角,如式(1)所示:其中,θ1、γ1、ψ1分别代表俯仰角、横滚角、航向角,Ax、Ay、Az分别代表三轴加速度信息,mx、my、mz代表三轴磁力计的强度信息。
4.根据权利要求3所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤3中,利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计,区分运动状态与静止状态,具体包括:通过阈值条件,对足部的运动状态进行判断,判断方法如式(2)所示:其中,Ath代表区分运动状态与静止状态的阈值,Anorm代表合加速度,当合加速度Anorm大于阈值Ath时,代表节点处于运动状态;当合加速度Anorm小于阈值时Ath,代表节点处于静止状态。
5.根据权利要求4所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤4中,当踝关节节点处于静止状态时,通过卡尔曼滤波技术进行数据融合,估计系统的误差并利用误差的估计值对系统参数进行校正,如式(3)所示:其中,F是误差模型与状态量所构成的系统矩阵,W为系统随机过程噪声序列,V是系统观测噪声序列;
当踝关节节点处于运动状态,通过陀螺仪对节点的姿态角信息进行更新,如式(4)所示:
其中,q0、q1、q2、q3是四元数信息,θ2、γ2、ψ2是实时姿态角信息,通过陀螺仪对四元数进行更新,可以得到实时的姿态角信息即节点的姿态信息,将实时姿态角信息与参考姿态角信息作差,得到姿态角的变换信息,如式(5)所示:
6.根据权利要求5所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤5中,利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息,具体包括:在实际运动过程中,三轴加速度计可以敏感到比力在载体坐标系的三个方向上的分力,通过分力的比例关系可以得到在载体坐标系下的矢量的角度信息,结合步骤4中姿态角的变化信息与载体坐标系下的矢量的角度信息,计算得到最终的相对于初始姿态角的矢量的角度信息,在行人的矢状面,通过矢量关系可以得到方向角度信息,如式(6)、式(7)所示:θy=arccos(Ay/Anorm) (6)
θsum=θy+θ (7)
其中,θy是载体坐标系的y轴加速度Ay与合加速度Anorm的夹角,θ是当前姿态角相比于初始姿态角的变化信息,θsum是经过姿态角变化补偿后的矢量在行人矢状面的角度变化信息。
7.根据权利要求6所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤6中,根据测距模块提供的两节点间的距离信息以及矢量的角度信息获取节点的实时位置信息,具体包括:在步骤5中可以得到每个时间片段内的矢量的角度信息,结合测距模块提供的两节点间的距离信息可以得到矢量的全部信息,最终,通过函数关系可以获取节点实时的位置信息,如式(8)所示:其中,L为测距模块测量的两节点间的距离信息,即矢量长度信息,α为矢量在矢状面的角度信息,即矢量的角度信息,y为矢状面的步进距离信息,h为矢状面的步高信息,整个过程中,需要对矢量的角度信息即α不断进行更新,在运动过程中,运动节点相对静止节点从后向前摆动即完成一个跨步的过程可以分为三个阶段,第一阶段是运动节点在静止节点的后方,第二阶段是运动节点从静止节点的后方摆动到静止节点的前方,第三阶段是运动节点在静止节点的前方,通过矢量的角度信息可以对三个阶段进行区分。
8.根据权利要求7所述的一种基于人体分布惯性节点矢量测距的三维位姿测量方法,其特征在于,所述步骤7中,综合行人单步的运动信息,实现人员定位功能,具体包括:通过步骤1到步骤6可以精确计算单步的步长信息及单步内的姿态信息,综合各单步的运动信息,实现三维空间下人员的定位功能,计算公式如下:其中,xstep是单次跨步的水平位移距离,ystep是单次跨步的垂直位移距离,l是单步的步长信息,ψ是节点提供的航向信息,xsum是上一次测量条件下相对于初始坐标的水平位移距离,ysum是上一次测量条件下相对于初始坐标的垂直位移距离,x是本次测量条件下相对于初始坐标的水平位移距离,y是本次测量条件下相对于初始坐标的垂直位移距离。