基于毫米波雷达及惯性测量单元的一种行人自主导航方法技术

本发明专利技术公开了一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其包括以下步骤：S1、在每只脚上安装一个惯性测量单元，并在任一只脚上安装毫米波雷达；S2、获取用户的步长信息和每只脚的模糊位置信息；S3、对用户双脚的模糊位置信息进行修正与融合，得到融合后的位置信息；S4、将融合后的信息作为用户的具体位置信息，判断是否继续导航，若是则返回步骤S2，否则结束导航。本方法通过毫米波雷达对双脚间的位姿信息进行测量，并将其作为融合卡尔曼滤波器的水平管测量，将累计误差的增长率的数量级从平方降低到了常数级，有效的降低了累积误差为整体定位精度产生的不良影响，对长距离、长时间的定位导航精度有显著的提高。

A pedestrian autonomous navigation method based on millimeter wave radar and inertial measurement unit

【技术实现步骤摘要】
基于毫米波雷达及惯性测量单元的一种行人自主导航方法
本专利技术涉及导航领域，具体涉及一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法。
技术介绍
随着社会的快速发展，导航技术无论在军用还是民用方面都得到了广泛的应用，在近些年也取得了长足的发展。在室外定位中，全球导航定位系统有着优秀的表现，然而，由于卫星信号穿透性较差，在高楼、地下建筑等室内场景中，卫星定位系统的表现不尽如人意。惯性导航技术作为一种不易受外界环境干扰，无需预先部署，无依赖，低成本的导航技术，在无GPS情况下的定位中有着独特的优势。惯性导航系统通过对惯性传感器(加速度计、陀螺仪)测得的加速度和角加速度进行二次积分，在已知起始点上进行累加，从而得到定位及导航轨迹。在这个过程中会不可避免的对噪声积分并叠加，进而增大了噪声对定位质量的影响，在长距离、长时间的定位中出现严重的轨迹发散现象，难以符合长时间定位的要求。
技术实现思路
针对现有技术中的上述不足，本专利技术提供的一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法解决了现有惯性导航系统会产生累计误差的问题。为了达到上述专利技术目的，本专利技术采用的技术方案为：提供一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其包括以下步骤：S1、在每只脚上安装一个惯性测量单元，并在任一只脚上安装毫米波雷达；S2、通过毫米波雷达获取用户的步长信息；以当前位置信息为基准，通过惯性测量单元获取用户每只脚的模糊位置信息；S3、将步长信息作为多传感器融合的卡尔曼滤波器的水平观测量，对用户双脚的模糊位置信息进行修正与融合，得到融合后的位置信息；S4、将融合后的信息作为用户的具体位置信息，判断是否继续导航，若是则返回步骤S2，否则结束导航。进一步地，步骤S2中通过毫米波雷达获取用户的步长信息的具体方法包括以下子步骤：S2-1-1、通过毫米波雷达发射信号并接收反射回来的信号；S2-1-2、根据反射回来的信号遍历每个所检测到的目标，获取每个检测到的目标的速度、距离和回波功率；S2-1-3、去除速度为0的目标、去除距离大于阈值1的目标，以及去除回波功率小于阈值2的目标；S2-1-4、在保留下来的目标中，根据公式：将归一化的回波功率作为权重，对当前每一个探测到的目标进行加权平均，得到用户的步长信息R；其中为保留下来的第i个目标的距离；为保留下来的第i个目标的回波功率。进一步地，S2-1-2中获取每个检测到的目标的速度和距离的具体方法为：将反射回来的信号进行混频后得到中频信号的频率Fs，根据公式：获取目标距离雷达的距离d；将获取的若干距离d按行排列，将排列好的矩阵的每一列进行快速傅里叶变换，将变换后得到的矩阵的行作为距离数值，将变换后得到的矩阵的列作为速度数值；其中c为光速；S为信号频率随时间的变化率。进一步地，阈值1为用户身高的0.8倍；所述阈值2为毫米波雷达发射功率的千分之一。进一步地，步骤S2中通过惯性测量单元获取用户每只脚的模糊位置信息的具体方法包括以下子步骤：S2-2-1、以每只脚中安装惯性测量单元的位置为坐标原点建立该脚的本体坐标系，读取惯性测量单元中陀螺仪的数据，并根据公式：Ct+1＝Ct(2I3×3+ΩtΔt)(2I3×3-ΩtΔt)-1获取t+1时刻每只脚的姿态矩阵；其中Ct为t时刻该脚的姿态矩阵；Ct+1为t+1时刻该脚的姿态矩阵；I3×3为3×3单位矩阵；Ωt为t时刻该脚角速率的反对称矩阵；Δt为相邻时刻的间隔时长；ωx、ωy和ωz分别为t时刻该脚上陀螺仪读取的x轴方向、y轴方向和z轴方向的角速率；S2-2-2、以用户的人体中心为坐标原点建立惯性导航坐标系，获取惯性测量单元中加速度计的数据，并根据公式：pt+1＝pt+vt+1Δt获取t+1时刻每只脚的模糊位置信息；其中为惯性导航坐标系下t时刻该脚的加速度；为本体系下t时刻该脚在该本体坐标系中的加速度；vt为t时刻该脚的速度；vt+1为t+1时刻该脚的速度；pt为t时刻该脚的位置信息；pt+1为t+1时刻该脚的模糊位置信息。进一步地，步骤S3的具体方法包括以下子步骤：S3-1、根据公式Xa0＝(Ca0,pa0,va0)T,Xb0＝(Cb0,pb0,vb0)T分别获取用户两只脚t+1时刻的状态向量Xa0和Xb0；其中Ca0、pa0和va0分别为未设置毫米波雷达的脚t+1时刻对应的姿态矩阵、位置信息和速度；Cb0、pb0和vb0分别为设置了毫米波雷达的脚t+1时刻对应的姿态矩阵、位置信息和速度；S3-2、根据公式：Pt+1＝FtPtFtT+Q更新每只脚t+1时刻对应的误差协方差矩阵；其中Pt+1为t+1时刻该脚的误差协方差矩阵；Pt为t时刻该脚的误差协方差矩阵；Ft为t时刻的状态转移矩阵；Q为状态转移噪声；(·)T为矩阵的转置；St为t时刻惯性测量单元的加速度；为t时刻惯性测量单元x轴方向的加速度；为t时刻惯性测量单元y轴方向的加速度；为t时刻惯性测量单元z轴方向的加速度；03×3为3×3的零矩阵；S3-3、判断当前速度是否为0，若是则进入步骤S3-4，否则进入步骤S3-7；S3-4、根据公式：Kt+1＝Pt+1HT(HPt+1HT+M)-1H＝{03×303×3I3×3}分别获取t+1时刻两只脚的卡尔曼增益Kt+1；其中H为观测矩阵，H中的两个零矩阵的位置分别对应观测到的该脚t+1时刻的状态向量中姿态矩阵和位置信息的位置，H中的单位矩阵对应观测到的该脚t+1时刻的状态向量中速度的位置信息；M为观测噪声；S3-5、根据公式：P't+1＝(I9×9-Kt+1H)Pt+1修正t+1时刻每只脚的误差协方差矩阵，得到修正后的误差协方差矩阵P'；其中Pt'+1为t+1时刻该脚对应的修正后的误差协方差矩阵；I9×9为9×9的单位矩阵；S3-6、根据公式：ε＝(εc,εp,εv)T＝Kt+1·vt+1C't+1＝(2I3×3+ε'cΔt)(2I3×3-ε'cΔt)-1Ct+1p't+1＝pt+1-εpv't+1＝vt+1-εvXa1＝(Ca1,pa1,va1)T,Xb1＝(Cb1,pb1,vb1)T修正t+1时刻惯性测量单元所在脚的姿态矩阵Ct+1、惯性测量单元所在脚的位置信息pt+1和惯性测量单元所在脚的速度vt+1，并更新该脚t+1时刻的状态向量，分别得到修正后的惯性测量单元所在脚的姿态矩阵C't+1、修正后的惯性测量单元所在脚的位置信息p't+1和修正后的惯性测量单元所在脚的速度v't+1，以及更新后的状态向量Xa1和Xb1；其中ε'c为参数εc的反对称矩阵；ε为修正数据；εp和εv均为中间参数；参数εc、εp和εv的值由Kt+1·vt+1决定；Ca1、pa1和本文档来自技高网...

