[发明专利]用于室内导航的超声波阵列辅助定位方法与系统

权利要求书

1.一种用于室内导航的超声波阵列辅助定位方法，用以辅助惯性导航系统，提供了定位主体和若干设于所述定位主体外围的超声波传感器，若干所述超声波传感器的发射超声波的方向的反向延长线均通过所述定位主体的中心，所述超声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距值；

该方法包括如下步骤：

S01：通过惯性导航系统获得所述定位主体的位置信息、类矩形环境的地图信息，进而得到所述定位主体的中心到类矩形环境的四个边缘的四个预测距离值；

S02：根据所述超声波传感器相对定位主体的位置、定位主体的航向以及超声波传感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实测距离值；

S03：将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越大，权重越小；

S04：依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离；

S05：依据最终的实际距离对定位主体进行位置的更新。

2.如权利要求1所述的用于室内导航的超声波阵列辅助定位方法，其特征在于：在所述步骤S02之前，还包括对所述超声波传感器筛选的过程，在该过程中，先计算每个所述超声波传感器相对于类矩形环境地图的朝向，然后根据朝向的角度阈值过滤掉部分超声波传感器，仅就未被过滤的超声波传感器实施后续步骤。

3.如权利要求1所述的用于室内导航的超声波阵列辅助定位方法，其特征在于：在所述步骤S03中，还包括对实测距离值筛选的过程，在该过程中，将实测距离值与预测距离值差值的绝对值与一个阈值比较，若大于该阈值，则将该实测距离值过滤掉，仅就未被过滤掉的实测距离值进行权重设定，进而实施后续步骤。

4.一种用于室内导航的超声波阵列辅助定位系统，用以辅助惯性导航系统，包括粗略位置获取模块、地图信息获取模块、距离预测模块、超声波阵列测距模块、航向获取模块和导航滤波模块，

所述粗略位置获取模块，用以通过所述惯性导航系统获得定位主体的位置信息；

所述地图信息获取模块，用以获得定位主体所处类矩形环境的地图信息；

所述距离预测模块，用以根据定位主体的位置信息和其所处类矩形环境的地图信息计算定位主体到类矩形环境四个边缘的距离，得到相应的预测距离值；

所述超声波阵列测距模块，至少包括若干安装于定位主体上的超声波传感器，所述超声波传感器用以测量超声波发射点到对应类矩形环境一条边缘的垂线距离，得到测距值；

所述航向获取模块，用以通过所述惯性导航系统获得定位主体的前进方向；

所述导航滤波模块，用以根据所述预测距离值、测距值和定位主体的前进方向，计算得到定位主体到类矩形环境四条边的实际距离，进而实现对定位主体位置的位置更新。

5.如权利要求4所述的用于室内导航的超声波阵列辅助定位系统定位系统，其特征在于：若干所述超声波传感器分布于所述定位主体的外围，且每个所述超声波传感器发射超声波的方向的反向延长线均通过所述定位主体的中心。

6.如权利要求4所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：所述导航滤波模块在用于计算得到定位主体到类矩形环境四条边的实际距离时，

所述导航滤波模块先用以根据所述超声波传感器相对定位主体的位置、定位主体的航向以及超声波传感器的测距值计算所述定位主体中心到类矩形环境的四个边缘的四个实测距离值；

所述导航滤波模块再用以将实测距离值与预测距离值进行比较，计算两者差值的绝对值，根据该差值的绝对值的大小设定不同实测距离值的不同的权重，其中，差值的绝对值越大，权重越小；

然后，所述导航滤波模块依据不同的权值对实测距离值进行加权平均求和，得到最终的实际距离；

最后，所述导航滤波模块依据最终的实际距离对定位主体进行位置的更新。

7.如权利要求6所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：在计算得到所述实测距离值之前，所述导航滤波模块还用以实施所述超声波传感器筛选的过程，在该过程中，先计算每个所述超声波传感器相对于类矩形环境地图的朝向，然后根据朝向的角度阈值过滤掉部分超声波传感器，仅就未被过滤的超声波传感器实施后续步骤。

8.如权利要求6所述的用于室内导航的超声波阵列辅助定位系统，其特征在于：在对实测距离值设置不同的权重之前，所述导航滤波模块还用以实施对实测距离值筛选的过程，在该过程中，将实测距离值与预测距离值差值的绝对值与一个阈值比较，若大于该阈值，则将该实测距离值过滤掉，仅就未被过滤掉的实测距离值进行权重设定，进而实施后续步骤。