CN104303123A - 路径规划自动驾驶仪 - Google Patents

Abstract

即使车辆远离期望路径，路径规划自动驾驶仪也可导向车辆。

Description

路径规划自动驾驶仪

相关申请

本申请涉及并要求题目为“路径规划自动驾驶仪”的第13/470,028号美国申请的优先权，该美国申请以全文引用的方式并入本文。

技术领域

本发明主要涉及用于车辆的自动控制系统。

背景技术

与草率的操作相比，根据精确的期望路径来导向农用拖拉机、施工车辆、矿用卡车、军用坦克或其它大型车辆通常会省时和节能。精确的农用拖拉机控制，例如，整体来说给农民、消费者和社会带来好处。农民能够更高效地劳动，且在肥料和杀虫剂上花费更少的钱。消费者享受高质量的低价产品，以及农场和农药的精确使用减少了浪费和过量的污水。

能够高精度、重复导向农用拖拉机、喷洒车、收割机等的自动驾驶仪系统给农场和农药使用带来好处。例如，农田通常由90英尺宽或甚至更大的水栅喷雾。当使用这么宽的水栅时，就需要谨慎以使从一个喷雾片到下一个喷雾片重叠几英尺。然而，该重叠可在拖拉机或喷洒车配备高性能的自动驾驶仪时减小至仅数英寸。由非双喷雾片边缘产生的节约在大面积农场上迅速增加。

现有的自动驾驶仪系统足以将车辆保持在预定路径上。这些自动驾驶仪是基于反馈控制技术的以及其可以使大型的农用拖拉机例如遵循航迹误差在一或两英寸内的线。图1A示出了在反馈控制下遵循路径的车辆；车辆105遵循路径110。当航迹误差XTE小于车辆轴距b时，该车辆处于小信号机制或“靠近”期望路径。

当车辆远离期望路径时，反馈自动驾驶仪表现不好。出现这样的常见情况包括从很远的地方加入路径或者在路径的末端调转方向以加入附近的路径。图1B示出了具有可能的路径125的车辆，该路径125可以由在大信号机制下使用小信号反馈自动驾驶仪引起。通过反馈自动驾驶仪将车辆115向路径120导向。当航迹误差大于车辆轴距时，该车辆处于大信号机制或离期望路径“远”。

针对常规的反馈自动驾驶仪，存在小信号机制下的性能和大信号机制下的可接受行为之间的权衡。例如，在小信号机制下保持小航迹误差的高反馈增益可导致陡峭的接近期望路径并在加入如图1B所示的路径时发生振荡。常规的自动驾驶仪在遭遇与期望路径之间的大偏差时，通过调用启发式的限制来避免不良的大信号行为。这些限制意味着大信号导向并非想象的那样有效。

在以上例子中，轴距作为特征长度尺度的例子用来区分小信号机制和大信号机制。也可以使用其它特征长度，例如，在特征自动驾驶仪响应时间内移动的距离。通常，小信号机制是指车辆自动驾驶仪表现为线性的时不变系统的任何情况。另一方面，大信号机制是指出现非线性行为(例如，转向角限值或转向角变化率限值)的情况。

不管车辆靠近或远离期望路径，都需要能够提供高性能导向的车辆自动驾驶仪。该自动驾驶仪应该不仅将车辆保持在路径上，而且应该从任何起始点将车辆有效地导向以加入路径。

附图说明

图1A示出了在反馈控制下遵循路径的车辆。

图1B示出了具有可能的路径的车辆，该路径可以由在大信号机制下使用小信号反馈自动驾驶仪引起。

图2A示出了具有在任意位置(x,y)、航向(φ)和曲率(θ)的特征的车辆；以及用于车辆遵循的期望路径。

图2B示出了遵循规划路径以加入期望路径的车辆。

图3示出了对牵引场地边界内的器械的拖拉机规划的掉头路径。

图4是包含反馈控制器和前馈控制器的路径规划自动驾驶仪的框图。

图5是基于路径规划算法的反馈控制系统的框图。

图6是在反馈模式下的反馈和前馈控制系统的框图。

图7是在结合的反馈和前馈模式下的反馈和前馈控制系统的框图。

图8是受约束的双积分器系统的反馈图。

图9是表示在从受约束的所有可能的航向和曲率的初始条件下转向期间行进的航迹距离的表面的图。

图10A示出了用于车辆加入直线期望路径的示例路径。

图10B示出了图10A所示的路径中时间与u、θ和φ的关系。

图11示出了两个示例性加入路径。

图12是在迭代路径规划方法中使用的曲率与时间或距离的关系的第一示意图。

图13是在迭代路径规划方法中使用的曲率与时间或距离的关系的第二示意图。

图14是在迭代路径规划方法中最终航迹误差与迭代次数的关系的示例性示意图。

图15A至图15D示出了掉头路径。

图16A和图16B示出了速度依赖于曲率限值和曲率变化率限值的示意图。

图17示出了对弯曲的期望路径的接近。

图18A和图18B示出了不一定最佳的加入路径。

具体实施方式

简介

以下描述的路径规划自动驾驶仪与常规自动驾驶仪相比提供了更复杂的车辆导向。图2A示出了在由任意位置(x,y)、航向(φ)和曲率(θ)限定的配置下的车辆205；以及用于车辆要遵循的期望路径210。路径规划自动驾驶仪创建了使车辆从任意初始配置(y,φ,θ)移动至期望路径的有效加入路径。初始配置可以靠近或远离期望路径。例如，图2B示出了遵循规划路径220的车辆215要加入期望路径225。

与常规自动驾驶仪不同，路径规划自动驾驶仪提供对约束(例如最大允许转向角或最大转向角变化率)的直接控制。这些约束可以是车速的函数。路径规划技术可作为反馈自动驾驶仪的补充前馈能力来实现或可替代所有反馈。

许多高级策略可应用于路径规划自动驾驶仪。可以以高达指定的最大角例如α(图2B中虚线230和期望路径225之间的角)加入路径。可以以特定的方向(在掉头中使用的能力)加入期望路径。图3示出了对在场地边界315内牵引器械310的拖拉机305所规划的掉头路径。规划路径320被设计成保持器械310与边界之间的距离大于器械宽度的一半(W)。规划路径320在与拖拉机当前的航向相反的方向上加入期望路径325。

系统概述

路径规划自动驾驶仪计算用于车辆要遵循的有效路径。该自动驾驶仪使用纯反馈或结合前馈和反馈的控制结构来沿着该路径导向车辆。描述了两种不同的用于计算有效路径的方法。

说明路径规划自动驾驶仪的用处的典型场景是加入期望路径。例如，在农业上，拖拉机操作者会希望加入用于种植、喷雾或收割的预定的直线。不要求在任何特定的位置处加入该直线。反之，操作者会只是想在舒适操作的约束下以最短的时间加入该直线。

将车辆从其初始位置导向到期望直线的加入路径具有连续的曲率，该曲率被限制为小于最大的曲率限值。这意味着加入路径没有急转弯且考虑了在车辆如何快速改变其曲率和航向上的实际限制。

