自动汽车引导与轨迹跟踪的制作方法

本专利申请要求于2016年11月10日递交的申请号为no.62/420，314的美国悬而未决申请的权益和优先权。该专利申请的全部内容通过引用并入本文中。

背景技术：

本发明通常涉及类似汽车的地面车辆的自动引导和控制，更具体地说，涉及用于向类似汽车的地面车辆提供转向和电动机控制信号的系统、方法和计算机程序产品。

根据美国国家公路交通安全管理局(nhtsa)，2014年有600万警察报告了高速公路碰撞事故，导致200多万人受伤和32,675人死亡。在这些交通事故中，94％归因于司机错误。据信，自动驾驶或“无人驾驶”汽车将显著减少此类事故。目前，几乎所有的汽车制造商都已经开始研发自动驾驶汽车，并且许多自动驾驶车辆已经过道路测试，甚至许可驾驶人员进行驾驶监督。

运输车辆运动控制可以被分类为路径跟随和轨迹跟踪。路径跟随是指在没有时间限制的情况下穿越规定路径，而轨迹跟踪允许车辆以规定时间穿过路径(中立追踪)，具有以下这样的形式(诸如一排)：以增加公路容量或用于自适应巡航控制(合作追踪)，或者追踪逃避车辆，如追捕逃犯车辆的警车(对抗性追踪)。传统的无人驾驶汽车设计用于路径跟随，并且具有有限的可操作性或实际应用。路径跟随引导通常采用所谓的arc-pure-pursuit算法，该算法不包含速度约束。该设计涉及控制器参数的经验确定，这也可能限制车辆的性能潜力。由于对车辆施加的运动速度的要求，轨迹跟踪已经证明，对于汽车而言比路径跟随更难以控制和引导的问题。

用于汽车的传统自动运动控制器使用用于转向和节气门的单独控制器，这倾向于进一步限制车辆的性能潜力。为了有效地应对类似汽车的地面车辆运动控制的非线性和时变性质，传统系统可以使用所谓的模型预测控制(mpc)技术。mpc使用当前计算的控制器增益、运行在有限的未来时间间隔内车辆运动的模拟。mpc然后根据模拟结果修改车辆模型，执行在线最优控制设计，以获得一组新的增益，使用这些增益来控制车辆到下一个决策时间步骤，并在每个控制决策步骤时重复该过程，通常每秒50至100次。这种控制器的计算量相当密集，但性能有限且缺乏稳定性。

因此，需要用于控制自动驾驶汽车的改进的方法、系统和计算机程序产品。

技术实现要素：

本发明的实施例提供了一种集成的三自由度(3dof)轨迹线性化轨迹跟踪控制器和用于中立、合作和对抗跟踪操作的纯追踪引导控制器。与顶级认知任务规划器一起，本发明的实施例可以便于完全自动的类似汽车的地面车辆操作。本发明的实施例还可以用作更先进的汽车失控防止和恢复控制器的基线控制器，并且可以适用于任何类似汽车的轮式机器人。

多环3dof轨迹线性化控制器被配置为利用非线性轮胎牵引力、非线性拖曳力和致动器动力学/动力学特性引导具有非线性刚体动力学/动力学特性的车辆。轨迹线性化控制器可以基于用于指数稳定性的奇异扰动(时间尺度分离)理论，并且同时控制纵向速度和转向角以遵循可行的引导轨迹。这种建模和设计方法通过缩小模型车上的计算机模拟和硬件测试结果得到支持。

本发明的实施例还可包括基于视线的纯追踪引导控制器，其能够进行轨迹跟踪。引导控制器可以生成3dof空间轨迹，其被提供给3dof轨迹跟踪控制器。由此产生的组合可以提供3dof运动控制系统，其具有用于汽车和类似汽车的移动机器人轨迹跟踪的集成(同时)转向和速度控制。

以上概述呈现了本发明的一些实施例的简化概述，以便提供对本文所述发明的某些方面的基本理解。发明内容并非旨在提供对本发明的广泛概述，也不旨在标识任何关键或重要元素或描绘本发明的范围。发明内容的唯一目的仅仅是以简化的形式呈现一些概念，作为对下面给出的详细描述的介绍。

附图说明

包含在本说明书中并构成说明书一部分的附图示出了本发明的各种实施例，并且与上面给出的本发明的一般描述以及下面给出的实施例的详细描述一起用于解释本发明的实施例。

图1是根据本发明的实施例的轨迹线性化控制器的示意图。

图2是可以由图1的轨迹线性化控制器用来控制车辆的多个参考系的示意图。

图3是轨迹跟踪控制器的示意图，该轨迹跟踪控制器包括引导控制器和转向控制器。

图4是误差坐标系的示意图。

图5是用于自动的类似汽车的地面车辆的引导、导航和控制系统的示意图。

图6是描绘被引导车辆和目标车辆之间的位置关系的示意图。

图7是可以与图5的引导、导航和控制系统使用的轨迹发生器的示意图，其包括视线搜寻器(seeker)模块、航向引导模块、速度引导模块和轨迹合成器模块。

图8是可以在图7的速度引导模块中使用的闭环系统的示意图。

图9是可以在图7的航向引导模块中使用的闭环系统的示意图。

图10是描绘被引导车辆和用于车道跟随的虚拟目标之间的位置关系的示意图。

图11是描绘用于跟踪具有车道约束的目标的引导车辆、虚拟目标和目标车辆之间的位置关系的示意图。

图12是描绘导航参考系中的轨迹线性化控制系统的示例性位置轨迹跟踪性能的曲线图。

图13是描绘轨迹线性化控制系统的示例性位置跟踪性能的多个曲线图的图形视图。

图14是描绘导航参考系中的轨迹线性化控制系统的示例性位置跟踪误差的多个曲线图的图形视图。

图15是描绘车体固定参考系中的轨迹线性化控制系统的示例性位置跟踪误差的多个曲线图的图形视图。

图16是描绘轨迹线性化控制系统的不同缩放速度下的示例性横向位置误差的多个曲线图的图形视图。

图17是描绘车体固定参考系中的轨迹线性化控制系统的示例性速度跟踪的多个曲线图的图形视图。

图18是描绘车体固定参考系中的轨迹线性化控制系统的示例性速度跟踪误差的多个曲线图的图形视图。

