CN107734457B - 智慧停车场导航***及方法 - Google Patents

Abstract

本发明公开一种智慧停车场导航***及方法，在手机用户终端，根据用户选择的车位信息得到需要到达的目标位置，再根据惯性导航和iBeacon蓝牙信标的组合定位信息得到实时位置，利用两个位置进行路径规划。本发明结合捷联式惯性导航和iBeacon蓝牙定位等技术，惯性导航是一种无须部署其它辅助设备，短期定位精度达到米级的自主导航方式。智能手机标配有加速度计和陀螺仪等惯性传感器，这些内置传感器使得手机用户本身具有了丰富的移动信息，为手机实现惯性导航提供了数据支持。

Description

智慧停车场导航***及方法

技术领域

本发明涉及停车场技术领域，具体涉及一种智慧停车场导航***及方法。

背景技术

在互联网无处不在的时代，智能手机的使用越来越普及，基于智能手机和移动互联网的开发和应用，已经变成了社会热点问题。另外，当今社会汽车越来越多，人们借助汽车，能快速穿梭于城市各个角落之间，但是停车难等问题也随之出现。如何实现停车场的集中管理，使其运行得高效有序，显得尤为重要。虽然智能停车场能够实现停车场的有序集中管理，但是现有智能停车场大多具有车位空闲汇总、停车LED指示和自动收费等功能，欠缺车位网络预订和室内停车导航功能，特别是GNSS高精度定位芯片在智能手机上的普及，以及手机自主室内定位技术和算法日益成熟，必将激发智能手机LBS应用市场的繁荣，从而解决现有智能停车场普遍存在室内无法定位、安装成本过高、或需要特殊设备等方面的不足。

发明内容

本发明所要解决的是机动车主停车难的问题，提供一种智慧停车场导航***及方法。

为解决上述问题，本发明是通过以下技术方案实现的：

智慧停车场导航***，由室内定位***和停车场车位检测***组成；上述室内定位***包括iBeacon蓝牙信标和手机用户终端；上述停车场车位检测***包括服务器、车位信息处理终端、车位检测器和车位信息显示屏；

车位检测器数量与停车位的数量一致，这些车位检测器分别布设在每个需要检测的停车位上方，并利用超声波检测原理检测停车位的使用情况；车位检测器的输出端连接到车位信息处理终端，车位信息处理终端处理车位信息，处理后的车位信息分两路传输，一路传输到停车场的车位信息显示屏上，以直观显示停车场的车位使用情况，另外一路传到服务器，服务器将车位信息通过无线网发送到手机用户终端；

停车场的顶部布设有一定数量的iBeacon蓝牙信标，每2个iBeacon蓝牙信标之间相互间隔，这些iBeacon蓝牙信标的通信范围将整个停车场覆盖；手机用户终端接收到iBeacon蓝牙信标的信息，同时采集内部自带的惯性传感器输出的数据而进行组合导航定位，并根据定位结果进行路径规划。

上述方案中，iBeacon蓝牙信标在停车场内布设时，需要进行iBeacon信标节点的ID规划，以数据负载中UUID值的差异作为信标节点的区分。

基于上述***的智慧停车场导航方法，包括步骤如下：

步骤1、停车场内的车位检测器检测利用超声波检测原理检测停车位的使用情况，并将车位信息送至车位信息处理终端；

步骤2、车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息，并对其进行汇总处理后，分别送至车位信息显示屏和手机用户终端上；

步骤3、车位信息显示屏将停车场的车位使用情况进行直观显示；

步骤4、停车场内的手机用户终端接收到附件iBeacon蓝牙信标所发送的信息，并调取手机用户终端内部自带的惯性传感器输出的数据而进行蓝牙和惯性组合导航定位；

步骤5、手机用户终端根据步骤2得到的车位信息和步骤4得到的定位结果，采用蚁群路径规划进行路径规划，并最终得到一条从用户当前所处位置到达目标空闲车位的最优路径。

上述步骤3中，车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息的过程如下：

步骤31、车位信息处理终端采用轮询模式对车位检测器进行访问，每次发一个地址至所有车位检测器上；

步骤32、车位检测器收到该地址后进行地址匹配，如果地址与自身地址符合，则调用发送程序给车位信息处理终端发送已经准备好的车位信息，而地址与自身地址不符合的车位检测器则保持监听状态；

步骤33、当车位信息处理终端把所有车位检测器都访问一轮之后，将所有车位检测器返回来的信息进行汇总处理。

上述步骤4中，蓝牙和惯性组合导航定位的过程如下：

步骤41、基于iBeacon蓝牙信标的信息实现蓝牙定位；

步骤411、实现初始化方法，利用该方法对扫描***进行注册，对iBeacon蓝牙信标扫描参数进行设置，扫描采集iBeacon蓝牙信标节点ID信息，并据此信息判断出iBeacon蓝牙信标定位区域，同时对iBeacon蓝牙信标列表进行读取，获取屏幕缩放比和室内地图等信息；

