基于MEMS传感器的车位级停车惯性导航系统及导航方法与流程

基于mems传感器的车位级停车惯性导航系统及导航方法
技术领域
1.本发明属于车辆导航技术领域，涉及一种基于mems传感器的车位级停车惯性导航系统，还涉及一种基于mems传感器的车位级停车惯性导航方法。


背景技术：

2.截止2020年底，我国汽车保有量达2.81亿，而我国停车位个数仅有1.19亿个。2021年4月6日公安部交通管理局最新统计资料显示，北上广深停车场泊位空置率达到44.6％，全国城市超九成车位的使用率小于50％，乱停车、找车位难、数据隔离等一系列“停车难”、“寻找车位难”问题引起社会广泛关注。
3.通过对国内的公共停车场和私人停车场的实地调研和数据分析，发现许多停车场和停车位布局过于复杂，导致停车位寻找困难，而停车场自身提供的车位诱导和反向寻车服务不够智能，车主难以寻找停车位。同时，对于在对智能停车的app调查中发现，发现暂无管理系统能够做到客户端实时车位智能诱导、移动式反向寻车等功能，故现状导航无法实现完整出行链精确诱导。
4.解决目前地下停车场定位导航精度低、停车场设施改造价格高昂的问题。目前地下导航技术定位情况不佳，且对于硬件设施的依赖高。本方案提出两种解决思路：一、削弱地下导航对于硬件设施的依赖，充分利用手机能够提供的数据信息。二、利用惯性导航取代目前的蓝牙、wifi等地下定位系统。对于惯性导航的不足，利用约束条件和数学模型进行迭代优化。


技术实现要素：