图19是描绘轨迹线性化控制系统的示例性偏航跟踪和偏航跟踪误差的多个曲线图的图形视图。

图20是描绘轨迹线性化控制系统的示例性偏航角度速率跟踪和跟踪误差的多个曲线图的图形视图。

图21是描绘示例性电动机电压和转向角致动器信号的多个曲线图的图形视图。

图21b是可以由类似汽车的地面车辆跟踪的附图8轨迹的图形视图。

图22是描绘使用基于视线的纯追踪引导系统的s形运动目标的导航参考系中的示例性轨迹跟踪的曲线图的图形视图。

图23是描绘用于图22的s形运动目标的导航参考系中的测试车辆的x轴和y轴位置的多个曲线图的图形视图。

图24是描绘图22的s形运动目标的导航参考系中的位置跟踪误差的多个曲线图的图形视图。

图25是描绘图22的s形运动目标的车体参考系中的位置跟踪误差的多个曲线图的图形视图。

图26是描绘用于图22的s形运动目标的车体参考系中的测试车辆的速度跟踪的多个曲线图的图形视图。

图26b是用于图22的s形运动目标的车体参考系中的测试车辆的速度跟踪误差的图形视图。

图26c是用于图22的s形运动目标的车体参考系中的测试车辆的偏航角度速率的图形视图。

图27是描绘图22的s形运动目标的测试车辆的视线角度的多个曲线图的图形视图。

图28是描绘图22的s形运动目标的车体参考系中测试车辆的范围误差的曲线图。

图29是描绘用于图22的s形运动目标的测试车辆的示例性偏航角度和偏航角度跟踪误差的多个曲线图的图形视图。

图30是描绘用于图22的s形运动目标的测试车辆的转向角的曲线图的图形视图。

图31是描绘用于图22的s形运动目标的测试车辆的侧滑角度的曲线图的图形视图。

图32是可用于实现图1至31的控制器、模块或系统中的一个或多个的示例性计算系统的示意图。

具体实施方式

本发明的实施例包括用于自动的类似汽车的地面车辆的控制系统，其提供具有三个自由度(3dof)的车辆运动控制。3dof车辆运动控制面临几个挑战。例如，车辆刚体、轮胎牵引力和各种拖曳力模型是高度非线性的。另一个挑战是，即使车辆动力学/动力学特性可以被视为时间不变，沿着依时间轨迹的跟踪误差动力学通常是时变的。此外，类似汽车的地面车辆受到非完整的运动学约束，这进一步需要时变和非平滑的引导和控制算法。

3dof运动控制可以被分类为路径跟随和轨迹跟踪。路径跟随仅要求车辆遵循指定的路径而没有时间限制。因此，路径跟随控制器系统仅需要处理车辆运动学/运动学特性。相反，轨迹跟踪控制系统要求车辆以给定的速度穿过规定的路径。轨迹跟踪比路径跟随更具挑战性，因为除了车辆运动学之外还必须考虑车辆动力学。本发明的实施例使用3dof运动控制来解决这些问题，用于非完整地面车辆轨迹跟踪与非线性车辆和力模型。

通常希望同时控制车辆的平移和旋转运动。然而，平移和旋转运动的车辆动力学是高度耦合和非线性的，这给控制系统带来了额外的挑战。此外，即使车辆动力学可能是时不变的，车辆的轨迹也是时变的，这意味着跟踪误差动力学也是时变的。

本发明的实施例使用轨迹线性化控制(tlc)来解决非完整地面车辆轨迹跟踪控制问题的挑战。tlc提供非线性时变控制器，控制器将非线性动态逆与线性时变反馈稳定性相结合。基于tlc的控制器可被视为增益调度控制器，其设计在轨迹上的每个点处，以提供稳健的稳定性。

现在参考图1，tlc系统100可以包括开环标称控制器102(例如，逆动力学模块)和闭环跟踪误差调节器104(例如，稳定控制器)。标称控制器100可以被配置为消除开环中的非线性车辆动力学106的非线性，从而减小跟踪误差。跟踪误差的这种减小可以便于非线性跟踪误差动力学的线性化，以用于稳定。使用线性时变(ltv)坐标变换和pd特征值分配，可以指数地稳定非线性和时变的跟踪误差动力学。

可以修改用于没有非完整约束的车辆的多环轨迹跟踪tlc，以通过解耦车体固定参考系(而非惯性参考系)中的位置误差，来处理类似汽车的车辆。该特征可以使车辆控制系统能够处理车辆横向运动的非完整约束。本文公开的车辆控制系统可以应用于任何类型的车辆，其中，可控性沿着每个车体轴线是不同的。

图2描绘了可用于控制车辆运动的多个参考系。参考系包括东北向下(ned)导航参考系(“n系”)、ned车体承载参考系(“nb系”)和车体固定参考系(“b系”)。n系可以包括原点on，其固定在地球表面上的感兴趣点处。n系可以被视为平坦的和惯性的，并且可以包括指向一个方向(例如，指向北方)的xn轴200，指向另一个方向(例如指向东方)的yn轴202，以及指向另一个方向(例如，向下)的zn轴(未示出)。

nb系可以与n系平行，但是在车辆的重心(cg)处具有原点onb。对于b系，原点ob也可以固定在车辆的cg上，其中，xb沿着车辆的纵向轴线指向前方，yb指向车辆的右舷侧，以及zb指向下方。如图2中所示，β是车辆侧滑角，χ是车辆行进方向角，αlos是从车辆到目标的n系中的视线角度，γ是b系中的视线角度，p＝[xnyn]^t是n系中的车辆位置，rb是b系中的范围矢量，p＝[xnyn]^t是n系中的车辆位置，v＝[uv]^t是b系中的速度，ψ是b系和nb系之间的偏航角度，r＝ψ是b系中的偏航角度速度，以及δ是前轮胎转向角。

在以下对类似汽车的地面车辆轨迹跟踪的描述中，可以做出一些假设以简化车辆运动的分析。这些假设可以包括车辆是刚体，车辆的重量在四个车轮中的每个车轮之间均匀分布，并且车辆在水平且平滑的道路上行驶。这些简化可以允许忽略俯仰和翻滚运动，并且纵向横向运动控制与转向控制相结合。由于类似汽车的地面车辆的非完整约束，假设横向力不会产生横向加速度。相反，假设横向加速度仅仅是旋转运动的结果。

