[发明专利]一种基于蓝牙的室内停车场定位与导航方法

权利要求书

1.一种基于蓝牙的室内停车场定位与导航方法，其特征在于，包括以下步骤：

步骤1、对停车场部署多个蓝牙基站，测量各位置各基站的蓝牙信号强度，即RSSI值，整理记录成蓝牙指纹库；

步骤2、将当前位置的RSSI值向量通过K近邻算法与指纹库中的数据进行比对，筛选出最符合的k个定位点，计算当前位置与各定位点RSSI值向量的差距dis_i，选取dis_i值最小的k个定位点，计算其坐标平均值作为初步定位坐标；

步骤3、将初步定位结果进行卡尔曼滤波计算，更新卡尔曼滤波参数，得到当前定位的最终定位坐标；

步骤4、根据当前位置选取最近空车位作为导航终点：在可行路径上每隔相同距离设置相连节点，以当前位置的最近节点为起点，进行宽度优先搜索，直至到达与空停车位相邻的节点，得到相隔节点数最少，即路径最短的空车位；

步骤5、计算最短路径，并通过此路径导航到路径最短的空车位。

2.根据权利要求1所述的基于蓝牙的室内停车场定位与导航方法，其特征在于，步骤2中计算当前位置与各定位点RSSI值向量的差距dis_i，公式如下：

式中，为当前位置接收到的j号基站的RSSI值，为指纹库中i号定位点的j号基站的RSSI值；初步定位坐标计算公式如下：

式中，(x_m，y_m)为被选中的定位点的坐标。

3.根据权利要求1所述的基于蓝牙的室内停车场定位与导航方法，其特征在于，步骤3中得到定位坐标(x，y)，横坐标计算步骤如下：

步骤3.1、初始化卡尔曼滤波参数：状态转移矩阵A、加速度对位置的影响矩阵u(k)、加速度影响系数矩阵B、保证计算过程符合数学要求的调整矩阵H、初始估计协方差P、过程激励噪声协方差Q、测量噪声协方差R；

3.2、利用卡尔曼滤波修正坐标值，公式如下：

X(k|k-1)＝AX(k-1|k-1)+Bu(k) (1)

P(k|k-1)＝AP(k-1|k-1)A^T+Q (2)

K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R]^-1 (3)

X(k|k)＝X(k|k-1)+K(k)(Z(k)-HX(k|k-1)) (4)

式中X(k|k-1)为当前时刻横坐标的预测值，X(k-1|k-1)为上一时刻的准确横坐标值，P(k|k-1)为当前时刻先验估计协方差，P(k-1|k-1)为上一时刻的后验估计协方差，K(k)为卡尔曼增益，Z(k)为K近邻算法得到的初步横坐标定位结果，X(k|k)为当前横坐标的最终结果；

3.3更新卡尔曼滤波参数：完成每轮计算之后需要对后验估计协方差进行更新，公式如下：

P(k|k)＝(I-K(k)H)P(k|k-1)

P(k|k)为当前后验估计协方差；纵坐标的卡尔曼滤波过程与横坐标相同。

4.根据权利要求1所述的基于蓝牙的室内停车场定位与导航方法，其特征在于，步骤5计算最短路径采用A*算法，具体过程如下：

F＝G+H

式中F为到达某一节点的代价，G为起点到该节点的距离，H为该节点到终点距离的估计值；

G＝G_father+|x-x_father|+|y-y_father|

H＝|x-x_destination|+|y-y_destination|

使用曼哈顿距离来计算H值，执行时反复遍历候选节点open list，获取F值最小的节点加入结果集，同时把该节点的临近节点加入open list，直至遍历至终点，最短路径即为从终点倒推至起点所获得的路径。