技术领域
[0001] 本
发明属于导航
定位技术领域，涉及一种基于相对位姿测量的行人导航方法。
背景技术
[0002] 行人导航为行人实时地提供可靠的
位置、速度、
姿态等导航信息，在人们的很多生产活动、社会活动和休闲活动中有很迫切的需求，研究行人导航方法和技术有极其重要的学术意义和实用价值。
[0003] 目前市场上多数行人导航产品，主要依靠地图匹配GPS定位。行人的运动特征具有主观性和随机性，大多数情况都是沿着人行道或者林荫道行走，或者在穿越深林、隧道或者进出城市
高层建筑区，或者在室内环境下，GPS
信号受限制，难以实现行人的持续导航定位。而且GPS的定位误差一般大于10米，达不到行人导航的
精度要求。
[0004] 依靠惯性导航的行人导航方法，虽然能够实现任何环境下持续的实时定位导航，但是惯性导航
传感器系统的输出只能提供准确的短期位置、速度、
加速度、姿态、和
角速率估计，测量数据包含漂移或偏差误差和噪声，长时间使用会有误差累计，失去导航定位的功能。
发明内容
[0005] 有鉴于此，本发明的目的在于提供一种基于相对位姿测量的全自主行人导航方法，实现行人室内室外无盲区、高精度定位。该方法是把
航天器交会对接中相对位姿测量的思想应用到行人
导航系统中的一种新型导航定位方法。
[0006] 为达到上述目的，本发明提供如下技术方案：
[0007] 本发明公开一种基于相对位姿测量的行人导航方法，涉及导航定位领域。该方法包括以下步骤：(1)在行人左右脚上分别安装3组相对位姿测量设备；(2)确定导航
坐标系；(3)使用安装在左右脚
鞋底的
电子开关检测行人左右脚与地面的
接触；(4)使用多目标测距
电路和光学相机分别测量出左右脚坐标系之间的相对位姿，进而分别测量出左右脚前后两次接触地面时的相对位姿；使用左右脚上的9轴惯性测量单元分别测量出左右脚前后两次接触地面时的相对位姿；(5)将三组位姿测量结果进行数据融合，行人行走过程就是不断重复步骤(3)-(5)的位姿测量过程；(6)确定左右脚坐标系分别相对于导航坐标系的相对位置和相对姿态；(7)左右脚行进轨迹及其姿态的显示。
[0008] 进一步，所述相对位姿测量是指测量行人左右脚坐标系之前的相对位置和相对姿态，和行人左右脚前后两次接触地面时的相对位置和相对姿态。
[0009] 进一步，所述在行人左右脚上分别安装3组相对位姿测量设备是指：右脚
鞋跟处安装9轴IMU惯性测量单元(3轴
加速度计，3轴
陀螺仪，3轴
电子罗盘)；右脚内侧安装至少3个测距信号Tx发射单元；右脚内侧安装至少3个光学特征点；右脚
鞋底前后2个位置分别安装电子开关模
块。左脚鞋跟处安装9轴IMU惯性测量单元(3轴加速度计，3轴陀螺仪，3轴电子罗盘)；左脚内侧安装至少3个测距信号Rx接收单元和测距
信号处理单元；左脚内侧安装光学相机；左脚鞋底前后2个位置分别安装电子开关模块。
[0010] 进一步，所述导航坐标系是指初始导航前左脚的惯性坐标系。
[0011] 进一步，所述使用安装在左右脚鞋底的电子开关检测行人左右脚与地面的接触是指：鞋底前后2个位置分别安装电子开关模块，用于检测行人从鞋跟接触地面(鞋底后面开关闭合，前面开关打开)到整个鞋底接触地面(鞋底前后两个开关均闭合)，再到鞋跟离开地面(鞋底后面开关打开，前面开关闭合)，鞋子完全离开地面(鞋底两个开关均打开)的过程。当两只脚均接触地面时，触发相对位姿测量电路工作。
[0012] 进一步，所述使用多目标测距电路测量出左右脚坐标系之间的相对位姿是指，测出右脚内侧安装至少3个测距信号Tx发射单元与左脚内侧安装至少3个测距信号Rx接收单元之间的距离，根据球面方程组，计算出右脚任一发射单元在左脚坐标系中的位置坐标，由于发射单元在右脚坐标系中坐标已知，从而计算出右脚坐标系与左脚坐标系之间的相对位置和相对姿态矩阵。
[0013] 进一步，所述发射单元和接收单元之间的多目标测距，可以采用
超声波测距或者无线电测距，各发射信号使用频分多址FDMA方式或者时分多址TDMA方式来相互区分。
[0014] 进一步，所述使用光学相机测量出左右脚坐标系之间的相对位姿是指，安装在左脚内侧的光学相机对右脚内侧的光学特征点进行闪光照相，测量出这些特征点在相机坐标系以及左脚坐标系中的位置坐标，由于这些光学特征点在右脚坐标系中坐标已知，从而可以计算出右脚坐标系与左脚坐标系之间的相对位置和相对姿态矩阵。
[0015] 进一步，所述使用左右脚上的9轴惯性测量单元分别测量出左右脚前后两次接触地面时的相对位姿是指，将惯性测量单元采集的3轴加速度，3轴
角速度，3轴磁
力计数据使用惯性导航
算法计算出左右脚前后两次接触地面时的相对位置和相对姿态。
[0016] 本发明的有益效果在于：(1)将航天器交会对接技术中的相对位姿测量思想用到行人导航系统中，实现了行人室内外全自主、无盲区、高精度定位；(2)可准确地测量出左右脚行进过程中的姿态，能够更准确地提取步态特征信息。
附图说明
[0017] 为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：
[0018] 图1为基于相对位姿测量的行人导航方法原理图
[0019] 图2为基于相对位姿测量的行人导航装置说明图
[0020] 图3为基于相对位姿测量的行人导航流程说明图
具体实施方式
[0021] 下面将结合附图，对本发明的优选
实施例进行详细的描述。
[0022] 图1是基于相对位姿态测量的行人导航方法原理图。如图所示，在行人1的左脚和右脚上分别构建左脚坐标系OL-XLYLZL和右脚坐标系OR-XRYRZR，记录行人迈出第一步之前100的行人左脚坐标系OL(0)-XL(0)YL(0)ZL(0)；左脚稳定地停在地面时，右脚迈出第一步并落地后201，启动第1次相对位姿测量，测量出右脚坐标系OR(1)-XR(1)YR(1)ZR(1)相对于左脚坐标系OL(0)-XL(0)YL(0)ZL(0)的相对位置V(1)和相对姿态R(1)，而后右脚稳定地停在地面时，左脚迈出第1步并落地101后，启动一次相对位姿测量，测量出右脚坐标系OR(1)-XR(1)YR(1)ZR(1)相对于左脚坐标系OL(1)-XL(1)YL(1)ZL(1)的相对位置V(2)和相对姿态R(2)。以此类推，左脚稳定地停在地面，右脚迈出第i步并落地后203，测量出右脚坐标系OR(i)-XR(i)YR(i)ZR(i)相对于左脚坐标系OL(i-1)-XL(i-1)YL(i-1)ZL(i-1)的相对位置V(2i-1)和相对姿态R(2i-1)，而后右脚稳定地停在地面时，左脚迈出第i步落地后104，测量出右脚坐标系OR(i)-XR(i)YR(i)ZR(i)相对于左脚坐标系OL(i)-XL(i)YL(i)ZL(i)的相对位置V(2i)和相对姿态R(2i)。
[0023] 第1次行进过程(先迈右脚后迈左脚)为：
[0024] (公式1)
[0025] (公式2)
[0026] 第i次行进过程(先迈右脚后迈左脚)为：
[0027] (公式3)
[0028] (公式4)
[0029] 第i+1次行进过程(先迈右脚后迈左脚)为：
[0030] (公式5)
[0031] (公式6)
[0032] 由(公式3)和(公式4)，可知：
[0033] (公式7)
[0034] 由(公式4)和(公式5)，可知：
[0035] (公式8)
[0036] (公式7)表明了前后两次左脚坐标系之间的相对位姿关系，据此，可以求出第i步左脚坐标系与初始左脚坐标系之间的位姿关系，如下所示：
[0037] (公式9)
[0038] RL(i)为第i步左脚坐标系相对于初始左脚坐标系的姿态矩阵，VL(i)为第i步左脚坐标系相对于初始左脚坐标系的相对位置，即第i步左脚坐标系的原点在初始左脚坐标系中的位置坐标。
[0039] (公式8)表明了前后两次右脚坐标系之间的相对位姿关系，据此，可以求出第i步右脚坐标系与初始左脚坐标系之间的位姿关系，如下所示：
[0040] (公式10)
[0041] RR(i)为第i步右脚坐标系相对于初始左脚坐标系的姿态矩阵，VR(i)为第i步右脚坐标系相对于初始左脚坐标系的相对位置，即第i步右脚坐标系的原点在初始左脚坐标系中的位置坐标。
[0042] 将位置坐标VL(i)，i＝1,2,3,…,n连接起来，就得到了行人左脚的行进轨迹，同样将位置坐标VR(i)，i＝1,2,3,…,n连接起来，就得到了行人右脚的行进轨迹。RL(i)，i＝1,2,3,…,n为行进过程中，左脚每一步与地面接触时的姿态矩阵，RR(i)，i＝1,2,3,…,n为行进过程中，右脚每一步与地面接触时的姿态矩阵。
[0043] 图2为基于相对位姿测量的行人导航装置说明图。导航装置由安装在左脚鞋子3上和右脚鞋子4上的测量设备构成。右脚4鞋跟处安装9轴IMU惯性测量单元40(3轴加速度计，3轴陀螺仪，3轴电子罗盘)，确定右脚惯性导航坐标系ORXRYRZR；右脚内侧安装至少3个测距信号Tx发射单元421,422,423；右脚内侧安装至少3个光学特征点431,432,433；
右脚鞋底前后2个位置分别安装电子开关模块410和411，用于检测右脚从鞋跟接触地面到整个鞋底接触地面，再到鞋跟离开地面，鞋子完全离开地面的过程。左脚1鞋跟处安装9轴IMU惯性测量单元30(3轴加速度计，3轴陀螺仪，3轴电子罗盘)，确定左脚惯性导航坐标系OLXLYLZL；左脚内侧安装至少3个测距信号Rx接收单元321,322,323，接收信号送往测距信号处理单元33，计算右脚任一Tx发射单元距3个接收单元321,322,323之间的距离，可以求出任一Tx发射单元在左脚坐标系中的位置坐标，在已知发射单元在右脚坐标系中坐标的情况下，计算出右脚坐标系ORXRYRZR与左脚坐标系OLXLYLZL之间的相对位置V和相对姿态矩阵R；左脚内侧安装光学相机32，用于测量右脚内侧的光学特征点在相机坐标系以及左脚坐标系中的位置坐标，在已知光学特征点在右脚坐标系中坐标的情况下，计算出右脚坐标系ORXRYRZR与左脚坐标系OLXLYLZL之间的相对位置V和相对姿态矩阵R；左脚鞋底前后
2个位置分别安装电子开关模块310和311，用于检测左脚从鞋跟接触地面到整个鞋底接触地面，再到鞋跟离开地面，鞋子完全离开地面的过程。
[0044] 图3是基于多目标测距的行人导航流程说明图。
[0045] 步骤50：行人左右脚上分别安装如图2所示的自主导航装置。
[0046] 步骤51：开始导航，确定导航坐标系。
[0047] 以开始导航前的左脚坐标系为导航坐标系，以后每步行走都可以看成一个周期过程。
[0048] 步骤52：左右脚电子开关状态检测步态。
[0049] 左脚鞋底前后两个电子开关闭合，右脚鞋底后面电子开关先闭合，前面电子开关后闭合，表明行人左脚
支撑地，迈右脚落地；同样地，右脚鞋底前后两个电子开关闭合，左脚鞋底后面电子开关先闭合，前面电子开关后闭合，表明行人右脚支撑地，迈左脚落地。当双脚均接触地面时，启动相对位姿测量步骤53，有三种相对位姿测量方法：步骤531，步骤532，步骤533。
[0050] 步骤531：基于多目标测距的相对位姿测量，包含步骤5311，步骤5312，步骤5313，步骤5314。
[0051] 步骤5311：当左右脚均接触地面时，启动多目标测距电路，测量出右脚每个测距特征点(Tx发射模块)与左脚3个以上测距接收单元Rx之间的距离；
[0052] 步骤5312：计算出右脚测距特征点在左脚坐标系中的坐标。
[0053] 步骤5313：确定右脚坐标系与左脚坐标系之间的
相位位置和相对姿态。
[0054] 步骤5314：确定左右脚坐标系前后两次接触地面的相对位置和相对姿态。
[0055] 步骤532：基于光学成像的相对位姿测量，包含步骤5321，步骤5322，步骤5323，步骤5324。
[0056] 步骤5321：当左右脚均接触地面时，左脚上的光学相机对右脚进行闪光照相；
[0057] 步骤5322：计算出右脚光学特征点在左脚坐标系中的坐标。
[0058] 步骤5323：确定右脚坐标系与左脚坐标系之间的相位位置和相对姿态。
[0059] 步骤5324：确定左右脚坐标系前后两次接触地面的相对位置和相对姿态。
[0060] 步骤533：基于惯性测量的相对位姿测量，包含步骤531，步骤532，步骤533。
[0061] 步骤5331：惯性导航单元实时进行
数据采集；
[0062] 步骤5332：确定左右脚坐标系前后两次接触地面的相对位置和相对姿态。
[0063] 步骤54：数据融合处理。
[0064] 将步骤531，步骤532，步骤533三种相对位姿测量方法测出的相对位置和相对姿态数据进行数据融合，然后进行下一步处理，回到步骤52，重复下一次行走过程。
[0065] 步骤55：确定左右脚坐标系分别相对于导航坐标系的相对位置和相对姿态。
[0066] 将数据融合后得到的左右脚坐标系相对位置和相对姿态转换为左右脚坐标系相对于初始导航坐标系的相对位置和相对姿态。
[0067] 步骤56：左右脚行进轨迹及其姿态的显示。
[0068] 行人每迈一步，都在导航坐标系中绘制行人左右脚的位置和姿态，这样就实现了行人的实时导航。
[0069] 最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明
权利要求书所限定的范围。