鉴于上述约束，状态空间方程可以使用以下等式，从六个自由度(6dof)刚体运动方程直接简化为3dof运动方程：

平移运动学：

其中，sψ＝sinψ和cψ＝cosψ。

平移动力学：

旋转运动学：

旋转动力学：

其中，f＝[fxfy]^t是车体框架力，nm是偏航力矩，m是车辆的总质量，并且izz是绕zb轴的惯性矩。

总纵向力fx和横向力fy可以写为：

fx＝fxtfcos(δ)+fxtr-fytfsin(δ)+frr+faero+fb

fy＝fxtfsin(δ)+fytfcos(δ)+fytr+fyst(方程式5)

其中，frr＝-crrmgsgn(u)是滚动阻力；是纵向气动阻力；fb＝δbfb，max是制动力；fxt和fyt分别是纵向和横向轮胎牵引力；fyst是轮胎横向静态阻力；以及下标f和r表示所讨论的力分别与前轴或后轴相关联。

对于轮式类似汽车的地面车辆，推进力和控制力以及转向力矩可以通过轮胎的牵引力来实现。由于这些牵引力的高度非线性行为，轮胎牵引力建模在设计类似汽车的地面车辆控制系统中可能是一个重大而艰巨的挑战。

在本发明的一个实施例中，纵向轮胎牵引力可以定义为：

其中，cα是4轮总轮胎刚度系数，并且α是纵向滑移角。纵向滑移角α可以定义为：

其中，reff和ωw分别是车轮的有效半径和角速度。

为了将控制器应用于反向驱动，可以将负号添加到限定函数α(u，ωw)的每个制动和加速度方程中。

前轴和后轴处的横向轮胎牵引力可以分别定义为：

其中，β定义轮胎侧滑角，δ是车辆转向角，以及cβ是车辆总转弯刚度。在本发明的一个实施例中，可以假设没有发生横向轮胎打滑。在这种假设下，横向力fy不能产生横向位移，从而施以非完整约束。

偏航力矩可以定义为：

其中，lf和lr是前轴和后轴相对于重心的纵向距离。如与方程7一样，对于反向驱动，可以将负号添加到方程9。

用于汽车和其他类似汽车的地面车辆的驱动致动器通常包括内燃机加机械制动器，或具有再生制动的电动机和机械制动器。转向致动器通常是液压或电动伺服系统。为了清楚起见，下面将本发明的实施例描述为使用电枢dc电动机作为驱动致动器，以及使用伺服电动机作为转向致动器。然而，本发明不限于将任何特定类型的致动器用于驱动致动器或转向致动器。

电枢控制的dc电动机可以通过以下一阶线性常微分方程来建模：

其中，fxl是纵向载荷力，ra是电枢的电阻，jm是传动系绕电动机轴的有效惯性矩，bm是传动系绕电动机轴的有效扭转粘性摩擦系数，km是电动机机电常数，以及reff是轮胎的有效半径。

转向伺服的动力学可以通过以下传递函数来建模：

其中，ωns是伺服电动机的带宽。

可以将非线性轨迹跟踪问题公式化为跟踪误差坐标中的稳定性问题。这可以揭示跟踪误差动力学对于时变标称轨迹是时变的，即使车辆动力学是时不变的。因此，tlc架构可以为该问题公式提供用于轨迹跟踪的解决方案。

非线性车辆动力学对控制输入信号的响应可以通过以下定义：

其中，ξ(t)∈rⁿ、μ(t)∈r^l、η(t)∈r^m分别表示车辆动力学、控制致动器和导航传感器的状态、输入和输出。映射h(·)：rⁿ×r^l→rⁿ和f(·)：rⁿ×r^l→rⁿ可以用均匀有界和连续的雅可比行列式界定。设和为标称状态、标称输入和标称输出轨迹，从而：

并将相应的跟踪误差和误差控制定义为：

可以允许将跟踪误差动力学写为：

其中，可以作为已知的时变参数处理。标称控制输入和标称状态可以通过车辆动力学的伪逆来确定。

由方程14定义的跟踪误差动力学可以是非线性的和时变的，其可以沿着标称轨迹线性化以获得：

x＝a(t)x+b(t)u，y＝c(t)x+d(t)u(方程16)

其中，以及

通过应用线性时变控制算法u＝k(t)x，可以稳定由方程15定义的线性化跟踪误差动力学。因此，tlc可以组合非线性反演和线性时变反馈稳定。由于稳定是指数变化的，这可以在存在规则和奇异扰动的情况下提供稳健的稳定性。如下所述，可以使用上面提出的tlc控制算法，来实现用于非完整地面车辆的3dof控制器。

图3描绘了示例性四环3doftlc轨迹跟踪控制器300的框图，其包括引导控制器302和转向控制器304。引导控制器302可包括引导外环标称控制器306、引导内环标称控制器308、引导标称分配模块310、引导控制分配模块312、引导外环反馈控制器314和引导内环反馈控制器316。转向控制器304可包括转向外环标称控制器318、转向内环标称控制器320、和转向控制分配模块322、转向外环反馈控制器324和转向内环反馈控制器326。引导控制器302和转向控制器304可以从车辆导航系统328接收输入信号，并且向车辆控制致动器模块330提供信号。车辆控制致动器模块330可以控制影响车辆运动学变化的控制器(例如，驱动和转向控制器)。标称控制器306、308、318、320中的每一个可以包括产生标称控制信号和/或状态的开环标称控制器。反馈控制器314、316、324、326中的每一个可以包括闭环跟踪误差控制器，其稳定相应控制信号和/或状态中的跟踪误差。

严格地说，车辆动力学可以指代由方程2和4表示的车辆的物理特性，其由车辆的质量和车辆中的质量分布产生。车辆运动学(由方程1和3表示)是限制车辆动力学的车辆的几何特性。总的来说，方程1至4被称为运动方程，其可以被分类地称为车辆动力学，并且由车辆动力学模块332来说明。由方程10a和10b表示的控制致动器(例如电动机和转向系统)是克服车辆动力学从而使车辆能够移动和操纵的致动器。轮胎是控制执行器，其将致动力和扭矩传递到车体，从而允许车辆加速和转弯。