【技术保护点】
1.一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，包括以下步骤：/nS1、在每只脚上安装一个惯性测量单元，并在任一只脚上安装毫米波雷达；/nS2、通过毫米波雷达获取用户的步长信息；以当前位置信息为基准，通过惯性测量单元获取用户每只脚的模糊位置信息；/nS3、将步长信息作为多传感器融合的卡尔曼滤波器的水平观测量，对用户双脚的模糊位置信息进行修正与融合，得到融合后的位置信息；/nS4、将融合后的信息作为用户的具体位置信息，判断是否继续导航，若是则返回步骤S2，否则结束导航。/n

【技术特征摘要】
1.一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，包括以下步骤：
S1、在每只脚上安装一个惯性测量单元，并在任一只脚上安装毫米波雷达；
S2、通过毫米波雷达获取用户的步长信息；以当前位置信息为基准，通过惯性测量单元获取用户每只脚的模糊位置信息；
S3、将步长信息作为多传感器融合的卡尔曼滤波器的水平观测量，对用户双脚的模糊位置信息进行修正与融合，得到融合后的位置信息；
S4、将融合后的信息作为用户的具体位置信息，判断是否继续导航，若是则返回步骤S2，否则结束导航。


2.根据权利要求1所述的基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，所述步骤S2中通过毫米波雷达获取用户的步长信息的具体方法包括以下子步骤：
S2-1-1、通过毫米波雷达发射信号并接收反射回来的信号；
S2-1-2、根据反射回来的信号遍历每个所检测到的目标，获取每个检测到的目标的速度、距离和回波功率；
S2-1-3、去除速度为0的目标、去除距离大于阈值1的目标，以及去除回波功率小于阈值2的目标；
S2-1-4、在保留下来的目标中，根据公式：