对于轮式车辆，曲率由车轮转向角和轴距来确定。路径规划自动驾驶仪可以与具有两个可转向的车轮和两个或更多个固定的车轮的类似卡车的车辆、中心铰链车辆、履带式车辆、三轮车等一起使用。一些车辆控制系统接受曲率作为直接输入而不接受转向角作为直接输入。

在假设车速恒定的情况下计算加入路径。每当速度改变时，可以重新计算加入路径，或者，不管车速是否改变，周期性地重新计算加入路径。

加入路径被构建成一系列直线段、圆形段和回旋曲线段。回旋曲线用作加入不同的恒定曲率的段；例如加入直线和圆弧。回旋曲线的曲率随着曲线长度而线性改变。回旋曲线也称为欧拉(Euler)螺旋线或科纽(Cornu)螺旋线。也可使用对回旋曲线的合理近似。

图4是包含反馈控制器和前馈控制器的路径规划自动驾驶仪的框图。该自动驾驶仪包括具有反馈控制器、前馈控制器、处理器和存储器的主单元405。该主单元的功能块可由通用微处理器、专用集成电路、或两者的结合来实现。

该主单元接收来自GNSS(全球导航卫星系统)接收器410和传感器415的输入。该主单元发送输出给执行器420。GNSS接收器410可使用来自全球定位系统、全球导航卫星系统、北斗/指南针或类似的卫星星座的任意组合的信号。该接收器可使用由广域增强系统和/或实时动态定位技术提供的微分校正来提高精确度。该接收器为主单元提供定期的位置、速度和时间的更新。

传感器415包括报告车辆转向角的转向传感器和向主单元报告报告车辆航向的航向传感器。航向可由GNSS和惯性测量的结合来感测。转向角可由放置在转向连杆上的机械传感器或转向连杆的惯性测量来感测。如果单独从GNSS测量值估算车辆踪迹的航向和曲率，则也可以省略传感器415。

执行器420包括用来操纵车辆的转向执行器。转向执行器可以转动车辆的转向轮或方向盘、或者其可以直接控制转向机构。在一些车辆上，转向执行器操作液压阀来控制转向连杆而无需移动操作者的转向轮。

路径规划自动驾驶仪可包括许多其它功能、传感器和执行器。为简单起见，图4中仅包含与路径规划操作相关的功能、传感器和执行器。显示和用户输入设备是在图4中省略的部件的例子。例如，用户输入对系统调整是必要的。图4的系统可在不同的模式(图5、图6和图7中示出了其中的三种模式)下运行。

路径规划自动驾驶仪可在反馈控制系统中使用路径规划技术或者其可在结合的反馈/前馈控制系统中使用路径规划。图5是基于路径规划算法的反馈控制系统的框图。在图5中，反馈控制器505通过向车辆510发送曲率变化率(u)或曲率(θ)命令信号来控制车辆510。由GNSS接收器、航向传感器和/或其它传感器测量的实际车辆航向(φ)和航迹误差(y)，与期望的曲率、航向和踪迹(θ,φ,y)相结合，以对控制器505形成误差信号。

以下详细描述由控制器505确定u或θ的方法。在这一点上可以说，控制器重复计算加入路径以将车辆从其当前位置导向至期望路径，以及产生使车辆遵循加入路径的控制命令。该控制器并不基于常规的比例积分微分(PID)技术。

图6和图7分别是在反馈模式以及结合的反馈和前馈模式下的反馈和前馈控制系统的框图。在图6和图7中，反馈控制器605通过向车辆610发送曲率(θ)命令来控制车辆610(可替选地，反馈控制器615可向车辆发送曲率变化率(u)命令)。由GNSS接收器、航向传感器和/或其它传感器测量的实际车辆航向(φ)和航迹误差(y)，与期望路径620的参数相结合，以对控制器605形成误差信号。与反馈控制器505不同，反馈控制器605可以是常规的PID控制器。

在图6和图7中，实线箭头表示有效的信号路径，而虚线箭头表示关闭的信号路径。因此，在图6中，反馈控制器利用由测量的路径和期望路径之间的差异导出的误差信号来生成反馈控制信号θ_FB。在车辆接近期望路径时可使用该模式。

当车辆远离期望路径时，控制系统切换至图7所示的模式。车辆远离期望路径的典型情形是在车辆被设立在该路径上之前占用自动驾驶仪系统时。例如，已经在农田上形成一组平行地带以导向拖拉机，以及拖拉机操作者可能希望自动驾驶仪导向拖拉机到最近的地带。当这种情况发生时，路径加入规划器615计算从车辆的当前位置到期望路径的有效路径。

路径加入规划器615计算加入路径以将车辆导向到期望路径，并提供与反馈控制信号θ_FB相结合的前馈控制信号θ_FF以导向车辆610。为了反馈控制器605的目的，加入路径是直到到达初始期望路径620的期望路径。因此，在此期间由路径加入规划器导出反馈参考信号(φ,y)。该路径加入规划器是结合图4描述的前馈机构。以下详细描述由前馈路径加入规划器所用的方法。

可替选地，一旦路径加入规划器615计算加入路径，继续到初始期望路径(从加入路径与初始期望路径相遇的点开始)的加入路径就可变为新的期望路径。于是，可以丢弃在加入路径与初始期望路径相遇的点之前的初始期望路径。

具有类似于如图6和图7的路径规划、反馈和前馈控制系统的路径规划、反馈和前馈控制系统的自动驾驶仪可以使用开关以在模式之间交替。该模式基于车辆与期望路径620的距离来选择；靠近该路径时使用反馈控制，远离该路径时使用结合的前馈和反馈控制。决定“近”和“远”的准则可以基于特征长度，例如车辆轴距或在特征响应时间内行进的距离、或可以基于是否达到转向角限值或转向角变化率限值。

可替选地，控制系统可在图7所示的模式下连续运行。当车辆靠近期望路径(620)时，来自路径加入规划器的输出信号小以及可对反馈控制系统产生微不足道的实际影响。因此，路径加入规划器可保持一直在线。

图5至图7示出，测量数据(φ,y)与期望路径数据结合，然后被发送给反馈控制器505或605。然而，可替选地，测量数据在与期望路径数据结合之前，其可由观测器(例如卡尔曼滤波器)估测。

以下详细描述两种不同的用于计算有效加入路径的方法：积分方法和迭代方法。首先在规划路径加入直线y＝0方面描述各方法。然后讨论加入曲线的扩展。在表1中定义了在以下讨论中使用的符号。

(x,y)	位置
		φ	航向
θ	曲率
		u	曲率变化率

表1：符号定义

这些术语用来描述车辆配置和车辆遵循的路径的特征。

积分方法