在轨迹跟踪控制器300的以下描述中，下标sim可用于识别与模拟或实际车辆状态相关联的变量和/或信号，下标sen可用于识别与通过导航传感器传递的感测信息或模拟车辆状态相关联的变量和/或信号，下标nom可用于识别与标称信号相关联的变量和/或信号，下标ctrl可用于识别与反馈控制器314、316、324、326输出相关联的变量和/或信号，下标err可用于识别与跟踪误差相关联的变量和/或信号，并且下标com可用于识别与控制器命令信号相关联的变量和/或信号。

相应的运动方程的动力学伪逆可以用于生成标称控制信号，并且反馈控制器314、316、324、326可以是用于稳定每个环中的跟踪误差的比例-积分(pi)控制器。引导外环可以接收来自轨迹发生器(未示出)的位置命令信号pcom和来自导航系统328的位置测量信号psen，并且使用这些信号来确定引导内环的速度控制信号vctrl。引导内环可以接收来自引导外环的速度命令信号vcom以及接收速度测量信号vsen，并使用这些信号来确定速度误差信号verr。

速度控制信号vctrl可以是包括b系中的纵向速度控制信号vx,ctrl和横向速度控制信号vy,ctrl的矢量信号，速度命令信号vcom可以是包括b系中的纵向速度命令信号vx,com和横向速度控制信号vy,com的矢量信号，速度测量信号vsen可以是包括b系中的纵向速度测量信号vx,sen和横向速度测量信号vy,sen的矢量信号，并且速度误差信号verr可以是包括b系中的纵向速度误差信号vx,err和横向速度误差信号vy,err的矢量信号。

可以基于速度误差信号verr确定用于引导环控制分配的力控制信号fctrl。速度命令信号vcom可以是标称速度信号vnom和速度控制信号vctrl之和，如由以下提供：

vcom＝vnom+vctrl

纵向速度误差信号vx,err可以等于纵向速度命令信号vx,com和纵向速度测量信号vx,sen之间的差，由下式提供：

vx,err＝vx,com-vx,sen

引导分配单元可基于力命令信号fcom和偏航命令ψcom来确定驱动命令信号δt，com，其可对应于内燃机的节气门位置或电枢控制的dc电动机的电压。力命令信号fcom可以是标称力信号fnom和力控制信号fctrl之和，如下所述提供：

fcom＝fnom+fctrl

图3中描绘的tlc轨迹跟踪控制器的一个有利特征是将车辆模型分为刚体模型、力模型和致动器模型，并且分别处理每个子模型。整个闭环系统可以包括四个环，每个环具有如图1所示完整的tlc结构。开环标称控制器306、308、318、320可以使用相应运动方程的动力学伪逆来产生标称控制信号和标称状态。反馈控制器314、316、324、326可各自使用比例-积分(pi)控制器来以指数方式稳定跟踪误差。

可以通过反演方程1来确定b系中的标称速度vnom信号以产生：

其中，是方程1中的矩阵，其中，ψ由其标称值ψnom替换。这里标称偏航信号ψnom可以如下所述定义，并且可以通过以下形式的伪微分器从位置命令信号确定：

位置跟踪误差perr＝psen-pcom可以通过解耦b系(而非n系)中的跟踪误差动力学来定义，以解释对车辆横向运动的非完整约束。参考图4，下行距离rb可以被定义为负位置误差-perr在车体系上的投影幅度，如下：

然后，可以通过以下方程给出线性化误差动力学：

pi控制算法可以如下给出：

其中，通过下式给出pi增益矩阵：

其中，ai，k＝diag[-ai1k-ai2k]，i＝1，2，3，4和k＝1，2是时变控制器参数，其是从期望的闭环动力学合成的，并且aij1由下式提供：

其中，ωn，i，j是期望的固有频率，ζij是期望动力学的期望恒定阻尼比，对于x和y信道，j＝1，2，并且对于两个引导环，i＝1，2。引导外环的输出可以组合标称控制器306的输出和反馈控制器314的输出，以产生速度命令信号vcom＝vnom+vctrl。

引导内环可以从引导外环接收速度命令信号vcom并且从导航系统328接收速度测量信号vsen，以确定用于引导环控制分配的力命令信号fcom。可以使用平移动力学方程2来确定标称力信号fnom以产生：

其中，可以通过使引导外环标称控制器306的输出vnom通过伪微分器来确定速度跟踪误差verr可以由引导外环反馈控制器314接收并且定义为verr＝vsen-vnom。

车体速度的线性化误差动力学由下式给出：

其中，a2(·)对应于方程2的矩阵。引导内环反馈控制器316可以确定由以下定义的力控制信号fctrl：

车体速度pi参数可以如下给出：

如上所述，在非完整的类似汽车的地面车辆中，b系中的横向力不会产生横向位移，即轮胎不会相对于路面滑动。因此，可能仅需要使用基于fnom和fctrl的x信道来控制驱动致动器。横向位置误差可以通过偏航角度ψ来校正，如下面更详细地描述的。

引导控制器302可以确定驱动命令信号δt，com和偏航命令信号ψcom。在刚体和水平地面假设下，旋转运动可以仅取决于偏航角度ψ。因此，侧倾角和俯仰角θ可以被约束为零或者被视为扰动。纵向力fx(例如，如由纵向力控制信号fx，ctrl表征)可以用于确定驱动控制信号δt，ctrl。驱动控制信号δt，ctrl可以与标称驱动信号δt，nom相加，以产生驱动命令信号δt，com，其对于包括dc电动机的驱动致动器可以对应于施加到dc电动机的电压。

对于包括其中δt＝em的dc电动机的驱动致动器，可以通过反演方程10来确定施加到dc电动机的电压以产生：

其中，ωm，mom由车轮的标称角速度ωw，nom确定。车轮的标称角速度ωw，nom可以通过反演方程6和方程7的非线性轮胎牵引力模型来确定，并且可以通过使ωw，nom经过伪微分器来确定。

再次参见图2，车辆的偏航角度ψ可以通过以下方式确定：

ψ＝χ-β

其中，χ是车辆的行进方向角，β是车辆的侧滑角。标称行进方向角χnom可以定义为：

因为β可以使动态逆不稳定，所以可以将其从标称动态逆中排除，例如，通过设置βnom＝0，使得ψnom＝χnom。通过允许反馈控制器管理β，偏航控制信号可以被定义为：

