一种基于人体下肢DH模型的行人惯性导航定位方法

一种基于人体下肢dh模型的行人惯性导航定位方法
技术领域
1.本发明属于行人惯性导航系统技术领域，涉及一种基于人体下肢dh模型的行人惯性导航定位方法。


背景技术：

2.随着mems技术的不断提升和应用的不断普及，行人惯性导航技术得到迅速发展目前行人惯性导航方法大多采用捷联式惯性导航技术，将惯性传感器固联于行人足部，根据足部与地面接触状态进行运动学分析，利用零速修正(zero velocity updata,zupt)方法实现自主性强、成本低的行人导航系统。
3.零速修正方法在支撑相建立足部速度为零的伪观测，能周期性地修正惯导系统误差，是约束惯导误差发散的有效手段，然而行人惯导系统中的航向状态具有较差的可观测性，航向漂移会引起很大位置误差，影响导航定位精度，仅利用零速修正方法难以满足可靠的行人导航定位需求。
4.专利号为2020108957717的申请公开了一种基于人体四节点运动约束的行人导航定位方法，根据先采集的行人的imu数据，解算行人的姿态、速度、位置信息，建立行人运动步态检测模型，进行零速修正而后根据基于双足不等式约束的滤波方法对行人航向进行修正，但利用该方法的导航系统的精度还是不够。


技术实现要素：

