用于室内导航的超声波阵列辅助定位方法与系统的制作方法
【专利摘要】本发明提供了一种用于室内导航的超声波阵列辅助定位方法和系统，该方法包括如下步骤：S01：通过惯性导航系统获得定位主体的位置信息、类矩形环境的地图信息，进而得到定位主体的中心到类矩形环境的四个边缘的四个预测距离值；S02：根据超声波传感器相对定位主体的位置、定位主体的航向以及超声波传感器的测距值计算定位主体中心到类矩形环境的四个边缘的四个实测距离值；S03：将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重；S04：依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离；S05：依据最终的实际距离对定位主体进行位置的更新。
【专利说明】用于室内导航的超声波阵列辅助定位方法与系统
【技术领域】
[0001] 本发明涉及惯性导航和室内定位导航技术，特别涉及一种用于室内导航的超声波 阵列辅助定位方法与系统。
【背景技术】
[0002] 在室外导航定位技术中，卫星导航系统，包括全球定位系统GPS以及我国自主研 发的北斗导航系统都已经得到了非常广泛的应用，极大地丰富了人们的生活。然而，当处在 室内环境下时，所能接收到的GPS信号或北斗卫星信号是非常微弱的，甚至根本接收不到， 这就需要提供室内导航与定位的新方法。
[0003] 目前的室内导航方法主要分为两种，一种是基于信标定位的方法，另一种是基于 惯性器件定位的方法。基于信标定位的方法，即在当前环境中先预设一定数量的位置固定 的信标，用户通过与这些信标的信息交互来获取自身当前的位置，如RFID定位、WIFI定位 等。基于惯性器件定位的方法，通过设备内置的加速度计、陀螺仪、磁力计等来感知定位目 标的运动方向和运动距离，逐步计算目标位置进行定位导航。
[0004] 基于信标定位的方法虽然精度高，但有以下两个缺陷：
[0005] (1).需要事先在环境中部署信标，所需成本很高；
[0006] (2).若发生灾害，信标极有可能会被摧毁，使得该条件下的室内定位导航不可进 行。
[0007] 而基于惯性器件定位的方法，不需要设定信标，因而成本相对较低，且由于惯性传 感器安装在可便携设备中，具有自主性，即使发生灾害或其他紧急情况也能够进行定位和 导航。虽然惯性导航有以上的诸多优点，但这种导航方法也有自己的不足之处，其最大的缺 点是长时间导航精度较低。因为惯性器件的误差是可累积的，当经过一定的时间后，导航定 位误差会迅速增长。
【发明内容】
[0008] 本发明要解决的技术问题是如何使惯性导航系统的定位结果更加精确，导航性能 更好。
[0009] 为了解决这一技术问题，本发明提供了一种用于室内导航的超声波阵列辅助定位 方法，用以辅助惯性导航系统，提供了定位主体和若干设于所述定位主体外围的超声波传 感器，若干所述超声波传感器的发射超声波的方向的反向延长线均通过所述定位主体的中 心，所述超声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得 到测距值；
[0010] 该方法包括如下步骤：
[0011] S01 :通过惯性导航系统获得所述定位主体的位置信息、类矩形环境的地图信息， 进而得到所述定位主体的中心到类矩形环境的四个边缘的四个预测距离值；
[0012] S02:根据所述超声波传感器相对定位主体的位置、定位主体的航向以及超声波传 感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实测距离值；
[0013] S03:将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值 的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越大，权重越小；
[0014] S04 :依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离；
[0015] S05 :依据最终的实际距离对定位主体进行位置的更新。
[0016] 在所述步骤S02之前，还包括对所述超声波传感器筛选的过程，在该过程中，先计 算每个所述超声波传感器相对于类矩形环境地图的朝向，然后根据朝向的角度阈值过滤掉 部分超声波传感器，仅就未被过滤的超声波传感器实施后续步骤。
[0017] 在所述步骤S03中，还包括对实测距离值筛选的过程，在该过程中，将实测距离值 与预测距离值差值的绝对值与一个阈值比较，若大于该阈值，则将该实测距离值过滤掉，仅 就未被过滤掉的实测距离值进行权重设定，进而实施后续步骤。
[0018] 本发明还提供了一种用于室内导航的超声波阵列辅助定位系统，用以辅助惯性导 航系统，包括粗略位置获取模块、地图信息获取模块、距离预测模块、超声波阵列测距模块、 航向获取模块和导航滤波模块，
[0019] 所述粗略位置获取模块，用以通过所述惯性导航系统获得定位主体的位置信息；
[0020] 所述地图信息获取模块，用以获得定位主体所处类矩形环境的地图信息；
[0021] 所述距离预测模块，用以根据定位主体的位置信息和其所处类矩形环境的地图信 息计算定位主体到类矩形环境四个边缘的距离，得到相应的预测距离值；
[0022] 所述超声波阵列测距模块，至少包括若干安装于定位主体上的超声波传感器，所 述超声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距 值；若干所述超声波传感器分布于所述定位主体的外围，且每个所述超声波传感器发射超 声波的方向的反向延长线均通过所述定位主体的中心。
[0023] 所述航向获取模块，用以通过所述惯性导航系统获得定位主体的前进方向；使用 的是惯性导航系统中输出的航向信息，记为heading。
[0024] 所述导航滤波模块，用以根据所述预测距离值、测距值和定位主体的前进方向，计 算得到定位主体到类矩形环境四条边的实际距离，进而实现对定位主体位置的位置更新。
[0025] 所述导航滤波模块在用于计算得到定位主体到类矩形环境四条边的实际距离时，
[0026] 所述导航滤波模块先用以根据所述超声波传感器相对定位主体的位置、定位主体 的航向以及超声波传感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四 个实测距离值；
[0027] 所述导航滤波模块再用以将实测距离值与预测距离值进行比较，计算两者差值的 绝对值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对 值越大，权重越小；
[0028] 然后，所述导航滤波模块依据不同的权值对实测距离值进行加权平均求和，得到 最终的实际距离；
[0029] 最后，所述导航滤波模块依据最终的实际距离对定位主体进行位置的更新。
[0030] 在计算得到所述实测距离值之前，所述导航滤波模块还用以实施所述超声波传感 器筛选的过程，在该过程中，先计算每个所述超声波传感器相对于类矩形环境地图的朝向， 然后根据朝向的角度阈值过滤掉部分超声波传感器，仅就未被过滤的超声波传感器实施后 续步骤。
[0031] 在对实测距离值设置不同的权重之前，所述导航滤波模块还用以实施对实测距离 值筛选的过程，在该过程中，将实测距离值与预测距离值差值的绝对值与一个阈值比较，若 大于该阈值，则将该实测距离值过滤掉，仅就未被过滤掉的实测距离值进行权重设定，进而 实施后续步骤。
[0032] 本发明在惯性器件定位的基础上，添加超声波阵列测距装置，将超声波传感器设 于定位主体上，从而进行融合定位导航，解决了累积误差问题，进一步的，本发明还将预测 距离值和实测距离值有效的运用与整合，最终使惯性导航系统的定位结果更加精确，导航 性能更好。
【专利附图】
【附图说明】
[0033] 图1是本发明一实施例中用于室内导航的超声波阵列辅助定位系统的模块示意 图；
[0034] 图2是本发明一实施例中定位主体距离预测示意图；
[0035] 图3是本发明一实施例中超声波阵列均匀分布示意图；
[0036] 图4是本发明另一实施例中超声波阵列非均匀分布示意图；
[0037] 图5是本发明一实施例中超声波传感器测距示意图；
[0038] 图6是本发明一实施例中导航滤波模块的操作流程图。
【具体实施方式】
[0039] 以下将结合图1至图6对本发明提供的用于室内导航的超声波阵列辅助定位方法 与系统进行详细的描述，其为本发明一可选的实施例，可以认为，本领域的技术人员在不改 变本发明精神和内容的范围内能够对其进行修改和润色。
[0040] 本实施例提供了一种用于室内导航的超声波阵列辅助定位方法，用以辅助惯性导 航系统，提供了定位主体和若干设于所述定位主体外围的超声波传感器，若干所述超声波 传感器的发射超声波的方向的反向延长线均通过所述定位主体的中心，所述超声波传感器 用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距值；
[0041] 该方法包括如下步骤：
[0042] S01 :通过惯性导航系统获得所述定位主体的位置信息、类矩形环境的地图信息， 进而得到所述定位主体的中心到类矩形环境的四个边缘的四个预测距离值；
[0043] S02 :根据所述超声波传感器相对定位主体的位置、定位主体的航向以及超声波传 感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实测距离值；
[0044] S03:将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值 的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越大，权重越小；
[0045] S04 :依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离；
[0046] S05 :依据最终的实际距离对定位主体进行位置的更新。
[0047] 在所述步骤S02之前，还包括对所述超声波传感器筛选的过程，在该过程中，先计 算每个所述超声波传感器相对于类矩形环境地图的朝向，然后根据朝向的角度阈值过滤掉 部分超声波传感器，仅就未被过滤的超声波传感器实施后续步骤。
[0048] 在所述步骤S03中，还包括对实测距离值筛选的过程，在该过程中，将实测距离值 与预测距离值差值的绝对值与一个阈值比较，若大于该阈值，则将该实测距离值过滤掉，仅 就未被过滤掉的实测距离值进行权重设定，进而实施后续步骤。
[0049] 本实施例还提供了一种用于室内导航的超声波阵列辅助定位系统，用以辅助惯性 导航系统，包括粗略位置获取模块、地图信息获取模块、距离预测模块、超声波阵列测距模 块、航向获取模块和导航滤波模块，具体来说，请参阅图1，本实施例中，所述粗略位置获取 模块、地图信息获取模块均与距离预测模块相连，所述距离预测模块、超声波阵列实际测距 模块、航向获取模块均与导航滤波模块相连。
[0050] 所述粗略位置获取模块，用以通过所述惯性导航系统获得定位主体的位置信息； 所述粗略位置获取模块使用的是惯性导航系统的输出信息，即惯性导航系统确定的定位主 体的位置信息，在二维地图中可表示为（x，y)。
[0051] 所述地图信息获取模块，用以获得定位主体所处类矩形环境的地图信息；所述地 图信息获取模块用于获取定位主体所处环境的地图信息，本发明针对的特定环境为类矩形 环境，比如方形的房间，长直走廊等等，地图信息即该类矩形环境的宽和长，记为wide和 length。
[0052] 所述距离预测模块，用以根据定位主体的位置信息和其所处类矩形环境的地图信 息计算定位主体到类矩形环境四个边缘的距离，得到相应的预测距离值；具体地，请参阅图 2,分别为右距离d_right，上距离d_up，左距离d_left，下距离d_down。对应地，有：
[0053] d_right = wide-x
[0054] d_up = long-y
[0055] d_left = x
[0056] d_down = y
[0057] 以上的模块距离预测模块、地图信息获取模块和粗略位置获取模块可用以实现步 骤S01 :通过惯性导航系统获得所述定位主体的位置信息、类矩形环境的地图信息，进而得 到所述定位主体的中心到类矩形环境的四个边缘的四个预测距离值；
[0058] 所述超声波阵列测距模块，至少包括若干安装于定位主体上的超声波传感器，所 述超声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距 值；
[0059] 若干所述超声波传感器分布于所述定位主体的外围，且每个所述超声波传感器发 射超声波的方向的反向延长线均通过所述定位主体的中心。
[0060] 请参阅图3和图4,超声波阵列分布在定位主体外围，超声发射方向的反向延长线 通过定位主体中心，这些传感器可均匀分布，如图3所示，在其他可选的实施例中，也可非 均匀分布，如图4所示，数量为不限定个数，但它们的分布位置是已知的，另外定位主体外 围不一定为圆形，可以为其他任何形状，比如若定位主体为遥控车，其外围可能呈圆形，也 可能为方形。亦即，超声波传感器的个数和位置分布完全可以是任意的。超声波阵列的分 布位置可用多个二元数组表示，即（Α，Θ J，（r2，Θ 2)，（r3，Θ 3)，…，（rn，θ n)，& (i = 1? n)表示超声波传感器到定位主体中心的距离，Θ = 1?n，n是超声波阵列装置中所有 传感器的个数）表不超声波传感器相对于定位主体坐标系中的正右方的夹角，逆时针时为 正值。
[0061] 超声波传感器由于其价格便宜，精度高而得到了广泛的应用，实际中使用超声波 传感器时大部分都是将超声波垂直入射到物体表面，通过飞行时间换算为距离获得测量距 离。在实验中我们发现，当超声波的入射角方向在一个较小的范围内时，测量得到的距离也 是垂直入射距离，即超声波传感器到物体表面的最短距离。此时，在已知地图信息的条件下 我们可以利用这个垂直入射距离对需要定位的主体进行位置修正，获取更加精确的定位结 果。超声波测量时是以扇形方式发射超声波，本实施例所用的是入射角较小时超声波发射 点到矩形环境一条边缘的垂线距离。
[0062] 所述航向获取模块，用以通过所述惯性导航系统获得定位主体的前进方向；所述 航向获取模块使用的是惯性导航系统中输出的航向信息，记为heading。如图5所示，其中 heading可以记做航向与地图坐标系中的正右方的夹角，逆时针时为正值，而heading与定 位主体坐标系的正右方的夹角为+90°。
[0063] 所述导航滤波模块，用以根据所述预测距离值、测距值和定位主体的前进方向，计 算得到定位主体到类矩形环境四条边的实际距离，进而实现对定位主体位置的位置更新。 [0064] 亦即，所述导航滤波模块通过融合距离预测模块、超声波阵列实际测距模块、航向 获取模块的信息，对定位主体的位置进行更新，得到更加精确的定位导航结果。导航滤波模 块是超声波阵列辅助定位方法的核心。用以实现以上所述的步骤S2至S5。
[0065] 所述导航滤波模块在用于计算得到定位主体到类矩形环境四条边的实际距离时， [0066] 所述导航滤波模块先用以根据所述超声波传感器相对定位主体的位置、定位主体 的航向以及超声波传感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四 个实测距离值；在计算得到所述实测距离值之前，所述导航滤波模块还用以实施所述超声 波传感器筛选的过程，在该过程中，先计算每个所述超声波传感器相对于类矩形环境地图 的朝向，然后根据朝向的角度阈值过滤掉部分超声波传感器，仅就未被过滤的超声波传感 器实施后续步骤。
[0067] 所述导航滤波模块再用以将实测距离值与预测距离值进行比较，计算两者差值的 绝对值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对 值越大，权重越小；在对实测距离值设置不同的权重之前，所述导航滤波模块还用以实施对 实测距离值筛选的过程，在该过程中，将实测距离值与预测距离值差值的绝对值与一个阈 值比较，若大于该阈值，则将该实测距离值过滤掉，仅就未被过滤掉的实测距离值进行权重 设定，进而实施后续步骤。
[0068] 然后，所述导航滤波模块依据不同的权值对实测距离值进行加权平均求和，得到 最终的实际距离；
[0069] 最后，所述导航滤波模块依据最终的实际距离对定位主体进行位置的更新。
[0070] 具体来说，请参考图5和图6,导航滤波模块的操作流程可以以下步骤来实现：
[0071] 步骤A :以地图坐标系右方为参考方向，逆时针时为正值，计算各个超声波传 感器在地图坐标系中发射超声波的方向，即9 i+heading-90 ° , Θ 2+heading_90 ° , 03+heading_9O。，…，0n+heading_9〇。，且通过取模运算转化为0。?360。之内。 因为只有入射角在±20度范围内的超声波得到的是垂直入射距离，所以筛选保留入 射角在±20°范围内的超声波传感器信息。即对于可用的超声波传感器，其入射角为 0° (360。）±20。，90° ±20。，180° ±20。，270° ±20。，这里用多个三元数组表 示，即（A，hd)，（r2, β2，(12)，（r3, β3，(13)，···，（rnl, 3nl，dnl)，其中?是角度过滤后的 超声波传感器的个数，rji = 1?ηι)表示超声波传感器到定位主体中心的距离， =1?nl)表示超声波传感器相对于地图坐标系正右方的夹角，逆时针为正值，即β i = mod( Θ i+heading-90。，360° )，ddi = 1?nl)表示超声波传感器的初始测距信息；
[0072] 步骤B :在经过步骤A筛选保留下来的超声波传感器阵列中，计算每个超声波传感 器的三元组信息换算到定位主体到类矩形环境的四个边缘的信息。当然，每个超声波传感 器只换算到类矩形环境的一边。具体地，请参阅图5,假设超声波传感器i相对于在地图坐 标系中的入射方向在±20°内，则该超声波传感器可对应定位主体到类矩形环境的右边的 距离，设其三元数组信息为（A，βρφ)，利用I获取入射角 Yi，这里假设β, = 10°，则 、=10。（若 1 = 95。，则、=|95-90|。=5。，若1 = 165。，则、=|165-180|。 = 15° )，得到入射角、后，可以计算该实测距离值(1/=1^(3〇8(、)+屯；
[0073] 步骤C :比较d/ (i = 1?nl)和相应的4个预测距离右距离d_right，上距离d_ up，左距离d_left，下距离d_down中的一个进行比较。具体地，仍以步骤B中给出的L = 10°为例，d/将和d_right进行比较。如果两者相差过大，即绝对值差超过一个阈值，比如 20cm，有| d/ -d_right | >20,则舍弃该d/，否则保留该距离信息，并记录和据测距离的差绝 对值Δ i ;
[0074] 步骤D :在保留下来的超声测距信息中，对于右、上、左、下每一边缘，根据距离差 Mi = l?n2，n2是距离差值过滤后的超声波传感器的个数）给予每个实测计算距离一定 的权值 Wi，若距离差\越小则权重越大，反之权重越小。具体计算方法为，以右侧边缘为 例，假设Aji = l?n_right，n_right是对应于右方的所有合适的超声传感器的个数）是 对应于右方的第i个实测计算距离的差值，d/则该d/的权重为：
【权利要求】
1. 一种用于室内导航的超声波阵列辅助定位方法，用以辅助惯性导航系统，提供了定 位主体和若干设于所述定位主体外围的超声波传感器，若干所述超声波传感器的发射超声 波的方向的反向延长线均通过所述定位主体的中心，所述超声波传感器用以测量超声波发 射点到对应类矩形环境一条边缘的垂线距离，得到测距值； 该方法包括如下步骤： 501 :通过惯性导航系统获得所述定位主体的位置信息、类矩形环境的地图信息，进而 得到所述定位主体的中心到类矩形环境的四个边缘的四个预测距离值； 502 :根据所述超声波传感器相对定位主体的位置、定位主体的航向以及超声波传感器 的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实测距离值； 503 :将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值的绝 对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越大，权重越小； 504 :依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离； 505 :依据最终的实际距离对定位主体进行位置的更新。
2. 如权利要求1所述的用于室内导航的超声波阵列辅助定位方法，其特征在于：在所 述步骤S02之前，还包括对所述超声波传感器筛选的过程，在该过程中，先计算每个所述超 声波传感器相对于类矩形环境地图的朝向，然后根据朝向的角度阈值过滤掉部分超声波传 感器，仅就未被过滤的超声波传感器实施后续步骤。
3. 如权利要求1所述的用于室内导航的超声波阵列辅助定位方法，其特征在于：在所 述步骤S03中，还包括对实测距离值筛选的过程，在该过程中，将实测距离值与预测距离值 差值的绝对值与一个阈值比较，若大于该阈值，则将该实测距离值过滤掉，仅就未被过滤掉 的实测距离值进行权重设定，进而实施后续步骤。
4. 一种用于室内导航的超声波阵列辅助定位系统，用以辅助惯性导航系统，包括粗略 位置获取模块、地图信息获取模块、距离预测模块、超声波阵列测距模块、航向获取模块和 导航滤波模块， 所述粗略位置获取模块，用以通过所述惯性导航系统获得定位主体的位置信息； 所述地图信息获取模块，用以获得定位主体所处类矩形环境的地图信息； 所述距离预测模块，用以根据定位主体的位置信息和其所处类矩形环境的地图信息计 算定位主体到类矩形环境四个边缘的距离，得到相应的预测距离值； 所述超声波阵列测距模块，至少包括若干安装于定位主体上的超声波传感器，所述超 声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距值； 所述航向获取模块，用以通过所述惯性导航系统获得定位主体的前进方向； 所述导航滤波模块，用以根据所述预测距离值、测距值和定位主体的前进方向，计算得 到定位主体到类矩形环境四条边的实际距离，进而实现对定位主体位置的位置更新。
5. 如权利要求4所述的用于室内导航的超声波阵列辅助定位系统定位系统，其特征在 于：若干所述超声波传感器分布于所述定位主体的外围，且每个所述超声波传感器发射超 声波的方向的反向延长线均通过所述定位主体的中心。
6. 如权利要求4所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：所述 导航滤波模块在用于计算得到定位主体到类矩形环境四条边的实际距离时， 所述导航滤波模块先用以根据所述超声波传感器相对定位主体的位置、定位主体的航 向以及超声波传感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实 测距离值； 所述导航滤波模块再用以将实测距离值与预测距离值进行比较，计算两者差值的绝对 值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越 大，权重越小； 然后，所述导航滤波模块依据不同的权值对实测距离值进行加权平均求和，得到最终 的实际距离； 最后，所述导航滤波模块依据最终的实际距离对定位主体进行位置的更新。
7. 如权利要求6所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：在计 算得到所述实测距离值之前，所述导航滤波模块还用以实施所述超声波传感器筛选的过 程，在该过程中，先计算每个所述超声波传感器相对于类矩形环境地图的朝向，然后根据朝 向的角度阈值过滤掉部分超声波传感器，仅就未被过滤的超声波传感器实施后续步骤。
8. 如权利要求6所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：在对 实测距离值设置不同的权重之前，所述导航滤波模块还用以实施对实测距离值筛选的过 程，在该过程中，将实测距离值与预测距离值差值的绝对值与一个阈值比较，若大于该阈 值，则将该实测距离值过滤掉，仅就未被过滤掉的实测距离值进行权重设定，进而实施后续 步骤。
【文档编号】G01S5/18GK104111445SQ201410324167
【公开日】2014年10月22日 申请日期:2014年7月9日 优先权日:2014年7月9日
【发明者】刘成旋, 钱久超, 裴凌, 邹丹平, 刘佩林, 郁文贤, 籍晨 申请人:上海交通大学