其中，vctrl是来自引导外环反馈控制器314的vctrl输出的第二信道，其可以由引导控制分配模块312接收。因此：

ψcom＝ψnom+ψctrl

转向控制器304的外环可以从引导控制器302接收偏航命令信号ψcom并且从导航系统328接收偏航测量信号ψsen，并且基于所述偏航命令信号和偏航测量信号确定内环的车体角度速率命令信号。标称偏航角度速率信号rnom可以通过反演方程3来确定，这产生：

其中，通过使ψcom通过伪微分器确定该环的跟踪误差控制算法可以通过以下方式提供：

其中，偏航跟踪误差信号ψerr＝ψsen-ψcom，控制增益给定为kp3＝a331，ki3＝a332(方程32)

并且由转向外环标称控制器318输出的偏航角度速率命令信号rcom是标称偏航角度速率rnom和偏航角度速率控制信号rctrl之和：

rcom＝rnom+rctrl

转向内环可以从转向外环反馈控制器324接收车体偏航角度速率命令信号rcom，并且从导航系统328接收偏航角度速率测量信号rsen，并且基于所述信号之间的差异确定偏航角度速率误差信号rerr。然后，转向内环反馈控制器326可以基于偏航角度速率误差信号rerr确定标称车体扭矩信号nm,nom。

为此，反演方程4可能导致如下的标称车体扭矩：

其中，通过使rnom通过伪微分器(例如，转向外环反馈控制器324)来确定由此可以基于从引导控制器302接收到的偏航命令信号ψcom和从导航系统328接收的偏航测量信号ψsen确定标称车体扭矩信号nm，com＝nm，nom+nm，ctrl。转向控制器304可以基于车体扭矩控制信号nm，ctrl和标称车体扭矩信号nm，com(例如，通过对信号求和)确定车体扭矩命令信号nm，com，使得nm，com＝nm，ctrl+nm，nom。转向控制分配模块322然后可以基于车体扭矩命令信号nm，com确定转向驱动命令信号δs，com。

内环的pi控制算法可以由以下定义：

其中，在车体角度速率跟踪误差为rerr＝rsen-rnom的情况下，控制增益由下式给出：

kp4＝izza431，ki4＝a432(方程35)，

并且转向内环的输出为

转向控制分配模块322可用于将力矩命令信号分配给转向角，并进一步确定转向角命令信号δs，com。可以通过反演方程10来确定转向角命令信号δs，com，以产生：

图5描绘了类似汽车的地面车辆引导、导航和控制(gn&c)系统500，其包括轨迹跟踪控制器300和引导轨迹发生器502，并且从导航系统328接收反馈信号，并向车辆致动器提供控制信号，车辆致动器由车辆控制致动器模块330表示。对于非线性系统，运动的稳定性本质上可以是基本局部的，并且吸引域可以是有限的。因此，当车辆跟踪误差超出吸引域时，例如当初始车辆状态远离任务轨迹时，引导轨迹生成器502可能是必要的，以产生可行的标称轨迹，该轨迹可以将车辆引导至任务轨迹。

一种类型的纯追踪引导确定将车辆的当前位置和车辆前方的目标点在标称路径上接合的圆周弧。然后，车辆在朝向标称路径向前移动时遵循所述弧。然而，基于弧的纯追踪引导系统不考虑车辆的速度，因此可能不适合于类似汽车的基于地面的车辆中的轨迹跟踪。

已经用于飞机追踪的另一种类型的纯追踪引导将追踪车辆的速度矢量与将车辆和真实或虚拟目标接合的视线对准。正常加速命令信号产生并被转换成用于操纵飞机的姿态命令信号。与享受完整6dof运动的飞机不同，类似于汽车的地面车辆受到非完整运动约束，因为横向力不能产生横向加速度。因此，产生正常加速命令信号的传统飞机视线纯追踪引导不能用于直接引导类似汽车的地面车辆。本发明的实施例通过为跟踪控制器生成可行的n系位置轨迹来克服该问题。

为此，修改的视线纯追踪引导系统可以被配置为与类似汽车的地面车辆轨迹跟踪引导一起操作。这可以通过如下实现：将视线纯追踪引导应用于轨迹跟踪代替用于非线性、非完整的、类似汽车的车辆的路径跟随，将视线解耦为车体系中的朝向和速度引导控制来克服横向非完整约束，并且在惯性系中为3dof惯性位置轨迹控制器生成引导轨迹，而不是生成加速命令信号。

现在参考图6，本发明的实施例可用于许多轨迹跟踪应用中，用于诸如车辆排之类的协同追踪，诸如在自适应巡航控制和车道跟踪中的被动追踪，或诸如车辆对车辆追逐之类(例如，无人驾驶执法车辆600追逐逃逸车辆602)的对抗性追踪。车辆对车辆追逐案例可以用作描述视线纯追踪引导设计的示例性情况。然而，视线纯追踪指导的其他应用也是可能的。

在以下对示例性视线纯追踪引导系统的描述中，假设追踪车辆的可操纵性至少与目标车辆一样好，并且道路状况是理想的。视线纯追踪引导轨迹发生器可具有多个目标。这些目标可以包括：获取b系中的视线矢量；将速度矢量与视线矢量对准；保持追逐车辆与目标之间的安全纵向距离；以及笛卡尔n系中生成标称引导轨迹pg。可以将标称引导轨迹pg作为位置命令信号pcom提供给轨迹跟踪控制器300。

n系视线角度αlos可以被定义为：

其中，和[pxpy]^t分别是目标和追踪车辆的n系位置。

将(方程37)设为n系中的范围矢量。视线纯追踪引导的目标可以是将速度矢量与视线矢量对准，以便满足以下等式：

其中，下标g表示变量是引导信号。

图7更详细地描绘了引导轨迹生成器502的示例性实施例。引导轨迹生成器502包括视线搜寻器模块700、航向引导模块701、速度引导模块702和轨迹合成器模块703。模块700至703中的每一个可以被配置为使得轨迹生成器502实现了特定的视线纯追求引导目标。

视线搜寻器模块700可以被配置为通过获取相对于纵轴xb的b系视线角度γ，来确定在b系中的视线矢量，并确定纵向下行距离rbx。可以使用方程37确定n系中的视线。b系视线角度γ可以确定为：

