一种基于路径规划的自动平行泊车算法

Ｐａｒａｌｌｅｌ ｐａｒｋｉｎｇ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ａｕｔｏｎｏｍｏｕｓ ｐａｔｈ ｐｌａｎｎｉｎｇ
ＬＩＮ Ｚｈｅｎ—ｚｈｅｎ ，ＬＩ Ｑｉｎｇ 一，ＬＩＡＮＧ Ｙａｎ－ｊｕ 一，ＣＨＥＮ Ｄａ—ｐｅｎｇ ， （１．Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｍｉｃｒｏｅｌｅｃｔｒｏｎｉｃｓ ｏｆ Ｃｈｉｎｅｓｅ Ａｃａｄｅｍｙ ｏｆ Ｓｃｉｅｎｃｅｓ，Ｂｅｉｊｉｎｇ １０００２９，Ｃｈｉｎａ；２．Ｓｍａｒｔ Ｃｅｎｔｅｒ ｉｎ Ｋｕｎｓｈａｎ ｏｆ ＩＭＥＣＡＳ，Ｋｕｎｓｈａｎ Ｉｎ— ｄｕｓｔｒｉａｌ Ｔｅｃｈｎｏｌｏｇｙ Ｒｅｓｅａｒｃｈ Ｉｎ ｓｔｉｔｕｔｅ，Ｓｕｚｈｏｕ Ｊｉａｎｇｓｕ ２１５３４７，Ｃｈｉｎａ）
前轮转 向角
符号 Ｌ
说 明 轴距 前悬 车宽 车辆运动速度
运动学方程 及点坐标方程为
＝ ｃ。ｓ ； ＝ ｓｉｎ ０；０＝｛ｓｉｎ妒
陷也存在 于文献 ［５］中。文 献［６］使用的双半径 法需要在 车速 为零 时改变转 向角 ，对 车辆损 伤较 大 ；文献 ［７］中提 出了 以五 阶多项式作 为平行泊 车的规划 路径 ，但其未对约束 空间加以限
ｙ
赖于问题 ，没有标准方法 ；改变遗传 因子法不能 解决非线 性问 题 ；罚函数法有效地将 遗传搜索 引导到解 空 间中有希 望的 区域去 ，通 过惩罚 不可行解 ，将 约束 问题 转换 为无约 束问题 。 目标 函数和它共同作 用以决定染色体的适应度 函数 。文献 ［１４］中提 出了一 种带拉 格朗 Ｅｔ乘 数 的罚 函数 ，将非 线性 约束 进行取对数和平方等处理 ，并且需要 不断增大拉格 朗 日乘 数 ， 进行多次迭代 ，计算复杂度高 。本文对泊车问题有针对性地设 计罚 函数 ，仅对无效参数 的个体实施有 别于正 常极值 的惩 罚 ， 能高效求得可行 的算法最优解 。
１．２ 改进 的 五 阶 多 项 式 路 径 规 划 根据文献 ［７］，在 已知泊 车起 止 位置 的条件 下 ，可 以计算
出表示 泊车路径的五 阶多 项式 。由于需 要满足 起止点 车身平 行 于 轴 、转 向角为零 的条 件 ，所需 车位较 大。实际泊 车过程 可有如下改进 ：在停 车路 径和转 向角之 间进行折 中，以尽 可能
所需要 的最小泊车位 。
小 的转 向角停放于平行车位 ，再 略微 向前 行驶至 车位正 中，同
收稿 日期 ：２０１１ １０－１４；修回 １３期 ：２０１１—１１－１６ 基金项 目：中国科 学院知识创新 工程重要方向资助项 目（ＫＧＣＸ２一Ｙｗ一１４８） 作者简介 ：林 蓁蓁（１９８５一），女 ，福建福 州人 ，硕士研究生 ，主要研 究方向为车载嵌入 式 系统（１ｉｎｚｈｅｎｚｈｅｎ＠ｉｍｅ．ａｓ．ａｎ）；李庆 （１９７２－），男 ，吉林 四平人 ，副研 究员，主要研究方向为 多传感器集成、数据 融合 ；梁艳 菊（１９８５一），女 ，河 南周 口人，博士研 究生，主要 研究方向为 图像拼接 、立体视 觉； 陈 大鹏 （１９６８一），男 ，安 徽 合 肥 人 ，研 究 员 ，博 导 ，主要 研 究 方 向 为 集 成 电路 、ＭＥＭＳ．
ｏ
Ｘ
图１ 车辆 的数学模型
图２ 泊车轨迹及约束空间示意
１．３ 平行 泊车的约束 空间
平行泊车的约束 空 间包括 最大 转 向角约束 和避 障 约束 。 前者保证路径中所需的转向角小 于车辆能达到 的最大转 向角 ， 后者保证泊车过程 中不与车位边缘发生碰撞 。
当个体在约束空间内时 ，罚 函数 为零 ，适应 度函数值 为终
须满足 ：
＜
。对 于避 障约束 ，车辆 的
＼ｌ ’ ， ）
工一
右侧不能与 车位 的右上顶点 （Ｐｘ，Ｐ ）相撞 。由文献 ［１１］，在倒
车过程 中保证车辆 的右前顶点不与车位的右上顶点相撞 即可 ，
如图 ２中的两个 圆点所示 ，当 ＞Ｐ 时 ，Ｙ ＞Ｐｙ。
仿真使用丰 田凯美瑞实车数据 ，车长 ４．８２５ ｉｎ，宽 １．８２ Ｉｎ，轴距 ２．７５５ ｉｎ，最大转向角 ４５。。以车位长度 ７．２ Ｉｎ为例 ，遗 传算 法采用每代 ８０个 个体 ，１００代 迭代过 程 ，如 图 ３所示 ，可 以看到迭代过程 收敛于 曲率近乎于零的个体 上 ，结果正确。
１．１ 车 辆 的 数 学 模 型
泊车过程中假定车辆保持恒定低速 ，因此侧滑 的影响忽略不
计 ，分析采用的车辆模型如图 １所示 ，各符号说 明如表 １所示。
表 １ 符
０
说 明 后轴 中心点坐标
右 前 顶 点 坐 标 车辆朝向与 轴夹角
摘 要 ：针对 城 市 中停 车位狭 小、现有 自动泊车 方法缺 乏 连 贯性 的 问题 ，提 出一 种 自动平 行 泊车 算 法。对 现 有 的五 阶 多项 式路径 规 划方 法加 以改进 ，并有 针对 性地 设计 罚 函数 ，采 用 遗传 算 法计 算 最佳 泊车路 径 和 最 小泊 车 空间 ，实现 自动 平行 泊车 。仿真 结果表 明 ，该 算法 能快速 有效地 完成 泊 车 ，车辆损伤 小 ，对 空 间的要 求 最低 。 关 键词 ：平行 泊车 ；路 径规 划 ；约束 空 间 ；遗 传算 法 ；二分 法 中图分类 号 ：ＴＰ１８２；ＴＰ２４２ 文献标 志码 ：Ａ 文章 编号 ：１００１—３６９５（２０１２）０５－１７１３—０３ ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００１—３６９５．２０１２．０５．０２９
ｋ＝ｄＯ／ｄｓ，结合曲线弧长公式 ｄｓ＝，／１十Ｙ ｄｘ和车辆的运动学 方程 ，可推得 ｓｉｎ ＝ 。因此 ，当 Ｙ”（ ）＝０时 ，满足条 件 ： ０。将 起止点坐标和 导数条件联 立 ，得到 关于轨 迹 系数 向量 Ａ 的非 齐次线性方程组 ，形 如 ＸＡ＝Ｙｏ 由于 ｒ（Ｘ）＝５，可知通 解 为 Ａ＝ ＋ｃ亭。其 中 ：田为特解 ， 为导 出组 的基 础解 系。因 此 ，平行泊车可能的轨迹 为一组 曲线 ，如 图 ２所示 。需要从 中 挑选在平行泊 车约束空 间之 内且指标最 优的 酶线 作为平行 泊 车的实施 曲线 。
第 ２９卷 第 ５期 ２０１２年 ５ 月
计 算 机 应 用 研 究
Ａｐｐｌｉｃａｔｉｏｎ Ｒｅｓｅａｒｃｈ ｏｆ Ｃｏｍｐｕｔｅｒｓ
Ｖ０１．２９ Ｎｏ．５ Ｍ ａｙ ２０１２
一 种 基 于路 径 规 划 的 自动 平 行 泊 车 算 法 冰
林蓁蓁 ，李 庆 ，梁艳菊 ，陈大鹏
（１．中 国科 学院微 电子研 究所 ，北京 １０００２９；２．昆 山市 工业 技 术研 究院 中国科 学 院微 电 子研 究所 昆 山感 知 中 心 ，江 苏 苏 州 ２１５３４７）
国外对于 自动平行泊 车的算法 研究起 步 于 ２０世 纪 ９０年 代 ，国内近几年来对该领域也有所 涉及。概括起来主要有两种 方法 ：基于模糊控制和基于路径规划 。文献 ［１，２］中提 出应 用 驾驶员 的经验整理 出控 制规 则 ，可减 小 泊车 过程 中 的位置 误 差 ，但控制过程缺乏连贯 的规划 性 ，需 要反 复调整 车速 和转 向 角 ，且前后挪移需要较 大的停 车空 间，难 以量化 。而 基于路 径 规划 的方法 根据车辆与停车位的相对位置算 出最佳泊 车路径 ， 便 于泊 车全 过程 的规划 控制 。文 献 ［３］提 出的路 径需 要不 停 地变速及换挡 Ｊ，不 符合 驾驶 员操 作 的实 际情 况。相 同 的缺
Ａ ｂｓｔｒａｃｔ： Ｉｎ ｏｒｄｅｒ ｔｏ ｓｏｌｖｅ ｐｒｏｂｌｅｍ ｓ ｏｆ ｎａｒｒｏｗ ｐａｒｋｉｎｇ ｓｐａｃｅ ｉｎ ｃｉｔｉｅｓ ａｎｄ ｌａｃｋ ｏｆ ｃｏｎｔｉｎｕｉｔｙ ｏｆ ｖｅｈｉｃｌｅ’Ｓ ｍｏｔｉｏｎ ｉｎ ｏｔｈｅｒ ｐａｒｋｉｎｇ ａｐｐｒｏａｃｈｅｓ，ｔｈｉｓ ｐａｐｅｒ ｐｒｏｐｏｓｅｄ ａｎ ａｕｔｏｎｏｍｏｕｓ ｐａｒｋｉｎｇ ａｌｇｏｒｉｔｈｍ ．Ｉｔ ｉｍｐｒｏｖｅｄ ｔｈｅ ａｐｐｒｏａｃｈ ｏｆ ｆ ｉｆ ｔｈ·ｏｒｄｅｒ ｐｏｌｙｎｏｍｉａｌ， ａｎｄ ｄｅｓｉｇｎｅｄ ｐｅｎａｌｔｙ ｆｕｎｃｔｉｏｎ ａｎｄ ｇｅｎｅｔｉｃ ａｌｇｏｒｉｔｈｍ ｔｏ ｃａｌｃｕｌａｔｅ ｔｈｅ ｂｅｓｔ ｐａｔｈ ａｎｄ ｔｈｅ ｔｉｇｈｔｅｓｔ ｐａｒｋｉｎｇ ｓｐａｃｅ，ｆ ｉｎａｌｌｙ ｒｅａｌｉｚｉｎｇ ａｕｔｏｍａｔｉｃ ｐａｒａｌｌｅｌ ｐａｒｋｉｎｇ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔ ｉｎｄｉｃａｔｅｓ ｔｈａｔ ｔｈｉｓ ｍｅｔｈｏｄ ｃａｎ ｐａｒｋ ａ ｃａｒ ｓｗｉｆｔｌｙ ａｎｄ ｅｆ ｉ ｃｉｅｎｔｌｙ ｗｉｔｈ ｓｌｉｇｈｔ ｉｍ— ｐａｃｔ ｏｎ ｖｅｈｉｃｌｅｓ ａｎｄ ｔｈｅ ｌｅａｓｔ ｓｐａｃｅ ｒｅｑｕｉｒｅｍｅｎｔ． Ｋ ｅｙ ｗｏｒｄｓ： ｐａｒａｌｌｅｌ ｐａｒｋｉｎｇ；ｐａｔｈ ｐｌａｎｎｉｎｇ； ｃｏｎｓｔｒａｉｎｅｄ ｓｐａｃｅ；ｇｅｎｅｔｉｃ ａｌｇｏｒｉｔｈｍ ；ｄｉｃｈｏｔｏｍｙ
算法 中取个体适应度函数的最小值为最佳个体 ，带罚函数 的适应度 函数为
＝ ｌ ｌ＋ｒ１ｍａｘ（０，Ｐ 一Ｙｒｆｉ）＋１＂２ｍａｘ（０，Ａ１）＋ｒ２ｍａｘ（０，△２）
其中 ：ｋ。为终点处 的曲率 ，是所求 的适 应度 函数 ，曲率 越小 ，车 轮在终 点处 的转 向角越小 ，曲线越佳 ；适应 度 函数 的后续部 分 为罚函数 ， 、ｒ 为惩罚 系数 ；Ｙｍ为 ＝ 时 ，车辆右 前方 顶 点的 ｙ轴坐标值 ，若 Ｐ 一Ｙ ＞０，说 明车辆 右侧 和车位发 生了 碰撞 ，差值越大 ，该参 数组离 可行 区 间越 远 ；△ ＝Ｌｍａｘ（Ｋ）一 ｓｉｎ ，△：＝一ｓｉｎ 一Ｌｍｉｎ（Ｋ），Ｋ为该个体 下多 项式 曲率 的变化范围。若 △ 和 △：大于零 ，说 明多项式表示 的路径超过 了车辆的最大转向角 ，差值越大 ，该参数组离可行 区间越远 。
州 Ｌ＋Ｌ，）ｃ０ｓ ＋詈ｓｉｎ
ｙｒｆ＝ｙ＋（ ＋Ｌ，）ｓｉｎ日一 ｃ刚
制 ，需 要较大的泊车空间 。 本文在速度恒定 的条 件下 ，提 出了改进 的五 阶多项 式路 径
规划方法 ，并给出其约束 空间 。在此 基础上 ，采 用遗传算 法在 可行的路径 曲线族 中精 确寻优 ，设计 了高效 的罚 函数 ，完成 自 动泊车 。此外 ，使用二分法搜 索可行 解空 间 ，计 算 出所 述方 法
· １７１４·
计 算 机 应 用 研 究
第 ２９卷
时调整转 向角与车辆平行 。本文据此改进泊车路径规划算法 ， 使其具有更大的适 应性。
设车辆经过调 整 ，位 于坐标为 （ ，Ｙ ）、Ｏｓ＝０、 ＝０的 泊车起 点。泊 车终 点 坐标 （ ，Ｙ ）为 车 位远 端 ，与车 头保 留 ２０ ｃｍ的安全距离 ，０ ＝０，以倒车 的方式 泊入 车位。泊 车轨迹 五阶多项式为 Ｙ（ ）：Ｏ，５ ＋０，４ｘ ＋ｆｔ３ ＋ｏ２ ＋０１ ＋‰。由 车身沿轨迹切线方 向可知 Ｙ （ ）＝０，ｙ （ ）：０。由曲率定义
点 曲率 ；当个体不在约束空间 内时 ，适应度函数值迅速增大 ，在
后续迭代步骤中复制较少或丢 弃。此方法对 约束条件 的线性
和可微性没有限制 ，从而遗传算法最终将停 留在约束空 间内且
适应度值最优的个体上。该个体 确定 了当前 车位条件 下的最
优起始位置和规划路径 。 ３ 仿 真结果
■
对于最大转角约束 ，由 ｓｉｎ ＝ｍ 可知 ，泊车轨迹的曲率必 ３．１ 平行泊车仿 真
随 着 汽 车 数 量 的 增 加 ，城 市 中停 车 位 的 空 间 逐 渐 减 小 ，给
经 验不足的驾驶员带来很 大 困扰 。本 文针 对泊 车中最 困难 的 １ 数 学模 型建 立
平 行泊车问题 ，提 出了一 种基 于路径 规划 的恒 速泊 车方法 ，帮 助 驾驶 员安全ｊＪ￣ ｒＪ地实现 自动泊车 。