用于自主驾驶车辆的路径规划的精确曲率估计算法
【技术领域】
[0001] 本发明主要涉及一种使用地图数据库的形状点来识别道路曲率的系统和方法，更 具体的，涉及一种在半自主或自主驾驶车辆上使用地图数据库形状点和多点多项式曲线拟 合算法来识别道路曲率的系统和方法。
【背景技术】
[0002] 现代化车辆的操作变得越来越自主，即，车辆能够在越来越少的驾驶员干预的前 提下提供驾驶控制。巡航控制系统已经在车辆上使用了数年，其中，车辆操作员可以设置特 定的车辆速度，该车辆将维持在这个速度，而不需要驾驶员操作节气门。适应性巡航控制 系统在本领域中最近得到了发展，其中，系统不仅维持设定的速度，并且当使用多个传感器 (例如雷达和相机）检测到前车缓慢移动的情况时还使得车辆自动减速。一些现代化车辆 还提供自主驻车，其中，车辆将自动提供停车用的转向控制。一些车辆系统在驾驶员做出会 影响车辆稳定性的急转弯变化时还会进行干预。一些车辆系统试图将车辆维持在道路车道 的中心附近。此外，全自主车辆已经证明了可以高达30mph在模拟郊区交通中行驶，遵守全 部的道路规则。
[0003] 随着车辆系统的改进，它们将变得越来越自主，以实现成为完全自主驾驶车辆的 目标。例如，未来的车辆很可能将应用自主系统，用于车道改道、通过、离开交通、进入交通 等等。平滑的操作和自动的车道对中以及车道改道控制对于自主驾驶车辆上的驾驶员和乘 客舒适是很重要的。然而，由于传感器和促动器的延时，测量到的车辆状态与实际的车辆状 态可能是不一样的。这种差别可导致不恰当的路径生成，这将影响车道改道生硬性。
[0004] 受让给本申请的受让人的于2012年5月1日授权的且合并在此作为参考的、名 称为 "Path Generation Algorithm for Automated Lane Centering and Lane Changing Control System(用于自动车道对中和车道改道控制系统的路径生成算法）"的美国专利 8, 170, 739公开了一种为自动车道对中和/或车道改道提供路径生成的系统。该系统采用 一个或多个前向相机，其检测车辆前方的车道标识线，用于识别车辆正在行驶的行驶车道。 相机所检测的道路标识信息用于确定车辆的中心车道，其可用于识别道路的曲率、车辆的 航向角度、车辆的位置等等。所需路径生成处理器接收来自相机的信号、车辆状态信息和车 辆的转向角以及车辆改道的请求。该系统还包括路径预测处理器，其基于包括车辆纵向速 度、车辆横向速度、车辆偏航率以及车辆转向角的车辆状态信息来预测车辆的路径。所需的 路径信息和预测的路径信息进行比较，生成错误信号，误差信号被发送给改道控制器，其提 供转向角信号以使得车辆转向并减小错误信号，其中，路径生成处理器采用五阶多项方程 式以基于输入信号确定车辆所需的路径。
[0005] 上述讨论的系统通常需要知晓道路曲率，以提供平滑的车辆路径规划。在本领域 中已知的是使用车上的GPS接收器和相关的地图数据库来识别车辆正在行驶的道路的曲 率。GPS接收器根据地球上的经度和炜度识别车辆的位置，地图数据库使用该位置来协调车 辆在哪条道路上，并提供多个形状点，所述形状点也识别车辆周围的经度和炜度的道路的 位置。地图数据库可用的形状点通常依赖于道路曲率因而是分开的，其中，对于更大曲率的 道路而言，形状点间隔更加紧密。形状点可用线连接，并将该线拟合成曲线方程式，道路的 曲率可被确定，其中，线的曲率穿过由数据库识别的每个形状点。然而，因为在道路调查中 存在错误，地图数据库的形状点可能并不精确位于道路的中心，可能多多少少偏离道路，这 使得在由形状点生成的曲线中产生错误。
[0006] 由于形状点位置具有错误，形状点之间的线的曲率可能无法精确地识别任何特定 位置下道路曲率的正确半径。由于地图数据库中形状点的局外点足够显著以使得道路曲率 的半径特别不准确，不能可靠地采用使用地图数据库形状点来识别道路曲率的已知技术。
【发明内容】
[0007] 本公开描述了一种在车辆系统中使用地图数据库的形状点识别道路曲率的系统 和方法。该方法包括确定用于确定道路曲率的关注的范围，并从地图数据库收集从车辆当 前位置至限定了道路位置的关注范围的端点的形状点。该方法将世界测地系统84(WGS84) 坐标系下的形状点转换为UTM坐标系，接着使用转换后的形状点拟合一组多项方程式以限 定曲率。该方法确定该一组多项方程式是否超过预定的曲率精确度阈值，如果是，那么使用 转换后的形状点将多组多项方程式与关注范围内的多个邻近道路段拟合。该方法接着使用 多组多项方程式的解来确定在任意道路位置处的道路曲率。
[0008] 本申请还包括下述技术方案：
[0009] 1. -种在车辆的车辆系统中确定道路曲率的方法，所述方法包括：
[0010] 确定关注的范围，以用于确定道路曲率；
[0011] 从车辆上的地图数据库收集从车辆的当前位置至限定道路位置的关注范围的端 点的形状点，其中，所述形状点通过世界测地系统84(WGS84)坐标系限定；
[0012] 将所述形状点从所述WGS84坐标系转换为通用横轴墨卡托投影（UTM)坐标系；
[0013] 使用转换后的形状点拟合一组多项方程式以限定道路曲率；
[0014] 确定该一组多项方程式超过预定的曲率精确度阈值；
[0015] 使用转换后的形状点将多组多项方程式与关注范围内的多个道路段拟合；以及
[0016] 使用多组多项方程式的解来确定在道路位置处的道路曲率。
[0017] 2.根据技术方案1所述的方法，其中，拟合一组多项方程式包括：拟合两个三阶多 项方程式以限定道路曲率。
[0018] 3.根据技术方案2所述的方法，其中，拟合一组多项方程式包括：基于弧长拟合两 个多项方程式，其中，弧长限定为任意两个形状点之间的距离。
[0019] 4.根据技术方案3所述的方法，其中，拟合一组多项方程式包括：限定每个形状点 为：
[0021] 其中，s是弧长，p和q是未知系数。
[0022] 5.根据技术方案4所述的方法，其中，拟合一组多项方程式包括：通过最小化成本 函数来解方程式。
[0023] 6.根据技术方案5所述的方法，其中，所述成本函数J限定为：
[0025] 7.根据技术方案5所述的方法，其中，确定一组多项方程式超过预定精确度阈值 包括：确定成本函数最小值超过精确度阈值。
[0026] 8.根据技术方案1所述的方法，其中，将多组多项方程式与多个道路段拟合包括： 将一组多项方程式与多个连续段单独地拟合，其中，重叠部分限定在两个段之间，并且虚拟 的形状点在重叠段中从之前的段被限定。
[0027] 9.根据技术方案8所述的方法，其中，每个道路段的每组多项方程式是两个三阶 多项方程式。
[0028] 10.根据技术方案1所述的方法，其中，从地图数据库收集形状点包括收集限定了 车辆后方道路的形状点。
[0029] 11.根据技术方案1所述的方法，其中，确定在任意道路位置处的道路曲率包括： 如下计算曲率κ :
[0031] 12.根据技术方案1所述的方法，其进一步包括：基于多项方程式的解来确定车辆 的航向角度。
[0032] 13.根据技术方案12所述的方法，其中，确定车辆航向角度包括使用方程式
[0033] 其中：
[0035] 并且其中，Sl是从起始点P i至点P 弧长。
[0036] 14. -种在车辆的车辆系统中确定道路曲率的方法，所述方法包括：
[0037] 从车辆上的地图数据库收集从车辆当前位置至限定道路位置的关注范围的端点 的形状点；
[0038] 使用形状点来拟合三阶多