γ＝αlos-ψsen

其中，ψsen是当前即时导航传感器感应到的偏航角度。b系中的下范围矢量rb可以由下式给出：

将设为追踪车辆和目标之间的范围，rbx＝rbcosγ和rby＝rbsinγ可以分别提供对xb和yb坐标轴的范围投影，以定义b系中的范围距离矢量的纵向分量(rbx)和横向分量(rby)，其中，γ是视线矢量和纵轴xb之间的角度，px,tgt和py,tgt是目标的、由目标相对位置信号ptgt指示的x和y坐标，以及px和py是由位置测量信号psen指示的追踪车辆的x和y坐标。尽管上述等式可以定义视线引导搜寻器的功能，但是本发明的实施例可以包括视线搜寻器，视线搜寻器确定相对于b系的范围矢量和视线，而不需要知道在n系中的目标相对位置。例如，视线搜寻器可包括相机、红外相机、声纳、雷达和/或激光雷达中的一个或多个。

航向引导模块701可以被配置为将速度矢量与视线矢量对准。标称车辆侧滑角βnom可以由反馈控制器管理。这可以允许在轨迹控制器中将标称车辆侧滑角βnom设定为零。因此，βg在引导轨迹生成器502中可以为零，在这种情况下，基于ψ＝χ-β，χg＝ψg。方程38然后可以重写为：

γ＝αlos-ψg＝0，其中，t→∞。(方程39)

因此，航向引导目标是使用引导角度ψg作为虚拟控制以指数方式稳定γ＝0，对于该虚拟控制，可以采用比例-积分-微分(pid)引导控制器来提供：

可以使用基于图9中所示的等效闭环系统的线性时不变控制器设计技术来确定pid增益。

速度引导模块702可用于维持追踪车辆与目标之间的安全纵向距离。为此，距离误差可以定义为rerr＝rbx-rset，其中，rset是安全纵向距离设置。通常，rset＝τ|vx|可以是速度相关参数，其中，τ是控制器的适当响应时间。

通过确定引导速度ug，可以采用pid控制器705随着t→∞以指数方式稳定为rerr→0，如下：

其中，并且sata(x)是由下式定义的饱和度函数：

可以选择极限rerr,max以限制由于大范围误差引起的最大引导加速度以及积分器抗卷绕方案。

轨迹合成器模块703可以用于使用航向引导模块701和速度引导模块702的输出，以在n系中构建标称引导轨迹pg，如下：

其中，vg是b系中的车辆横向速度。因为侧滑角βg＝0，vg＝0。考虑到防滑，术语umax可以定义汽车的最大速度，其可以基于操作和路面状况实时自适应地设定。

图8描绘了可用于分析速度引导模块702的稳定性的等效闭环系统，其包括求和模块800、pid模块802、积分器模块804和传递函数模块806。图9描绘了可用于分析航向引导模块701的稳定性的等效闭环系统，其包括求和模块900、pid模块902、积分器模块904和传递函数模块906。在图8和图9中，整个闭环位置轨迹跟踪控制器可以使用以下传递函数，以不同带宽在b系中解耦为转向和位置控制：

上述视线纯追踪引导控制器可用于在没有道路约束的情况下追踪不协同目标。然而，控制器还可以用于协同追踪，其中，目标将其坐标ptgt广播给追踪者。对于中立追踪(例如车道保持)，搜寻器可以指定车辆前方的虚拟目标，例如，如图10所示的车道中心的点。然后，可以将视线角度γ定义为车体系x轴xb和下范围矢量rb之间的角度γ。

图11描绘了具有车道约束的车辆追踪。为了将车辆引导到目标，系统可以假设两个目标，其中，一个目标是真实目标1100，而另一个目标是虚拟目标1102，例如，车道中心的点，其中，γ1是在车辆x轴与实际目标下范围矢量rb1之间的角度，以及γ2是车辆x轴与虚拟目标下范围矢量rb2之间的角度。如果γerrl>γerr2，则车辆追逐车道约束的虚拟目标，否则车辆追逐真实目标。

实验结果-轨迹跟踪控制器

为了关注运动控制问题，并且为了便于控制器设计的验证，3dof轨迹跟踪控制器被设计用于具有再生制动和伺服电动机驱动转向的1/6比例dc电动机驱动汽车。用于3dof汽车的控制器接收位置轨迹命令信号，并确定用于电动机和转向伺服的相应电压。

表i-测试无线电控制车辆的车辆建模参数

和是用于数据分析和模拟的计算机应用程序，其可从美国马萨诸塞州natick的mathworks公司获得，用于对轨迹跟踪控制器300进行建模。使用表i中的系统参数，车辆刚体模型是使用方程1至4建立的，以及轮胎牵引力模型是使用方程5至7建立的，以及各种非线性拖曳力和致动器模型是通过方程9和10给出的。表i中的系统参数基于无线电控制的“monstertruck”(巨轮卡车)，其用作测试车辆并且可以从美国德克萨斯州mckinney的traxxas公司获得。monstertruck被制造商归类为1/10比例，但它约为乘用车的1/6，并被视为1/6比例的汽车，用于缩放测试速度性能。测试的场景包括目标是遵循期望的轨迹并保持期望的速度的场景。饱和度要求|δ|≤30°和|u|≤7m/s也施加在轨迹跟踪控制器300上。

模拟中使用的控制器参数示于表ii中。标称控制器带宽，即伪微分器中的固有频率ωn，是根据相应致动器的最大响应时间或操作要求的最大可允许带宽来设定的。闭环带宽，即每个闭环特征值的固有频率ωn，被最初设置为直接内环的带宽的三分之一。所有阻尼比最初设定为0.7。然后，调整参数以获得最理想的性能和瞬态行为。

表ii-3dof运动控制器系数

图12描绘了曲线图1200-1202，其示出了位置命令信号pcom、标称位置pnom和车辆沿地面在n系中的模拟位置psim的二维视图。由于命令信号pcom是满足转向和速度饱和约束的可行轨迹，因此信号/位置基本上彼此共同延伸。

图13描绘了针对每个状态变量x和y的轨迹跟踪控制器300的模拟位置跟踪性能。在n系和b系中的相应跟踪误差分别在图14和15中示出。图14中所示的n系中的位置误差perr可以是车辆当前位置相对于标称位置pnom的偏差。