通过以下方程(1)描述在x-y平面中的连续曲率轨迹的时间演化：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\φ}\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right]=\left[\begin{array}{c}\mathrm{\υ}\·\cos \mathrm{\φ}\left(t\right)\\ \mathrm{\υ}\·\sin \mathrm{\φ}\left(t\right)\\ \mathrm{\υ}\·\mathrm{\θ}\left(t\right)\\ u\left(t\right)\end{array}\right]---\left(1\right)$

遵循由方程(1)限定的轨迹的车辆具有航向φ、曲率θ和曲率变化率u。车辆的速度为υ。

待解决的问题是：在约束下，找出在最短的时间内将车辆导向到目标线的路径。待加入的目标线为x轴(y＝0)以及从整体上考虑x位置会降低。由于方程(1)不随时间发生改变，因此初始时间可设置为t＝0。

最佳控制问题的正式表述是：针对从t＝0至t＝t_f的所有时间找出u的分段的连续解，使得满足以下约束(2)-(6)且最小化最终时间t_f。

$\left[\begin{array}{c}y\left(0\right)\\ \mathrm{\φ}\left(0\right)\\ \mathrm{\θ}\left(0\right)\end{array}\right]=\left[\begin{array}{c}{y}_0\\ {\mathrm{\φ}}_0\\ {\mathrm{\θ}}_0\end{array}\right]---\left(2\right)$

|φ₀|≤π    (3)

$\left[\begin{array}{c}y\left(t_f\right)\\ \mathrm{\φ}\left(t_f\right)\\ \mathrm{\θ}\left(t_f\right)\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\φ}}_f\\ 0\end{array}\right]---\left(4\right)$

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}y\left(t\right)\\ \mathrm{\φ}\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right]=\left[\begin{array}{c}\mathrm{\υ}\·\sin \mathrm{\φ}\left(t\right)\\ \mathrm{\υ}\·\mathrm{\θ}\left(t\right)\\ \mathrm{\μ}\·u\left(t\right)\end{array}\right]---\left(5\right)$

|θ(t)|≤θ_max>0    (6)

此处θ_max是最大的允许曲率以及μ是最大的允许曲率变化率。μ归一化u，使得|u|≤1(注意：μ>0)。方程(2)和方程(3)表示初始条件；方程(4)表示最终条件。方程(5)表示该问题的动态范围，而方程(6)是曲率的约束。

物理上，φ＝φ±n·2π，因此φ_f＝n·2π，其中n是整数。在一些情况下，航向会受限制。例如，可以要求车辆以不大于距离该直线的最大角的航向接近目标线。在这种情况下，|φ(t)|≤φ_max，其中0<φ_max≤π。

上述问题是复杂的。与其直接解决它，不如更容易地考虑一个更简单的问题：计算将车辆从其初始航向和曲率导向到某一期望的最终航向和零曲率的路径。换言之，忽略y坐标或航迹误差。现在控制问题的表述变为：针对从t＝0至t＝t_f的所有时间找出u(-1≤u≤1)的分段的连续解，使得满足以下约束(7)-(10)且最小化最终时间t_f。

$\left[\begin{array}{c}\mathrm{\&phi;}\left(0\right)\\ \mathrm{\&theta;}\left(0\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&phi;}}_0\\ {\mathrm{\&theta;}}_0\end{array}\right]---\left(7\right)$

$\left[\begin{array}{c}\mathrm{\&phi;}\left(t_f\right)\\ \mathrm{\&theta;}\left(t_f\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&phi;}}_f\\ 0\end{array}\right]---\left(8\right)$

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}\mathrm{\&phi;}\left(t\right)\\ \mathrm{\&theta;}\left(t\right)\end{array}\right]=\left[\begin{array}{c}\mathrm{\&upsi;}\&CenterDot;\mathrm{\&theta;}\left(t\right)\\ \mathrm{\&mu;}\&CenterDot;u\left(t\right)\end{array}\right]---\left(9\right)$

θ(t)∈{θ|-θ_max≤θ≤θ_max}    (10)

此处φ_f≠0表示任何期望的最终航向。

该问题是所谓的“双积分器”问题的例子，解决方案是公知的。根据φ和θ的值，最佳输入u(t)是0、1或-1。令Δφ＝φ-φ_f以及并定义：

$f_1\left(\mathrm{\&Delta;\&phi;}\right)=\sqrt{-\frac{2\mathrm{\&mu;}}{\mathrm{\&upsi;}}\mathrm{\&Delta;\&phi;}},$ 其中φ_c≤Δφ≤0    (11)

$f_2\left(\mathrm{\&Delta;\&phi;}\right)=-\sqrt{\frac{2\mathrm{\&mu;}}{\mathrm{\&upsi;}}\mathrm{\&Delta;\&phi;}},$ 其中0≤Δφ≤φ_c    (12)

其可合并为：

$f_3\left(\mathrm{\&Delta;\&phi;}\right)=-\mathrm{sgn}\left(\mathrm{\&Delta;\&phi;}\right)\sqrt{\frac{2\mathrm{\&mu;}}{\mathrm{\&upsi;}}\left|\mathrm{\&Delta;\&phi;}\right|},$ 其中|Δφ|≤φ_c    (13)

则最佳输入θ(t))为：

                    Δφ＝0         θ＝0

0        ，其中     Δφ>φ_c       θ＝-θ_max

                    Δφ<-φ_c      θ＝θ_max

                    Δφ≥φ_c       θ≠-θ_max

1        ，其中    0≤Δφ≤φ_c    θ>f₂(Δφ)          (14)

                    -φ_c≤Δφ<0    θ>f₁(Δφ)

                    Δφ≤φ_c          θ≠θ_max

-1       ，其中       -φ_c≤Δφ≤0      θ<f₁(Δφ)

                    0<Δφ≤φ_c        θ≤f₂(Δφ)

图8是受上述给定的约束的双积分器系统的反馈图。该反馈图示出了在(Δφ,θ)平面中的两个示例性轨迹。第一个轨迹从A处开始并在0处结束，而第二个轨迹从E处开始并在0处结束。从A处开始，曲率(θ)增大直到在C处达到最大曲率限值。在B处，曲率改变符号。从C处到D处曲率最大且恒定。曲率从D处到0处减小。第二个示例性轨迹从E处开始并在0处结束。从E处到F处，曲率减小。从F处到0处,曲率增大。

当最佳输入(14)应用于遵守方程(1)的车辆时，车辆执行最佳转弯。遵循图8的从A处到0处或从E处到0处的最佳平面轨迹(Δφ,θ)导致在(x,y)平面中的最佳转弯。从A处到0处的最佳平面轨迹(Δφ,θ)导致由回旋曲线(从A处到B处、B处到C处和D处到0处)和圆弧(从C处到D处)构成的在(x,y)平面中的最佳转弯。从E处到0处的最佳平面轨迹(Δφ,θ)导致由两个回旋曲线(从E处到F处和F处到0处)构成的在(x,y)平面中的最佳转弯。