步骤412、将预设的设备列表与扫描到的iBeacon蓝牙信标进行过滤匹配，标记出相关的蓝牙设备；

步骤413、获取相关蓝牙设备的RSSI值和对应绝对坐标等信息，并保存在特设数组集合中以供室内定位算法调用；

步骤414、根据扫描接收到的蓝牙数据进行载体坐标计算；

步骤42、基于惯性传感器输出的数据实现惯性定位；

步骤421、注册惯性传感器***；

步骤422、调用传感器数据获取方法进行事件监听，该方法的参数为惯性传感器事件的对象，包含了惯性传感器所获得的角速度与加速度等信息，一旦相应信息发生变化即开始收集数据；

步骤423、获得加速度和角速率的信息；

步骤424、进行惯性导航计算；

步骤425、判断固定的惯性导航周期是否结束，若没有则返回步骤424；

步骤43、利用UKF组合算法导航方法，将步骤41所得的iBeacon蓝牙定位信息和步骤步骤42所得的惯性导航定位信息进行数据融合定位；

步骤431、对步骤41和步骤42的定位结果进行时间同步化匹配，寻找同时刻的定位结果；

步骤432、根据惯性导航的解算结果和信标锚点的已知坐标构建UKF的状态转移矩阵；

步骤433、根据步骤41中的蓝牙定位结果，计算其与各个信标锚点之间的距离；

步骤434、利用步骤433中的距离信息和步骤42惯性导航定位结果，构建***测量方程相关矩阵；

步骤435、计算Sigma点，进行UKF的时间更新和测量更新，实现数据融合解算；

步骤436、获得融合定位结果。

上述步骤414的子步骤如下：

步骤4141、调出特设数组中当期时刻各信标iBeacon信号强度RSSI值，并作相应的降噪处理；

步骤4142、将每个信标的RSSI值与指纹库中对应信标序列的RSSI值进行比配，找到指纹库中最为相近的(RSSI)_e值；

步骤4143、调出指纹库中(RSSI)_e值对应的参考坐标值x_i,y_i；

步骤4144、利用下式计算RSSI值与指纹库中个参考点的信号强度均值

的欧式距离d_i；

步骤4145、利用下列下式计算每个参考点的权重W_i；

步骤4146、结合参考点位置坐标和权重W_i，利用下式计算加权平均，并获得测点蓝牙定位位置；

上述各式中，

为指纹库中每个信标锚点对应的信号强度的均值，RSSI为接收各个信标的实时信号强度值，j＝1,2,…,m，m为本时刻使用到指纹库中信标锚点的个数；i＝1,2,…,g，g为本时刻接收到有用的RSSI的蓝牙个数。

上述步骤424的子步骤如下：

步骤4241、判断接收到的加速度和角速度的时间是否在设定的导航周期内，若是，则执行下一步骤；若不是，则结束本时刻的数据解算而直接输出导航定位；

步骤4242、利用本时刻的加速度值和角速度值对四元数即时修正；

步骤4243、利用四元数，根据下式计算姿态旋转矩阵

式中，q₀为四元数的标量部分，q₁为四元数的x轴分量，q₂为四元数的y轴分量，q₃为四元数的z轴分量；

步骤4244、利用姿态旋转矩阵

将三轴上的加速度值从载体坐标系转换到导航坐标系；

步骤4245、根据接收数据的时间间隔T，分别对导航坐标系三轴上的加速度进行积分，获得三轴上的速度，并对速度进行相应的误差补偿；

步骤4246、利用误差补偿后三轴的速度和时间间隔T，计算位置分量，并进行位置补偿，然后获得该时刻的三轴位置。

上述步骤432的子步骤如下：

步骤4321、提取惯性导航解算后的导航坐标系中三轴位置量、加速度量及姿态旋转矩阵

步骤4322、利用下式构建加速度矩阵S(fⁿ _k)；

式中，f^E _k、f^N _k和f^U _k分别为导航坐标系下东、北、天三个方向上的比力值，k为解算的时刻；

步骤4323、判断是否在组合导航解算周期内，若是，则提取有效的RSSI对应的信标锚点坐标值；若不是，结束本轮的组合导航计算，直接输出惯性导航结果；

步骤4324、利用下式构建状态旋转矩阵F^I _k；

式中，T为惯性***采样周期，0_3×3为3阶零矩阵，I_3×3为3阶单位，

为阵姿态旋转矩阵。

上述步骤5中，采用蚁群路径规划进行路径规划的过程如下：

步骤51、设置参数并初始化蚁群变量；

步骤52、根据伪随机比例原则，蚂蚁实现构造路径和状态转移；

步骤53、分析蚂蚁路线，利用下式更新信息素局部；

τ(r,s)←(1-α)τ(r,s)+αΔτ(r,s)