b系误差是perr在b系上的投影。图15示出了在车辆达到稳定状态之后纵向跟踪误差快速收敛到零。最大横向跟踪误差rby在约21秒和61秒处约为0.09m，这约为测试车辆的轨道宽度的26％。这些误差可能是由于当路径急转弯时的大转弯速率和角加速度，如图20中所描绘的偏航速率曲线2000所示的在大约21和61秒处。

通常，较高的速度可能引起转弯处较大的横向跟踪误差。图16可以解释这种现象。如曲线1600所示，以6米/秒的速度驾驶车辆可使最大横向跟踪误差达到0.3米(86％的轨道宽度)，而u＝2m/s则将最大跟踪误差降低至0.01m(3％轨道宽度)，如曲线1602所示。测试轨迹要求1/6比例的测试车辆以u＝4m/s(参见曲线1604)驾驶，对于全尺寸汽车，其将是大约54mph。图17示出了b系中的速度跟踪，并且相应的跟踪误差在图18中示出。方向跟踪性能由图19中的曲线1900-1902示出，通过曲线1906描绘了相应的跟踪误差。ψcom和ψnom之间的差异可以通过车辆侧滑角β来解释。角度速率跟踪在图20中呈现。图21描绘了用于dc电动机和电动伺服的致动器信号，其是平滑的并且恰好在饱和极限内。

已经使用上述模型车辆执行了硬件实施和测试。控制器参数和跟踪图8轨迹的测试情况下面分别在表iii和图21b中示出。该测试仅使用车载传感器完成，而没有任何全局参考传感器，如摄像头、信标或gps。用车载mems陀螺仪测量角速度和偏航角度，最大角度速率为1rad/sec。使用车载加速度计和电动机轴编码器测量b系速度和相对位置。采用扩展卡尔曼滤波器来融合加速度计和编码器数据。轨迹是最小的圆圈和硬件能够承受的最大角度速率。车辆从导航参考系的原点，以零速度开始。仅对于车载传感器，结果出乎意料地准确和平滑。

上述本发明的实施例包括用于使用tlc的非线性、非完整的、类似汽车的地面车辆的3dof轨迹跟踪控制器设计。模拟结果表明，该车辆可以良好性能跟踪可行轨迹。这被认为是用于非完整车辆轨迹跟踪的第一个tlc控制器。实验结果证明，tlc控制器为mpc和其他汽车控制器提供了有效的替代方案。通过考虑传感器误差、动力学和噪声；车体与车轮之间的可变形连接；以及翻滚和俯仰运动、轮胎打滑和车辆稳定性，。

视线纯追踪引导的实验结果

使用matlab/simulink实施并模拟根据以上描述的视线纯追踪引导控制器。上述1/6比例的traxxas远程控制模型汽车用于通过视线纯追踪引导控制器收集实验数据。速度引导控制器中使用的参数是ζ＝0.7，ωnx＝3rad/sec，并且ωny＝5.5rad/sec。获得实验结果的附加参数见表ii。

表ii-losppg测试无线电控制车辆的pid参数

图22至图30描绘了matlab/simulink模拟结果。在模拟中，rset＝1.5m，控制器响应时间τ＝0.5s。图22描绘了在n系中示出的二维轨迹，而图23描绘了位置跟踪结果。目标在原点北部20米处初始化，沿着直线-s-直线轨道向东移动速度为4m/s，以证明在直线和转弯条件下的跟踪性能。受控车辆初始化为从原点朝向北方从0m/s加速。实线图2202表示目标轨迹，虚线图2204表示纯追踪引导生成轨迹，以及点划线图2206表示追逐车辆轨迹。

图24描绘了n系跟踪误差，而图25描绘了b系跟踪误差。图26示出了车体系速度跟踪，其示出了轨迹跟踪和路径跟随之间的差异。纵向速度跟踪图2602是平滑的，而横向速度跟踪图2604示出了一些振荡瞬态2606，其对应于小的侧滑角β，如图31所示，它影响所需的向心加速度和s形转弯的偏航力矩。1/6比例的车辆以0m/s的初始速度启动，并且在大约60秒时获得4m/s的目标速度，相当于24m/s，或者对于全尺寸的汽车为54mph。图26b描绘了车体系速度跟踪误差verr，图26c描绘了偏航率和偏航率跟踪误差。

图27和28分别示出了视线误差γ和范围误差rerr。γ和rerr都收敛到0，这满足了引导目标。图29描绘了具有相应跟踪误差的偏航角度ψ跟踪。图30描绘了曲线3002，其示出了相对于时间绘制的车辆转向角δ。总体地，曲线3002表示转弯是平滑的。图31描绘了示出侧滑角β相对于时间的曲线图3102。

现在参考图32，可以使用一个或多个计算机设备或系统(例如示例性计算机系统3200)来实施上述本发明的实施例。计算机系统3200可以包括处理器3202、存储器3204、输入/输出(i/o)接口3206以及人机接口(hmi)3208。计算机系统3200还可以经由i/o接口3206和/或网络3212可操作地耦接到一个或多个外部资源3210。计算机可以被配置为支持对致动器3222和导航传感器3224控制的实时通信信道。

处理器3202可以包括从微处理器、微控制器、数字信号处理器、微计算机、中央处理单元、现场可编程门阵列、可编程逻辑器件、状态机、逻辑电路、模拟电路、数字电路中选择的一个或多个设备或者基于存储在存储器3204中的操作命令来操纵(模拟或数字)信号的任何其他设备。存储器3204可以包括单个存储器设备或多个存储器设备，包括但不限于只读存储器(rom)、随机存取存储器(ram)、易失性存储器、非易失性存储器、静态随机存取存储器(sram)、动态随机存取存储器(dram)、闪存、高速缓冲存储器或能够存储信息的任何其他设备。存储器3204还可以包括数据存储设备，诸如硬盘驱动器、光盘驱动器、磁带驱动器、非易失性固态设备或能够存储信息的任何其他设备。数据库3214可以驻留在存储器3204中，并且可以用于收集和组织由本文描述的各种系统和模块使用的数据。