因此，最佳转弯定义为通过将最佳双积分器输入(14)应用到连续曲率方程(1)中获得的路径。最佳转弯包括：(i)回旋曲线、(ii)在回旋曲线之前的圆弧、(iii)在回旋曲线之前的回旋曲线、或(iv)在回旋曲线之前的圆弧之前的回旋曲线。理论上，最佳转弯以最短的时间将车辆导向到目标配置(例如零航向和曲率)。实际上，最佳转弯至少可以用作在遵守航向、曲率、曲率变化率或其它约束的情况下创建从初始配置到目标配置的有效加入路径。

以上描述的方法可扩展至航迹误差。该控制问题表述变为：针对从t＝0至t＝t_f的所有时间，找出u(-1≤u≤1)的分段的连续解，使得满足以下约束(15)-(18)且最小化最终时间t_f。

$\left[\begin{array}{c}y\left(0\right)\\ \mathrm{\&phi;}\left(0\right)\\ \mathrm{\&theta;}\left(0\right)\end{array}\right]=\left[\begin{array}{c}{y}_0\\ {\mathrm{\&phi;}}_0\\ {\mathrm{\&theta;}}_0\end{array}\right]---\left(15\right)$

$\left[\begin{array}{c}y\left(t_f\right)\\ \mathrm{\&phi;}\left(t_f\right)\\ \mathrm{\&theta;}\left(t_f\right)\end{array}\right]=\left[\begin{array}{c}{y}_f\\ 0\\ 0\end{array}\right]---\left(16\right)$

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}y\left(t\right)\\ \mathrm{\&phi;}\left(t\right)\\ \mathrm{\&theta;}\left(t\right)\end{array}\right]=\left[\begin{array}{c}\mathrm{\&upsi;}\&CenterDot;\sin \mathrm{\&phi;}\left(t\right)\\ \mathrm{\&upsi;}\&CenterDot;\mathrm{\&theta;}\left(t\right)\\ \mathrm{\&mu;}\&CenterDot;u\left(t\right)\end{array}\right]---\left(17\right)$

θ(t)∈{θ|-θ_max≤θ≤θ_max}    (18)

目标线是x轴且φ_f＝0。该问题及其解不随y值变化，因此任何轨迹可在y值上变化，使得y_f＝0。同样地，如果y₀选择正确，则y_f＝0。使y_f＝0的y₀的值被定义为

或者，用另一种方式说：如果则从航向、曲率和航迹误差(φ₀,θ₀,y₀)的初始条件开始，到零航向和零曲率的最佳转弯将在x轴上结束。是通过对所有允许的φ值和θ值进行∫y(t)dt反向积分直到达到期望的航向和曲率得到的：

$y\left(t\right)=y_0+\underset{0}{\overset{t}{\&Integral;}}\mathrm{\&upsi;}\&CenterDot;\sin \left(\mathrm{\&phi;}\left(t\right)\right)\mathrm{dt}---\left(19\right)$

$\mathrm{\&phi;}\left(t\right)={\mathrm{\&phi;}}_0+\underset{0}{\overset{t}{\&Integral;}}\mathrm{\&upsi;}\&CenterDot;\mathrm{\&theta;}\left(t\right)\mathrm{dt}---\left(20\right)$

$\mathrm{\&theta;}\left(t\right)={\mathrm{\&theta;}}_0+\underset{0}{\overset{t}{\&Integral;}}\mathrm{\&mu;}\&CenterDot;u\left(t\right)\mathrm{dt}---\left(21\right)$

其中合适的u(t)由方程(14)确定。

图9是对所有允许的φ和θ的的图。在图9中，v＝1、μ＝0.6以及θ_max＝50度。所有y(t_f)＝φ(t_f)＝θ(t_f)＝0的解位于画出的表面。特别地，与图8类似的轨迹以暗曲线画在该表面上。

包括航迹误差的整个问题的解可分为两种情况：有航向约束和没有航向约束。当没有航向约束时：

其中φ_f＝2πn(n是整数)的选择决定第一转弯是向右还是向左。实际上，两种可能性都可以计算以及可使用产生轨迹更短的可能性。

解(22)给出了当时到目标线上的最佳转弯。如果则使最佳转弯朝向垂直目标线的线；即到航向为的线。朝向该垂直线的转弯，或随后沿着该垂直线的行进，产生与以下相交的轨迹

$\left[\begin{array}{c}\stackrel{\&OverBar;}{y}(\mathrm{\&phi;},\mathrm{\&theta;})\\ \mathrm{\&phi;}\\ \mathrm{\&theta;}\end{array}\right]---\left(23\right)$

因此总能找到达到最终期望条件的解。

当存在航向约束时：

解(24)给出了当时到目标线上的最佳转弯。如果则使最佳转弯朝向相对于目标线以角min(φ_max,π/2)倾斜的线。(min(x,y)等于x,y中的最小值。)朝向该线的转弯，或随后沿着该线的行进，产生与以下相交的轨迹

$\left[\begin{array}{c}\stackrel{\&OverBar;}{y}(\mathrm{\&phi;},\mathrm{\&theta;})\\ \mathrm{\&phi;}\\ \mathrm{\&theta;}\end{array}\right]---\left(25\right)$

因此总能找到达到最终期望条件的解。

解(22)或(24)可在图5的反馈控制器505中使用。

图10A示出了用于车辆加入直线期望路径的示例路径，而图10B示出了图10A所示的路径中时间与u、θ和φ的关系。在图10A中，x轴(y＝0)1005为待加入的目标线。其中示出了将车辆从初始条件(y₀＝2.5,φ₀＝0.3,θ₀＝0)导向到目标线的加入路径。该加入路径包括回旋曲线1010、圆弧1015、回旋曲线1020、直线1025、回旋曲线1030、圆弧1035和回旋曲线1040。在图10B中，不连续的线1045(例如)表示输入u，分段的连续线1050表示曲率θ，平滑的线1055表示航向φ，虚线1060表示曲率限制以及点划线1065表示航向限制。u的最佳解只有三个可能的值：1、0或-1。在圆形段和直线段内，曲率θ的斜度是平坦的；在回旋曲线段内θ的斜度反映了最大的允许转向角变化率。航向1055平滑变化；其最大斜度反映了最大的允许转向角。

加入路径可通过数值求解方程(1)以及在每个时间步长根据解(22)或(24)选择u来计算。可替选地，(19)、(20)或(21)可被分析积分以及x坐标可从以下获得：

$x\left(t\right)=x_0+\underset{0}{\overset{t}{\&Integral;}}\mathrm{\&upsi;}\&CenterDot;\cos \left(\mathrm{\&phi;}\left(t\right)\right)\mathrm{dt}---\left(26\right)$

以下步骤可用来计算从初始车辆配置[y₀φ₀θ₀]开始到目标线的加入路径：

(1)    (a)如果则转至步骤(5)。

(b)如果且φ₀＝-sgn(φ₀)·φ_max(或)且θ₀＝0，则转至步骤(3)。

(c)否则，继续步骤(2)。

(2)计算到相对于目标线以角±φ_max或倾斜的线上的最佳转弯。将该轨迹的这个部分结束的时间称为t₁。则：

(a)如果则转至步骤(5)。