步骤54、重复步骤52到步骤53，直到蚁群中全部蚂蚁寻找到最优路径，一次循环结束；

步骤55、利用下两式全局更新信息素；

τ(r,s)←(1-ρ)τ(r,s)+ρΔτ(r,s)

步骤56、判断是否达到预设的循环次数或终止条件，如果达到，则转至步骤57，否则，不断执行步骤52到步骤55；

步骤57、获得最优路径；

上述各式中，τ(r,s)为r、s之间的信息素，Δτ(r,s)为r、s之间的信息素增量，r表示目标当前所在的结点，s表示蚂蚁即将访问的网格，α表示信息素挥发系数，L_gb表示当前全局最佳路径，ρ为残留因子。

与现有技术相比，本发明具有如下特点：

(1)将iBceaon信标基站的位置信息引入卡尔曼滤波器***状态变量中，同时，在***观测方程将基站位置信息消去以减少组合导航模型对基站位置信息的依赖；

(2)利用UKF，将手机惯性导航和iBeacon蓝牙信标信息融合滤波，在智能手机上实现停车场室内定位；

(3)调用高德软件公司的高德地图API接口，实现室外定位导航及市内停车场搜索等功能；

(4)采用超声波探头设计车位检测器，实现停车场车位检测，开发后台服务器，通过移动互联网，实现基于Andriod平台的车位查询、网上预订和自动收费等功能；

(5)利用改进蚁群算法完成停车场室内外路径规划，实现停车指引和反向寻车；

(6)具有定位精度高、实用性强、易于安装和维护的特点，能满足平台***开发目的和要求，具有实用性强、经济性高等特点，并且移动互联，契合“互联网+”时代位置经济蓬勃发展的需要。

附图说明

图1为智慧停车场导航***的原理图。

图2为惯性导航算法流程图。

图3为基于UKF的组合导航算法流程图。

图4为组合导航应用程序实现流程图。

图5为蚁群算法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实例，并参照附图，对本发明进一步详细说明。

(一)智慧停车场导航***

参见图1，智慧停车场导航***，由iBeacon蓝牙信标、手机用户终端、服务器、车位信息处理终端、车位检测器和车位信息显示屏组成。iBeacon蓝牙信标和手机用户终端组成室内定位***；服务器、车位信息处理终端、车位检测器和车位信息显示屏组成停车场车位检测***。

将车位检测器布设在需要检测的停车位上方，利用超声波检测原理检测停车位的使用情况。车位检测器为超声波车位检测器，车位检测器由上而下发出超声波，根据反射波精确测量出反射面到探测器的距离，由此准确地检测出每个车位的停车情况。通过串口(RS485通讯接口)将车位检测器与车位信息处理终相连，车位信息上传到车位信息处理终端，并处理车位信息，处理后的车位信息分两路传输：一路通过串口传输到停车场的车位信息显示屏上，以直观提示车主停车导航；另外一路传到服务器；服务器将车位信息，通过无线网发送到手机用户终端。服务器与手机用户终端进行通信时，手机用户终端与服务器之间进行点对点的通信，而手机用户终端相互之间不需要相互通信。用户在APP上进行车位搜索，搜索信息和服务器传送的车位信息进行匹配，最后得到符合要求的停车场车位信息，用户据此信息预订车位。在停车场的顶部，根据不同的要求合理地布设一定数量的iBeacon蓝牙信标；手机用户终端接收到iBeacon蓝牙信标的信息，同时采集内部自带的惯性传感器输出的数据而进行组合导航定位。

用户室内停车场导航定位时，定位设备包括手持终端和iBeacon蓝牙信标两部分。蓝牙信标被有序布设在停车场的车道和车位等场所，具体布设需要根据定位效果而确定。通常，每40平米布设一个蓝牙信标。进行iBeacon信标节点的ID规划时，以数据负载中UUID值的差异作为信标节点的区分。十六位UUID(Universally Unique Identifier，通用唯一识别码)的前十位沿用默认编号，后六位中的前两位用于标示特定的室内环境，作为实测环境下区域空间的划分；中间两位设置为00000000；最后两位则用来表示特定的室内环境中信标节点的相对坐标。

(二)慧停车场手机导航方法

为了提高停车场定位导航***的精度和灵活性，智慧停车场导航方法，即INS/iBeacon组合导航算法，将iBceaon蓝牙信标的位置信息引入***状态变量中，降低观测量对基站信息的依赖，提高了停车场定位导航***的灵活性。同时，本发明提出利用无迹卡尔曼滤波(UKF)实现组合导航***的定位数据融合，以提高目标载体的定位信息预估精度。

