[发明专利]低成本高精度的高架上下坡检测方法

权利要求书

1.一种低成本高精度的高架上下坡检测方法，其特征在于，采用GNSS+IMU+里程计组合定位导航系统，包括以下步骤：

步骤1，输入组合定位导航系统的信息；

步骤2，若地图匹配在高架出入口匝道上或附近，则执行步骤3；

步骤3，检查组合定位导航系统给出的俯仰角是否有效可用，若无效或还处在校准中，则等待进入组合定位导航状态且俯仰角可用为止；若有效，执行步骤4；

步骤4，获取组合定位导航系统输出的俯仰角和里程计的速度；

步骤5，对俯仰角和速度进行低通滤波，滤除噪声；

步骤6，抓取俯仰角在进入匝道之前或刚进入匝道时的初始均值作为零偏；

步骤7，计算零偏补偿后的俯仰角和零偏补偿后的高度差；

步骤8，判断是上高架或者下高架或者平坡；

步骤9，重复步骤4-步骤8，直至地图匹配不在高架出入口匝道上或附近；

步骤10，清除计算状态，结束上下坡检测；

所述步骤1中组合定位导航系统的信息包括姿态角信息、里程计的速度信息、地图匹配给出的是否在高架出入口匝道上或附近信息；

所述步骤5中设当前时刻为第n秒，低通滤波后的俯仰角记为P_n，低通滤波后的速度记为V_n，步骤6中的零偏记为P₀；

按照1Hz输入组合定位导航系统的姿态角信息、里程计的速度信息、地图匹配给出的是否在高架出入口匝道上或附近信息；

步骤7中计算零偏补偿后的俯仰角Pn＇和零偏补偿后的高度差Hn如下：

Pn＇＝Pn-P0，Hn＝Hn-1+sin(Pn-P0)*Vn，其中Hn-1为n-1时刻计算出的高度差；

步骤8具体包括以下步骤：

若P_n＇和P_n在连续一段时间T内均大于正阈值A和B，且当前匹配的地图的匝道坡度属性为上坡，则判断当前为上高架；

若P_n＇和P_n在连续一段时间T内均小于负阈值C和D，且当前匹配的地图的匝道坡度属性为下坡，则判断当前为下高架；

当P_n＇和P_n均不满足阈值条件时，若H_n大于正阈值E且当前匹配的地图的匝道坡度属性为上坡，则判断当前为上高架；

若H_n大于负阈值F且当前匹配的地图的匝道坡度属性为下坡，则判断当前为下高架；

若上述条件均不满足，则判断当前为平坡；

阈值A、B、C、D、E、F和时间T根据里程计的速度信息进行动态调整。

2.如权利要求1所述的一种低成本高精度的高架上下坡检测方法，其特征在于，高架出入口匝道附近是指距离高架出入口匝道小于50m。