Location and path planning in underground parking lot for intelligent vehicles

面向智能汽车的地下停车场定位与路径规划 Location and path planning in underground parking lot for intelligent vehicles 1 2 first-author 李祎承（1988-），男，河北沧州人，副教授，博士生导师，2018年于武汉理工大学获得博士学位，主要从事智能汽车高精度定位技术。 E-mail： liyicheng070@ujs.edu.cn http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41464324&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41464331&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41464327&type= 22.01332855 32.00399780 李祎承 （1988-），男，河北沧州人，副教授，博士生导师，2018年于武汉理工大学获得博士学位，主要从事智能汽车高精度定位技术。 E-mail： liyicheng070@ujs.edu.cn liyicheng070@ujs.edu.cn 1 1 2 3 4 corresp E-mail： xushc@tsinghua.edu.cn E-mail： xushc@tsinghua.edu.cn 许述财（1978-），男，副研究员，博士，博士生导师，2007年于吉林大学学获得博士学位，主要从事智能汽车主被动安全技术。 E-mail：xushc@tsinghua.edu.cn http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465402&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465412&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465407&type= 22.01333618 32.00400543 许述财 （1978-），男，副研究员，博士，博士生导师，2007年于吉林大学学获得博士学位，主要从事智能汽车主被动安全技术。 E-mail： xushc@tsinghua.edu.cn xushc@tsinghua.edu.cn 江苏大学 汽车工程研究院，江苏 镇江 212013 江苏大学 汽车工程研究院，江苏 镇江 212013 Automotive Engineering Research Institute， Jiangsu University， Zhenjiang 212013， China Automotive Engineering Research Institute， Jiangsu University， Zhenjiang 212013， China 清华大学 苏州汽车研究院（相城），江苏 苏州 215133 清华大学 苏州汽车研究院（相城），江苏 苏州 215133 Suzhou Automotive Research Institute， Tsinghua University， Suzhou 215133， China Suzhou Automotive Research Institute， Tsinghua University， Suzhou 215133， China 安徽工程大学 机械工程学院，安徽 芜湖 241000 安徽工程大学 机械工程学院，安徽 芜湖 241000 School of Mechanical Engineering， Anhui Polytechnic University， Wuhu 241000， China School of Mechanical Engineering， Anhui Polytechnic University， Wuhu 241000， China 清华大学 汽车安全与节能国家重点实验室，北京 100084 清华大学 汽车安全与节能国家重点实验室，北京 100084 State Key Laboratory of Automotive Safety and Energy， Tsinghua University， Beijing 100084， China State Key Laboratory of Automotive Safety and Energy， Tsinghua University， Beijing 100084， China 为了解决智能车辆在全球定位系统（Global Positioning System， GPS）信号缺失的地下停车场中的定位与路径规划问题，本文使用双目离线建图、单目在线定位的视觉方法在场景中进行定位，并使用改进的路径规划算法规划全局行驶路线。首先，利用双目相机在地下停车场中的多个节点位置进行场景采集，节点的不同信息分层存储入视觉多层级地图。然后，利用单目相机进行由粗到精的地图匹配和位姿计算实现快速高精度定位。最后，使用改进的路径规划算法在地图中对各路口位置进行检测并生成路口路径图（Intersection Roadmap， IRM），全局可行驶路线根据定位结果和路径图规划得到。实验结果表明：使用单目相机的视觉定位平均误差为1.3 m，平均误差率为7.4%，基于路口路径图的规划算法相比概率路径图（Probabilistic Roadmap， PRM）算法，规划时间由5 s左右缩短至0.2 s以内，路径长度缩短了约9.56%。定位导航系统能够实时准确地实现车辆在停车场环境中的定位与路径规划，具备良好的实用性。 To solve the location and path planning challenges for intelligent vehicles in GPS-restricted underground parking lots， we employ a vision-based approach that combines binocular offline mapping and monocular online localization to trace the intelligent vehicles within the scene. The improved path planning algorithm is used to plan the global route. First， the system uses a binocular camera to capture scenes at multiple nodes in the underground parking lot， which are then stored in different layers of a hierarchical map. The monocular camera is subsequently utilized to perform coarse-to-fine map matching and calculate the pose of the vehicle， ensuring high-speed and accurate positioning. Finally， the improved path planning algorithm is achieved by detecting intersections and generating a roadmap. The experimental results demonstrate that the average monocular positioning error and average error rate are 1.3 m and 7.4%， respectively， the planning time is reduced from approximately 5 s to less than 0.2 s， and the path length is shortened by approximately 9.56%. This positioning and navigation system is practical for real-time positioning and path planning of intelligent vehicles in parking lot environments. 智能汽车 室内定位 停车场导航 路径规划 路口路径图 intelligent vehicle indoor location parking lot navigation path planning intersection roadmap（IRM） 1 引 言 1 近年来，计算机处理能力的提高以及人工智能的快速发展掀起了汽车行业自动驾驶研究的热潮，关于智能车辆的研究在学术及工业领域中都受到了广泛的关注。在典型框架实现中，智能车辆系统可被分为感知系统（Perception System）和决策系统（Decision Making System） ［ 1 1 ］ 。其中，定位是智能车辆感知系统的核心功能之一，车辆定位效果的好坏将直接影响后续的规划与控制。决策系统则负责在获取定位及其它感知信息之后规划出可行驶路线。 对于定位来说，在开阔的室外场景中，智能车辆借助全球导航卫星系统（Global Navigation Satellite System， GNSS）、惯性导航系统（Inertial Navigation System， INS）或者两者结合可以取得较高的定位精度 ［ 2 2 ］ ，但在城市建筑物密集区域，由于多径效应，全球定位系统（Global Positioning System， GPS）的定位精度将会下降，而在室内场景，特别是在地下停车场中，GPS信号的缺失会使得此类系统快速失效。 目前，多种室内定位系统（Indoor Positioning System， IPS）被开发出来以解决室内定位问题，不同的IPS可能会使用不同的传感器和不同的算法，目前常用的室内定位方法主要包括航位推算（Dead Reckoning， DR） ［ 3 3 - 6 6 3-6 ］ 、基于通讯技术的定位 ［ 7 7 - 9 9 7-9 ］ 和基于同时建图与定位（Simultaneous Location and Mapping， SLAM）的方法 ［ 10 10 - 13 13 10-13 ］ 。DR方法将自身加速度、角度等传感器数据进行积分得到自身位置，其积分原理使得这类技术会产生较大的累积误差。基于通讯技术的定位方法通过场景中的无线信号进行定位，但是这类方法通常需要在场景中预先架设设备，并且难以获取车辆的精确姿态，而精确位姿是智能车辆后续自主决策与控制所必需的，这使得这类基于无线的定位技术很难应用于智能车辆室内定位。SLAM方法是目前智能车辆在室内场景定位时应用较多的一种方法，SLAM方法利用如相机、激光雷达之类的传感器在不同时刻的数据通过拼接对周围环境进行建图并同时给出载体在地图中的定位，但是其过高的复杂度使得计算资源会被大幅消耗。将SLAM方法拆分成建图、定位两个步骤，用先建图后定位的方式在保证精度的前提下减少复杂度是一种可行的方式。 在决策层面，智能车辆根据感知信息进行路径规划等工作。路径规划算法根据理论的不同大体上可以分为智能算法、基于图搜索的算法、基于采样的算法等 ［ 14 14 ］ 。智能算法对自然现象进行仿生，以较低的成本对具有不确定性的复杂问题进行解决，遗传算法 ［ 15 15 ］ 、粒子群算法 ［ 16 16 ］ 、蚁群算法 ［ 17 17 ］ 等常用的智能优化方法都可以应用于路径规划，但这类方法需要大规模的训练且计算量较大，难以应用于大规模环境中的精确求解。图搜索算法利用可视图方法对场景进行建模并在可视图中进行图搜索，是目前应用最为广泛的算法，A *［ 18 18 ］ 、D *［ 19 19 ］ 等都是典型的图搜索算法，这类方法需要对环境预先进行建模，并可与其他方法结合使用取得更好的效果。基于采样的方法则可以在没有构建好可视图之前用随机采样的方式对环境进行探索，之后可以选择结合图搜索方法规划出可行驶路线，典型的采样方法包括概率路径图（Probabilistic Roadmap， PRM） ［ 20 20 ］ 、快速扩展随机树（Rapidly-exploring Random Tree，RRT） ［ 21 21 ］ 等。 通过上述分析可以得知，采用先建图、后定位的策略在室内场景中进行定位是一种较优的方法，同时，基于采样的路径规划方法较为适合道路构造简单的地下停车场环境。因此，本文针对上述分析及地下停车场的特点，对地下停车场的定位及路径规划算法进行了设计。采用双目离线建图、单目在线定位的策略实现低成本高精度室内定位，并通过选取路口作为路径图生成节点，提出了一种基于路口路径图（Intersection Roadmap， IRM）的路径规划算法，提升了其在资源占用率、定位规划效果及实时性方面的整体表现。 2 地下停车场定位方法 1 2.1　多层级地图构建 2 地下停车场多层级地图是实现智能汽车定位的基础，多层级地图的构建使得单次制图，多次定位得到实现。多层级地图建图时采用一台双目摄像机，将地下停车场分成一系列的数据采集节点，每个节点间距为2 m。为了能够在定位匹配时能够同时保证高效性与准确性，多层级地图将每个节点采集场景进行处理，得到不同层次的信息并分别存入3个层级：（1）BoW（Bag of Words） ［ 22 22 ］ 特征层，特征层对左右目图像分别提取视觉局部特征，并将左目特征与BoW模型结合，构成BoW特征层；（2）场景结构层，将左右目局部特征进行匹配，并通过三角法计算三维信息构成场景结构层；（3）节点坐标层，即节点在地下停车场坐标系下的真实坐标，坐标信息的真实值通过节点间场景结构位姿计算综合手工测量得出，同时借助如车道线一类的环境参照物可以标定出节点采集时镜头的朝向。在此基础上得到这些节点的节点坐标系与地图坐标系的旋转矩阵，并作为推算剩余节点坐标系与地图坐标系之间旋转矩阵的基准定位结果的相对位姿，在结合坐标信息后可转化为绝对位姿。文章构建的多层级视觉地图的数据采集如 图1 图1 所示。 2.2　多层级地图中的视觉定位 2 2 . 2 . 1　节点级定位 3 本文中车辆的节点级定位主要依靠词袋法将车载相机拍摄画面与视觉地图库中图像进行匹配，得出与车载相机拍摄画面最相似的地图库图像，词袋模型进行图像匹配主要分为三个步骤：特征提取及描述，视觉词典构造，节点匹配。 步骤1：特征提取及描述 对地图库中图像进行预处理，将各图像灰度化及直方图均衡化并缩放至同一尺度，利用ORB（Oriented Fast and Rotated Brief） ［ 23 23 ］ 算法提取预处理后各图像的特征点与描述符并生成特征描述子。 步骤2：视觉词典构造 采用K-means ［ 24 24 ］ 聚类方法将提取到的特征构造成单词表，对各地图库图像的词频数进行统计得出词频直方图并生成对应的特征频描述子。 步骤3：节点匹配 采用同样方法提取出待定位点图片的特征频描述子，并计算该描述子与视觉地图库中所有的特征频描述子的海明距离，距离最小的特征频描述子对应的节点即为与车辆最近的地图节点。 2 . 2 . 2　位姿级定位 3 粗定位将车辆定位在视觉地图中某节点附近，通过透视 <math id="M1"><mi>n</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465504&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465499&type= 1.86266661 2.79399991 点（Perspective n-points， PnP） ［ 25 25 ］ 求解可以实现车辆的位姿级定位。首先对检索得出节点是否为最近节点进行确认，其次通过标定矫正后双目相机的拍摄画面及定位节点对车辆的位姿进行确定。 位姿估计的主要求解目标是计算出车辆所在位置相对于视觉地图中对应节点位置的位移向量 <math id="M2"><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465593&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465557&type= 1.26999998 2.79399991 和旋转矩阵 <math id="M3"><mi mathvariant="bold-italic">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465552&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465547&type= 2.87866688 2.79399991 。当有 n 个特征点时，将第 i 个特征点记作 <math id="M4"><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>,</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465477&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465472&type= 5.24933338 4.14866638 将其世界坐标记作 <math id="M5"><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>X</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd><mtd><msub><mrow><mi>Y</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd><mtd><msub><mrow><mi>Z</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465487&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465482&type= 22.52133369 4.74133301 ，将其投影的像素坐标记作 <math id="M6"><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>u</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465494&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465491&type= 13.97000027 4.74133301 ，对于透视 <math id="M7"><mi>n</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465504&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465499&type= 1.86266661 2.79399991 点问题，在齐次坐标下有： <math display="block" id="M8"><mtable><mtr><mtd><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>u</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><mo>…</mo></mtd><mtd><msub><mrow><mi>u</mi></mrow><mrow><mi>n</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>v</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>v</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><mo>…</mo></mtd><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>n</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">1</mn></mtd><mtd><mn mathvariant="normal">1</mn></mtd><mtd><mo>…</mo></mtd><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced><mo>≅</mo><mi mathvariant="bold-italic">K</mi><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mrow><mtable><mtr><mtd><mi mathvariant="bold-italic">R</mi></mtd><mtd><mi mathvariant="bold-italic">t</mi></mtd></mtr></mtable></mrow></mtd></mtr></mtable></mrow></mfenced><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>X</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>X</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><mo>…</mo></mtd><mtd><msub><mrow><mi>X</mi></mrow><mrow><mi>n</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>Y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>Y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><mo>…</mo></mtd><mtd><msub><mrow><mi>Y</mi></mrow><mrow><mi>n</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>Z</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></mtd><mtd><msub><mrow><mi>Z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mtd><mtd><mo>…</mo></mtd><mtd><msub><mrow><mi>Z</mi></mrow><mrow><mi>n</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">1</mn></mtd><mtd><mn mathvariant="normal">1</mn></mtd><mtd><mo>…</mo></mtd><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced></mrow></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465514&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465509&type= 76.62332916 18.54199982 ， 其中： <math id="M9"><mi mathvariant="bold-italic">K</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465524&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465519&type= 3.13266683 2.79399991 代表车载相机的内参数矩阵， <math id="M10"><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465593&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465557&type= 1.26999998 2.79399991 为平移向量， <math id="M11"><mi mathvariant="bold-italic">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465552&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465547&type= 2.87866688 2.79399991 为车载相机坐标系与模型中的摄像机坐标系间的旋转矩阵。 当有6个及以上的特征点时，可以根据上式求出 <math id="M12"><mi mathvariant="bold-italic">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465552&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465547&type= 2.87866688 2.79399991 和 <math id="M13"><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465593&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465557&type= 1.26999998 2.79399991 ，以此可以求出当前位置与节点的距离： <math display="block" id="M14"><mi>P</mi><mo>=</mo><mo>-</mo><msup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><mo>⋅</mo><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465603&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465598&type= 18.71133423 3.04800010 . 3 路口中心检测与路径规划算法 1 PRM是一种经典的采样路径规划算法，其在场景中进行随机采样后，将各采样点在其邻域范围内进行碰撞检测并连线构建路径图，规划时将起点、终点添加至路线图中并进行寻路，这种单次构图、多次规划的规划策略简单而高效，但随机采样使得规划路径并不一定能达到最优且有可能规划失败，增加采样点可以在一定程度上提高规划效果，但规划时间会大幅增加。针对此问题，本文提出了一种路口路径图（IRM）规划方法，首先提取出道路的边线，之后在提取到的边线基础上计算出路口中心，利用路口中心作为采样节点来构建路径图，克服了PRM算法的缺点。 3.1　道路边线检测 2 3 . 1 . 1　道路轮廓提取 3 Canny边缘检测是目前应用非常广泛的一种边缘检测算法，该算法使用基于梯度幅值的双阈值方法对图像轮廓、边缘进行检测，本文使用Canny边缘检测算法提取道路轮廓。 Canny算法对输入图像依次进行去噪、图像梯度计算和非极大值抑制，最终使用双阈值方法确定出边缘。在梯度计算时，Canny边缘检测利用一阶微分算子计算图像中各点处的梯度幅值和梯度方向，以图像某点 <math id="M15"><mo stretchy="false">(</mo><mi>i</mi><mo>,</mo><mtext> </mtext><mi>j</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465606&type= 7.95866632 3.97933316 为例，其两方向偏导分别如 式（3） 式（3） 、 式（4） 式（4） 所示： <math display="block" id="M16"><msub><mrow><mi>G</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>=</mo><mfrac><mrow><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>-</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>+</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo>,</mo><mi>j</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>-</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo>,</mo><mi>j</mi></mrow></mfenced></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465621&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465616&type= 95.25000000 8.97466660 ， <math display="block" id="M17"><msub><mrow><mi>G</mi></mrow><mrow><mi>y</mi></mrow></msub><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>=</mo><mfrac><mrow><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>-</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo>,</mo><mi>j</mi></mrow></mfenced><mo>+</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>-</mo><mi>I</mi><mfenced separators="|"><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo>,</mo><mi>j</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465631&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465626&type= 95.08066559 8.97466660 ， 其中， <math id="M18"><mi>I</mi><mo stretchy="false">(</mo><mo>⋅</mo><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465639&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465634&type= 4.82600021 3.55599999 代表某点处灰度值，此时点 <math id="M19"><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465650&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465644&type= 7.11199999 4.74133301 处的梯度幅值 <math id="M20"><mi>G</mi><mo stretchy="false">(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465660&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465654&type= 10.49866676 3.97933316 和梯度方向 <math id="M21"><mi>θ</mi><mo stretchy="false">(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465668&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465664&type= 9.48266602 3.97933316 分别为： <math display="block" id="M22"><mi>G</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>=</mo><mroot><mrow><msubsup><mrow><mi>G</mi></mrow><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>+</mo><msubsup><mrow><mi>G</mi></mrow><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced></mrow><mrow/></mroot></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465680&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465673&type= 47.07466888 6.09600019 ， <math display="block" id="M23"><mi>θ</mi><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced><mo>=</mo><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mfenced separators="|"><mrow><mfrac><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>y</mi></mrow></msub><mfenced separators="|"><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></mfenced></mrow></mfrac></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465687&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465684&type= 39.70866776 12.10733318 . 将Canny算法应用于地图场景得到的道路轮廓如 图2 图2 所示。 3 . 1 . 2　道路边线检测 3 在提取的道路轮廓图中，通过Hough直线变换 ［ 26 26 ］ 可以得出道路的边线。Hough变换将笛卡尔空间或极坐标空间映射至霍夫空间，原空间中的点会映射为霍夫空间中的一条线，而原空间中的线会映射为霍夫空间中的一个点。根据变换性质，在霍夫坐标系下，多条直线的交点在原空间中很可能是条直线。因此，当霍夫坐标系内交于某点的曲线数达到某设定阈值时，就认为原空间中存在相应的一条直线，在道路轮廓图中使用Hough直线变换即可得出道路边线的检测结果。 由于Hough变换的检测原理，边线检测结果中，单条直线附近容易出现距离很近的冗余直线，因此需要在边线检测基础上进行优化，通过形态学闭运算，对检测图像先膨胀后腐蚀，从而将距离很近的线条整合为单条直线，通过上述步骤即可得出道路边线检测的拟合结果，如 图3 图3 所示。 3.2　路径图生成与路径规划 2 3 . 2 . 1　道路边线交点检测 3 Shi-Tomasi角点检测 ［ 27 27 ］ 方法在经典的Harris角点检测方法基础上对角点响应函数进行了改进，Shi-Tomasi方法以一个小窗口在图像中进行滑动，并计算窗口内部的像素值变化量 <math id="M24"><mi>E</mi><mo stretchy="false">(</mo><mi>u</mi><mo>,</mo><mtext> </mtext><mi>v</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465729&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465724&type= 12.69999981 3.55599999 ： <math display="block" id="M25"><mi>E</mi><mfenced separators="|"><mrow><mi>u</mi><mo>,</mo><mi>v</mi></mrow></mfenced><mo>≈</mo><mfenced open="[" close="]" separators="|"><mrow><mi>u</mi><mo>,</mo><mi>v</mi></mrow></mfenced><mi>M</mi><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>u</mi></mtd></mtr><mtr><mtd><mi>v</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465739&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465734&type= 35.64466476 8.55133343 ， <math display="block" id="M26"><mtable><mtr><mtd><mrow><mi>M</mi><mo>=</mo><munder><mo movablelimits="false">∑</mo><mrow><mfenced separators="|"><mrow><mi>x</mi><mo>,</mo><mi>y</mi></mrow></mfenced></mrow></munder><mrow><mi>ω</mi><mfenced separators="|"><mrow><mi>x</mi><mo>,</mo><mi>y</mi></mrow></mfenced><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi>I</mi></mrow><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><msub><mrow><mi>I</mi></mrow><mrow><mi>x</mi></mrow></msub><msub><mrow><mi>I</mi></mrow><mrow><mi>y</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>I</mi></mrow><mrow><mi>x</mi></mrow></msub><msub><mrow><mi>I</mi></mrow><mrow><mi>y</mi></mrow></msub></mtd><mtd><msubsup><mrow><mi>I</mi></mrow><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></mrow></mrow></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465749&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465745&type= 43.60333252 9.82133293 ， 其中： <math id="M27"><mo stretchy="false">[</mo><mi>u</mi><mo>,</mo><mi>v</mi><mo stretchy="false">]</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465759&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465755&type= 9.22866726 3.55599999 为窗口中心像素坐标； <math id="M28"><mi>ω</mi><mo stretchy="false">(</mo><mi>x</mi><mo>,</mo><mi>y</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465767&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465761&type= 11.93799973 3.97933316 为设定的窗口响应函数； <math id="M29"><msub><mrow><mi>I</mi></mrow><mrow><mi>x</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465791&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465771&type= 2.53999996 3.72533321 ， <math id="M30"><msub><mrow><mi>I</mi></mrow><mrow><mi>y</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465801&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465796&type= 2.45533323 3.72533321 为相应的灰度偏导。 像素梯度矩阵的特征值可以计算出来并用于角点响应函数计算，记相应的特征值为 <math id="M31"><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465813&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465807&type= 2.96333337 3.72533321 、 <math id="M32"><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465824&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465818&type= 2.96333337 3.72533321 ，根据特征值可对窗口区域进行非极大值抑制，Shi-Tomasi角点检测方法中设定的角点响应函数为： <math display="block" id="M33"><mi>R</mi><mo>=</mo><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mfenced separators="|"><mrow><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465833&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465828&type= 25.06133461 3.97933316 . <math id="M34"><mi>R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465854&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465849&type= 2.79399991 2.79399991 值可用于非极大值抑制及角点检测， <math id="M35"><mi>R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465854&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465849&type= 2.79399991 2.79399991 值大于设定阈值时，即认为检测出角点， 图4 图4 中展示了边线交点检测的结果。 3 . 2 . 2　路口中心检测 3 3.2.1节中检测出了各道路边线交点，但各交点所归属的路口位置仍是未知的，因此无法根据交点直接计算出路口中心坐标，针对此问题，采用K-means聚类算法来在多个交点中得出相应的路口中心，K-means均值聚类随机选取 <math id="M36"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465877&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465874&type= 1.69333339 2.79399991 个点作为分类中心点，并将每个数据点放入距离最近的中心点所在的类中，计算各个分类数据点中心点的平均值作为新的中心点并循环迭代至不发生变化， 图5 图5 中展示了最终的路口检测结果。 <math display="block" id="M37"><msup><mrow><mi mathvariant="bold-italic">X</mi></mrow><mrow><mi mathvariant="bold-italic">*</mi></mrow></msup><mo>=</mo><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">g</mi><mtext> </mtext><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>j</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>k</mi></mrow></munderover></mstyle><mrow><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>n</mi></mrow></munderover></mstyle><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">X</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>j</mi></mrow></msubsup><mo>-</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></mfenced></mrow></mrow><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465886&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465881&type= 48.85266495 8.97466660 ， <math display="block" id="M38"><mi>k</mi><mo>=</mo><mfrac><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">u</mi><mi mathvariant="normal">m</mi><mo stretchy="false">(</mo><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi><mi>p</mi></mrow></msub><mo stretchy="false">)</mo></mrow><mrow><mn mathvariant="normal">4</mn></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465894&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465890&type= 21.75933456 8.21266651 ， 其中， <math id="M39"><msub><mrow><mi>P</mi></mrow><mrow><mi>i</mi><mi>p</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465903&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41465900&type= 4.23333359 3.72533321 表示道路边线交点，num（·）表示点数量。 路口节点坐标在初次计算完成后，可供PRM算法解算路径多次使用，无需重复计算。 3 . 2 . 3　基于IRM的路径规划 3 在得到了各路口坐标之后，使用与PRM算法类似的路径图生成策略可以构建出基于路口节点的路径图。 路径图构造时首先对检测到的路口节点进行检测，当节点位于障碍物内部时，将其作为误检测进行剔除。当节点检测完成后，将剩余的节点进行连线，所有节点均连线完成后即成功完成了IRM的构造，连线时需要保证各节点的连线不能与障碍物重叠，为了节省计算量，缩短路径检索时间，各节点只连接自己邻域范围内的点，太远的点不进行连接， 图6 图6 中展示了在地下停车场地图中生成的IRM。 当车辆在地下停车场环境中规划全局可行驶路线时，在给定目标位置作为终点后，利用路径图构造方法将其添加至路径图中，同时使用本文提出的由粗到精视觉定位方法对车辆进行定位，并将定位结果作为规划起点，起点随着车辆行驶进行实时更新并以同样的方式添加至路径图中。起点、终点均添加至路径图后，可利用A * 、Dijkstra等图搜索算法在路径图中找到最短路径。本文在实验环节对于不同算法规划出的路径进行了对比，并将IRM与PRM算法的规划时间和规划路径长度进行了测试。 4 实验与结果 1 为验证算法可靠性，选取江苏大学汽车工程研究院一楼车库和镇江市吾悦广场购物中心地下停车场作为实验场景，选用ZED2双目相机作为实验数据采集设备， 图7 图7 中展示了使用的数据采集设备， 图8 图8 中展示了实验场景。 在完成多层级地图构建的停车场区域内进行多尺度定位的验证。为方便图像数据的处理，使用地图构建时的测试车辆和ZED2相机记录车辆行驶时的相机录像。设定ZED2录制视频为15帧每秒，并手动测量每一段行驶轨迹的长度作为真值，共采集30条轨迹，长度在15 m到30 m。 将录制的所有视频按帧截取图片，利用多尺度定位方法得到每一帧图像在停车场地图中的定位信息，连接所有待定位点，得到计算轨迹。 多尺度定位中首先验证节点级定位的准确率。截取所有轨迹视频总共得到2 657帧待定位图像。其中最近节点的匹配率为63%，最近两个节点匹配率为82%，最近三个节点匹配率为98%。尽管有很多点没有匹配到距离最近的节点，PnP算法在5 m内具有较高的计算精度，最近三个节点匹配率为98%，这意味着这些点距离匹配到的节点距离不超过3 m。在精确定位的过程中依然能够顺利计算相对节点的位姿信息。 通过算法的自检过程，完成定位的待定位点为1 903个，定位成功率为71.6%。比较所有计算得到轨迹与对应真值，其中最大误差2.3 m。平均误差为1.3 m，平均误差率为7.4%。 同时，为了比较规划算法的优劣，对本文使用的IRM算法和典型的A * 算法、PRM算法、蚁群算法、RRT算法进行了对比。 图9 图9 中展示了不同算法规划出的路径，可以看出IRM算法规划的全局路径最为合理，同时文章将IRM算法和PRM算法在不同参数下的规划时间和规划路径长度进行了对比， 表1 表1 中展示了不同参数下的规划算法数据平均结果。 算法 采样点数量 邻域距离/Dp（pixel） 规划时间/s 路径长度/Dp（pixel） 4 PRM 200 500 5.94 1 025.8 200 1 000 9.37 959.6 500 500 36.13 953 500 1 000 56.15 948.8 IRM 18 500 0.097 927 从 图9 图9 的规划结果对比中可看出，在均未经过进一步的路径平滑或节点剪枝等处理之前，IRM算法规划出的路径在各个规划结果中最为合理且规划路径最短，实验结果表明，在规划路径的合理性及可行性上，IRM算法可以取得较优的效果。 另外，由于PRM算法与IRM算法在路径图生成后的路径搜索部分原理相同，因此对IRM算法和PRM算法进行了对比分析。从 表1 表1 中可以看出，在增加了采样点数量或者邻域距离时，PRM算法的规划出的路径长度会有所下降，但受限于其随机采样原理，其路径长度无法达到最优，同时PRM算法在路径图生成时会占用大量的计算资源，从 表1 表1 中可以看出采样点数量及邻域距离增加后，尽管规划效果会有所提升，但规划时间会快速增长；当采样点数量由200增长至500，邻域距离由500像素增长至1 000像素时，规划时间会由5 s左右增长至接近1 min，这样的规划代价是难以接受的；并且PRM的随机性会使得在不同的节点采样生成不同的规划路径，与原始的PRM算法相比，IRM算法的规划时间由5 s左右缩短至0.2 s以内，路径长度缩短了约9.56%。可以看出IRM算法能够稳定的输出在极短时间内实时生成的最短路径，IRM算法的实时性和规划可行性均得到了保证。 5 结 论 1 针对车辆在地下停车场中的定位导航问题，本文围绕路径规划问题展开研究，针对地下停车场环境的特点，利用计算机视觉技术根据视觉地图对车辆进行定位。导航采用的路径规划方法中使用路口中心坐标取代传统PRM算法的随机采样，使用单目相机的视觉定位平均误差为1.3 m，平均误差率为7.4%。基于路口节点的规划算法相比原始的PRM算法，规划时间由5 s左右缩短至0.2 s以内，路径长度缩短了约9.56%。定位导航系统能够实时准确地实现车辆在停车场环境中的导航，具备良好的实用性，IRM算法在规划时间大幅缩短的情况下得到了最优的路径，实现了在地下停车场环境中对司机寻路的引导。 参考文献 BADUE C ， GUIDOLINI R ， CARNEIRO R V ， et al . Self-driving cars： a survey ［J］. Expert Systems With Applications ， 2021 ， 165 ： 113816 . doi: 10.1016/j.eswa.2020.113816 http://dx.doi.org/10.1016/j.eswa.2020.113816 Self-driving cars： a survey 刘鹏飞 . 里程计辅助的高精度车载GNSS/INS组合导航系统 ［J］. 光学 精密工程 ， 2020 ， 28 （ 4 ）： 979 - 987 . 里程计辅助的高精度车载GNSS/INS组合导航系统 LIU P F . High-precision vehicle GNSS/INS integrated navigation system aided by odometer ［J］. Opt. Precision Eng. ， 2020 ， 28 （ 4 ）： 979 - 987 . （in Chinese） High-precision vehicle GNSS/INS integrated navigation system aided by odometer HOU X Y ， BERGMANN J . Pedestrian dead reckoning with wearable sensors： a systematic review ［J］. IEEE Sensors Journal ， 2021 ， 21 （ 1 ）： 143 - 152 . doi: 10.1109/jsen.2020.3014955 http://dx.doi.org/10.1109/jsen.2020.3014955 Pedestrian dead reckoning with wearable sensors： a systematic review GU F Q ， KHOSHELHAM K ， YU C Y ， et al . Accurate step length estimation for pedestrian dead reckoning localization using stacked autoencoders ［J］. IEEE Transactions on Instrumentation and Measurement ， 2019 ， 68 （ 8 ）： 2705 - 2713 . doi: 10.1109/tim.2018.2871808 http://dx.doi.org/10.1109/tim.2018.2871808 Accurate step length estimation for pedestrian dead reckoning localization using stacked autoencoders KUNHOTH J ， KARKAR A ， AL-MAADEED S ， et al . Indoor positioning and wayfinding systems： a survey ［J］. Human-Centric Computing and Information Sciences ， 2020 ， 10 ： 18 . doi: 10.1186/s13673-020-00222-0 http://dx.doi.org/10.1186/s13673-020-00222-0 Indoor positioning and wayfinding systems： a survey 周楚霖 ， 陈敬东 ， 黄凡 . 基于无迹粒子滤波的WiFi-PDR融合室内定位技术 ［J］. 计算机科学 ， 2022 ， 49 （ S1 ）： 606 - 611 . doi: 10.11896/jsjkx.210700108 http://dx.doi.org/10.11896/jsjkx.210700108 基于无迹粒子滤波的WiFi-PDR融合室内定位技术 ZHOU C L ， CHEN J D ， HUANG F . WiFi-PDR fusion indoor positioning technology based on unscented particle filter ［J］. Computer Science ， 2022 ， 49 （ S1 ）： 606 - 611 . （in Chinese） . doi: 10.11896/jsjkx.210700108 http://dx.doi.org/10.11896/jsjkx.210700108 WiFi-PDR fusion indoor positioning technology based on unscented particle filter 孙森震 ， 李广云 ， 冯其强 ， 等 . 可见光通信与双目视觉的室内定位 ［J］. 光学 精密工程 ， 2020 ， 28 （ 4 ）： 834 - 843 . 可见光通信与双目视觉的室内定位 SUN S Z ， LI G Y ， FENG Q Q ， et al . Indoor positioning based on visible light communication and binocular vision ［J］. Opt. Precision Eng. ， 2020 ， 28 （ 4 ）： 834 - 843 . （in Chinese） Indoor positioning based on visible light communication and binocular vision LIU F ， LIU J ， YIN Y Q ， et al . Survey on WiFi-based indoor positioning techniques ［J］. IET Communications ， 2020 ， 14 （ 9 ）： 1372 - 1383 . doi: 10.1049/iet-com.2019.1059 http://dx.doi.org/10.1049/iet-com.2019.1059 Survey on WiFi-based indoor positioning techniques YANG D G ， ZHU Z L ， ZHANG J C ， et al . The overview of human localization and vital sign signal measurement using handheld IR-UWB through-wall radar ［J］. Sensors （Basel， Switzerland） ， 2021 ， 21 （ 2 ）： 402 . doi: 10.3390/s21020402 http://dx.doi.org/10.3390/s21020402 The overview of human localization and vital sign signal measurement using handheld IR-UWB through-wall radar 杜航原 ， 郝燕玲 ， 赵玉新 ， 等 . 用概率假设密度滤波实现同步定位与地图创建 ［J］. 光学 精密工程 ， 2011 ， 19 （ 12 ）： 3064 - 3073 . doi: 10.3788/ope.20111912.3064 http://dx.doi.org/10.3788/ope.20111912.3064 用概率假设密度滤波实现同步定位与地图创建 DU H Y ， HAO Y L ， ZHAO Y X ， et al . Implementation of SLAM by probability hypothesis density filter ［J］. Opt. Precision Eng. ， 2011 ， 19 （ 12 ）： 3064 - 3073 . （in Chinese） . doi: 10.3788/ope.20111912.3064 http://dx.doi.org/10.3788/ope.20111912.3064 Implementation of SLAM by probability hypothesis density filter 尹芳 ， 吴云 . 空间非合作旋转目标的模型重建与位姿优化 ［J］. 光学 精密工程 ， 2019 ， 27 （ 8 ）： 1854 - 1862 . doi: 10.3788/ope.20192708.1854 http://dx.doi.org/10.3788/ope.20192708.1854 空间非合作旋转目标的模型重建与位姿优化 YIN F ， WU Y . Model reconstruction and pose optimization of non-cooperative rotating space target ［J］. Opt. Precision Eng. ， 2019 ， 27 （ 8 ）： 1854 - 1862 . （in Chinese） . doi: 10.3788/ope.20192708.1854 http://dx.doi.org/10.3788/ope.20192708.1854 Model reconstruction and pose optimization of non-cooperative rotating space target TAHERI H ， XIA Z C . SLAM； definition and evolution ［J］. Engineering Applications of Artificial Intelligence ， 2021 ， 97 ： 104032 . doi: 10.1016/j.engappai.2020.104032 http://dx.doi.org/10.1016/j.engappai.2020.104032 SLAM； definition and evolution KOLHATKAR C ， WAGLE K . Review of SLAM algorithms for indoor mobile robot with LIDAR and RGB-D camera technology ［C］. Innovations in Electrical and Electronic Engineering ， 2021 ： 397 - 409 . doi: 10.1007/978-981-15-4692-1_30 http://dx.doi.org/10.1007/978-981-15-4692-1_30 Review of SLAM algorithms for indoor mobile robot with LIDAR and RGB-D camera technology 采国顺 ， 刘昊吉 ， 冯吉伟 ， 等 . 智能汽车的运动规划与控制研究综述 ［J］. 汽车安全与节能学报 ， 2021 ， 12 （ 3 ）： 279 - 297 . doi: 10.3969/j.issn.1674-8484.2021.03.002 http://dx.doi.org/10.3969/j.issn.1674-8484.2021.03.002 智能汽车的运动规划与控制研究综述 CAI G S ， LIU H J ， FENG J W ， et al . Review on the research of motion planning and control for intelligent vehicles ［J］. Journal of Automotive Safety and Energy ， 2021 ， 12 （ 3 ）： 279 - 297 . （in Chinese） . doi: 10.3969/j.issn.1674-8484.2021.03.002 http://dx.doi.org/10.3969/j.issn.1674-8484.2021.03.002 Review on the research of motion planning and control for intelligent vehicles 李艳生 ， 万勇 ， 张毅 ， 等 . 基于人工蜂群-自适应遗传算法的仓储机器人路径规划 ［J］. 仪器仪表学报 ， 2022 ， 43 （ 4 ）： 282 - 290 . 基于人工蜂群-自适应遗传算法的仓储机器人路径规划 LI Y S ， WAN Y ， ZHANG Y ， et al . Path planning for warehouse robot based on the artificial bee colony-adaptive genetic algorithm ［J］. Chinese Journal of Scientific Instrument ， 2022 ， 43 （ 4 ）： 282 - 290 . （in Chinese） Path planning for warehouse robot based on the artificial bee colony-adaptive genetic algorithm 陈秋莲 ， 郑以君 ， 蒋环宇 ， 等 . 基于神经网络改进粒子群算法的动态路径规划 ［J］. 华中科技大学学报（自然科学版） ， 2021 ， 49 （ 2 ）： 51 - 55 . 基于神经网络改进粒子群算法的动态路径规划 CHEN Q L ， ZHENG Y J ， JIANG H Y ， et al . Improved particle swarm optimization algorithm based on neural network for dynamic path planning ［J］. Journal of Huazhong University of Science and Technology （Natural Science Edition） ， 2021 ， 49 （ 2 ）： 51 - 55 . （in Chinese） Improved particle swarm optimization algorithm based on neural network for dynamic path planning 肖金壮 ， 余雪乐 ， 周刚 ， 等 . 一种面向室内AGV路径规划的改进蚁群算法 ［J］. 仪器仪表学报 ， 2022 ， 43 （ 3 ）： 277 - 285 . 一种面向室内AGV路径规划的改进蚁群算法 XIAO J Z ， YU X L ， ZHOU G ， et al . An improved ant colony algorithm for indoor AGV path planning ［J］. Chinese Journal of Scientific Instrument ， 2022 ， 43 （ 3 ）： 277 - 285 . （in Chinese） An improved ant colony algorithm for indoor AGV path planning 张韬 ， 项祺 ， 郑婉文 ， 等 . 基于改进A * 算法的路径规划在海战兵棋推演中的应用 ［J］. 兵工学报 ， 2022 ， 43 （ 4 ）： 960 - 968 . doi: 10.12382/bgxb.2021.0209 http://dx.doi.org/10.12382/bgxb.2021.0209 基于改进A*算法的路径规划在海战兵棋推演中的应用 ZHANG T ， XIANG Q ， ZHENG W W ， et al . Application of path planning based on improved A * algorithm in war gaming of naval warfare ［J］. Acta Armamentarii ， 2022 ， 43 （ 4 ）： 960 - 968 . （in Chinese） . doi: 10.12382/bgxb.2021.0209 http://dx.doi.org/10.12382/bgxb.2021.0209 Application of path planning based on improved A* algorithm in war gaming of naval warfare 秦旭 ， 黄晓华 ， 马东明 ， 等 . 基于改进D * 算法的巡检机器人路径规划 ［J］. 组合机床与自动化加工技术 ， 2022 （ 6 ）： 10 - 13 . 基于改进D*算法的巡检机器人路径规划 QIN X ， HUANG X H ， MA D M ， et al . Path planning of inspection robot based on improved D * algorithm ［J］. Modular Machine Tool ＆ Automatic Manufacturing Technique ， 2022 （ 6 ）： 10 - 13 . （in Chinese） Path planning of inspection robot based on improved D* algorithm 吕进锋 ， 马建伟 ， 李晓静 . 基于改进的随机路径图及和声算法的舰船航线规划 ［J］. 控制理论与应用 ， 2020 ， 37 （ 12 ）： 2551 - 2559 . doi: 10.7641/CTA.2020.90979 http://dx.doi.org/10.7641/CTA.2020.90979 基于改进的随机路径图及和声算法的舰船航线规划 LÜ J F ， MA J W ， LI X J . Route planning based on improved probabilistic roadmap and harmony search ［J］. Control Theory ＆ Applications ， 2020 ， 37 （ 12 ）： 2551 - 2559 . （in Chinese） . doi: 10.7641/CTA.2020.90979 http://dx.doi.org/10.7641/CTA.2020.90979 Route planning based on improved probabilistic roadmap and harmony search 王海芳 ， 张瑶 ， 朱亚锟 ， 等 . 基于改进双向RRT * 的移动机器人路径规划算法 ［J］. 东北大学学报（自然科学版） ， 2021 ， 42 （ 8 ）： 1065 - 1070， 1142 . 基于改进双向RRT*的移动机器人路径规划算法 WANG H F ， ZHANG Y ， ZHU Y K ， et al . Mobile robot path planning based on improved bidirectional RRT * ［J］. Journal of Northeastern University （Natural Science） ， 2021 ， 42 （ 8 ）： 1065 - 1070， 1142 . （in Chinese） Mobile robot path planning based on improved bidirectional RRT* 李新德 ， 刘苗苗 ， 徐叶帆 ， 等 . 一种基于2D和3D SIFT特征级融合的一般物体识别算法 ［J］. 电子学报 ， 2015 ， 43 （ 11 ）： 2277 - 2283 . 一种基于2D和3D SIFT特征级融合的一般物体识别算法 LI X D ， LIU M M ， XU Y F ， et al . A recognition algorithm of generic objects based on FeatureLevel fusion of 2D and 3D SIFT descriptors ［J］. Acta Electronica Sinica ， 2015 ， 43 （ 11 ）： 2277 - 2283 . （in Chinese） A recognition algorithm of generic objects based on FeatureLevel fusion of 2D and 3D SIFT descriptors 齐咏生 ， 陈培亮 ， 刘利强 ， 等 . 基于单目视觉的动态环境同步定位与多地图构建算法 ［J］. 农业机械学报 ， 2022 ， 53 （ 4 ）： 280 - 292 . doi: 10.6041/j.issn.1000-1298.2022.04.030 http://dx.doi.org/10.6041/j.issn.1000-1298.2022.04.030 基于单目视觉的动态环境同步定位与多地图构建算法 QI Y S ， CHEN P L ， LIU L Q ， et al . Simultaneous localization and multi-mapping algorithm in dynamic environment based on monocular vision ［J］. Transactions of the Chinese Society for Agricultural Machinery ， 2022 ， 53 （ 4 ）： 280 - 292 . （in Chinese） . doi: 10.6041/j.issn.1000-1298.2022.04.030 http://dx.doi.org/10.6041/j.issn.1000-1298.2022.04.030 Simultaneous localization and multi-mapping algorithm in dynamic environment based on monocular vision 李健 ， 曹垚 ， 王宗敏 ， 等 . 融合k-means聚类和Hausdorff距离的散乱点云精简算法 ［J］. 武汉大学学报·信息科学版 ， 2020 ， 45 （ 2 ）： 250 - 257 . 融合k-means聚类和Hausdorff距离的散乱点云精简算法 LI J ， CAO Y ， WANG Z M ， et al . Scattered point cloud simplification algorithm integrating k-means clustering and Hausdorff distance ［J］. Geomatics and Information Science of Wuhan University ， 2020 ， 45 （ 2 ）： 250 - 257 . （in Chinese） Scattered point cloud simplification algorithm integrating k-means clustering and Hausdorff distance 王中宇 ， 李亚茹 ， 郝仁杰 ， 等 . 基于点特征的单目视觉位姿测量算法 ［J］. 红外与激光工程 ， 2019 ， 48 （ 5 ）： 169 - 176 . doi: 10.3788/irla201948.0517002 http://dx.doi.org/10.3788/irla201948.0517002 基于点特征的单目视觉位姿测量算法 WANG Z Y ， LI Y R ， HAO R J ， et al . Monocular vision pose measurement algorithm based on points feature ［J］. Infrared and Laser Engineering ， 2019 ， 48 （ 5 ）： 169 - 176 . （in Chinese） . doi: 10.3788/irla201948.0517002 http://dx.doi.org/10.3788/irla201948.0517002 Monocular vision pose measurement algorithm based on points feature 李霞 ， 苏筠皓 ， 岳振超 ， 等 . 基于中值点Hough变换玉米行检测的导航线提取方法 ［J］. 农业工程学报 ， 2022 ， 38 （ 5 ）： 167 - 174 . doi: 10.11975/j.issn.1002-6819.2022.05.020 http://dx.doi.org/10.11975/j.issn.1002-6819.2022.05.020 基于中值点Hough变换玉米行检测的导航线提取方法 LI X ， SU J H ， YUE Z C ， et al . Extracting navigation line to detect the maize seedling line using Median-point Hough transform ［J］. Transactions of the Chinese Society of Agricultural Engineering ， 2022 ， 38 （ 5 ）： 167 - 174 . （in Chinese） . doi: 10.11975/j.issn.1002-6819.2022.05.020 http://dx.doi.org/10.11975/j.issn.1002-6819.2022.05.020 Extracting navigation line to detect the maize seedling line using Median-point Hough transform 骆开庆 ， 邓军灿 ， 蔡伟博 ， 等 . 基于Shi-Tomasi角点验证的线段提取算法优化方法 ［J］. 华南师范大学学报（自然科学版） ， 2022 ， 54 （ 1 ）： 113 - 121 . 基于Shi-Tomasi角点验证的线段提取算法优化方法 LUO K Q ， DENG J C ， CAI W B ， et al . Line segment extraction algorithm optimization based on Shi-tomasi corner detector ［J］. Journal of South China Normal University （Natural Science Edition） ， 2022 ， 54 （ 1 ）： 113 - 121 . （in Chinese） Line segment extraction algorithm optimization based on Shi-tomasi corner detector 10.37188/OPE.20233105.0757 TP391.4 A 1004-924X（2023）05-0757-10 李祎承,杨东晓,高翔等.面向智能汽车的地下停车场定位与路径规划[J].光学精密工程,2023,31(05):757-766. 李祎承,杨东晓,高翔等.面向智能汽车的地下停车场定位与路径规划[J].光学精密工程,2023,31(05):757-766. LI Yicheng,YANG Dongxiao,GAO Xiang,et al.Location and path planning in underground parking lot for intelligent vehicles[J].Optics and Precision Engineering,2023,31(05):757-766. LI Yicheng,YANG Dongxiao,GAO Xiang,et al.Location and path planning in underground parking lot for intelligent vehicles[J].Optics and Precision Engineering,2023,31(05):757-766. 国家重点研发计划项目（2018YFE0204302）；国家自然科学基金（52272418， U20A20333， 52002154）；江苏省自然科学基金（BK20190853）；江苏省重点研发计划（BE2020083-2， BE2021011-2） 2022-09-06 2022-09-28 2023-03-10 2023-03-22 光学精密工程 5 31