在室外，用户驾车出行，进行室外导航时，在本平台配套的手机导航APP上，通过高德软件公司的API地图接口，调用高德地图服务，完成停车场位置搜索和导航。在室内，利用超声波车位检测器检测停车场车位信息，而信息经过处理后显示在停车场内的显示屏幕中，同时，更新至网络服务器。用户在手机APP上端查询车位信息，而服务器根据用户查询需求将车位信息发送到智能手机终端。用户根据车位信息选定需要到达的车位，实现车位预约，并以此确定停车导航的终点位置。用户手机根据接收到的iBeacon蓝牙信标的RSSI和ID，确定停车导航的初始位置。通过初始位置和终点位置，在经过数字图像处理的停车场地图上利用改进蚁群算法规划出一条最优路径。在运动过程中，手机APP利用UKF实现INS/iBeacon组合导航数据融合定位，而用户则根据规划出来的路径和定位结果，在停车场内便捷地停车。

本发明融合了诸多技术和重点实现方法，如手机惯性导航、iBeacon室内定位、UKF算法、蚂蚁路径规划和车位检测技术等。

Ⅰ.车位检测

本发明超声波车位探测***具有基于RS485总线的主从结构。

从机每隔一定时间负责驱动超声波模块进行测距，然后根据距离判断车位空忙，并将之保存在从机RAM里面。***采用HC-SR04超声波模块作为车位探测器，通过IO口TRIG触发，发出超声波。分析汽车或地面的反射波，可以精确测量出反射面到探测器的距离，由此测出每个车位的停车情况。

主机(即车位信息处理终端)采用轮询模式对从机(即车位检测器)进行访问，每次发一个地址，所有从机收到后进行地址匹配，如果地址与自身符合，则调用发送程序给主机发送已经准备好的结果，而地址与自身不符合的从机则保持监听状态；当主机把所有从机都访问一轮之后，将所有从机返回来的信息进行汇总处理，并显示每个车位的空忙情况。

大型停车场内可以选用MAX487接口芯片，其负载能力强大，可以实现数以千计的车位检测***。

Ⅱ.INS/iBeacon组合导航算法

捷联惯性导航以牛顿力学为基础，采用加速度计和陀螺仪完成载体加速度和角速度的测量，构建“数学平台”——姿态旋转矩阵解算载体的姿态，并将加速度转换到导航坐标系，计算得到运动目标速度和位置，是一种不依赖外部辅助信息的自主导航方式。目前，许多智能手机配备了惯性传感器，开发者可以通过Framework API的传感器类，获取所需要的传感器数值，进行导航计算。iBeacon是苹果公司推出的一种新型的位置感知技术，是低功耗的蓝牙传输技术(BLE)。iBeacon根据射频场强随距离衰减的模型计算距离，其将自身设为一个信息基站，当有智能终端进入指定区域时，标记该终端的位置信息。iBeacon的信号强度采用RSSI值表示，可以通过RSSI值的变化来判断智能终端与iBeacon设备之间的距离。

本发明利用UKF实现INS/iBeacon组合导航***的数据融合滤波。在UKF算法中，为可获得更多的观测信息设定而利用采样值变换得到的一系列sigma点，从而对***状态的协方差和均值做了更为准确的估计。同时，在观测方程或状态方程中不需要非线性变换，从而避免了线性化误差。对于组合导航非线性***而言，UKF与其他滤波方法相比，有着运算量少、估计效果显著的优点。

蓝牙定位不依赖于前一时刻的估算，仅根据当前时刻的信号观测值来进行离散位置估计。惯性导航虽然可以实现短期高精度的自主定位导航，但是智能手机上配备的惯性传感器精度较低，若长时间进行导航计算，则存在较大的积分误差。因此，在一定的导航周期内，需要利用UKF组合导航算法，将蓝牙定位与手机惯性导航定位信息融合，以达到长时间室内导航定位的目的。

(1)iBeacon蓝牙定位算法

iBeacon蓝牙定位时，通过RSSI值的规律性变化计算iBeacon设备与智能手机与的距离。本发明使用“近邻法”实现iBeacon室内定位。在近邻法中，先采集各个参考基站的RSSI指纹信息，然后根据实时的iBeacon信号强度RSSI值计算其欧式距离。

再计算各参考点的权重，如式(2)所示，

最后，加权平均，利用参考点位置坐标计算待测点位置。

基于iBeacon蓝牙信标的信息实现蓝牙定位的过程如下：

步骤11、实现初始化方法，利用该方法对扫描***进行注册，对iBeacon蓝牙信标扫描参数进行设置，扫描采集iBeacon蓝牙信标节点ID信息，并据此信息判断出iBeacon蓝牙信标定位区域，同时对iBeacon蓝牙信标列表进行读取，获取屏幕缩放比和室内地图等信息；

步骤12、将预设的设备列表与扫描到的iBeacon蓝牙信标进行过滤匹配，标记出相关的蓝牙设备；

步骤13、获取相关蓝牙设备的RSSI值和对应绝对坐标等信息，并保存在特设数组集合中以供室内定位算法调用；

步骤14、根据扫描接收到的蓝牙数据进行载体坐标计算。

(2)手机惯性导航定位算法