5.发明目的：针对以上缺点，本发明提供一种基于人体下肢dh模型的行人惯性导航定位方法，解决了行人惯性导航中航向发散问题，减少行人导航定位的累积误差，提高导航定位精度。
6.技术方案：为解决上述问题，本发明一种基于人体下肢dh模型的行人惯性导航定位方法，包括以下步骤：
7.(1)采集行人足绑式imu数据并进行预处理；
8.(2)利用dh准则，建立人体下肢模型；在人体下肢静止阶段时，于左大腿、右大腿、左小腿、右小腿的位置进行传感器的安装和校准；
9.(3)实时解算左足踝关节节点、右足踝关节节点共两节点的imu数据并进行零速检测，检测结果处于零速状态时进行零速修正，修正过程中获得左右双足的状态量；
10.(4)通过步骤(2)的传感器采集腿部四个节点数据并进行姿态的实时解算，获得实时更新的姿态角，通过正向运动学实时求解双足距离矢量；所述的四个节点分别为左腿髋关节节点、右腿髋关节节点、左腿膝关节节点、右腿膝关节节点；
11.(5)根据步骤(3)获得的左右双足的状态量，结合步骤(4)解算得到的双足距离矢量建立双足距离等式约束，并结合滤波方法对位置、速度和姿态进行修正，得到修正后的速度、姿态和位置。
12.进一步的，步骤(4)具体为：
13.(4.1)采集腿部四个节点数据，利用捷联惯导算法实时解算腿部四个节点imu姿态角，姿态角包括航向角ψ、俯仰角θ、滚转角γ；所述的四个节点分别为左腿髋关节节点、右腿髋关节节点、左腿膝关节节点、右腿膝关节节点；
14.(4.2)利用正向运动学求解双足距离矢量
15.(4.2.1)根据建立的dh模型参数，获得单只腿部节点的关节之间的齐次变换矩阵所述的关节i包括关节1、关节2、关节3、关节4，关节1、关节2、关节3对应髋关节的三个自由度，关节4对应膝关节的一个自由度；公式为：
[0016][0017]
其中，θi表示关节i的旋转角，i取1、2、3、4；θ1数值为单只腿部髋关节节点的imu航向角；θ2数值为单只腿部髋关节节点的imu滚转角，θ3数值为单只腿部髋关节节点的imu俯仰角；θ4数值为单只腿部膝关节节点的imu俯仰角，且θ1、θ2、θ3、θ4的数值均取自同一只腿的节点姿态角的数值；αi表示关节i的扭角，di表示表示关节i与关节i-1之间连杆的偏置；
[0018]
(4.2.2)获取单只腿部节点中髋关节与该只腿部踝关节的齐次变换矩阵公式为：
[0019][0020]
(4.2.3)提取第四列中的前三个元素得到单只腿部节点中髋关节与该只腿部踝关节的位置相对关系
[0021]
(4.2.4)足部两节点imu的位置相对关系表示为：
[0022][0023]
其中，表示左侧足绑式imu与左侧髋关节的位置相对关系，表示右侧足绑式imu与右侧髋关节的位置相对关系；t＝[-d 0 0]
t
表示左右侧髋关节的位置相对关系，d表示左右髋关节间距。
[0024]
进一步的，步骤(5)具体包括：
[0025]
(5.1)联立双足状态量，构建系统方程：
[0026]
x＝[x
l xr]
[0027]
其中，x
l
表示左脚imu的导航误差状态量、xr表示右脚imu的导航误差状态量；
[0028]
(5.2)根据x
l
和xr解算得到的双足imu相对距离模值为：
[0029]
dc＝f(l
·
x)
[0030]
其中,f( )为向量欧式距离求解函数,l＝[03×
6 i3×
3 03×
15
ꢀ‑
i3×
3 03×9]；
[0031]
(5.3)根据双足距离矢量获得足部两节点imu之间的相对距离模值为：
[0032][0033]
(5.4)建立双足等式约束，获得量测方程：
[0034]zdis
＝δd＝d
c-d
dh
＝f(l
·
x)-d
dh
[0035]
(5.5)根据系统方程和量测方程应用扩展卡尔曼滤波获得最优估计误差状态量后，对位置、速度、姿态和imu器件误差进行再次修正。
[0036]
进一步的，步骤(2)中的传感器的校准公式为：
[0037][0038]
其中，r
trans
表示初始旋转矩阵，θs表示初始姿态俯仰角，γs表示初始姿态的滚转角，ψs表示初始姿态的航向角。
[0039]
进一步的，步骤(1)中预处理具体包括以下步骤：
[0040]
(1.1)从imu静态数据中计算并扣除陀螺仪的零偏
▽b：
[0041][0042]
其中，s0表示静态区间开始时刻，s1表示静态区间结束时刻，w表示陀螺仪输出；
[0043]
(1.2)利用静态下加速度计的比力输出计算初始的姿态角，包括足部俯仰角θ和足部滚转角γ：
[0044][0045][0046]
其中，g
x
为足部imu的x轴的比力输出；gy为足部imu的y轴的比力输出；gz为足部imu的z轴的比力输出。
[0047]
进一步的，步骤(2)和步骤(3)中实时解算均采用捷联惯导方法，具体为：
[0048]
姿态更新算法公式如下：
[0049][0050]
其中，q表示四元数矩阵，t
k-1
表示k-1时刻，tk表示k时刻，即k-1时刻的下一采样时刻；ψ表示航向角、θ表示俯仰角、γ表示滚转角，表示姿态；
[0051]
速度更新算法公式如下：
[0052][0053]
其中，表示在k+1时刻载体在地理系下速度的微分，vn(t
k+1
)是k+1时刻载体在地理系下的运动速度；表示机体系到导航系的坐标转换矩阵；代表加速度计比力输出；为地球自转角速度在导航坐标系下的投影，表示导航系相对于地球系下的角速度在导航坐标系下的投影；gn表示当地重力加速度；
[0054]
位置更新算法公式如下：
[0055][0056]
其中，l表示代表经度，λ表示纬度，h表示高度；rm表示地球子午面内曲率半径，rn表示地球卯酉面内曲率半径；ve表示东向的速度矢量、vn表示北向的速度矢量、vu表示天向的速度矢量；δt表示采样时间间隔。
[0057]
进一步的，步骤(3)中零速检测具体为：
[0058][0059][0060][0061][0062]
zvd(k)＝s1(k)&s2(k)&s3(k)&s4(k)
[0063]
其中，代表第k至k+n时刻内的加速度的均值、σ
a,k:k+n
代表第k至k+n时刻内的加速度的标准差、代表第k至k+n时刻内的角速度的均值、σ
w,k:k+n
代表第k至k+n时刻内的角速度的标准差；t
a,min
为加速度的最小阈值、t
a,max
为加速度的最大阈值、t
σa
为加速度标准差的阈值、tw为角速度的阈值、为角速度标准差的阈值；当s1(k)、s2(k)、s3(k)、s4(k)同时为1时，zvd(k)＝1，表示当前为零速状态。
[0064]
进一步的，步骤(3)中零速修正算法具体为：
[0065]
当检测足部处于零速状态时，利用卡尔曼滤波计算进行误差估计，构建18维的卡尔曼滤波器状态量x：
[0066][0067]
其中，表示东北天方向的平台误差角，δv表示东北天方向的速度误差，δp表示位置误差；εb表示陀螺的零偏误差，εr表示陀螺的一阶马尔可夫漂移误差，
▽a表示加速度计的一阶马尔可夫漂移误差；
[0068]
建立状态方程：
[0069][0070]
其中，a表示状态转移矩阵，g表示系统噪声转移矩阵，w表示系统噪声；
[0071]
建立量测方程：
[0072]zzupt
＝δv＝v
n-v
zupt
＝hx+v
[0073]
其中，z
zupt
表示观测量，即为imu速度误差量δv，vn为捷联惯导解算得到的imu速度矢量，v
zupt
的理论值为[0 0 0]
t
，h是观测矩阵，v为观测噪声矩阵，表示速度观测不确定性；
[0074]
根据状态方程和量测方程构建kalman滤波器，求解得到最优估计状态量
[0075]
进一步的，步骤(5.5)中利用系统方程和量测方程应用扩展卡尔曼滤波具体为；
[0076][0077][0078][0079][0080]
其中，a表示状态转移矩阵，q表示状态转移协方差矩阵，表示k时刻的先验估计协方差，p
k-1
表示表示k-1时刻的后验估计协方差，kk表示卡尔曼滤波增益，j表示雅可比矩阵，r表示测量噪声协方差，z表示观测量，表示由系统方程得到k时刻的状态先验估计，xk由kalman滤波器求解得到的最优估计，表示k时刻的后验估计协方差，i表示单位矩阵。
[0081]
进一步的，步骤(5.4)中获得量测方程后作泰勒级数展开并保留线性项后得到线性观误差量测方程：
[0082]zdis
＝δd＝d
c-d
dh
＝j
·
x+nd[0083]
其中，nd为观测噪声矩阵，j表示非线性函数相对于系统状态向量x的雅可比矩阵。
[0084]
有益效果：本发明相对于现有技术，其显著优点是：通过或建立dh模型，并利用腿部四节点的实时更新的姿态角求解双足距离矢量，结合双足获得的状态量构建出双足等式约束进行误差修正，有效弥补了零速修正算法难以约束航向漂移的问题，有效抑制了惯性导航系统的航向误差发散。
附图说明
[0085]
图1所示为本发明的流程图；
[0086]
图2所示为本发明中构建的dh模型；
[0087]
图3所示为本发明构建的dh模型中腿部四节点的关节自由度示意图；
[0088]
图4所示为本发明中腿部四节点imu的安装校准示意图；
[0089]
图5所示为本发明所述方法定位结果与传统方法结果对比图。
具体实施方式
[0090]
下面结合附图对本发明的技术方案进一步说明。
[0091]
如图1所示，一种基于人体下肢dh模型的行人惯性导航定位方法，包括如下步骤：
[0092]
步骤一：采集行人双足足绑式imu数据，利用输出的1000个静态数据，进行陀螺仪的零偏校正和初始姿态求解。
[0093]
(1)从imu静态数据中计算并扣除陀螺仪的零偏
▽b：
[0094][0095]
其中，s0表示静态区间开始时刻，s1表示静态区间结束时刻，w表示陀螺仪输出；
[0096]
(2)利用静态下加速度计的比力输出计算初始的姿态角，包括足部俯仰角θ和足部滚转角γ：
[0097][0098][0099]
其中，g
x
为足部imu的x轴的比力输出；gy为足部imu的y轴的比力输出；gz为足部imu的z轴的比力输出。
[0100]
步骤二：如图2所示，利用denavit-hartenberg(dh)准则，建立人体下肢模型，在静止阶段，于左大腿、右大腿、左小腿、右小腿的位置进行传感器的安装和校准；通过传感器采集腿部四个节点的数据，所述四个节点分别为左腿髋关节节点、右腿髋关节节点、左腿膝关节节点、右腿膝关节节点；主要包括：
[0101]
(1)忽略肢体微小差异及关节间的微小平移，确定下肢关节运动自由度：如图3所示，髋关节具有3个自由度，膝关节具有1个自由度；
[0102]
(2)确定行人大腿小腿长度，大腿长度为l1小腿长度为l2，确定左右髋关节宽度为d；
[0103]
(3)根据人体下肢长度建立腿部dh模型参数，如下表1：
[0104]
表1
[0105][0106]
其中，如图3所示，θi表示关节i的旋转角，i取1、2、3、4；(关节i非腿部四个关节节点，关节i分别对应四个关节节点中各关节节点所代表的自由度；关节1、关节2、关节3分别对应髋关节的三个自由度，关节4对应膝关节的一个自由度)。
[0107]
θ1～θ4的取值由腿部imu姿态角提供，具体的，θ1数值为单只腿部髋关节节点的imu
航向角；θ2数值为单只腿部髋关节节点的imu滚转角，θ3数值为单只腿部髋关节节点的imu俯仰角；θ4数值为单只腿部膝关节节点的imu俯仰角，且θ1、θ2、θ3、θ4的数值均取自同一只腿的节点姿态角的数值；αi表示关节i的扭角，di表示表示关节i与关节i-1之间连杆的偏置；
[0108]di
表示表示关节i与关节i-1之间连杆的偏置；忽略肢体微小差异及关节间的微小平移，当人体正常站立时，髋、膝、踝关节连线垂直于水平面，因此均设置为0；ai表示关节i与关节i-1之间连杆的长度；αi表示关节i的扭角、offseti表示关节i的补偿角；当人体模型确定，仅仅是θi需要借助imu求解，其余参数都已确定；
[0109]
(4)在安装腿部imu时，由于人体皮肤不规则，有部分的凹凸与倾角，为实现腿部四节点imu三轴坐标系一致，如图4所示，需要进行传感器的安装校正。传感器安装校准具体通过初始旋转矩阵r
trans
来实现，初始对准阶段，传感器进行初始姿态求解得到俯仰角θs和滚转角γs和航向角ψs。理论中，人体下肢自然垂直与地面，下肢三轴坐标系与地理系保持一致。因此初始旋转矩阵r
trans
计算公式为：
[0110][0111]
其中，r
trans
表示初始旋转矩阵，θs表示初始姿态俯仰角，γs表示初始姿态的滚转角，ψs表示初始姿态的航向角。
[0112]
步骤三、实时解算左右双足两节点imu数据，并同步进行零速检测，利用零速更新算法修正足部双节点导航累计误差，得到修正后的速度、姿态和位置。具体的，以足绑式惯性传感器的中心为坐标原点o，右向为x轴，y轴垂直于x轴指向前向，z轴指向上，三轴满足右手螺旋定则。采集足绑式惯性传感器输出的1000个静态数据，根据初始静态区间中加速度和角速度的数据特性，获得零速检测的加速度和角速度阈值。
[0113]
(1)利用捷联惯导算法解算足部两节点imu的姿态、速度和位置；
[0114]
四元数姿态更新算法公式如下：
[0115][0116][0117]
其中，q表示四元数矩阵，t
k-1
表示k-1时刻，tk表示k时刻，即k-1时刻的下一采样时刻，δθ表示采样时间内陀螺仪三轴的角度增量，δθ表示由δθ构成的反对称矩阵，ψ表示航向角、θ表示俯仰角、γ表示滚转角，表示姿态；
[0118]
速度更新算法公式如下：
[0119][0120]
其中，表示在k+1时刻载体在地理系下速度的微分，vn(t
k+1
)是k+1时刻载体在地理系下的运动速度；表示机体系到导航系的坐标转换矩阵；代表加速度计比力输出；为地球自转角速度在导航坐标系下的投影，表示导航系相对于地球系下的角速度在导航坐标系下的投影；gn表示当地重力加速度；
[0121]
位置更新算法公式如下：
[0122][0123]
其中，l表示代表经度，λ表示纬度，h表示高度；rm表示地球子午面内曲率半径，rn表示地球卯酉面内曲率半径；ve表示东向的速度矢量、vn表示北向的速度矢量、vu表示天向的速度矢量；δt表示采样时间间隔。
[0124]
(2)以加速度模值、角速度标准差、加速度模值、角速度标准差作为步态检测条件进行零速检测；
[0125][0126][0127][0128][0129]
zvd(k)＝s1(k)&s2(k)&s3(k)&s4(k)
[0130]
其中，代表第k至k+n时刻内的加速度的均值、σ
a,k:k+n
代表第k至k+n时刻内的加速度的标准差、代表第k至k+n时刻内的角速度的均值、σ
w,k:k+n
代表第k至k+n时刻内的角速度的标准差；t
a,min
为加速度的最小阈值、t
a,max
为加速度的最大阈值、为加速度标准差的阈值、tw为角速度的阈值、为角速度标准差的阈值；当s1(k)、s2(k)、s3(k)、s4(k)同时为1时，zvd(k)＝1，表示当前为零速状态。
[0131]
(3)当检测到足部为零速状态时，利用卡尔曼滤波计算进行误差估计，得到修正后的位置、速度，否则不进行修正；
[0132]
(3.1)当检测足部处于零速状态时，利用卡尔曼滤波计算进行误差估计，构建18维的卡尔曼滤波器状态量x：
[0133][0134]
其中，分别表示东向、北向、天向的平台误差角；δv＝[δv
e δv
n δvu]
t
分别为东向、北向、天向的速度误差；δp＝[δl δλ δh]
t
分别表示经度、纬度、高度的误差；分别表示陀螺x轴、y轴、z轴的零偏误差；分别
表示陀螺x轴、y轴和z轴的一阶马尔可夫漂移误差；分别表示加速度计x轴、y轴和z轴的一阶马尔可夫漂移误差；
[0135]
(3.2)建立状态方程：
[0136][0137]
其中，a表示状态转移矩阵，g表示系统噪声转移矩阵，w表示系统噪声；
[0138]
(3.3)建立量测方程：
[0139]zzupt
＝δv＝v
n-v
zupt
＝hx+v
[0140]
其中，z
zupt
表示观测量，即为imu速度误差量δv，vn为捷联惯导解算得到的imu速度矢量，v
zupt
的理论值为[0 0 0]
t
，h是观测矩阵，v为观测噪声矩阵，表示速度观测不确定性；
[0141]
(3.4)根据状态方程和量测方程构建kalman滤波器，求解得到最优估计状态量即姿态、速度、位置的估计误差；从步骤(3.2)捷联惯导解算结果中相应扣除前九维状态量得到修正后的姿态、速度和位置，在imu数据中扣除后九维状态量对imu的零偏误差和一阶马尔可夫漂移误差进行修正。
[0142]
利用上述方法针对左足和右足采集的imu数据分别解算获得左脚imu的导航误差状态量x
l
、右脚imu的导航误差状态量xr。
[0143]
步骤四：利用dh模型正向运动学，实时求解双足距离；
[0144]
(4.1)采集腿部四个节点数据，利用上述捷联惯导算法实时解算腿部四个节点imu姿态角，姿态角即航向角ψ、俯仰角θ、滚转角γ；
[0145]
(4.2)利用正向运动学求解双足距离矢量
[0146]
(4.2.1)根据dh模型参数，获得左腿或者右腿中四个关节节点的关节之间齐次变换矩阵该矩阵表示了关节i相对于关节i-1的空间位置与姿态关系，公式为：
[0147][0148]
(4.2.2)获取左腿髋关节与左侧踝关节或者右侧腿髋关节与右侧踝关节的齐次变换矩阵公式为：
[0149][0150]
(4.2.3)提取第四列中的前三个元素得到左腿髋关节与左侧踝关节或者右侧腿髋关节与右侧踝关节的位置相对关系
[0151]
(4.2.4)足部两节点imu的位置相对关系表示为：
[0152][0153]
其中，表示左侧足绑式imu与左侧髋关节的位置相对关系，表示右侧足绑式imu与右侧髋关节的位置相对关系；t＝[-d 0 0]
t
表示左右侧髋关节的位置相对关系，d
表示左右髋关节间距。
[0154]
步骤五：计算每次采样时刻下捷联惯导解算下的双足距离，建立双足距离等式约束；
[0155]
(5.1)联立双足状态量，构建系统方程：
[0156]
x＝[x
l xr]
[0157]
其中，x
l
表示左脚imu的导航误差状态量、xr表示右脚imu的导航误差状态量；
[0158]
(5.2)根据x
l
和xr解算得到的双足imu相对距离模值为：
[0159]
dc＝f(l
·
x)
[0160]
其中,f()为向量欧式距离求解函数,l＝[03×
6 i3×
3 03×
15
ꢀ‑
i3×
3 03×9]；
[0161]
(5.3)根据下肢dh模型和解算得到的双足距离矢量得到安装在足部两节点imu之间的相对距离模值为：
[0162][0163]
(5.4)引入基于dh模型的双足等式约束，建立双足等式约束方程，并构建量测方程：
[0164]zdis
＝δd＝d
c-d
dh
＝f(l
·
x)-d
dh
[0165]
将上式非线性方程作泰勒级数展开并保留线性项后得到线性误差量测方程：
[0166]zdis
＝δd＝d
c-d
dh
＝j
·
x+nd[0167]
其中，nd为观测噪声矩阵，j表示非线性函数相对于系统状态向量x的雅可比矩阵。
[0168][0169][0170]
其中，x
l
表示修正后的左足imu的x轴位置，y
l
表示修正后的左足imu的y轴位置，z
l
表示修正后的左足imu的z轴位置；xr表示修正后的右足imu的x轴位置，yr表示修正后的右足imu的y轴位置，zr表示修正后的右足imu的z轴位置；ρ表示由dh模型得到的双足距离。
[0171]
(5.5)根据上述步骤(5.1)、(5.4)得到的系统方程和量测方程应用扩展卡尔曼滤波获得最优估计误差状态量后，对位置、速度、姿态和imu器件误差进行再次修正。
[0172]
具体为；
[0173][0174][0175][0176][0177]
其中，a表示状态转移矩阵，q表示状态转移协方差矩阵，表示k时刻的先验估计
协方差，p
k-1
表示表示k-1时刻的后验估计协方差，kk表示卡尔曼滤波增益，j表示雅可比矩阵，r表示测量噪声协方差，z表示观测量，表示由系统方程得到k时刻的状态先验估计，xk由kalman滤波器求解得到的最优估计，表示k时刻的后验估计协方差，i表示单位矩阵；
[0178]
求解最优估计误差状态量xk后，对位置、速度、姿态和imu器件误差进行再次修正。
[0179]
如图5所示，实验场景在教学实验楼内，路线是长度为56.8m，宽度为47m的矩形，路线总长度为622.8m，测试者以1.5m/s的步行前进，利用传统零速修正方法解算结果如图虚线所示，起终点误差大于20米；利用本发明即一种基于人体下肢dh模型的行人导航定位方法解算结果如图实线所示，航向误差大大减小，起终点位置误差为1.7米，定位精度大幅提升。