(b)如果且则转至步骤(4)。

(c)否则，继续步骤(3)。

(3)增加直线直到到达时间t₃，其中t₃满足转至步骤(5)。

(4)找出t₂，t₀<t₂<t₁，其中t₂满足丢弃对所有t>t₂的时间计算的轨迹。继续步骤(5)。

(5)计算到目标线上的最佳转弯。

如果不用数学符号表示，从任何初始车辆航迹误差、航向和转向角开始，这些步骤为：

(1)     (a)如果车辆距离目标线的距离恰好是开始最后的转弯的距离，则转至步骤(5)。

(b)如果车辆距离目标线的距离不是开始最后的转弯的距离，并且如果航向是相对于目标线最大的允许角(或与目标线垂直)，则转至步骤(3)。

(c)否则，继续步骤(2)。

(2)计算到相对于目标线以最大的允许角倾斜或与目标线垂直的线上的最佳转弯。

(d)如果该转弯在距离目标线的距离恰好是开始最后的转弯的距离处结束，则转至步骤(5)。

(e)如果在该转弯过程中的某个点处，达到距离目标线的距离恰好是开始最后的转弯的距离，则转至步骤(4)。

(f)否则，继续步骤(3)。

(3)增加在当前航向的方向上在距离目标线的距离恰好是开始最后的转弯的距离处结束的直线；然后转至步骤(5)。

(4)找出达到恰好的距离的点。从该点继续，转至步骤(5)。

(5)计算到目标线的最佳转弯。

通过以上步骤(1)-(5)计算的加入路径包括一个或多个回旋曲线以及也可包括圆弧和直线。加入路径结构的一个示例是：回旋曲线-直线-回旋曲线-圆弧-回旋曲线。也可以是回旋曲线、圆弧和直线的其它22种组合。通常，从任意初始条件到目标线的加入路径包括：(i)最佳转弯、(ii)在最佳转弯之前的直线、(iii)在最佳转弯之前的最佳转弯、或(iv)在最佳转弯之前的直线之前的最佳转弯。注意步骤(2)和(3)中指定的线可具有零长度，在该情况下，一个最佳转弯之后直接是另一最佳转弯。

图11示出了两个示例性加入路径。在图11中，x轴1105是目标线。较长的加入路径包括至航向为的最佳转弯1110、直线1115和到目标线上的最佳转弯1120。较短的路径示出了如何遵循以上步骤(4)和(5)，在处中断至航向为的最佳转弯1125和从该点处增加到目标线的最佳转弯1130。在这两种情况下，是由车辆拥有者或操作者指定的最大的允许接近航向(φ_max)。

注意，对于图11中的较短的路径，或其它类似的路径，与根据步骤(2)计算完整的转弯并根据步骤(4)找出与相交的点不同，可以在计算转弯时检查每个点是否达到其它小的流程的变化无疑是可能的。

加入路径可由图6和图7的路径加入规划器使用积分方法来计算。

迭代方法

计算加入路径的替选方法是基于考虑例如在图12和图13中所示的曲率与时间(或距离)的关系的曲线图。在该方法中，朝向目标线的转弯被初始化并允许在一定的时间内进行。之后，计算转弯的结果，转弯是否在相反方向的结果可被构建成导致在具有零航向和曲率的目标线上结束的轨迹。如果没有实现满意的转弯，则以允许进行另一时间增量的初始转弯重复该过程。该迭代方法可以很方便地实现为微处理器的代码。与积分方法不同，迭代方法并不容易获得分析解。

图12是在迭代路径规划方法中使用的曲率与时间或距离的关系的第一示意图。在图12中，时间用作横坐标；速度假设为常量，因此作为替代也可以使用距离。在该示出的情况下，已经允许初始的正转弯进行直到点B，在该点处转弯在相反方向解除。转向角限值包含在曲率限值±φ_max中，这是因为转向角和曲率由类似卡车的车辆中的轴距关联和由其它车辆类型中的类似参数关联。转向角变化率限值包含在θ(t)的最大斜度中。在θ(t)曲线下的阴影区域(即θ(t)的时间积分)与航向变化成正比；也可参见上述(20)。

现在，详细考虑图12：在时间t＝0处，曲率θ＝0、航向φ＝φ_i以及航迹误差y＝y_i。正转弯以最大转向角变化率开始直到θ达到θ_max。θ保持在θ_max直到点B。对于每个点B，测试在点C的航向以判断其是否超过最大航向限制。如果超过最大航向限制，则点B增加得过远，且作为替代需要使用之前的增量。在图12中，没有达到最大航向限制且曲率从点B逐步减小直到达到最小限值-θ_max。以下讨论的图13示出了达到最大航向限制的情况。

对于每个点B，图12中的部分图表示负曲率，即，点C的右边部分被构建成具有足够的区域以使得最终的航向φ_f＝0。换言之，初始航向φ_i、累计的正航向变化(用“+”表示的阴影区域)与累计的负航向变化(用“-”表示的阴影区域)之和等于0。

接着，计算最终航迹误差y_f。如果y_f＝0，则问题已经解决；若y_f≠0，则点B被增加一个时间步长并重复刚刚概述的过程。

图13是在迭代路径规划方法中使用的曲率与时间或距离的关系的第二示意图。图13与图12类似。然而，在图13中，初始曲率不是0，因此开始条件为(θ_i≠0,φ_i,y_i)。曲率未达到θ_max，但是航向达到其最大值，这导致在点B之前的零曲率(恒定航向)线。点B在时间上增加直到负曲率可以被构建以引起最终条件(θ_f＝0,φ_f＝0,y_f＝0)。

该迭代方法并不提供决定初始正或负(即左或由)转弯是否引起较短的加入路径的先验的机制。由给定开始条件的初始正转弯获得的结果可与由相同条件的初始负转弯获得的结果进行比较。于是可在实际中使用引起较短加入路径的初始转弯。

该迭代方法执行直到最终的航迹误差为0或至少最小化至可由常规的反馈自动驾驶仪处理的范围内。小于车辆轴距或一些其它特征长度的航迹误差可足够接近。图14是最终航迹误差与迭代次数的关系的示例性曲线图。如图14所示，最终航迹误差y_f可随着点B的进一步迭代初步增加。然而，一旦航迹误差变成B迭代次数的递减函数，就可以获得B需要增加多远以实现可接受的最终航迹误差的估计，而不需要尝试每个中间B迭代。例如，可使用梯度下降方法来估计最佳的B。

通过迭代方法产生的路径可用在图6和图7的路径加入规划器中。

扩展

通过积分或迭代方法计算的加入路径可用在先进的策略中。可包括依赖于约束的速度。可扩展任一种方法以加入非直线的期望路径。最终，非最佳加入路径有时可能是令人满意的。

180度转弯是路径规划策略的一个示例，其可特别应用于农业。耕地、施肥、播种及其它操作通常沿着一组平行的贯穿农田的排来执行。在到达一排的末端时，农用车掉头并加入下一排。路径规划自动驾驶仪可使用以上描述的积分或迭代方法来产生从一排到另一排的加入路径。