本发明采用“东北天”地理坐标系作为导航坐标系。智能手机配备的惯性传感器有加速度计和陀螺仪等，分别安装在手机载体坐标系的x、y、z轴上，以测量三轴上的加速度a^b _E、a^b _N、a^b _U和角速度ω_E、ω_N、ω_u。这些加速度和角速度存在***误差和随机误差，可以使用数字低通滤波器对其进行误差处理，然后根据捷联式惯性导航原理计算出目标载体的位置。手机惯性导航算法流程图如图2所示。

利用误差处理后的角速度，更新四元数Q并计算姿态旋转矩阵

四元数的微分方程矩阵形式为(4)式。

其中q₀、q₁、q₂、q₃为上一时刻的四元数，ω_E、ω_N、ω_u为经过误差修正后的角速率，对式(4)求解出下一时刻的四元数。利用四元数构成姿态旋转矩阵

如(5)式所示。

利用姿态旋转矩阵可以求解出各个时刻的姿态角θ、ψ、γ，并将原本在载体坐标系的加速度转化到导航坐标系，即将a^b转换为aⁿ。在导航坐标系上对加速度aⁿ进行积分，积分时间为采用周期T，得到速度增量ΔV，再对ΔV积分得到位移增量Δs。已知初值位置的情况下，通过三轴位移增量Δs的累积可以得到手机的位置x、y、z。另外各个时刻的姿态旋转矩阵的计算需要上一时刻的速度和位置的反馈值。

基于惯性传感器输出的数据实现惯性定位的过程如下：

步骤21、注册惯性传感器***；

步骤22、调用传感器数据获取方法进行事件监听，该方法的参数为惯性传感器事件的对象，包含了惯性传感器所获得的角速度与加速度等信息，一旦相应信息发生变化即开始收集数据；

步骤23、获得加速度和角速率的信息；

步骤24、进行惯性导航计算；

步骤25、判断固定的惯性导航周期是否结束，若没有则返回步骤24。

(3)UKF组合导航算法

在INS/iBeacon组合导航算法中，UKF算法需要构建***的状态方程和量测方程。

在构建状态方程时，三维姿态误差角、三维速度误差和三维位置误差可列为导航信息误差，而加速度计和陀螺仪漂移零偏的随机常数因对***精度的影响较大也列入状态方程，由此构成15维导航信息误差。在组合导航中，iBeacon蓝牙信标基站的位置向量是已知和固定的，于是可引入各锚点位置向量，以提高预估精度。

其中，下标k代表k时刻，φ_k是包括航向角误差、横滚角误差和俯仰角误差的姿态误差；δVⁿ _k和δPⁿ _k分别是导航坐标系中的速度误差和位置误差；▽^b _k和ε^b _k是k时刻三个轴向的加速度计误差和陀螺漂移；[M₁ M₂ … M_m]为蓝牙信标锚点的位置向量坐标，

m为锚点的总数。

INS解算状态转移矩阵的微分方程可用式(7)描述。

其中

式中：[f^E _k f^N _k f^U _k]为3个方向的比力值，分别指向导航坐标系的东向、北向和天向。所以k时刻***的状态方程可描述为：

其中，F^M _k＝I_2m×2m。ω_k为***噪声，是均值为零的正态分布，其协方差矩阵为Q_k。F^I _k为k时刻INS解算误差状态转移矩阵，其表达式为

即

其中，T为***采样周期。

***的观测方程可以表示为式(11)

式中(11)中：[P^(I) _E,k,P^(I) _N,k]为k时刻INS推导的移动目标位置，d^(U) _i,k为k时刻iBeacon测量得到的距离。h_i(X_k)可以表示为式(12)。

式(12)中：

为第i个锚节点位置的理论值，而[P^(I) _E,k,P^(I) _N,k]与移动目标位置的理论值[P^(t) _E,k,P^(t) _N,k]之间存在的误差关系如下：

式(11)中v_i(d^(U) _i,k,ε)为：

v_i(d^(U) _i,k,ε)＝-2d^(U) _i,kε+ε² (14)

本发明以April Beacon硬件制造商提供的iBeacon SDK库为蓝牙定位开发基础程序库，蓝牙信号采集平台为支持BLE4.0的智能Andriod手机，选取SENSE_DELAY_FASTEST作为传感器更新速率。

利用UKF组合算法导航方法，将步骤1所得的iBeacon蓝牙定位信息和步骤步骤2所得的惯性导航定位信息进行数据融合定位的过程如图3所示：

步骤31、对步骤1和步骤2的定位结果进行时间同步化匹配，寻找同时刻的定位结果；

步骤32、根据惯性导航的解算结果和信标锚点的已知坐标构建UKF的状态转移矩阵；

步骤33、根据步骤1中的蓝牙定位结果，计算其与各个信标锚点之间的距离；

步骤34、利用步骤33中的距离信息和步骤2惯性导航定位结果，构建***测量方程相关矩阵；

