路径规划自动驾驶仪的制作方法
【专利摘要】即使车辆远离期望路径，路径规划自动驾驶仪也可导向车辆。
【专利说明】路径规划自动驾驶仪
[0001] 相关申请
[0002] 本申请涉及并要求题目为"路径规划自动驾驶仪"的第13/470, 028号美国申请的 优先权，该美国申请以全文引用的方式并入本文。
【技术领域】
[0003] 本发明主要涉及用于车辆的自动控制系统。
【背景技术】
[0004] 与草率的操作相比，根据精确的期望路径来导向农用拖拉机、施工车辆、矿用卡 车、军用坦克或其它大型车辆通常会省时和节能。精确的农用拖拉机控制，例如，整体来说 给农民、消费者和社会带来好处。农民能够更高效地劳动，且在肥料和杀虫剂上花费更少的 钱。消费者享受高质量的低价产品，以及农场和农药的精确使用减少了浪费和过量的污水。
[0005] 能够高精度、重复导向农用拖拉机、喷洒车、收割机等的自动驾驶仪系统给农场和 农药使用带来好处。例如，农田通常由90英尺宽或甚至更大的水栅喷雾。当使用这么宽的 水栅时，就需要谨慎以使从一个喷雾片到下一个喷雾片重叠几英尺。然而，该重叠可在拖拉 机或喷洒车配备高性能的自动驾驶仪时减小至仅数英寸。由非双喷雾片边缘产生的节约在 大面积农场上迅速增加。
[0006] 现有的自动驾驶仪系统足以将车辆保持在预定路径上。这些自动驾驶仪是基于反 馈控制技术的以及其可以使大型的农用拖拉机例如遵循航迹误差在一或两英寸内的线。图 1A示出了在反馈控制下遵循路径的车辆；车辆105遵循路径110。当航迹误差XTE小于车 辆轴距b时，该车辆处于小信号机制或"靠近"期望路径。
[0007] 当车辆远离期望路径时，反馈自动驾驶仪表现不好。出现这样的常见情况包括从 很远的地方加入路径或者在路径的末端调转方向以加入附近的路径。图1B示出了具有可 能的路径125的车辆，该路径125可以由在大信号机制下使用小信号反馈自动驾驶仪引起。 通过反馈自动驾驶仪将车辆115向路径120导向。当航迹误差大于车辆轴距时，该车辆处 于大信号机制或离期望路径"远"。
[0008] 针对常规的反馈自动驾驶仪，存在小信号机制下的性能和大信号机制下的可接受 行为之间的权衡。例如，在小信号机制下保持小航迹误差的高反馈增益可导致陡峭的接近 期望路径并在加入如图1B所示的路径时发生振荡。常规的自动驾驶仪在遭遇与期望路径 之间的大偏差时，通过调用启发式的限制来避免不良的大信号行为。这些限制意味着大信 号导向并非想象的那样有效。
[0009] 在以上例子中，轴距作为特征长度尺度的例子用来区分小信号机制和大信号机 制。也可以使用其它特征长度，例如，在特征自动驾驶仪响应时间内移动的距离。通常，小 信号机制是指车辆自动驾驶仪表现为线性的时不变系统的任何情况。另一方面，大信号机 制是指出现非线性行为（例如，转向角限值或转向角变化率限值）的情况。
[0010] 不管车辆靠近或远离期望路径，都需要能够提供高性能导向的车辆自动驾驶仪。 该自动驾驶仪应该不仅将车辆保持在路径上，而且应该从任何起始点将车辆有效地导向以 加入路径。
【专利附图】
【附图说明】
[0011] 图1A示出了在反馈控制下遵循路径的车辆。
[0012] 图1B示出了具有可能的路径的车辆，该路径可以由在大信号机制下使用小信号 反馈自动驾驶仪引起。
[0013] 图2A示出了具有在任意位置（x，y)、航向（Φ)和曲率（Θ)的特征的车辆；以及 用于车辆遵循的期望路径。
[0014] 图2B示出了遵循规划路径以加入期望路径的车辆。
[0015] 图3示出了对牵引场地边界内的器械的拖拉机规划的掉头路径。
[0016] 图4是包含反馈控制器和前馈控制器的路径规划自动驾驶仪的框图。
[0017] 图5是基于路径规划算法的反馈控制系统的框图。
[0018] 图6是在反馈模式下的反馈和前馈控制系统的框图。
[0019] 图7是在结合的反馈和前馈模式下的反馈和前馈控制系统的框图。
[0020] 图8是受约束的双积分器系统的反馈图。
[0021] 图9是表示在从受约束的所有可能的航向和曲率的初始条件下转向期间行进的 航迹距离的表面的图。
[0022] 图10A示出了用于车辆加入直线期望路径的示例路径。
[0023] 图10B示出了图10A所示的路径中时间与u、Θ和φ的关系。
[0024] 图11示出了两个示例性加入路径。
[0025] 图12是在迭代路径规划方法中使用的曲率与时间或距离的关系的第一示意图。
[0026] 图13是在迭代路径规划方法中使用的曲率与时间或距离的关系的第二示意图。
[0027] 图14是在迭代路径规划方法中最终航迹误差与迭代次数的关系的示例性示意 图。
[0028] 图15A至图15D示出了掉头路径。
[0029] 图16A和图16B示出了速度依赖于曲率限值和曲率变化率限值的示意图。
[0030] 图17示出了对弯曲的期望路径的接近。
[0031] 图18A和图18B示出了不一定最佳的加入路径。
【具体实施方式】
[0032] 简介
[0033] 以下描述的路径规划自动驾驶仪与常规自动驾驶仪相比提供了更复杂的车辆导 向。图2A示出了在由任意位置（x，y)、航向（Φ)和曲率（Θ)限定的配置下的车辆205 ; 以及用于车辆要遵循的期望路径210。路径规划自动驾驶仪创建了使车辆从任意初始配置 (y，Φ，Θ )移动至期望路径的有效加入路径。初始配置可以靠近或远离期望路径。例如，图 2B示出了遵循规划路径220的车辆215要加入期望路径225。
[0034] 与常规自动驾驶仪不同，路径规划自动驾驶仪提供对约束（例如最大允许转向角 或最大转向角变化率）的直接控制。这些约束可以是车速的函数。路径规划技术可作为反 馈自动驾驶仪的补充前馈能力来实现或可替代所有反馈。
[0035] 许多高级策略可应用于路径规划自动驾驶仪。可以以高达指定的最大角例如 α (图2B中虚线230和期望路径225之间的角）加入路径。可以以特定的方向（在掉头中 使用的能力）加入期望路径。图3示出了对在场地边界315内牵引器械310的拖拉机305 所规划的掉头路径。规划路径320被设计成保持器械310与边界之间的距离大于器械宽度 的一半（W)。规划路径320在与拖拉机当前的航向相反的方向上加入期望路径325。
[0036] 系统概述
[0037] 路径规划自动驾驶仪计算用于车辆要遵循的有效路径。该自动驾驶仪使用纯反馈 或结合前馈和反馈的控制结构来沿着该路径导向车辆。描述了两种不同的用于计算有效路 径的方法。
[0038] 说明路径规划自动驾驶仪的用处的典型场景是加入期望路径。例如，在农业上，拖 拉机操作者会希望加入用于种植、喷雾或收割的预定的直线。不要求在任何特定的位置处 加入该直线。反之，操作者会只是想在舒适操作的约束下以最短的时间加入该直线。
[0039] 将车辆从其初始位置导向到期望直线的加入路径具有连续的曲率，该曲率被限制 为小于最大的曲率限值。这意味着加入路径没有急转弯且考虑了在车辆如何快速改变其曲 率和航向上的实际限制。
[0040] 对于轮式车辆，曲率由车轮转向角和轴距来确定。路径规划自动驾驶仪可以与具 有两个可转向的车轮和两个或更多个固定的车轮的类似卡车的车辆、中心铰链车辆、履带 式车辆、三轮车等一起使用。一些车辆控制系统接受曲率作为直接输入而不接受转向角作 为直接输入。
[0041] 在假设车速恒定的情况下计算加入路径。每当速度改变时，可以重新计算加入路 径，或者，不管车速是否改变，周期性地重新计算加入路径。
[0042] 加入路径被构建成一系列直线段、圆形段和回旋曲线段。回旋曲线用作加入不同 的恒定曲率的段；例如加入直线和圆弧。回旋曲线的曲率随着曲线长度而线性改变。回旋 曲线也称为欧拉（Euler)螺旋线或科纽（Cornu)螺旋线。也可使用对回旋曲线的合理近似。
[0043] 图4是包含反馈控制器和前馈控制器的路径规划自动驾驶仪的框图。该自动驾驶 仪包括具有反馈控制器、前馈控制器、处理器和存储器的主单元405。该主单元的功能块可 由通用微处理器、专用集成电路、或两者的结合来实现。
[0044] 该主单元接收来自GNSS (全球导航卫星系统）接收器410和传感器415的输入。 该主单元发送输出给执行器420。GNSS接收器410可使用来自全球定位系统、全球导航卫 星系统、北斗/指南针或类似的卫星星座的任意组合的信号。该接收器可使用由广域增强 系统和/或实时动态定位技术提供的微分校正来提高精确度。该接收器为主单元提供定期 的位置、速度和时间的更新。
[0045] 传感器415包括报告车辆转向角的转向传感器和向主单元报告报告车辆航向的 航向传感器。航向可由GNSS和惯性测量的结合来感测。转向角可由放置在转向连杆上的 机械传感器或转向连杆的惯性测量来感测。如果单独从GNSS测量值估算车辆踪迹的航向 和曲率，则也可以省略传感器415。
[0046] 执行器420包括用来操纵车辆的转向执行器。转向执行器可以转动车辆的转向轮 或方向盘、或者其可以直接控制转向机构。在一些车辆上，转向执行器操作液压阀来控制转 向连杆而无需移动操作者的转向轮。
[0047] 路径规划自动驾驶仪可包括许多其它功能、传感器和执行器。为简单起见，图4中 仅包含与路径规划操作相关的功能、传感器和执行器。显示和用户输入设备是在图4中省 略的部件的例子。例如，用户输入对系统调整是必要的。图4的系统可在不同的模式（图 5、图6和图7中示出了其中的三种模式）下运行。
[0048] 路径规划自动驾驶仪可在反馈控制系统中使用路径规划技术或者其可在结合的 反馈/前馈控制系统中使用路径规划。图5是基于路径规划算法的反馈控制系统的框图。 在图5中，反馈控制器505通过向车辆510发送曲率变化率（u)或曲率（Θ)命令信号来控 制车辆510。由GNSS接收器、航向传感器和/或其它传感器测量的实际车辆航向（Φ)和航 迹误差（y)，与期望的曲率、航向和踪迹（θ，Φ， 7)相结合，以对控制器505形成误差信号。
[0049] 以下详细描述由控制器505确定u或Θ的方法。在这一点上可以说，控制器重复 计算加入路径以将车辆从其当前位置导向至期望路径，以及产生使车辆遵循加入路径的控 制命令。该控制器并不基于常规的比例积分微分（PID)技术。
[0050] 图6和图7分别是在反馈模式以及结合的反馈和前馈模式下的反馈和前馈控制系 统的框图。在图6和图7中，反馈控制器605通过向车辆610发送曲率（Θ)命令来控制车 辆610 (可替选地，反馈控制器615可向车辆发送曲率变化率（u)命令）。由GNSS接收器、 航向传感器和/或其它传感器测量的实际车辆航向（Φ)和航迹误差（y)，与期望路径620 的参数相结合，以对控制器605形成误差信号。与反馈控制器505不同，反馈控制器605可 以是常规的PID控制器。
[0051] 在图6和图7中，实线箭头表示有效的信号路径，而虚线箭头表示关闭的信号路 径。因此，在图6中，反馈控制器利用由测量的路径和期望路径之间的差异导出的误差信号 来生成反馈控制信号Θ ΡΒ。在车辆接近期望路径时可使用该模式。
[0052] 当车辆远离期望路径时，控制系统切换至图7所示的模式。车辆远离期望路径的 典型情形是在车辆被设立在该路径上之前占用自动驾驶仪系统时。例如，已经在农田上形 成一组平行地带以导向拖拉机，以及拖拉机操作者可能希望自动驾驶仪导向拖拉机到最近 的地带。当这种情况发生时，路径加入规划器615计算从车辆的当前位置到期望路径的有 效路径。
[0053] 路径加入规划器615计算加入路径以将车辆导向到期望路径，并提供与反馈控制 信号9 FB相结合的前馈控制信号9FF以导向车辆610。为了反馈控制器605的目的，加入 路径是直到到达初始期望路径620的期望路径。因此，在此期间由路径加入规划器导出反 馈参考信号（Φ，7)。该路径加入规划器是结合图4描述的前馈机构。以下详细描述由前馈 路径加入规划器所用的方法。
[0054] 可替选地，一旦路径加入规划器615计算加入路径，继续到初始期望路径（从加入 路径与初始期望路径相遇的点开始）的加入路径就可变为新的期望路径。于是，可以丢弃 在加入路径与初始期望路径相遇的点之前的初始期望路径。
[0055] 具有类似于如图6和图7的路径规划、反馈和前馈控制系统的路径规划、反馈和前 馈控制系统的自动驾驶仪可以使用开关以在模式之间交替。该模式基于车辆与期望路径 620的距离来选择；靠近该路径时使用反馈控制，远离该路径时使用结合的前馈和反馈控 制。决定"近"和"远"的准则可以基于特征长度，例如车辆轴距或在特征响应时间内行进 的距离、或可以基于是否达到转向角限值或转向角变化率限值。
[0056] 可替选地，控制系统可在图7所示的模式下连续运行。当车辆靠近期望路径（620) 时，来自路径加入规划器的输出信号小以及可对反馈控制系统产生微不足道的实际影响。 因此，路径加入规划器可保持一直在线。
[0057] 图5至图7示出，测量数据（Φ，7)与期望路径数据结合，然后被发送给反馈控制 器505或605。然而，可替选地，测量数据在与期望路径数据结合之前，其可由观测器（例如 卡尔曼滤波器）估测。
[0058] 以下详细描述两种不同的用于计算有效加入路径的方法：积分方法和迭代方法。 首先在规划路径加入直线y = 〇方面描述各方法。然后讨论加入曲线的扩展。在表1中定 义了在以下讨论中使用的符号。
【权利要求】
1. 一种用于将车辆导向到期望路径的方法，包括： 创建从初始车辆配置到所述期望路径的加入路径，所述加入路径包括第一最佳转弯； 以及， 使用自动驾驶仪以沿着所述加入路径引导所述车辆。
2. 如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之前的 直线。
3. 如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之后的 第二最佳转弯。
4. 如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之后的 直线和在所述直线之后的第二最佳转弯。
5. 如权利要求1所述的方法，其中，所述第一最佳转弯包括第一回旋曲线。
6. 如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之 前的圆弧。
7. 如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之 后的第二回旋曲线。
8. 如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之 后的圆弧和在所述圆弧之后的第二回旋曲线。
9. 如权利要求1所述的方法，其中，所述期望路径是直线。
10. 如权利要求9所述的方法，其中，所述加入路径离所述直线不超过最大角Φ_。
11. 如权利要求1所述的方法，其中，所述加入路径不超过最大曲率θ_。
12. 如权利要求1所述的方法，其中，所述加入路径不超过最大曲率变化率μ。
13. 如权利要求1所述的方法，其中，创建加入路径包括： 在从所述初始车辆配置到具有平行于所述期望路径的航向和零曲率的配置的最佳转 弯期间，计算垂直于所述期望路径所穿过的距离3#,,,氏），其中Φ〇和叭是所述初始车辆 配置中所述车辆的航向和曲率；以及， 如果所述初始车辆配置中相对于所述期望路径的所述车辆的航迹误差等于] 则计算从所述初始车辆配置到所述期望路径的所述第一最佳转弯。
14. 如权利要求13所述的方法，还包括，如果Φ。等于最大接近角Φ_且0。= 〇,则 使航向为Φ。的直线在所述第一最佳转弯的前面。
15. 如权利要求1所述的方法，其中，创建加入路径包括： 计算从所述初始车辆配置到相对于所述期望路径以最大角Φ_倾斜的直线的所述第 一最佳转弯； 继续所述直线直到到达在到具有平行于所述期望路径的航向和零曲率的配置的最佳 转弯期间垂直于所述期望路径所穿过的距离？其中Φ和Θ是所述车辆的航向和曲 率；以及， 计算从所述直线到所述期望路径的第二最佳转弯。
16. 如权利要求1所述的方法，其中，创建加入路径包括： 计算从所述初始车辆配置到相对于所述期望路径以最大角φ_倾斜的直线的所述第 一最佳转弯；以及， 计算从所述第一最佳转弯上的点到所述期望路径的第二最佳转弯。
17. 如权利要求16所述的方法，其中，所述点距离所述期望路径为，其中]#，的 是在到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间垂直于所述期望 路径所穿过的距离，其中Φ和Θ是所述车辆的航向和曲率。
18. 如权利要求1所述的方法，还包括： 在车速改变时，创建从当前车辆配置到所述期望路径的新的加入路径。
19. 如权利要求1所述的方法，还包括： 在最大的允许航向Φ_改变时，创建从当前车辆配置到所述期望路径的新的加入路 径。
20. 如权利要求1所述的方法，还包括： 在最大的允许曲率Θ _改变时，创建从当前车辆配置到所述期望路径的新的加入路 径。
21. 如权利要求1所述的方法，还包括： 在最大的允许曲率变化率μ改变时，创建从当前车辆配置到所述期望路径的新的加 入路径。
22. 如权利要求1所述的方法，还包括： 定期创建从当前车辆配置到所述期望路径的新的加入路径。
23. 如权利要求1所述的方法，其中所述自动驾驶仪包括反馈控制器和前馈路径加入 规划器。
24. 如权利要求1所述的方法，其中，所述自动驾驶仪包括全球导航卫星系统GNSS接收 器。
25. 如权利要求1所述的方法，其中，所述自动驾驶仪包括车辆航向传感器。
26. 如权利要求1所述的方法，其中，所述自动驾驶仪包括车辆转向角传感器。
27. 如权利要求1所述的方法，其中，所述自动驾驶仪包括车辆转向执行器。
28. 如权利要求1所述的方法，其中，所述自动驾驶仪包括微处理器和存储器。
29. 如权利要求1所述的方法，其中，所述初始车辆配置包括初始航向和近似零曲率， 所述期望路径是平行于所述初始航向但具有与所述初始航向相差180度的航向的直线，以 及所述加入路径被构建成使其保持在垂直于所述期望路径的边界内。
30. -种用于将车辆导向到期望路径的方法，包括： 创建从初始车辆配置到所述期望路径的加入路径，所述加入路径包括在第一方向上的 第一转弯和在与所述第一方向相反的第二方向上的第二转弯；以及， 使用自动驾驶仪以沿着所述加入路径引导所述车辆。
31. 如权利要求30所述的方法，其中，所述期望路径是直线。
32. 如权利要求31所述的方法，其中，所述加入路径离所述直线不超过最大角Φ_。
33. 如权利要求30所述的方法，其中，所述加入路径不超过最大曲率θ_。
34. 如权利要求30所述的方法，其中，所述加入路径不超过最大曲率变化率μ。
35. 如权利要求30所述的方法，其中，所述加入路径还包括直线。
36. 如权利要求30所述的方法，其中，所述加入路径具有末端，在该末端处所述加入路 径和所述期望路径具有相同的航向和曲率，且这两个路径之间的航迹误差最小。
37. 如权利要求30所述的方法，其中，所述第一转弯包括回旋曲线。
38. 如权利要求30所述的方法，其中，所述第二转弯包括回旋曲线。
39. -种用于将车辆导向到期望路径的方法，包括： 创建从初始车辆配置到与所述期望路径相切的直线的第一加入路径，所述直线在所述 期望路径上与所述初始车辆配置最近的点处相切，所述加入路径包括第一最佳转弯； 自所述第一加入路径上的点重复以上步骤以建立整体连续曲率的加入路径，所述整体 连续曲率的加入路径在足够靠近所述期望路径处结束，使得能够使用反馈技术来将所述车 辆从所述整体加入路径的末端导向到所述期望路径；以及， 使用自动驾驶仪以沿着所述加入路径引导所述车辆。
40. -种车辆自动驾驶仪，包括： 微处理器和存储器； 全球导航卫星系统GNSS接收器，所述GNSS接收器提供车辆位置、速度和时间估计给所 述微处理器； 反馈控制器和前馈控制器，所述反馈控制器基于测量的航向和y偏移数据和期望路径 生成车辆曲率命令，所述前馈控制器基于计算的从所述车辆位置到所述期望路径的加入路 径生成车辆曲率命令；以及， 车辆转向执行器。
41. 如权利要求40所述的自动驾驶仪，其中，所述反馈控制器和所述前馈控制器位于 所述微处理器内。
42. 如权利要求40所述的自动驾驶仪，其中，所述加入路径不超过最大的允许曲率 ΘΜΧ。
43. 如权利要求42所述的自动驾驶仪，其中，所述最大的允许曲率θ_是根据车辆运 行期间记录的数据估计得到的。
44. 如权利要求40所述的自动驾驶仪，其中，所述加入路径不超过最大的允许曲率变 化率μ。
45. 如权利要求44所述的自动驾驶仪，其中，所述最大的允许曲率变化率μ是根据车 辆运行期间记录的数据估计得到的。
【文档编号】G05D1/02GK104303123SQ201380024793
【公开日】2015年1月21日 申请日期:2013年5月8日 优先权日:2012年5月11日
【发明者】J·W·皮克, S·普雷尼斯 申请人:天宝导航有限公司