5.本发明的目的是提供一种基于mems传感器的车位级停车惯性导航系统及导航方法，解决目前地下停车场定位导航精度低、停车场设施改造价格高昂的问题。
6.本发明所采用的技术方案是，一种基于mems传感器的车位级停车惯性导航系统，包括：
7.物理层，为该导航系统运行提供数据支撑，由停车场设备提供停车场车位分布图、车位状态，手机用户端陀螺仪用于测出手机角度的变化数据、加速度变化数据；
8.中转层，包括信息传输层，信息传输层包括有nb
‑
iot基站和停车场主机，nb
‑
iot基站负责将物理层采集的数据进行转发，停车场主机将网络层下达的指令进行转发，中转层保障了数据传输的有效性，可靠性和安全性，统一了开发的接口，便于设备的进扩展以及未来的开发；
9.网络层，对各个停车场的设备进行管理以及处理物理层采集的数据，将处理后的结果呈现给应用层的用户，网络层由中心主机组成，把停车场主机发送的数据传送至中心主机进行算法计算，网络层为系统功能的底层规律进行负责；
10.应用层，主要用于与用户进行信息交互，将网络层的数据通过手机用户端以地图及文字的方式呈现给用户；
11.用户层，该层代表车主，使用语音助手取代直接的人机交互以确保技术实现的安全性，语音助手首先要得到人说话的音频，根据内容进行执行操作并回复，再将回复通过语音的形式播放出来。
12.本发明所采用的另一个技术方案是，一种基于mems传感器的车位级停车惯性导航方法，应用上述一种基于mems传感器的车位级停车惯性导航系统，包括以下步骤：
13.步骤1、根据停车场内部道路结构及车位分布进行建模，完成信息转化，停车场内的车位检测器利用图像处理原理检测停车位的使用情况，并将车位信息送至车位信息处理端；
14.步骤2、调取步骤1中的停车场标准图及车位编号，对车位进行最优分配，将车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息，并对其进行汇总处理后，发送至手机用户终端上；
15.步骤3、利用手机用户端调取信息，判断用户状态，通过惯性导航算法进行定位，并使用拐点修正、bp神经网络算法对定位信息进行修正。手机用户终端根据步骤2得到的车位信息和定位结果，采用最短路径规划进行路径规划，并最终得到一条从用户当前所处位置到达目标空闲车位的最优路径；
16.步骤4、将步骤3中得到的最优路径发送至手机用户终端，将结果展示给用户。
17.本发明的特点还在于：
18.步骤1包括以下步骤：
19.步骤1.1、对车位进行编码，包括以下步骤：
20.步骤1.1.1、根据《停车位基础编码规则》生成车位编码；
21.步骤1.1.2、对车位编码简化，仅提取停车场码、停车位码、0
‑
3号角点位置码、同时增加车位状态编码，采用一位编码，空车位为0，占用车位为1，其他情况为2，得到简化编码；
22.步骤1.1.3、对步骤1.1.2得到的简化编码的数据库存储调用机制；
23.步骤1.1.4、对车位信息进行数据监控；
24.步骤1.2、停车场道路信息进行转化，规则如下：
25.步骤1.2.1、获取停车场道路结构平面图；
26.步骤1.2.2、由于停车场内部道路结构较为简单，利用有向道路网络模型对上述结构平面图进行转化，生成停车场标准图，停车场标准图包括停车场内的所有节点集及邻接矩阵，每个节点对应一个唯一的id号，通过节点id即获得节点坐标及出度、入度。
27.步骤1.1.3包括以下步骤：
28.步骤1.1.3.1、系统将依据简化编码将总数据库分为若干子数据库，各停车场下的停车位码、角点位置码及车位状态码形成的子数据库构成总数据库；
29.步骤1.1.3.2、系统录入车位时根据停车位码获取车位信息，即角点位置码及车位状态码，依据简化编码进行分类存储；
30.步骤1.1.3.3、对车位的所有查询基于该数据存储调用机制，调取总数据库内车位信息时依据搜索目标先定位子数据库再定位具体字节进行遍历比对，且车位信息在手机用户端展示默认只展示车位状态码；
31.步骤1.1.4包括以下步骤：
32.步骤1.1.4.1、通过接收车位检测器采集的数据来达到实时监控的目的，数据的获
取通过基于soap的web service接口的方式进行；
33.步骤1.1.4.2、车位数据接口传递的内容为:时间，token和数据。时间是为了进行记录，token是为了对调用者进行验证；
34.步骤1.1.4.3、双方预先约定一个key,当接口接收到数据后，会将时间和key进行md5加密，并与token进行比较，验证是否为合法请求，确保数据的准确和安全，数据采用了自定义的数据传输协议格式；
35.步骤1.1.4.4、通过已有的图像处理技术，车位相机采集的基本数据有:车位状态，时间，车牌号、车型，品牌，颜色信息；为了保证常用数据可以被快速解析，提升系统效率，系统会预先约定好公共数据的名称，以便直接解析。
36.步骤1.1.4.5、车位信息数据变动时对子数据库进行变动，每1分钟将子数据库信息同步到总数据库，避免总数据库频繁变动的同时确保总数据库信息可靠性，便于统一管理。
37.步骤2中，对最优车位的分配方法如下：
38.步骤2.1、获取停车场标准图，包含停车场节点集与邻接矩阵，获取停车场节点坐标，获取停车场各个区空车位数量与位置坐标；若为多层停车场，在以上信息基础上还需获取停车场总层数；
39.步骤2.2、在使各个区域车辆数量趋近相同的前提下，以优先级选择某一区域，对该区域内空车位进行遍历：以停车场入口为初始节点，分别将各车位距离最近节点作为目的节点代入dijkstra算法计算得出最优路径，对每个车位对应的路径分别存储；
40.步骤2.3、获取存储的路径，计算路径以下信息：路径长度、拐点个数、跨越的层数，商场入口直线距离；
41.步骤2.4、对步骤3所得的信息进行加权平均，记为路径复杂度，具体计算公式为：
[0042][0043]
式(1)中，a为路径复杂度，l为停车场标准图所有边长和，l为路径长度，n为停车场标准图节点个数，n为拐点个数。g为总层数，g为跨越层数；
[0044]
步骤2.5、对步骤2.4所得的所有路径的路径复杂度进行比较，筛选出复杂度最小的路径，路径对应车位即为最优车位；
[0045]
步骤2.6、将最优车位编号发送至移动端，确认发送成功后解除主机端程序占用并释放该程序缓存。
[0046]
6.根据如权利要求2的一种基于mems传感器的车位级停车惯性导航方法，步骤3包括以下步骤：
[0047]
步骤3.1、区分用户端当前状态，包括以下步骤:
[0048]
步骤3.1.1、调取用户手机端陀螺仪测出手机角度的变化数据、加速度变化数据；
[0049]
步骤3.1.2、去除异常值，删除坏值，包括以下步骤：
[0050]
步骤3.1.2.1、删除数据不完整的坏值；
[0051]
步骤3.1.2.2、判断数据是否服从正正态，若服从，通过3σ原则进行异常值的删除，若不服从，通过四分位距法进行异常值的删除；
[0052]
步骤3.1.3、通过陀螺仪测出的手机角度变化检测到人体重心的偏移程度，判断是
否在行走，如果有重心的偏移，则处于步行状态，如果没有，则处于驾驶状态；
[0053]
步骤3.2、利用惯性定位算法分别计算驾驶状态下、步行状态下当前位置；
[0054]
步骤3.3、对驾驶状态下的位置进行拐点修正；
[0055]
步骤3.4、对驾驶状态和步行状态下的位置通过b
‑
p神经网络模型对每一段路线进行修正；
[0056]
步骤3.5、确定用户定位后，根据目标车位进行首次路线规划，包括以下步骤：
[0057]
步骤3.5.1、获取停车场标准图，包含停车场节点集与邻接矩阵。根据最优车位编号在数据库中查找最优车位位置坐标；
[0058]
步骤3.5.2、根据最优车位位置坐标与停车场各节点坐标计算两点间直线距离，比较该距离，得出最优车位的最近节点标号并进行存储；
[0059]
步骤3.5.3、将最近节点作为目标节点，停车场入口作为初始节点代入dijkstra算法进行计算，得出首次规划路径；
[0060]
步骤3.6、若用户偏离既定路线，则根据最新定位进行二次动态规划，直至用户到达目标车位，否则一直重复步骤3.6，直至用户回到二次动态规划的路线，包括以下步骤：
[0061]
步骤3.6.1、确定首次规划存储的最近节点j，j为标号与入口节点i，获取停车场标准图，停车场标准图包含停车场节点集与邻接矩阵；
[0062]
步骤3.6.2、对于标准图中每一个节点k判断邻接矩阵是否有distance(i，j)<distance(i，k) distance(k，j)，若成立则令distance(i，j)＝distance(i，k) distance(k，j)，构建矩阵path并对节点k进行存储，具体为path(i，j)＝k。遍历全部节点后distance(i，j)即为最短路径距离；根据path矩阵，对最优路径经历的全部拐点中排在i与j中间的拐点进行连接，得到动态规划路径经过的全部拐点；
[0063]
步骤3.6.3、根据停车场标准图判断动态规划路径依次经过的边，即可得出动态规划路径
[0064]
步骤3.6.4、清除首次规划路径，显示动态规划路径。
[0065]
步骤3.6.5、当用户点击完成停车或用户离开停车场时，存储停车车位编号，解除移动端程序占用。当用户车辆驶出停车场时将停车车位编号发送至主机端作为用户历史停车记录的一部分，释放移动端相关存储。
[0066]
步骤3.4包括以下步骤：
[0067]
步骤3.4.1、以拐点为路段划分依据，在每一路段中通过b
‑
p神经网络模型进行修正；
[0068]
步骤3.4.2、通过车辆动力学模型计算输出速度；
[0069]
步骤3.4.3、记录车辆动力学的输出速度，从停车场入口开始计时的计时变量t，并将这两者作为b
‑
p神经网络模型的输入；
[0070]
步骤3.4.4、确认隐含层层数为2，激活函数；
[0071]
步骤3.4.5、输出量的值为时间t内走过的位移，与停车场平面图中当前路段在速度v，时间t下应该得到的理论位移信息对比，得出误差，反向传播，进行修正每一层之间的权重；
[0072]
步骤3.4.6、当遇到拐点时，按照拐点修正得到的速度，时间重置为0为b
‑
p神经网络模型重新的输入，重复3.4.2步骤，直至遇到下一个拐点，或者结束导航。
[0073]
步骤3.4.2包括以下步骤实施：
[0074]
步骤3.4.2.1、建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异后模型可等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型；
[0075]
步骤3.4.2.2、据牛顿力学，车辆的动力学模型为
[0076]
mv
mbx
＝mv
mby
w
mz
‑
2f
tf sinδf
‑
2f
sf cosδf
‑
2f
sr
ꢀꢀ
(2)
[0077]
mv
mby
＝
‑
mv
mbx
w
mz
2f
tf cosδf
‑
2f
sf
ꢀꢀ
(3)
[0078][0079]
i
z
w
mz
＝2a f
tf sinδf 2a f
sf cosδf
‑
2bf
sr
ꢀꢀ
(4)
[0080]
式(2)～(4)中:v
mbx
、v
mby
和w
mz
分别为车辆的横向速度、纵向速度和横摆角速度；m、i
z
分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ,除以从方向盘到前轮的转向传动比来确定；c
d
为空气阻力系数；a
f
为车辆前向面积；ρ
a
为空气密度；f
tf
、f
tr
分别为作用在单个前轮和后轮上的纵向力；f
sf
、f
sr
分别为作用在单个前轮和后轮上的横向力；
[0081]
步骤3.4.2.3、利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量u＝[δf,f
tf
，f
tr
]
t
；
[0082]
步骤3.4.2.4、通过四阶龙格库塔法求解微分方程可得载体坐标系下车辆的横向速度v
mbx
、纵向速度v
mby
和横摆角速度w
mz
，根据ins解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度:
[0083][0084]
步骤3.4.2中b
‑
p神经网络的激活函数为
[0085][0086]
式(6)中，e为自然常数。
[0087]
本发明的有益效果是：数学算法与惯性导航技术的融合应用，摆脱外部信息依赖，解决地下停车场导航问题，完善出行末端导航；在打造全出行链智慧停车服务，致力于解决动态交通末端向静态交通过渡问题，打通动态交通与静态交通之间的联系，解决车主在停车场内诱导停车、反向寻车及后续消费服务等问题，完善用户出行服务。智能有序的管理，提高停车场的安全性和经济性并大幅提升停车场的停车效率。车位级导航基础上融合大数据分析实现车位变化预测及偏好分析，提高用户出行效率，改善出行体验。优化区域资源配置，解决城市出行、城市交通和改善城市的面貌，助力于城市交通信息化发展，可产生巨大的经济效应。
附图说明
[0088]
图1是本发明的系统示意图；
[0089]
图2是本发明的车位编码组成示意图；
[0090]
图3是本发明的惯性导航的惯性定位工作原理图；
[0091]
图4是本发明的车辆动力学简化模型示意图；
[0092]
图5是本发明的b
‑
p神经网络模型各层信息传播示意图。
具体实施方式
[0093]
下面结合附图和具体实施方式对本发明进行详细说明。
[0094]
一种基于mems传感器的车位级停车惯性导航系统，如图1所示，包括：
[0095]
物理层，为该导航系统运行提供数据支撑，由停车场设备提供停车场车位分布图、车位状态，手机用户端陀螺仪用于测出手机角度的变化数据、加速度变化数据；
[0096]
中转层，包括信息传输层，信息传输层包括有nb
‑
iot基站和停车场主机，nb
‑
iot基站负责将物理层采集的数据进行转发，停车场主机将网络层下达的指令进行转发，中转层保障了数据传输的有效性，可靠性和安全性，统一了开发的接口，便于设备的进扩展以及未来的开发；
[0097]
网络层，对各个停车场的设备进行管理以及处理物理层采集的数据，将处理后的结果呈现给应用层的用户，网络层由中心主机组成，把停车场主机发送的数据传送至中心主机进行算法计算，网络层为系统功能的底层规律进行负责；
[0098]
应用层，主要用于与用户进行信息交互，将网络层的数据通过手机用户端以地图及文字的方式呈现给用户；
[0099]
用户层，该层代表车主，使用语音助手取代直接的人机交互以确保技术实现的安全性，语音助手首先要得到人说话的音频，根据内容进行执行操作并回复，再将回复通过语音的形式播放出来。
[0100]
一种基于mems传感器的车位级停车惯性导航方法，应用上述的一种基于mems传感器的车位级停车惯性导航系统，包括以下步骤：
[0101]
步骤1、根据停车场内部道路结构及车位分布进行建模，完成信息转化，停车场内的车位检测器检测利用图像处理原理检测停车位的使用情况，并将车位信息送至车位信息处理端；
[0102]
步骤2、调取步骤1中的停车场标准图及车位编号，对车位进行最优分配，将车位信息处理终端接收到停车场内所有车位检测器反馈回的车位信息，并对其进行汇总处理后，发送至手机用户终端上；
[0103]
步骤3、利用手机用户端调取信息，判断用户状态，通过惯性导航算法进行定位，并使用拐点修正、bp神经网络算法对定位信息进行修正。手机用户终端根据步骤2得到的车位信息和定位结果，采用最短路径规划进行路径规划，并最终得到一条从用户当前所处位置到达目标空闲车位的最优路径；
[0104]
步骤4、将步骤3中得到的最优路径发送至手机用户终端，将结果展示给用户。
[0105]
步骤1包括以下步骤：
[0106]
步骤1.1、对车位进行编码，包括以下步骤：
[0107]
步骤1.1.1、如图2所示，根据中国交通运输协会颁布的关于团体标准《停车位基础编码规则》，对停车场内车位进行车位编号确认，停车位编码包括统一地址编码段、空间区位编码段、管理属性编码段和校验码段4部分，统一地址编码包括市辖区编码、街道编码、社
区编码、基础网格编码、建筑物编码、停车场编号、车位编号7个字段，共27位，空间区位编码段应包括0号角点空间位置码、1
‑
3号角点空间位置码、高度码共3个字段、共计41位，管理属性编码段包括采集时间编码，位置属性分类码，适停车辆分类码、经营性质分类码、形态分类码、建筑物性质分类码、产权分类码、用途分类码8个字段，共计15位。
[0108]
步骤1.1.2、由于车位编码结构过长，为方便人工标注和辩读，对车位编码简化，仅提取停车场码、停车位码、0
‑
3号角点位置码、同时增加车位状态编码，采用一位编码，空车位为0，占用车位为1，其他情况为2，得到简化编码；
[0109]
步骤1.1.3、对步骤1.1.2得到的简化编码的数据库存储调用机制；
[0110]
步骤1.1.4、对车位信息进行数据监控；
[0111]
步骤1.2、停车场道路信息进行转化，规则如下：
[0112]
步骤1.2.1、获取停车场道路结构平面图；
[0113]
步骤1.2.2、由于停车场内部道路结构较为简单，利用有向道路网络模型对上述结构平面图进行转化，生成停车场标准图，停车场标准图包括停车场内的所有节点集及邻接矩阵，每个节点对应一个唯一的id号，通过节点id即可获得节点坐标及出度、入度。
[0114]
步骤1.1.3包括以下步骤：
[0115]
步骤1.1.3.1、为避免对庞大数据库进行检索，系统将依据简化编码将总数据库分为若干子数据库，各停车场下的停车位码、角点位置码及车位状态码形成的子数据库构成总数据库；
[0116]
步骤1.1.3.2、系统录入车位时根据停车位码获取车位信息，即角点位置码及车位状态码，依据简化编码进行分类存储；
[0117]
步骤1.1.3.3、对车位的所有查询基于该数据存储调用机制，调取总数据库内车位信息时依据搜索目标先定位子数据库再定位具体字节进行遍历比对，且车位信息在手机用户端展示默认只展示车位状态码；
[0118]
步骤1.1.4包括以下步骤：
[0119]
步骤1.1.4.1、通过接收车位检测器采集的数据来达到实时监控的目的，数据的获取通过基于soap的web service接口的方式进行；
[0120]
步骤1.1.4.2、车位数据接口传递的内容为:时间，token和数据。时间是为了进行记录，token是为了对调用者进行验证；
[0121]
步骤1.1.4.3、双方预先约定一个key,当接口接收到数据后，会将时间和key进行md5加密，并与token进行比较，验证是否为合法请求，确保数据的准确和安全，数据采用了自定义的数据传输协议格式；
[0122]
步骤1.1.4.4、通过已有的图像处理技术，车位相机采集的基本数据有:车位状态，时间，车牌号、车型，品牌，颜色等信息；为了保证常用数据可以被快速解析，提升系统效率，系统会预先约定好公共数据的名称，如车牌号，车位状态，以便直接解析。
[0123]
步骤1.1.4.5、车位信息数据变动时对子数据库进行变动，每1分钟将子数据库信息同步到总数据库，避免总数据库频繁变动的同时确保总数据库信息可靠性，便于统一管理。
[0124]
步骤2中，对最优车位的分配方法如下：
[0125]
步骤2.1、获取停车场标准图，包含停车场节点集与邻接矩阵，获取停车场节点坐
标，获取停车场各个区(a、b、c、d区)空车位数量与位置坐标；若为多层停车场，在以上信息基础上还需获取停车场总层数；
[0126]
步骤2.2、在使各个区域车辆数量趋近相同的前提下，以优先级(a区>b区>c区>d区)选择某一区域，对该区域内空车位进行遍历：以停车场入口为初始节点，分别将各车位距离最近节点作为目的节点代入dijkstra算法计算得出最优路径，对每个车位对应的路径分别存储；
[0127]
步骤2.3、获取存储的路径，计算路径以下信息：路径长度、拐点个数(经过的节点数)、跨越的层数，商场入口直线距离；
[0128]
步骤2.4、对步骤3所得的信息进行加权平均，记为路径复杂度，具体计算公式为：
[0129][0130]
式(1)中，a为路径复杂度，l为停车场标准图所有边长和，l为路径长度，n为停车场标准图节点个数，n为拐点个数。g为总层数(层)，g为跨越层数；
[0131]
步骤2.5、对步骤2.4所得的所有路径的路径复杂度进行比较，筛选出复杂度最小的路径，路径对应车位即为最优车位；
[0132]
步骤2.6、将最优车位编号发送至移动端，确认发送成功后解除主机端程序占用并释放该程序缓存。
[0133]
如图3所示，步骤3包括以下步骤：
[0134]
步骤3.1、区分用户端当前状态，包括以下步骤:
[0135]
步骤3.1.1、调取用户手机端陀螺仪测出手机角度的变化数据、加速度变化数据；
[0136]
步骤3.1.2、去除异常值，删除坏值，包括以下步骤：
[0137]
步骤3.1.2.1、删除数据不完整的坏值；
[0138]
步骤3.1.2.2、判断数据是否服从正态，若服从，通过3σ原则进行异常值的删除，若不服从，通过四分位距法进行异常值的删除；
[0139]
步骤3.1.3、通过陀螺仪测出的手机角度变化检测到人体重心的偏移程度，判断是否在行走，如果有重心的偏移，则处于步行状态，如果没有，则处于驾驶状态；(或者通过绘制加速度的变化图像，若是cos图像则可以判断是步行，反之为驾驶)
[0140]
步骤3.2、利用惯性定位算法分别计算驾驶状态下、步行状态下当前位置；
[0141]
步骤3.3、对驾驶状态下的位置进行拐点修正；
[0142]
步骤3.4、对驾驶状态和步行状态下的位置通过b
‑
p神经网络模型对每一段路线进行修正；
[0143]
步骤3.5、确定用户定位后，根据目标车位进行首次路线规划，包括以下步骤：
[0144]
步骤3.5.1、获取停车场标准图，包含停车场节点集与邻接矩阵。根据最优车位编号在数据库中查找最优车位位置坐标；
[0145]
步骤3.5.2、根据最优车位位置坐标与停车场各节点坐标计算两点间直线距离，比较该距离，得出最优车位的最近节点标号并进行存储；
[0146]
步骤3.5.3、将最近节点作为目标节点，停车场入口(或商场入口，以下统称为入口)作为初始节点代入dijkstra算法进行计算，得出首次规划路径；
[0147]
步骤3.6、若用户偏离既定路线，则根据最新定位进行二次动态规划，直至用户到
达目标车位，否则一直重复步骤3.6，直至用户回到二次动态规划的路线，包括以下步骤：
[0148]
步骤3.6.1、确定首次规划存储的最近节点j，j为标号与入口节点i，获取停车场标准图，停车场标准图包含停车场节点集与邻接矩阵；
[0149]
步骤3.6.2、对于标准图中每一个节点k判断邻接矩阵是否有distance(i，j)<distance(i，k) distance(k，j)，若成立则令distance(i，j)＝distance(i，k) distance(k，j)，构建矩阵path并对节点k进行存储，具体为path(i，j)＝k。遍历全部节点后distance(i，j)即为最短路径距离；根据path矩阵，对最优路径经历的全部拐点中排在i与j中间的拐点进行连接，得到动态规划路径经过的全部拐点；
[0150]
步骤3.6.3、根据停车场标准图判断动态规划路径依次经过的边，即可得出动态规划路径
[0151]
步骤3.6.4、清除首次规划路径，显示动态规划路径。
[0152]
步骤3.6.5、当用户点击完成停车或仅用户离开停车场时，存储停车车位编号，解除移动端程序占用。当用户车辆驶出停车场时将停车车位编号发送至主机端作为用户历史停车记录的一部分，释放移动端相关存储。
[0153]
步骤3.4包括以下步骤：
[0154]
步骤3.4.1、以拐点为路段划分依据，在每一路段中通过b
‑
p神经网络模型进行修正；
[0155]
步骤3.4.2、通过车辆动力学模型计算输出速度；
[0156]
步骤3.4.3、记录车辆动力学的输出速度，从停车场入口开始计时的计时变量t，并将这两者作为b
‑
p神经网络模型的输入；
[0157]
步骤3.4.4、确认隐含层层数为2，激活函数；
[0158]
步骤3.4.5、输出量的值为时间t内走过的位移，与停车场平面图中当前路段在速度v，时间t下应该得到的理论位移信息对比，得出误差，反向传播，进行修正每一层之间的权重；
[0159]
步骤3.4.6、当遇到拐点时，按照拐点修正得到的速度，时间重置为0为b
‑
p神经网络模型重新的输入，重复3.4.2步骤，直至遇到下一个拐点，或者结束导航。
[0160]
步骤3.4.2包括以下步骤实施：
[0161]
步骤3.4.2.1、建立横向、纵向、横摆三自由度非线性车辆动力学模型，忽略左右轮差异后模型可等效简化为前、后车轮分别集中在车辆前、后轴中点而构成的一个自行车模型；
[0162]
从图4中可看出：x
n
oy
n
为导航坐标系，x
n
轴向东，y
n
轴向北；x
b
oy
b
为固定在车辆质心上的载体坐标系，x
b
轴与载体横轴重合，向右为正，y
b
轴与载体纵轴重合，向前为正。
[0163]
步骤3.4.2.2、如图4所示，据牛顿力学，车辆的动力学模型为
[0164]
mv
mbx
＝mv
mby
w
mz
‑
2f
tf sinδf
‑
2f
sf cosδf
‑
2f
sr
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0165]
mv
mby
＝
‑
mv
mbx
w
mz
2f
tf cosδf
‑
2f
sf
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0166][0167]
i
z
w
mz
＝2a f
tf sinδf 2a f
sf cosδf
‑
2bf
sr
ꢀꢀ
(4)
[0168]
式(2)～(4)中:v
mbx
、v
mby
和w
mz
分别为车辆的横向速度、纵向速度和横摆角速度；m、
i
z
分别为车辆的质量、绕竖轴的转动惯量；a、b分别为汽车前轮和后轮轮轴中心到质心的距离；δf为前轮转向角，由方向盘转角传感器测得的方向盘转角δ,除以从方向盘到前轮的转向传动比来确定；c
d
为空气阻力系数；a
f
为车辆前向面积；ρ
a
为空气密度；f
tf
、f
tr
分别为作用在单个前轮和后轮上的纵向力；f
sf
、f
sr
分别为作用在单个前轮和后轮上的横向力；
[0169]
步骤3.4.2.3、利用方向盘转角传感器和车轮力传感器信息得到车辆动力学模型的控制输入向量u＝[δf,f
tf
，f
tr
]
t
；
[0170]
步骤3.4.2.4、通过四阶龙格库塔法求解微分方程可得载体坐标系下车辆的横向速度v
mbx
、纵向速度v
mby
和横摆角速度w
mz
，根据ins解算的姿态矩阵，得到导航坐标系下由动力学模型计算的速度:
[0171][0172]
如图5所示，步骤3.4.2中b
‑
p神经网络的激活函数为
[0173][0174]
式(6)中，e为自然常数。