步骤35、计算Sigma点，进行UKF的时间更新和测量更新，实现数据融合解算；

步骤36、获得融合定位结果。

应用程序分蓝牙定位和惯性导航定位两个部分进行，经时间同步匹配后利用UKF实现组合导航。组合导航应用程序实现流程图如图4所示。

蓝牙定位实现步骤如下：

iB_Step1：初始化时实现init()方法，利用该方法对扫描***进行注册，对iBeacon扫描参数进行设置，扫描采集信标节点ID信息，并据此信息判断出iBeacon定位区域，同时对iBeacon设备列表进行读取，获取屏幕缩放比并室内地图等信息。

iB_Step2：实现onBeaconDiscovered()方法，将预设的设备列表与扫描到的iBeacon设备进行过滤匹配，标记出相关的蓝牙设备。

iB_Step3：实现onGetDiscovered()方法，获取相关蓝牙设备的RSSI值和绝对坐标等信息，并保存在特设数组集合中以供室内定位算法调用。

iB_Step4：实现CalculatePosition()方法，根据扫描接收到的数据进行载体坐标计算，算法如前文iBeacon蓝牙定位算法所述。

惯性导航定位实现步骤如下：

INS_Step1：注册传感器***；

INS_Step2：调用OnSensoiChanged()方法进行事件监听，该方法的参数为传感器事件的对象，包含了传感器所获得的角速度与加速度等信息，一旦相应信息发生变化即开始收集数据；

INS_Step3：获得加速度和角速率的信息；

INS_Step4：编写INSCalculate()方法进行惯性导航计算，其算法如前文惯性导航定位算法所述。

INS_Step5：判断固定的惯性导航周期是否结束，若没有则返回INS_Step4。

在iB_Step4和INS_Step5之后，进行时间同步化匹配，寻找同时刻的定位结果，然后利用UKF组合算法导航CorrectionPosition()方法，将iBeacon蓝牙定位信息和惯性导航定位信息进行数据融合定位。

Ⅲ.蚁群路径规划

蚁群算法具有并行运算的特点，可以高效计算满足特定要求的最优解，被广泛应用于地图路径规划当中，具有良好的适应性能，可以便捷地与相关定位算法融合。但是当环境复杂度增加时，利用蚁群算法进行路径规划时容易出现停滞现象。本发明对蚁群算法进行改进，简化应用环境，建立的网格模型，实现路径规划。

如果用固定网格模型表示无碰撞自由目标位形空间，那么路径规划可以描述为：分别给定起始点和终点，在模型中寻找出一条满足相应的性能指标连续曲线来连接始点和终点。工作环境可由许多大小相同的网格来划分，每个格子的颜色表示可能对应的状态。假设当颜色为灰色时表示障碍物，当为其它颜色时，则表示自由空间。在静态环境下采用改进蚁群算法进行路径规划，该算法利用特定的网格对室内工作的空间进行建模，为提高算法的迭代速率，重新制定了相关变量的实现和发散方式。

利用下列式子全局更新改进蚁群***信息素：

τ(r,s)←(1-α)τ(r,s)+αΔτ(r,s) (15)

式中，α表示信息素挥发系数，L_gb表示当前全局最佳路径，r表示目标当前所在的结点，s表示蚂蚁即将访问的网格，Δτ表示单位轨迹信息素量。利用下列式子局部更新改进蚁群***信息素：

τ(r,s)←(1-ρ)τ(r,s)+ρΔτ(r,s) (17)

其中，ρ表示持久性系数，即残留因子，1-ρ表示信息素的衰减系数。

蚁群算法实现的流程图如图5所示。改进蚁群算法的实现步骤如下：

Step1：设置参数并初始化蚁群变量；

Step2：根据伪随机比例原则，蚂蚁实现构造路径和状态转移；

Step3：分析蚂蚁路线，利用式(15)更新信息素局部；

Step4：重复上面两个步骤，直到蚁群中全部蚂蚁寻找到最优路径，一次循环结束；

Step5：利用式(16)和(17)全局更新信息素；

Step6：预设的循环次数或终止条件，不断执行Step2到Step5，；

Step7：获得最优路径；

上述算法迭代次数少，且可以得到满足一定性能的最优路径。

在手机用户终端，根据用户选择的车位信息得到需要到达的目标位置，再根据惯性导航和iBeacon蓝牙信标的组合定位信息得到实时位置，利用两个位置进行路径规划。在应用程序上，调用PathPlan()方法进行路径规划，该方法用到室内地图识别算法和改进蚁群路径规划算法。室内地图识别时，需要对停车场地图进行数字图像处理，以提取出地图上的道路区域等信息，并进行网格化，具体实现编写在DImage()方法中，而蚁群路径规划算法具体实现编写在AntColonyOptimizationPP()方法中。通过实时路径规划，最终得到一条从起始位置避开所有障碍物最终到达目标位置的最优路径，利用ShowPisition()方法将最优路径通过二维地图上的标记在用户APP界面上显示。用户可以根据此定位信息实现室内停车导航。