处理器3202可以在驻留在存储器3204中的操作系统3216的控制下操作。操作系统3216可以管理计算机资源，使得被实施为一个或多个计算机软件应用程序(例如驻留在存储器中的应用程序3218)的计算机程序代码可以具有由处理器3202执行的指令。操作系统3216可以是实时操作系统，其在数据进入时以很少或无缓冲延迟地处理数据，并且具有明确定义和一致的时间约束。在替选实施例中，处理器3202可以直接执行应用程序3218，在这种情况下，可以省略操作系统3216。一个或多个数据结构3220也可以驻留在存储器3204中，并且可以由处理器3202、操作系统3216和/或应用程序3218使用，以存储和/或操纵数据。

i/o接口3206可以提供机器接口，该机器接口将处理器3202可操作地耦接到其他设备和系统，例如网络3212和/或外部资源3210。应用程序3218因此可以经由i/o接口3206进行通信、而与网络3212和/或外部资源3210协同工作，以提供包括本发明实施例的各种特征、功能和/或模块。应用程序3218还可以具有由一个或多个外部资源3210执行的程序代码，或者依赖于由计算机系统3200外部的其他系统或网络组件提供的功能和/或信号。实际上，考虑到几乎无穷无尽的硬件和软件配置是可行的，本领域技术人员将理解，本发明的实施例可以包括以下应用程序：其位于计算机系统3200外部，分布在多个计算机或其他外部资源3210之间，或者由通过网络3212作为服务提供的计算资源(硬件和软件)提供的应用程序，例如云计算设备。

hmi3208可以可操作地耦接到计算机系统3200的处理器3202，以允许用户直接与计算机系统3200交互。hmi3208可以包括视频和/或字母数字显示器、触摸屏、扬声器、以及能够向用户提供信息的任何其他合适的音频和视觉指示器。hmi3208还可以包括输入设备和控制器，例如字母数字键盘、指示设备、键盘、按钮、控制旋钮、麦克风等，其能够接受来自用户的命令或输入并将所输入的输入发送到处理器3202。

总体而言，被执行以实施本发明的实施例的例程，无论是作为操作系统的一部分还是作为特定应用程序、组件、程序、对象、模块或命令序列或其子集实施，在本文中都可以被称为“计算机程序代码”，或简称为“程序代码”。程序代码通常包括计算机可读命令，其在不同时间驻留在计算机中的各种存储器和存储设备中，并且当由计算机中的一个或多个处理器读取和执行时，使得该计算机执行操作，该操作是执行操作所必需的，和/或体现本发明实施例的各个方面的元件。用于执行本发明实施例的操作的计算机可读程序指令可以是例如汇编语言或以一种或多种编程语言的任何组合编写的源代码或目标代码。

可以基于在本发明的具体实施例中实现它的应用来识别本文描述的各种程序代码。然而，应当理解，随后的任何特定程序术语仅仅是为了方便而使用，因此本发明不应限于仅用于由这种术语识别和/或暗示的任何特定应用。此外，考虑到可以将计算机程序组织成例程、过程、方法、模块、对象等的一般无限数量的方式，以及可以在各种软件层之间分配的程序功能的各种方式，其驻留在通常的计算机中(例如，操作系统、库、api、应用程序、小程序、基于web的服务等)，应当理解，本发明的实施例不限于本文描述的程序功能的特定组织和分配。

本文描述的任何应用程序/模块中体现的程序代码能够以各种不同的形式单独地或共同地分配为程序产品。特别地，程序代码可以使用其上具有计算机可读程序指令的计算机可读存储介质来分配，以使处理器执行本发明实施例的各方面。

本质上是非暂时性的计算机可读存储介质可以包括以用于存储信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动的有形介质，例如计算机可读指令、数据结构、程序模块或其他数据。计算机可读存储介质还可以包括ram、rom、可擦除可编程只读存储器(eprom)、电可擦除可编程只读存储器(eeprom)、闪存或其他固态存储器技术、便携式光盘只读存储器(cd-rom)、或其他光学存储器、磁带盒、磁带、磁盘存储器或其他磁存储设备，或可用于存储所需信息并且可由计算机读取的任何其他介质。计算机可读存储介质本身不应被解释为暂时性信号(例如，无线电波或其他传播的电磁波，通过诸如波导的传输介质传播的电磁波，或通过导线传输的电信号)。计算机可读程序指令可以经由网络下载到计算机、另一种类型的可编程数据处理装置，或来自计算机可读存储介质的另一设备，或外部计算机或外部存储设备。

存储在计算机可读介质中的计算机可读程序指令可用于引导计算机、其他类型的可编程数据处理装置或其他设备以特定方式起作用，使得存储在计算机可读介质中的指令产生包括实施流程图、序列图和/或框图中指定的功能、动作和/或操作的指令的制品。计算机程序指令可以被提供给通用计算机、专用计算机或其他可编程数据处理装置的一个或多个处理器，以制造机器，从而经由一个或多个处理器执行的指令使得执行一系列计算，以实施流程图、序列图和/或框图中指定的功能、动作和/或操作。

在某些替选实施例中，流程图、序列图和/或框图中指定的功能、动作和/或操作可以与本发明实施例一致被重新排序、串行处理和/或同时处理。此外，任何流程图、序列图和/或框图可以包括比与本发明的实施例一致的那些更多或更少的块。

本文使用的术语仅用于描述具体实施例的目的，并不意图限制本发明的实施例。如本文所使用的，单数形式“一”、“一个”和“该”旨在也包括复数形式，除非上下文另有明确说明。将进一步理解，当术语“包括”和/或“包含”在本说明书中使用时，指定所述特征、整数、动作、步骤、操作、元件和/或组件的存在，但不排除存在或添加一个或多个其他特征、整数、动作、步骤、操作、元素、组件和/或其组。此外，在详细说明或权利要求中使用术语“包括”、“具有”、“具有(has)”、“带有”、“由......组成”或其变型的范围内，这些术语旨在以类似于术语“包含”的方式包括在内。

尽管已经通过对各种实施例的描述说明了所有本发明，尽管已经相当详细地描述了这些实施例，但申请人并不打算将所附权利要求的范围限制或以任何方式限制于这些细节。本领域技术人员容易想到其他优点和修改。因此，本发明在其更广泛的方面不限于具体细节、代表性装置和方法，以及所示和所述的说明性示例。因此，在不脱离申请人的一般发明构思的精神或范围的情况下，可以偏离这些细节。