图15A至图15D示出了掉头路径。在图15A至图15D的每一幅图中，弯曲的加入路径连接两个直线路径，例如，农田的排。加入路径使用以上描述的方法来计算。在图15A中，两个直线路径(y＝0和y＝2)之间的距离正好合适，以至于最大曲率的加入路径连接这两个直线路径而不超出它们。在图15B中，两个直线路径之间的距离较远，以至于加入路径包括垂直的直线部分。在图15C中，两个直线路径之间的距离太近，以至于无法在不超出这两个直线路径的情况下加入这两个直线路径。该路径可在任一方向被遵循，以至于超出部分按要求横跨开始直线或结束直线。在图15D中，加入路径同时超出两个直线路径。该加入路径被构建为开始于半路点(y＝两个直线路径之间距离的一半、φ＝-π/2和θ＝θ_max)并计算到线y＝0的加入路径。这产生了如图所示的加入路径的一半；另一半是镜像图像。

一旦计算了图15A至图15D中的任一加入路径，就可以知道该路径在x方向所需的距离。该信息可用来在合适的点处开始掉头，使得车辆(或被车辆牵引的器械)保持在如图3所示的边界内。

掉头是路径规划的用途的说明性示例。当然，并不要求待加入的直线是平行的。以上描述的路径规划方法是通用的并适应于任何初始的航迹误差、航向和转向角条件。

积分路径规划方法和迭代路径规划方法假设车速是常量。如果速度改变，则需要考虑该速度改变来计算新的加入路径。对于任何给定的车速，可设置诸如曲率和曲率变化率的参数的最大限值。对于典型的农用车，这些限值对应于最大的允许转向角和转向角变化率。图16A和图16B示出了速度依赖于曲率限值和曲率变化率限值的示意图。

在图16和图16B中，曲率θ和曲率变化率dθ/dt分别被绘制成对速度v的实曲线。这些曲线示出了如何调整自动驾驶仪的示例。随着车速增加，曲率和曲率变化率下降。虚线(“最大”)表示在每幅图中的最大限值。曲率和曲率变化率与速度的关系的曲线的形状可调整为适合车辆操作者或自动驾驶仪制造商的偏好。

曲率与速度的关系和曲率变化率与速度的关系可以多种不同的方式调整。一种方式是通过一系列测试策略来驾驶车辆以发现在多个速度下车辆的最大能力。自动驾驶仪可记录车辆遵循的路径，包括速度，并从记录的数据中导出最大曲率和曲率变化率。第二种方式是使自动驾驶仪接受用户输入以指定具有已知能力的车辆的特定品牌和型号。第三种方式是要求车辆操作者如其所期望自动驾驶仪驾驶车辆那样积极地驾驶车辆。之后，该自动驾驶仪可在操作者驾驶时记录该车辆遵循的路径并使用记录的数据来导出最大的操作者期望的曲率和曲率变化率。一些操作者没有实现其车辆的所有调整能力。将自动驾驶仪的性能与操作者的舒适度相匹配有助于避免操作者在使用自动驾驶仪对车辆进行控制时受到惊吓。

到目前为止，已经就产生到直线的加入路径描述了路径规划技术。然而，也可以加入弯曲的路径。加入曲线的总体策略是使用路径规划来导向车辆足够靠近该曲线以进行反馈技术。首先产生到最近的曲线切线的加入路径。随着加入路径接近曲线，最近的切线发生改变；则产生到新的最近的切线的新的加入路径。产生和遵循到最近的切线的加入路径的过程可重复进行以构建在足够靠近该弯曲的路径处结束整个加入路径，使得可使用反馈技术来导向车辆剩下的路。

图17示出了接近弯曲的期望路径。图17固定点1705和1710用三角形来表示。曲线1715是待加入的期望路径。与固定点1705最近的期望路径的切线标为“1”而与固定点1710最近的期望路径的切线标为“2”。虚线连接每个固定点到各自的切线。曲线1720是从第一固定点到第一切线“1”的加入路径，而曲线1725是从第二固定点到第二切线“2”的加入路径。在弯曲的路径1715的任一侧的点划线表示与弯曲的路径“近”的最大航迹误差。与弯曲的路径之间的距离比两个点划线曲线中的任一个更近的车辆可被小信号反馈自动驾驶仪导向至该弯曲的路径上。如果更远，可使用例如基于路径规划技术的大信号前馈自动驾驶仪。

按照以下过程构建到弯曲的期望路径1715的加入路径。从固定点1705开始，计算到曲线1715的最近点的切线“1”。使用路径规划技术，例如积分或迭代方法，来计算到切线“1”的加入路径1720。在到达固定点1710时，计算到曲线1715上的当前最近点的切线“2”。使用路径规划技术来计算到切线“2”的加入路径1725。该过程可重复足够多的次数以构建平滑的加入路径。

在计算完加入路径后，车辆在如图4至图7中所描述的自动驾驶仪的导向下遵循该加入路径。一旦车辆靠近该弯曲的路径，则由反馈自动驾驶仪导向剩余的路。虚曲线1730是由对曲线1715的反馈导向产生的踪迹。

基于车辆启动时与该路径距离多远和/或该路径的弯曲程度，可以确定何时以及多长时间一次重新计算到弯曲的期望路径上的当前最近点的新切线。新切线也可以定期计算而与距离或路径曲率无关。

如上所述，不管是加入弯曲的或直线的期望路径，都可通过重新计算加入路径来适应车速变化。然而，高速策略比低速策略占用更多的空间，路径规划自动驾驶仪可以将这一点考虑在内。在规划靠近边界的策略(例如在农田的边缘的掉头)时，路径规划自动驾驶仪可计算对于当前的速度和更高的速度的加入路径。例如，如果当前以最大车速的50％运行，则还可以计算适合于最大车速的90％的附加路径。这样，自动驾驶仪可在车辆操作者决定加速的情况下提供余地。期望较高的速度有助于防止困在引起意想不到的高速加入路径的策略中。也可通过指定最大曲率变化率μ小于规定的车辆绝对限值来约束路径规划自动驾驶仪。较小的μ值可导致较小的急转弯。每当航向限值、曲率限值或曲率变化率限值改变时，都可重新计算路径。

在一些情况下，并不能得出最佳的、时间最短的加入路径。图18A和图18B示出了不一定最佳的加入路径。在图18A和图18B中，期望路径是线y＝0。在图18A中，加入路径从(0,-1.8)附近开始；在图18B中，加入路径从(0,4)附近开始。

图18A中的加入路径是保持初始航向直到足够接近目标线(例如到达)以开始最佳的最终转弯的路径。该路径并不一定是最佳的，因为可允许更陡峭的接近目标线。图18B中的加入路径是保持初始曲率直到足够接近目标线(例如到达)以开始最佳的最终转弯。该路径并不一定是最佳的，因为可允许更大的曲率。因此，图18A和图18B的加入路径从(可能的)非最佳路径开始并以最佳的最终转弯结束。