本发明结合捷联式惯性导航和iBeacon蓝牙定位等技术，设计实现一款智慧停车场手机导航平台。惯性导航是一种无须部署其它辅助设备，短期定位精度达到米级的自主导航方式。智能手机标配有加速度计和陀螺仪等惯性传感器，这些内置传感器使得手机用户本身具有了丰富的移动信息，为手机实现惯性导航提供了数据支持。

需要说明的是，尽管以上本发明所述的实施例是说明性的，但这并非是对本发明的限制，因此本发明并不局限于上述具体实施方式中。在不脱离本发明原理的情况下，凡是本领域技术人员在本发明的启示下获得的其它实施方式，均视为在本发明的保护之内。

Claims (8)

1.智慧停车场导航方法，其特征是，包括步骤如下：

步骤1、停车场内的车位检测器检测利用超声波检测原理检测停车位的使用情况，并将车位信息送至车位信息处理终端；

步骤2、车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息，并对其进行汇总处理后，分别送至车位信息显示屏和手机用户终端上；

步骤3、车位信息显示屏将停车场的车位使用情况进行直观显示；

步骤4、停车场内的手机用户终端接收到附件iBeacon蓝牙信标所发送的信息，并调取手机用户终端内部自带的惯性传感器输出的数据而进行蓝牙和惯性组合导航定位；即：

步骤41、基于iBeacon蓝牙信标的信息实现蓝牙定位；

步骤411、实现初始化方法，利用该方法对扫描***进行注册，对iBeacon蓝牙信标扫描参数进行设置，扫描采集iBeacon蓝牙信标节点ID信息，并据此信息判断出iBeacon蓝牙信标定位区域，同时对iBeacon蓝牙信标列表进行读取，获取屏幕缩放比和室内地图等信息；

步骤412、将预设的设备列表与扫描到的iBeacon蓝牙信标进行过滤匹配，标记出相关的蓝牙设备；

步骤413、获取相关蓝牙设备的RSSI值和对应绝对坐标等信息，并保存在特设数组集合中以供室内定位算法调用；

步骤414、根据扫描接收到的蓝牙数据进行载体坐标计算；

步骤42、基于惯性传感器输出的数据实现惯性定位；

步骤421、注册惯性传感器***；

步骤422、调用传感器数据获取方法进行事件监听，该方法的参数为惯性传感器事件的对象，包含了惯性传感器所获得的角速度与加速度等信息，一旦相应信息发生变化即开始收集数据；

步骤423、获得加速度和角速率的信息；

步骤424、进行惯性导航计算；

步骤425、判断固定的惯性导航周期是否结束，若没有则返回步骤424；

步骤43、利用UKF组合算法导航方法，将步骤41所得的iBeacon蓝牙定位信息和步骤42所得的惯性导航定位信息进行数据融合定位；

步骤431、对步骤41和步骤42的定位结果进行时间同步化匹配，寻找同时刻的定位结果；

步骤432、根据惯性导航的解算结果和信标锚点的已知坐标构建UKF的状态转移矩阵；

步骤433、根据步骤41中的蓝牙定位结果，计算其与各个信标锚点之间的距离；

步骤434、利用步骤433中的距离信息和步骤42惯性导航定位结果，构建***测量方程相关矩阵；

步骤435、计算Sigma点，进行UKF的时间更新和测量更新，实现数据融合解算；

步骤436、获得融合定位结果；

步骤5、手机用户终端根据步骤2得到的车位信息和步骤4得到的定位结果，采用蚁群路径规划进行路径规划，并最终得到一条从用户当前所处位置到达目标空闲车位的最优路径。

2.根据权利要求1所述的智慧停车场导航方法，其特征是，步骤3中，车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息的过程如下：

步骤31、车位信息处理终端采用轮询模式对车位检测器进行访问，每次发一个地址至所有车位检测器上；

步骤32、车位检测器收到该地址后进行地址匹配，如果地址与自身地址符合，则调用发送程序给车位信息处理终端发送已经准备好的车位信息，而地址与自身地址不符合的车位检测器则保持监听状态；

步骤33、当车位信息处理终端把所有车位检测器都访问一轮之后，将所有车位检测器返回来的信息进行汇总处理。

3.根据权利要求1所述的智慧停车场导航方法，其特征是，步骤414的子步骤如下：

步骤4141、调出特设数组中当期时刻各信标iBeacon信号强度RSSI值，并作相应的降噪处理；

步骤4142、将每个信标的RSSI值与指纹库中对应信标序列的RSSI值进行比配，找到指纹库中最为相近的(RSSI)_e值；

步骤4143、调出指纹库中(RSSI)_e值对应的参考坐标值x_i,y_i；

步骤4144、利用下式计算RSSI值与指纹库中每个参考点的信号强度均值

的欧式距离d_i；