将归一化的回波功率作为权重，对当前每一个探测到的目标进行加权平均，得到用户的步长信息R；其中为保留下来的第i个目标的距离；为保留下来的第i个目标的回波功率。


3.根据权利要求2所述的基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，所述S2-1-2中获取每个检测到的目标的速度和距离的具体方法为：
将反射回来的信号进行混频后得到中频信号的频率Fs，根据公式：



获取目标距离雷达的距离d；将获取的若干距离d按行排列，将排列好的矩阵的每一列进行快速傅里叶变换，将变换后得到的矩阵的行作为距离数值，将变换后得到的矩阵的列作为速度数值；其中c为光速；S为信号频率随时间的变化率。


4.根据权利要求2所述的基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，所述阈值1为用户身高的0.8倍；所述阈值2为毫米波雷达发射功率的千分之一。


5.根据权利要求1所述的基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，所述步骤S2中通过惯性测量单元获取用户每只脚的模糊位置信息的具体方法包括以下子步骤：
S2-2-1、以每只脚中安装惯性测量单元的位置为坐标原点建立该脚的本体坐标系，读取惯性测量单元中陀螺仪的数据，并根据公式：
Ct+1＝Ct(2I3×3+ΩtΔt)(2I3×3-ΩtΔt)-1



获取t+1时刻每只脚的姿态矩阵；其中Ct为t时刻该脚的姿态矩阵；Ct+1为t+1时刻该脚的姿态矩阵；I3×3为3×3单位矩阵；Ωt为t时刻该脚角速率的反对称矩阵；Δt为相邻时刻的间隔时长；ωx、ωy和ωz分别为t时刻该脚上陀螺仪读取的x轴方向、y轴方向和z轴方向的角速率；
S2-2-2、以用户的人体中心为坐标原点建立惯性导航坐标系，获取惯性测量单元中加速度计的数据，并根据公式：






pt+1＝pt+vt+1Δt
获取t+1时刻每只脚的模糊位置信息；其中为惯性导航坐标系下t时刻该脚的加速度；为本体系下t时刻该脚在该本体坐标系中的加速度；vt为t时刻该脚的速度；vt+1为t+1时刻该脚的速度；pt为t时刻该脚的位置信息；pt+1为t+1时刻该脚的模糊位置信息。


6.根据权利要求5所述的基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其特征在于，所述步骤S3的具体方法包括以下子步骤：
S3-1、根据公式
Xa0＝(Ca0,pa0,va0)T,Xb0＝(Cb0,pb0,vb0)T
分别获取用户两只脚t+1时刻的状态向量Xa0和Xb0；其中Ca0、pa0和va0分别为未设置毫米波雷达的脚t+1时刻对应的姿态矩阵、位置信息和速度；Cb0、pb0和vb0分别为设置了毫米波雷达的脚t+1时刻对应的姿态矩阵、位置信息和速度；
S3-2、根据公式：
Pt+1＝FtPtFtT+Q


...

【专利技术属性】
技术研发人员：阎波，李柯蒙，魏震益，肖卓凌，
申请(专利权)人：电子科技大学，
类型：发明
国别省市：四川;51