如图18A和图18B所示的非最佳路径从操作者舒适的角度来说可能是令人满意的。在这两种情况下，非最佳路径开始于导向车辆继续在其已有的路径上行驶：在图18A的情况下航向不变；在图18B的情况下转向角不变。该方式提供了从操作者控制到车辆的自动驾驶仪控制的平滑、平稳的过渡。当然，如果车辆没有最终到达最佳的最终转弯的位置(例如)，则导致非最佳路径的策略并不可取。例如，如果初始航向使车辆远离目标路径，则不能保持初始航向。

已经描述了用于计算加入路径的路径规划自动驾驶仪和方法。即使车辆远离期望路径，该自动驾驶仪也能提供有效的车辆导向。该自动驾驶仪还提供对诸如转向角变化率的参数的直接控制，而不依赖于启发式的限制。

所公开的实施方式的以上描述被提供使得本领域技术人员能够制造或使用本发明。本领域技术人员很容易对这些实施方式进行各种改进，此处定义的原理可应用于其它实施方式而不脱离本发明的范围。因此，本发明不意图限于本文示出的实施方式而是符合与本文公开的原理和新特征一致的最宽范围。

优选包括本文描述的所有元件、部件和步骤。应当理解，这些元件、部件和步骤中的任一者可被其它元件、部件和步骤替代或全部删除，这对于本领域技术人员来说是显而易见的。

构思

本文至少披露了以下构思：

构思1、一种用于将车辆导向到期望路径的方法，包括：

创建从初始车辆配置到所述期望路径的加入路径，所述加入路径包括第一最佳转弯；以及，

使用自动驾驶仪以沿着所述加入路径引导所述车辆。

构思2、如构思1所述的方法，其中所述加入路径还包括在所述第一最佳转弯之前的直线。

构思3、如构思1所述的方法，其中所述加入路径还包括在所述第一最佳转弯之后的第二最佳转弯。

构思4、如构思1所述的方法，其中所述加入路径还包括在所述第一最佳转弯之后的直线和在所述直线之后的第二最佳转弯。

构思5、如构思1所述的方法，其中所述第一最佳转弯包括第一回旋曲线。

构思6、如构思5所述的方法，其中所述第一最佳转弯还包括在所述第一回旋曲线之前的圆弧。

构思7、如构思5所述的方法，其中所述第一最佳转弯还包括在所述第一回旋曲线之后的第二回旋曲线。

构思8、如构思5所述的方法，其中所述第一最佳转弯还包括在所述第一回旋曲线之后的圆弧和在所述圆弧之后的第二回旋曲线。

构思9、如前述构思中任一项所述的方法，其中所述期望路径是直线。

构思10、如构思9所述的方法，其中所述加入路径离所述直线不超过最大角φ_max。

构思11、如前述构思中任一项所述的方法，其中所述加入路径不超过最大曲率θ_max。

构思12、如前述构思中任一项所述的方法，其中所述加入路径不超过最大曲率变化率μ。

构思13、如构思1所述的方法，其中创建加入路径包括：

在从所述初始车辆配置到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间，计算垂直于所述期望路径所穿过的距离其中φ₀和θ₀是所述初始车辆配置中所述车辆的航向和曲率；以及，

如果所述初始车辆配置中相对于所述期望路径的所述车辆的航迹误差等于则计算从所述初始车辆配置到所述期望路径的所述第一最佳转弯。

构思14、如构思13所述的方法，还包括如果φ₀等于最大接近角φ_max且θ₀＝0，则使航向为φ₀的直线在所述第一最佳转弯的前面。

构思15、如构思1所述的方法，其中创建加入路径包括：

计算从所述初始车辆配置到相对于所述期望路径以最大角φ_max倾斜的直线的所述第一最佳转弯；

继续所述直线直到到达在到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间垂直于所述期望路径所穿过的距离其中φ和θ是所述车辆的航向和曲率；以及，

计算从所述直线到所述期望路径的第二最佳转弯。

构思16、如构思1所述的方法，其中创建加入路径包括：

计算从所述初始车辆配置到相对于所述期望路径以最大角φ_max倾斜的直线的所述第一最佳转弯；以及，

计算从所述第一最佳转弯上的点到所述期望路径的第二最佳转弯。

构思17、如构思16所述的方法，其中所述点距离所述期望路径为其中是在到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间垂直于所述期望路径所穿过的距离，其中φ和θ是所述车辆的航向和曲率。

构思18、如前述构思中任一项所述的方法，还包括：

在车速改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

构思19、如前述构思中任一项所述的方，还包括：

在最大的允许航向φ_max改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

构思20、如前述构思中任一项所述的方法，还包括：

在最大的允许曲率θ_max改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

构思21、如前述构思中任一项所述的方法，还包括：

在最大的允许曲率变化率μ改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

构思22、如前述构思中任一项所述的方法，还包括：

定期创建从当前车辆配置到所述期望路径的新的加入路径。

构思23、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括反馈控制器和前馈路径加入规划器。

构思24、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括GNSS接收器。

构思25、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括车辆航向传感器。

构思26、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括车辆转向角传感器。

构思27、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括车辆转向执行器。

构思28、如前述构思中任一项所述的方法，其中所述自动驾驶仪包括微处理器和存储器。

构思29、如前述构思中任一项所述的方法，其中所述初始车辆配置包括初始航向和近似零曲率，所述期望路径是平行于所述初始航向但具有与所述初始航向相差180度的航向的直线，以及所述加入路径被构建成使其保持在垂直于所述期望路径的边界内。

构思30、一种用于将车辆导向到期望路径的方法，包括：

创建从初始车辆配置到所述期望路径的加入路径，所述加入路径包括在第一方向上的第一转弯和在与所述第一方向相反的第二方向上的第二转弯；以及，

使用自动驾驶仪以沿着所述加入路径引导所述车辆。

构思31、如构思30至38中任一项所述的方法，其中所述期望路径是直线。

构思32、如构思31所述的方法，其中所述加入路径离所述直线不超过最大角φ_max。

构思33、如构思30至38中任一项所述的方法，其中所述加入路径不超过最大曲率θ_max。

构思34、如构思30至38中任一项所述的方法，其中所述加入路径不超过最大曲率变化率μ。

构思35、如构思30至38中任一项所述的方法，其中所述加入路径还包括直线。

构思36、如构思30至38中任一项所述的方法，其中所述加入路径具有末端，在该末端处所述加入路径和所述期望路径具有相同的航向和曲率，且这两个路径之间的航迹误差最小。

构思37、如构思30至38中任一项所述的方法，其中所述第一转弯包括回旋曲线。

构思38、如构思30至38中任一项所述的方法，其中所述第二转弯包括回旋曲线。

构思39、一种用于将车辆导向到期望路径的方法，包括：

创建从初始车辆配置到与所述期望路径相切的直线的第一加入路径，所述直线在所述期望路径上与所述初始车辆配置最近的点处相切，所述加入路径包括第一最佳转弯；