步骤4145、利用下列下式计算每个参考点的权重W_i；

步骤4146、结合参考点位置坐标和权重W_i，利用下式计算加权平均，并获得测点蓝牙定位位置；

上述各式中，

为指纹库中每个信标锚点对应的信号强度的均值，RSSI为接收各个信标的实时信号强度值，j＝1,2,…,m，m为本时刻使用到指纹库中信标锚点的个数；i＝1,2,…,g，g为本时刻接收到有用的RSSI的蓝牙个数。

4.根据权利要求1所述的智慧停车场导航方法，其特征是，步骤424的子步骤如下：

步骤4241、判断接收到的加速度和角速度的时间是否在设定的导航周期内，若是，则执行下一步骤；若不是，则结束本时刻的数据解算而直接输出导航定位；

步骤4242、利用本时刻的加速度值和角速度值对四元数即时修正；

步骤4243、利用四元数，根据下式计算姿态旋转矩阵

式中，q₀为四元数的标量部分，q₁为四元数的x轴分量，q₂为四元数的y轴分量，q₃为四元数的z轴分量；

步骤4244、利用姿态旋转矩阵

将三轴上的加速度值从载体坐标系转换到导航坐标系；

步骤4245、根据接收数据的时间间隔T，分别对导航坐标系三轴上的加速度进行积分，获得三轴上的速度，并对速度进行相应的误差补偿；

步骤4246、利用误差补偿后三轴的速度和时间间隔T，计算位置分量，并进行位置补偿，然后获得该时刻的三轴位置。

5.根据权利要求1所述的智慧停车场导航方法，其特征是，步骤432的子步骤如下：

步骤4321、提取惯性导航解算后的导航坐标系中三轴位置量、加速度量及姿态旋转矩阵

步骤4322、利用下式构建加速度矩阵S(fⁿ _k)；

式中，f^E _k、f^N _k和f^U _k分别为导航坐标系下东、北、天三个方向上的比力值，k为解算的时刻；

步骤4323、判断是否在组合导航解算周期内，若是，则提取有效的RSSI对应的信标锚点坐标值；若不是，结束本轮的组合导航计算，直接输出惯性导航结果；

步骤4324、利用下式构建状态旋转矩阵F^I _k；

式中，T为惯性***采样周期，0_3×3为3阶零矩阵，I_3×3为3阶单位，

为阵姿态旋转矩阵。

6.根据权利要求1所述的智慧停车场导航方法，其特征是，步骤5中，采用蚁群路径规划进行路径规划的过程如下：

步骤51、设置参数并初始化蚁群变量；

步骤52、根据伪随机比例原则，蚂蚁实现构造路径和状态转移；

步骤53、分析蚂蚁路线，利用下式更新信息素局部；

τ(r,s)←(1-α)τ(r,s)+αΔτ(r,s)

步骤54、重复步骤52到步骤53，直到蚁群中全部蚂蚁寻找到最优路径，一次循环结束；

步骤55、利用下两式全局更新信息素；

τ(r,s)←(1-ρ)τ(r,s)+ρΔτ(r,s)

步骤56、判断是否达到预设的循环次数或终止条件，如果达到，则转至步骤57，否则，不断执行步骤52到步骤55；

步骤57、获得最优路径；

上述各式中，τ(r,s)为r、s之间的信息素，Δτ(r,s)为r、s之间的信息素增量，r表示目标当前所在的结点，s表示蚂蚁即将访问的网格，α表示信息素挥发系数，L_gb表示当前全局最佳路径，ρ为残留因子。

7.实现权利要求1所述智慧停车场导航方法的智慧停车场导航***，其特征是，由室内定位***和停车场车位检测***组成；上述室内定位***包括iBeacon蓝牙信标和手机用户终端；上述停车场车位检测***包括服务器、车位信息处理终端、车位检测器和车位信息显示屏；

车位检测器数量与停车位的数量一致，这些车位检测器分别布设在每个需要检测的停车位上方，并利用超声波检测原理检测停车位的使用情况；车位检测器的输出端连接到车位信息处理终端，车位信息处理终端处理车位信息，处理后的车位信息分两路传输，一路传输到停车场的车位信息显示屏上，以直观显示停车场的车位使用情况，另外一路传到服务器，服务器将车位信息通过无线网发送到手机用户终端；

停车场的顶部布设有一定数量的iBeacon蓝牙信标，每2个iBeacon蓝牙信标之间相互间隔，这些iBeacon蓝牙信标的通信范围将整个停车场覆盖；手机用户终端接收到iBeacon蓝牙信标的信息，同时采集内部自带的惯性传感器输出的数据而进行组合导航定位，并根据定位结果进行路径规划。

8.根据权利要求7所述的智慧停车场导航***，其特征是，iBeacon蓝牙信标在停车场内布设时，需要进行iBeacon信标节点的ID规划，以数据负载中UUID值的差异作为信标节点的区分。

Images