自动泊车系统中核心算法的研究

第１７卷第３期　２０１２年６月　文章编号：１００７—０２４９（２Ｏｌ２）０３－００７１－０６　电路与系统学报　Ｊ０ＵＲＮＡＬ　ＯＦ　ＣＩＲＣＵＩＴＳ　ＡＮＤ　ＳＹＳＴＥＭＳ　ＶＯ１．１７　Ｎｏ．３　Ｊｕｎｅ，２０１２　自动泊车系统中核心算法的研究　周泓，　王文飞荇　（浙江大学仪器科学与工程学系，浙江杭州３１００２７）　摘要。对实现自动泊车系统涉及到的三个核心算法做了探索与研究。提出参考车泊车位模型，据此设计泊车位搜　索算法得到泊车位的空间信息；根据泊车位空间信息，通过｝Ａ车路径生成算法计算出一条最合理的行车路径；通过由　车控运动算法驱使车子按照预定路径运动到预定的车位上；最后在嵌入式系统上对这三个算法进行可行性验证。　关键词　自动泊车；泊车位搜索；泊车路径；运动控制　中图分类号：ＴＰ３　文献标识码；Ａ　１　引言　随着生活质量的提高，汽车的保有量快速增长，随之而来的是泊车位的紧张，这点在大城市中尤　为突出。而完成紧凑泊车位空间下的泊车成为许多新手和女性车主的一个难题。为此全世界的汽车厂　商开始研发自动泊车系统，在无人工干预的情况下替代司机完成泊车任务。　国内外研究机构则已经在这方面做了一些研究，但是并没有提出完整的解决算法＿Ｊ　Ｊ，或者是停　留在仿真阶段ｌ２］，要在真车上实现该系统还有一定的距离，本文将提出完成自动泊车系统中需要用到　的三个核心算法，包括泊车位搜索算法、路径生成算法、运　一　一　一　动控制算法，并搭建嵌入式系统在电动车上完成这些算法的　ｌ　Ｈ　笨甏Ｈ运动控制ｌ　验证。—］＝兰—　］一　一　２　算法框图　图１　自动泊车算法框图　基于超声波和机器视觉的自动泊车系统的算法框图如图１所示。主要步骤如下：　２．１泊车路径生成　泊车位的搜索依赖于泊车路径的制定策略。基于到车子本身的机械精度和系统实现复杂度，路径　的生成需要遵循以下原则：　Ａ．完成路径所需要的动作必须尽可能少。因为每个动作的精度误差会传递到下一个动作，动作越　多，精度越差。　Ｂ．在每个动作的实施过程中，车子的转向轮（绝大部分　为前轮）的角度需要保持一致。因为系统是通过嵌入式系统　实现的，而嵌入式系统的性能有限，转向轮角度保持一致能　够将运动轨迹的计算归结为几何问题，反之需要涉及复杂的　积分问题，这对嵌入式系统的性能是一个挑战。　按照这２个原则，生成的泊车路径在２个动作内完成，　转向轮（一般为前轮）在每个动作过程中都保持最大的偏角。　２．２泊车位搜索　一囊——　——　：．　工三　丑　．：　噩噩　横向泊车位　实现自动泊车首先要在停车场中找到合适的泊车位。对　于停车场中有连续多个空位很多的情况，车位空间很足，并　收稿日期：２０１　０－０８—３１　图２泊车位车位模型　修订日期：２０１０－０９．０６　通信作者　Ｅ－ｍａｉｌ：ｓａｇｒｅｓ２００４＠１６３．ｃｏｍ　７２　电路与系统学报　第１７卷　不需要使用自动泊车系统就能够轻松泊车，所以这里只考虑泊车位空间并不可观，并且泊车位的前后　已经有车停放的情形，如图２所示。　对于合适参考车模型的停车　位，使用超声波传感器探测车声　两侧的障碍物距离，根据障碍物　的距离来确定是否有合适的停车　位。　Ｌ　２．３　运动控制　在搜索停车位过程中自动泊　车系统需要控制车子沿着直线向　图３阿克曼两轮转向要求　图４车身几何尺　ｒ　前运动，而为了保证车子准确沿着直线运动，需要使用惯性导航仪的反馈来实现转向轮的方向校正。　在控制车子沿着制定好的泊车路径运动过程中需要精确测量出车身的位移，这需要借助光电编码　器来记录当前的位移值。　最后，使用步进电机分别对方向盘、汽车油门、　刹车控制车子的运动、停止、加速、减速。　ｆ　＋一　一一Ｓ——ｔｅｐＯ直线阶段Ｌｉｎｅ０　一一一一一一一一一一一一一一一一一．　３　泊车路径生成　３．１　阿克曼转向梯形公式　为使汽车实现车轮无侧滑的转向，车轮的偏转必　须满足阿克曼关系，即在汽车前轮定位角都等于零、　行走系统为刚性、汽车行驶过程中无侧向力的前提下，　整个转向过程中全部车轮必须围绕同一瞬时中心相对　于地面作圆周滚动，例如对于图１所示两轮转向情况，　前内轮转角ｂ与前外轮转角ａ之间应满足如下阿克曼　转向特性公式ｌｊｊ：　Ｂ／Ｌ＝ｃｏｔａ—ｃｏｔｆｌ　（１）　现代汽车的前轮转向机构都遵循这一个关系，这　也是设计泊车路径的一个基本依据。　３．２纵向泊车路径　车身的几何尺寸如图４所示。　对于和车子运动方向平行的纵向泊车位，分成４　个阶段完成，分别是Ｓｔｅｐ０、Ｓｔｅｐ１、Ｓｔｅｐ２、Ｓｔｅｐ３，　如图５所示。　图５纵向泊车路径　Ｓｔｅｐ３直线阶　其中，Ｓｔｅｐ０阶段从车尾与可用车位头部对齐　的地方开始直线向前运动适当距离；Ｓｔｅｐｌ阶段转　向轮　的偏角为Ｏｏ，后轮　转动距离为　，车身　绕圆心的转动角度为　；Ｓｔｅｐ２阶段转向轮　的偏　角为Ｏｒ，后轮Ｂ．转动距离为Ｓ，，车身绕圆心的转动　角度为　，且有　＝　。ｏｏ的绝对值等于　，并且　等于车　的最大转角，此时车身的运动半径为车子　的最小转弯半径Ｒ　；Ｓｔｅｐ３阶段为　让车子停在车　位中间而向前运动适当距离。在经历这４阶段后，　图６纵向泊车路径的儿何关系图　第３期　周泓等：自动泊车系统中核心算法的研究　７３　车身应该和Ｓｔｅｐ０之前保持平行。图６是泊车路径过程中各个重要几何参数的抽象图，其中　分别是前轮以ｏｏ转角运动时内向轮转弯半径、外向轮转弯半径，这是汽车本身的参数。　一　、　。　分别是车身与泊车位的横向距离、泊车位宽度，通过测量得到。要完成这个泊车过程，需要计算出直　线Ｐｏ　长度、　角度。根据Ｓｔｅｐ０、Ｓｔｅｐｌ之后车身的横向位移为　＋　可以得到下面的等式：　【　，一　＋　＝＾　Ｑ０＋Ｍ２　Ｑ１　｛Ｍ０Ｑｏ＝ＯｏＭ０～ＯｏＱｏ＝Ｒ　ｊ　一Ｒ　。　×ｃｏｓ（Ｇ）　１Ｍ２Ｑｌ＝０１Ｑ１一ＱＩＭ２＝（Ｒ　ｉ　＋　，．）一（Ｒ　ｉ　＋　，）×ｃｏｓ（　）　＿ａｒｃｃ。ｓ　ｌ　２型　竺１　×Ｒ　ｉ　＋　，』　（２）　由此得到４个阶段的运动参数：　Ｓｔｅｐ０阶段向前运动的距离Ｐ０　：　尸０　：ＰｏＰ２一　＝ＯｏＭ３一　尸　＝０ｏ０ｌ×ｓｉｎ（８ｏ）一√Ｒ：　一Ｒ　（２×Ｒ　＋　，）×ｓｉｎ（８ｏ）一４Ｒｍ　一尺　ｉ　（３）　睹’为：　＝在这里可以同时得到车位的最小长度　．厂———　——————＝－　Ｌ　，，＝　：　＋　＝√月　一　¨ｕ　＋　图７横向泊车路径　不过这个最小长度还没考虑安全裕度前轮和车头的距离　因此更加合理的最小长度　Ｐ’　应该为：　Ｌｍｉｎ＿　＊＝　：　＋　，　（４）　由此式可知，最小车位长度取决于车子本身的物理参数。　Ｓｔｅｐ１阶段后轮　转动距离：　ＭｏＭｌ＝　×Ｒ　。　Ｓｔｅｐ２阶段后轮　转动距离：　ＭｌＭ２：　×（Ｒ　ｉ　＋　Ｓｔｅｐ３阶段后轮　转动距离：　Ｌｓｔｅｐ３＝　３．３横向泊车路径　对于和车子运动方向垂直的横向泊车位，分成４个阶段完成，分别是Ｓｔｅｐ０、Ｓｔｅｐｌ、Ｓｔｅｐ２、Ｓｔｅｐ３，　如图７所示。Ｓｔｅｐ０阶段将车子从车尾与车位边线对齐的位置后退到合适的位置；Ｓｔｅｐｌ阶段将车子前　进左转　；Ｓｔｅｐ２阶段将车子右转后退　；Ｓｔｅｐ３阶段将车位刚好调整到与参考车头平齐的位置上。　如图８所示，完成Ｓｔｅｐ０，需要计算出Ⅳ。Ｊｖ，；完成Ｓｔｅｐ１需要计算　；完成Ｓｔｅｐ２需要计算　，而　＝　一　；完成Ｓｔｅｐ３需要计算　。　…莲　一　（５）　一　）　（６）　㈩　图８横向泊车路径的几何关系图　ｆ　＝Ｍ４Ｍ３＋坞Ｑ０　由：｛Ｍ４Ｍ３＝Ｍ５Ｍ１×ｓｉｎ（５２）＝（Ｌ　一Ｌ６）×ｓｉｎ（６ｏ）　｝Ｍ３Ｑｏ＝ＯｏＭｏ—ＯｏＭ３＝Ｒ　ｉ　一Ｒ　ｉ　×ｃｏｓ（￣ｏ）　解此方程可得：　＝ａｒｃｓｉｎ（｛ｑ（Ｒ　ｉ　一　）　×（　一Ｌｂ）　一［（　一Ｌｂ）　＋Ｒｍｉｎ２］×（　～２ｘＲ　×　）　一ｒ　ｏ　（尺　一　）×（　一三　）｝／［（　。　一Ｌｂ）　＋Ｒｍｉｎ　２】）　７４　＝电路与系统学报　第１７卷　（９）　ＯｏＮｌ—ｏｏ　＝（２×Ｒ　＋　）×ｃｏｓ（８ｏ）一（　＋　埔＋Ｒ　．　）　由此，Ｓｔｅｐ０时Ｂ．的运动距离为：　ｒ———：————＿：－　．ｖ】Ⅳ３＝ＮｌＯ１一Ⅳ３　＝（２×Ｒ　ｍ＋　）×ｓｉｎ（￣ｏ）一√尺　一　Ｓｔｅｐｌ时Ｂ．轮的运动距离为：　Ｍｌ＝　×Ｒ　ｊ　（１　０）　（１１）　Ｓｔｅｐ２时Ｂ　轮的运动距离为：　ｖ。＝（詈一　）×　Ｒ　＋　ｃ　２，　（１３）　Ｓｔｅｐ３时Ｂ．轮的运动距离为：　，　３＝　，一　一Ｌ６　上述计算得到的各个参量都是　的函数。　是Ｓｔｅｐｌ阶段车头偏离运动方向的最大距离，在实际情况　卜，停车场的出入道有双车道以上的宽度，因此　可以取的一个合理值就是车身宽度　：　，。这时，　车位的最小长度也是由车身的物理参数决定。　４　泊车位搜索　４．１超声波测距原理　测距原理为声波发射器向某一方向发射超声波，在　发射时刻的同时开始计时，超声波在空气中传播，途中　碰到障碍物就立即返回来，超声波接收器收到反射波就　图９纵向停车位搜索示意图　立即停止计时，通过两者的时间差ｆ，利用公式　＝　计算距离，其中　为发射器与障碍物之间的距离，　２为声波在介质中的传播速率：　３３　５×（１＋　…　ｒ为摄氏温度。　４．２超声波测距实现泊车位搜索　由３．２、３．３的分析可知，无论是纵向还是横向，泊车位最小长度都只取决于车本身的物理参数。　如　９是总线泊车位搜索的示意图，在进入停车场后，选择需要停泊的侧向（图９是选择左边）后丌　启自动泊车系统，系统开始接管车的控制。系统驱使车子匀速直线向前运动，开启位于车身尾部的超　声波传感器进行测距。　如果测到的是一个非空车位或者无效车位，则宽度　标车位的距离，将　Ｗｐａｒｋ　＜　满足　＜　那么　就是车身与Ｆ１　心赋值为　。　满足　如果测到的是一个有效空位车，宽度　的情况则将　清零，如果Ｌ　则丌始累加空位长度￡　，如果中途出现　Ｌ　ｍｌ，　则找到　合适车位，停ＩＩ：向前。　对于横向泊车位的搜索与上述情形类似。　图１０参考车的非理想长方休　４＿３　非理想参考车的处理　４．２中将参考车简化为一个理想的长方体，　实际的车身有各种凸凹，影响最大的是车头牟尾并　是简单的平面，如图１０所示。　在前轮附近会采集到最小值，而在车头最前部位会采集到最大值。从安全角度考虑，　一　应当取所有满足　＜　中值最小的　：　７６　电路与系统学报　第１７卷　值表示偏左，负值表示偏右；　为横向误差，正值表示偏上，负值表示偏下。　７　结论　本文针对自动泊车提出了三个核心算法，包括泊车位搜索算法、路径生成算法、运动控制算法，　并在模拟小车上对这三个算法进行了初步的实验，实验结果表明在算法能够比较好地实现自动泊车。　目前算法对机械精度要求比较高，需要进一步降低算法精度对机械精度的依赖。另外目前还停留在模　拟车的试验阶段，在条件允许的情况下应该在真车上做更多的测试取得更多的实验数据来改进算法。　参考文献：　［Ｉ】　郭孔辉，　姜辉，　张建伟，等．基于模糊逻辑的自动平行泊车转向控制器ｆＪ］．吉林大学学报（】＿学版），２００９，３９（２）：２３６—２４０　［２】　盂惠霞，　耿俊梅綦于ＭＡＴＬＡＢ的模糊控制汽车倒车仿真系统Ⅲ中原Ｊ一学院学报，２００９，２０（１）：７５　７７．　［３］　周祥基，　钱瑞明．汽乍转向传动技术及其发展…．机械制造与ｆ／动化，２００４，３３（６）：９．１２，１　５．　［４］　卢文涛，　李永奎，邓志刚等．转向控制算法及ＭＡＴＬＡＢ的实现Ⅲ．沈阳农业大学学报，２００５，３６（３）：３５８—３６０．　［５］　何建国，　张文胜，赵根庄．智能泊车引导方法的研究与实现［Ｊ１ｌ地理空问信息，２００７，５（２）：３－５．　种应用于自动泊车系统的车位检测方法…．传感器ｔ廿界，２００９，３（１）：２４．２７．　［７］　Ｒｙｕ　Ｙｏｕｎｇ—ＷｈＯ，Ｏｈ　Ｓｅ—ｙｏｕｎｇ，Ｋｉｍ　Ｓａｍ－ｙｏｎｇ．Ｒｏｂｕｓｔ　ａｕｔｏｍａｔｉｃ　ｐａｒｋｉｎｇ　ｗｉｔｈｏｕｔ　ｏｄｏｍｅｔ　ｒｙ　ｕｓｉｎｇ　ｅｎｈａｎｃｅｄ　ｆｕｚｚｙ　ｌｏｇｉｃ　ｃｏｎｔ　ｒｏｌｌｅｔ［Ａ】．ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｆｕｚｚｙ　Ｓｙｓｔｅｍｓ［ｃ］．２００６．５２１—５２７．　［８］　Ｃｈｉｕ　Ｃｈｉａｎ－ｓｏｎｇ，Ｌｉａｎ　Ｋｕａｎｇ—ｙｏｗ，Ｌｉｕ　Ｐｅｔｅｒ，Ｆｕｚｚｙ　ｇａｉｎ　ｓｃｈｅｄｕｌｉｎｇ　ｆｏｒ　ｐａｒａｌｌｅｌ　ｐａｒｋｉｎｇ　ａ　ｃａｒｌｉｋｅ　ｒｏｂｏｔ［Ｊ１ｌ　ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｃｏｎｔｒｏｌ　Ｓｙｓｔｅｍｓ　Ｔｅｃｈｎｏｌｏｇｙ，２００５，Ｉ　３（６）：１　０８４—１　０９２．　［９］Ｔｈｏｍａｓ　Ｄ　Ｇｉｌｌｅｓｐｉｅ．Ｆｕｎｄａｍｅｎｔａｌｓ　ｏｆＶｅｈｉｃｌｅ　Ｄｙｎａｍｉｃｓ［Ｊ］．Ｓｏｃｉｅｔｙ　ｏｆＡｕｔｏｍｏｔｉｖｅ　Ｅｎｇｉｎｅｅｒｓ，ｌｎｃ，１　９９２．２７５—３０９．　［１　０］Ｐｏｌａｋ　Ｊ　Ｗ，ＨｉＩｔｏｎ　Ｉ　Ｃ，Ａｘｈａｕｓｅｎ　Ｋ　Ｗ，ｅｔ　ａ１．Ｐａｒｋｉｎｇ　ｇｕｉｄａｎｃｅ　ａｎｄ　ｉｎｆｏｒｍａｔｉｏｎ　ｓｙｓｔｅｍｓ：ｐｅｒｆｏｒｍ　ａｎｃｅ　ａｎｄ　ｃａｐａｂｉｌｉｔｙ…．Ｔｒａｆ－ｉｃｔ　Ｅｎｇｉｎｅｅｒｉｎｇ　ａｎｄ　Ｃｏｎｔｒｏｌ，１９９０，３１（１Ｏ）：５１９－５２４．　作者简介：周泓（１９７４一），男，浙江绍兴人，副教授，博士，主要从事汽车电子及嵌入式系统方面的研究；王文飞，　，硕士研究生。　Ｃｏｒｅ　ａｌｇｏｒｉｔｈｍ　ｒｅｓｅａｒｃｈ　ｉｎ　ａｕｔｏｍａｔｉｃ　ｐａｒｋｉｎｇ　ｓｙｓｔｅｍ　ＺＨＯＵ　Ｈｏｎｇ，ＷＡＮＧ　Ｗｅｎ—ｆｅｉ　（Ｄｅｐａｒｔｍｅｎｔ　ｏｆ　Ｉｎｓｔｒｕｍｅｎｔ　Ｓｃｉｅｎｃｅ　ａｎｄ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｚｈｅｊｉａｎｇ　Ｕｎｉｖｅｒｓｉｔｙ，Ｈａｎｇｚｈｏｕ　３　１　００２７，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｔｈｒｅｅ　ｃｏｒｅ　ａｌｇｏｒｉｔｈｍｓ，ｗｈｉｃｈ　ａｒｅ　ｕｓｅｄ　ｉｎ　ａｕｔｏｍａｔｉｃ　ｐａｒｋｉｎｇ　ｓｙｓｔｅｍ，ａｒｅ　ｓｔｕｄｉｅｄ．Ｇｅｏｇｒａｐｈｉｃ　ｉｎｆｏｒｍａｔｉｏｎ　ｏｆ　ｐａｒｋｉｎｇ　ｓｐａｃｅ　ｉｓ　ｃｏｌｌｅｃｔｅｄ　ｔｈｒｏｕｇｈ　ｐａｒｋｉｎｇ　ｓｃａｎｎｉｎｇ　ａｌｇｏｒｉｔｈｍ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ｒｅｆｅｒｅｎｃｅ　ｃａｒ　ｐａｒｋｉｎｇ　ｍｏｄｅｌ　ｗｈｉｃｈ　ｉｓ　ｐｒｅｓｅｎｔｅｄ．　Ａｃｃｏｒｄｉｎｇ　ｔｏ　ｇｅｏｇｒａｐｈｉｃ　ｉｎｆｏｒｍａｔｉｏｎ，ｔｈｅ　ｍｏｓｔ　ｒｅａｓｏｎａｂｌｅ　ｐａｒｋｉｎｇ　ｐａｔｈ　ｉｓ　ｃａｌｃｕｌａｔｅｄ　ｔｈｒｏｕｇｈ　ｐａｔｈ　ｇｅｎｅｒａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ．Ｔｈｅ　ｃａｒ　ｉｓ　ｄｒｉｖｅｎ　ｔｏ　ｔａｒｇｅｔ　ｓｐａｃｅ　ａｌｏｎｇ　ｗｉｔｈ　ｔｈｅ　ｓｃｈｅｄｕｌｅｄ　ｐａｔｈ　ｔｈｒｏｕｇｈ　ｃｏｎｔｒｏｌｌｉｎｇ　ａｌｇｏｒｉｔｈｍ．Ａｔ　ｌａｓｔ　ｔｈｅ　ｆｅａｓｉｂｉｌｉｔｙ　ｏｆ　ｔｈｒｅｅ　ａｌｇｏｌ’ｉｔｈｍｓ　ｉｓ　ｖａｌｉｄａｔｅｄ　ｉｎ　ｔｈｅ　ｅｍｂｅｄｄｅｄ　ｓｙｓｔｅｍ．　Ｋｅｙ　ｗｏｒｄｓ：ａｕｔｏｍａｔｉｃ　ｐａｒｋｉｎｇ；ｐａｒｋｉｎｇ　ｓｃａｎｎｉｎｇ；ｐａｒｋｉｎｇ　ｐａｔｈ；ｍｏｖｉｎｇ　ｃｏｎｔｒｏｌｌｉｎｇ