自所述第一加入路径上的点重复以上步骤以建立整体连续曲率的加入路径，所述整体连续曲率的加入路径在足够靠近所述期望路径处结束，使得能够使用反馈技术来将所述车辆从所述整体加入路径的末端导向到所述期望路径；以及，

使用自动驾驶仪以沿着所述加入路径引导所述车辆。

构思40、一种车辆自动驾驶仪，包括：

微处理器和存储器；

GNSS接收器，所述GNSS接收器提供车辆位置、速度和时间估计给所述微处理器；

反馈控制器和前馈控制器，所述反馈控制器基于测量的航向和y偏移数据和期望路径生成车辆曲率命令，所述前馈控制器基于计算的从所述车辆位置到所述期望路径的加入路径生成车辆曲率命令；以及，

车辆转向执行器。

构思41、如构思40至45中任一项所述的自动驾驶仪，其中所述反馈控制器和所述前馈控制器位于所述微处理器内。

构思42、如构思40至45中任一项所述的自动驾驶仪，其中所述加入路径不超过最大的允许曲率θ_max。

构思43、如构思42所述的自动驾驶仪，其中所述最大的允许曲率θ_max是根据车辆运行期间记录的数据估计得到的。

构思44、如构思40至45中任一项所述的自动驾驶仪，其中所述加入路径不超过最大的允许曲率变化率μ。

构思45、如构思44所述的自动驾驶仪，其中所述最大的允许曲率变化率μ是根据车辆运行期间记录的数据估计得到的。

Claims (45)

1.一种用于将车辆导向到期望路径的方法，包括：

创建从初始车辆配置到所述期望路径的加入路径，所述加入路径包括第一最佳转弯；以及，

使用自动驾驶仪以沿着所述加入路径引导所述车辆。

2.如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之前的直线。

3.如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之后的第二最佳转弯。

4.如权利要求1所述的方法，其中，所述加入路径还包括在所述第一最佳转弯之后的直线和在所述直线之后的第二最佳转弯。

5.如权利要求1所述的方法，其中，所述第一最佳转弯包括第一回旋曲线。

6.如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之前的圆弧。

7.如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之后的第二回旋曲线。

8.如权利要求5所述的方法，其中，所述第一最佳转弯还包括在所述第一回旋曲线之后的圆弧和在所述圆弧之后的第二回旋曲线。

9.如权利要求1所述的方法，其中，所述期望路径是直线。

10.如权利要求9所述的方法，其中，所述加入路径离所述直线不超过最大角φ_max。

11.如权利要求1所述的方法，其中，所述加入路径不超过最大曲率θ_max。

12.如权利要求1所述的方法，其中，所述加入路径不超过最大曲率变化率μ。

13.如权利要求1所述的方法，其中，创建加入路径包括：

在从所述初始车辆配置到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间，计算垂直于所述期望路径所穿过的距离其中φ₀和θ₀是所述初始车辆配置中所述车辆的航向和曲率；以及，

如果所述初始车辆配置中相对于所述期望路径的所述车辆的航迹误差等于则计算从所述初始车辆配置到所述期望路径的所述第一最佳转弯。

14.如权利要求13所述的方法，还包括，如果φ₀等于最大接近角φ_max且θ₀＝0，则使航向为φ₀的直线在所述第一最佳转弯的前面。

15.如权利要求1所述的方法，其中，创建加入路径包括：

计算从所述初始车辆配置到相对于所述期望路径以最大角φ_max倾斜的直线的所述第一最佳转弯；

继续所述直线直到到达在到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间垂直于所述期望路径所穿过的距离其中φ和θ是所述车辆的航向和曲率；以及，

计算从所述直线到所述期望路径的第二最佳转弯。

16.如权利要求1所述的方法，其中，创建加入路径包括：

计算从所述初始车辆配置到相对于所述期望路径以最大角φ_max倾斜的直线的所述第一最佳转弯；以及，

计算从所述第一最佳转弯上的点到所述期望路径的第二最佳转弯。

17.如权利要求16所述的方法，其中，所述点距离所述期望路径为其中是在到具有平行于所述期望路径的航向和零曲率的配置的最佳转弯期间垂直于所述期望路径所穿过的距离，其中φ和θ是所述车辆的航向和曲率。

18.如权利要求1所述的方法，还包括：

在车速改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

19.如权利要求1所述的方法，还包括：

在最大的允许航向φ_max改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

20.如权利要求1所述的方法，还包括：

在最大的允许曲率θ_max改变时，创建从当前车辆配置到所述期望路径的新的加入路径。

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所述的方法，其中，所述加入路径离所述直线不超过最大角φ_max。

33.如权利要求30所述的方法，其中，所述加入路径不超过最大曲率θ_max。

34.如权利要求30所述的方法，其中，所述加入路径不超过最大曲率变化率μ。

35.如权利要求30所述的方法，其中，所述加入路径还包括直线。

36.如权利要求30所述的方法，其中，所述加入路径具有末端，在该末端处所述加入路径和所述期望路径具有相同的航向和曲率，且这两个路径之间的航迹误差最小。

37.如权利要求30所述的方法，其中，所述第一转弯包括回旋曲线。

38.如权利要求30所述的方法，其中，所述第二转弯包括回旋曲线。

39.一种用于将车辆导向到期望路径的方法，包括：

创建从初始车辆配置到与所述期望路径相切的直线的第一加入路径，所述直线在所述期望路径上与所述初始车辆配置最近的点处相切，所述加入路径包括第一最佳转弯；

自所述第一加入路径上的点重复以上步骤以建立整体连续曲率的加入路径，所述整体连续曲率的加入路径在足够靠近所述期望路径处结束，使得能够使用反馈技术来将所述车辆从所述整体加入路径的末端导向到所述期望路径；以及，

使用自动驾驶仪以沿着所述加入路径引导所述车辆。

40.一种车辆自动驾驶仪，包括：

微处理器和存储器；

全球导航卫星系统GNSS接收器，所述GNSS接收器提供车辆位置、速度和时间估计给所述微处理器；

反馈控制器和前馈控制器，所述反馈控制器基于测量的航向和y偏移数据和期望路径生成车辆曲率命令，所述前馈控制器基于计算的从所述车辆位置到所述期望路径的加入路径生成车辆曲率命令；以及，

车辆转向执行器。

41.如权利要求40所述的自动驾驶仪，其中，所述反馈控制器和所述前馈控制器位于所述微处理器内。

42.如权利要求40所述的自动驾驶仪，其中，所述加入路径不超过最大的允许曲率θ_max。

43.如权利要求42所述的自动驾驶仪，其中，所述最大的允许曲率θ_max是根据车辆运行期间记录的数据估计得到的。

44.如权利要求40所述的自动驾驶仪，其中，所述加入路径不超过最大的允许曲率变化率μ。

45.如权利要求44所述的自动驾驶仪，其中，所述最大的允许曲率变化率μ是根据车辆运行期间记录的数据估计得到的。