具体实施方式

以下结合附图对本公开的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本公开，并不用于限制本公开。

图1是根据一示例性实施例示出的一种轨迹规划方法的流程图，如图1所示，轨迹规划方法，包括以下步骤S11～S14。

在步骤S11中，在车辆行驶的过程中，根据车辆当前的状态信息和控制信息，建立车辆的预测控制模型，预测控制模型描述第一坐标系中车辆在每个未来时刻下的控制信息对相应未来时刻的下一时刻下的状态信息的约束。

目前通过先规划、后控制的解耦架构规划车辆未来轨迹时，规划模块单纯根据道路约束对计划轨迹进行求解，不计及车辆的状态信息(包括位置连续性和速度连续性)、控制信息(包括转向角极限和转向速率极限)等，只能通过惩罚函数的人为设计和调参(如经典的三阶导惩罚)进行轨迹可实现性的偏好模拟或后置择取，因此通过先规划、后控制的解耦架构得到的符合道路安全约束的计划轨迹可能是不可实现的。例如计划轨迹中包括一个曲率超限的右转轨迹，将该曲率超限的右转轨迹发送给控制模块，控制模块对该轨迹执行时必然会出现偏差，例如一个已尽最大能力、仍不及计划轨迹的右转，进而导致实际轨迹出现安全风险。

由此，针对先规划、后控制的解耦架构规划车辆未来轨迹时，不考虑车辆惯性与能力(状态信息和控制信息)、规划轨迹可实现性没有保障的问题，提出一种轨迹规划方法，本公开的轨迹规划方法中，在车辆行驶的过程中，通过根据车辆当前的状态信息和控制信息，构建Cartesian坐标系的车辆的预测控制模型；根据车辆所在道路当前的车道中心线，构建将Cartesian坐标系下的状态信息转换为Frenet坐标系下的状态信息的预测状态变换模型；根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，构建Frenet坐标系下的预测道路约束模型，通过上述三个模型将车辆惯性量、控制量以及车辆行驶道路同时进行考量，实现对车辆规控一体的轨迹优化，得到可实现性与安全性兼具的最优计划轨迹，提升车辆通行能力。

基于上述思路，在车辆行驶的过程中，根据当前车辆实时的状态信息和控制信息，以及结合车辆的参数(例如车辆轴距)，可建立预测控制模型，该预测控制模型能够根据当前车辆实时的状态信息和控制信息预测当前时刻之后的多个时刻下的控制信息，且该预测控制模型描述车辆在每个未来时刻下的控制信息对相应未来时刻的下一时刻下的状态信息的约束。

其中，状态信息表征车辆的惯性量，包括位置、航向角和速度等。控制信息表征对车辆的操纵量，包括车辆的转向角、加速度等。其中，例如可通过定位模块中获取车辆的状态信息，可通过底盘模块读取车辆的控制信息。

预测控制模型可能的形式可以包括运动学模型/动力学模型、四轮模型/自行车模型等不同形式，可统一表征为：

其中，p^c为车辆在Cartesian坐标系下的状态信息(包含位置的横坐标X、位置的纵坐标Y、车辆的航向角ψ、车辆的速度V等惯性量)，u为控制信息(包含车辆的转向角δ、加速度a等操纵量)。状态转移方程表征车辆在t时刻的控制信息(操纵量)对状态信息(惯性量)的影响，不等式约束u_min≤u_t≤u_max，表征车辆在t时刻的行为极限，Δu_min≤Δu_t≤Δu_max表征t时刻和t时刻的上一时刻之间的控制信息的变化量。

在步骤S12中，根据车辆所在道路当前的车道中心线，建立车辆的预测状态变换模型，预测状态变换模型描述第一坐标系中车辆在每个未来时刻下的状态信息对第二坐标系中车辆在相应未来时刻下的状态信息的约束，其中，第一坐标系与第二坐标系为不同的坐标系。

为了将Cartesian坐标系(第一坐标系)中的状态信息转换为Frenet坐标系(第二坐标系)中，本公开可根据车辆所在道路当前的车道信息，建立预测状态变换模型。预测状态变换模型例如可以表征为：

其中，p^f为车辆在Frenet坐标系中的状态信息：包含纵向坐标S、横向坐标L、车辆与参考线之间的夹角φ、车辆与左边界的横向距离ΔL^lf、车辆与右边界的横向距离ΔL^rt等。

图2是根据一示例性实施例示出的一种第一坐标中车辆的状态信息和第二坐标系中车辆的状态信息之间的转换关系示意图。

如图2所示，在Cartesian坐标系中车辆的状态信息包括：位置的横坐标为X，纵坐标为Y，车辆与横坐标之间的夹角(航向角)为ψ，将车辆在Cartesian坐标系中的状态信息映射到Frenet坐标系中后，得到车辆在Frenet坐标系中的状态信息包括：车辆的纵向坐标S、横向坐标L、车辆与参考线之间的夹角φ。

在步骤S13中，根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，建立第二坐标系中车辆的预测道路约束模型，预测道路约束模型描述第二坐标系中车辆在每个未来时刻下的可行驶区域对车辆在相应未来时刻下的状态信息的约束。

为了确保车辆行车安全，本公开可根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，建立第二坐标系中车辆的预测道路约束模型。预测道路约束模型例如可以表征为：

其中，p^f为车辆的Frenet状态量(包含纵向坐标S、横向坐标L、参考线夹角φ、与左边界的横向距离ΔL^lf、与右边界的横向距离ΔL^rt等)。本质上，道路约束模型是对车辆的行车安全的考量。

在步骤S14中，根据预测控制模型、预测状态变换模型和预测道路约束模型，得到车辆在多个未来时刻下的目标状态信息，并根据目标状态信息，得到车辆在由多个未来时刻形成的时段内的规划轨迹。

一种实施方式中，根据预测控制模型、预测状态变换模型和预测道路约束模型，例如可通过如下方式得到车辆在多个未来时刻下的目标状态信息：

将多个未来时刻逐一作为目标时刻，获取车辆在目标时刻下的预估状态信息和预估控制信息。之后，根据预估状态信息、预估控制信息，对预测控制模型进行线性化处理，得到对应目标时刻的线性预测控制模型，根据预估状态信息，对预测状态变换模型进行线性化处理，得到对应目标时刻的线性预测状态变换模型，根据线性预测控制模型、线性预测状态变换模型和预测道路约束模型，得到车辆在目标时刻下的目标状态信息。

由于预估控制模型h(·)通常是非线性的，为避免形成非凸非线性优化问题，需要对预测控制模型线性化处理。如果我们对本帧将要解得的未来时刻(t时刻)的p^c值和u值有一个较好的先验估计就能对h(·)进行如下的一阶泰勒展开：

其中，由于不同时刻有各自的先验估计(预估状态信息和预估控制信息)，对预测控制模型线性化处理后不同时刻的系数矩阵也是不同的。

在获取不同时刻的预估状态信息和预估控制信息时，由于计划轨迹有较好帧间连续性，因此大多数时候上帧下发的t时刻下的状态信息和控制信息可以直接作为本帧所需获取的t时刻下的预估状态信息和预估控制信息。不过，在纵向决策突变(如减速避让)的个别帧，还需要估计一个变速系数(根据道路约束模型中的纵向约束计算)，根据该系数对上帧轨迹的状态信息和控制信息进行拉伸、而后再对(p^c,u^c)重新采样，得到最终的预估状态信息和预估控制信息

针对根据车辆所在道路当前的车道中心线建立的预测状态变换模型，由于车道中心线(参考线)不是完美的直线，预测状态变换模型即表征为的g(·)就是非线性的，为避免形成非凸非线性优化问题，需要对预测状态变换模型进行线性化处理。

同理，由于已经对本帧将要解得的未来时刻(记为t时刻)的p^c值有一个较好的先验估计就能对g(·)进行如下的一阶泰勒展开：

其中，由于不同时刻有各自的先验估计，线性化后不同时刻的系数矩阵也是不同的。

事实上，状态变换中的计算是以坐标变换为基础的。为了对Cartesian-Frenet坐标变换进行微分，可借助弗莱纳公式(Frenet–Serret公式)进行推导，得到以下微分关系：

其中，κ为曲率，τ为单位切向量，n为单位法向量，均为Frenet参考线的描述。e_X＝[1 0]^T，e_Y＝[0 1]^T。

针对根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，建立第二坐标系中车辆的预测道路约束模型，由于Frenet坐标系中的状态信息涉及的非线性关系已经纳入对预测状态变换模型进行处理，只要Frenet坐标系中的状态信息的设计恰当，预测道路约束模型就可以建模成为上述表征的这样的线性不等式形式，从而避免对预测道路约束模型的线性化工作。

此外，预测道路约束模型的一种更好的约束方式乐意表征为如下的软约束形式：

其中ε是人为引入的松弛变量；w是人为配置的软度权重，是非负的向量参数，表征各行约束的软度(若w的某行设置为0，则该行约束为硬约束)。相较于硬约束，采用软约束的预测道路约束模型具有两大优势：一是可以体现靠边厌恶(硬约束只要不越界，对求解器就是无差别的；而多层软约束下，靠近边界时求解器的惩罚会加重)，避免车辆贴着边界或障碍物行驶；二是可以避免车辆越界(如偶发压线)后出现求解器无解的情况。

一种实施方式中，通过线性预测控制模型、线性预测状态变换模型和预测道路约束模，在表征车辆惯性与能力(车辆惯性量与控制量)和道路安全(车辆的可行驶区域)的同时考虑下，可通过如下方式得到车辆在目标时刻下的目标状态信息：

获取通过线性预测控制模型、线性预测状态变换模型和预测道路约束模型预测的车辆在目标时刻下的预测状态信息集合，根据预测状态信息集合，得到车辆在目标时刻下的目标状态信息。

其中，例如可通过如下方式得到目标时刻下的目标状态信息：

根据预测状态信息集合，获取目标时刻下的状态信息阈值，根据状态信息阈值，构建在目标时刻下的目标函数，根据预测状态信息集合，对目标函数进行二次规划处理，以获得能够使目标函数的函数值最小的目标状态信息。

示例地，例如可按照如下方式构建目标函数：

其中，表示预测状态信息集合，其同时包含第一坐标系中的状态信息p_t ^c和第二坐标系中的状态信息p_t ^f，为状态信息阈值，为状态跟踪项，||Δu_t||_R为控制平顺项，ε¹为软约束罚项，Q、R是正定的权重矩阵，记号||p||_Q表示二次型p^TQp，t为目标时刻。

根据预测状态信息集合，对目标函数进行二次规划处理，得到目标状态信息之后，根据目标状态信息，可得到车辆在由多个未来时刻形成的时段内的最优的规划轨迹(即状态信息序列)，例如记为同时可一并得到对应的最优的控制信息序列，例如记为

此外，由于规划轨迹和控制信息序列均被确定得到，一种实施方式中，可将控制信息序列一并下发至车辆的底盘的控制系统，以使车辆底盘根据控制信息序列控制车辆，进而避免通过自动驾驶的经典架构中控制模块来完成“轨迹→控制量”的转化，再将控制量下发给车辆底盘控制系统的过程，去除对控制模块的依赖，免去控制模块求解“轨迹→控制量”时对车载系统计算的负担，降低车载计算单元的成本。

通过上述技术方案，在车辆行驶的过程中，通过根据车辆当前的状态信息和控制信息，构建Cartesian坐标系的车辆的预测控制模型；通过根据车辆所在道路当前的车道中心线，构建将Cartesian坐标系下的状态信息转换为Frenet坐标系下的状态信息的预测状态变换模型；通过根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，构建Frenet坐标系下的预测道路约束模型，之后根据上述三个模型将车辆惯性量、控制量以及车辆行驶道路同时进行考量，实现对车辆规控一体的轨迹优化，得到可实现性与安全性兼具的最优计划轨迹，提升车辆通行能力。

图3是根据一示例性实施例示出的一种轨迹规划装置的框图300。参照图3，轨迹规划装置应用于自动驾驶车辆，轨迹规划装置包括：

第一确定模块301，用于在车辆行驶的过程中，根据车辆当前的状态信息和控制信息，建立车辆的预测控制模型，预测控制模型描述第一坐标系中车辆在每个未来时刻下的控制信息对相应未来时刻的下一时刻下的状态信息的约束；

第二确定模块302，用于根据车辆所在道路当前的车道中心线，建立车辆的预测状态变换模型，预测状态变换模型描述第一坐标系中车辆在每个未来时刻下的状态信息对第二坐标系中车辆在相应未来时刻下的状态信息的约束，其中，第一坐标系与第二坐标系为不同的坐标系；

第三确定模块303，用于根据车辆当前的地图信息和车辆所在道路当前的障碍物信息，建立车辆的预测道路约束模型，预测道路约束模型描述第二坐标系中车辆在每个未来时刻下的可行驶区域对车辆在相应未来时刻下的状态信息的约束；

处理模块304，用于根据预测控制模型、预测状态变换模型和预测道路约束模型，得到车辆在多个未来时刻下的目标状态信息，并根据目标状态信息，得到车辆在由多个未来时刻形成的时段内的规划轨迹。

可选地，处理模块304采用如下方式根据预测控制模型、预测状态变换模型和预测道路约束模型，得到车辆在多个未来时刻下的目标状态信息：

将多个未来时刻逐一作为目标时刻，获取车辆在目标时刻下的预估状态信息和预估控制信息；

根据预估状态信息、预估控制信息，对预测控制模型进行线性化处理，得到对应目标时刻的线性预测控制模型；

根据预估状态信息，对预测状态变换模型进行线性化处理，得到对应目标时刻的线性预测状态变换模型；

根据线性预测控制模型、线性预测状态变换模型和预测道路约束模型，得到车辆在目标时刻下的目标状态信息。

可选地，处理模块304采用如下方式根据线性预测控制模型、线性预测状态变换模型和预测道路约束模型，得到车辆在目标时刻下的目标状态信息：

获取通过线性预测控制模型、线性预测状态变换模型和预测道路约束模型预测的车辆在目标时刻下的预测状态信息集合；

根据预测状态信息集合，得到车辆在目标时刻下的目标状态信息。

可选地，处理模块304采用如下方式根据预测状态信息集合，得到目标时刻下的目标状态信息：

获取目标时刻下的状态信息阈值，根据状态信息阈值，构建在目标时刻下的目标函数；

根据预测状态信息集合，对目标函数进行二次规划处理，以获得能够使目标函数的函数值最小的目标状态信息。

可选地，第一坐标系中车辆在每个未来时刻下的状态信息包括：车辆的位置坐标信息、速度信息和航向角信息；

第二坐标系中车辆在每个未来时刻的状态信息，包括：

车辆的纵向坐标、横向坐标、车辆与车道中心线之间的夹角、车辆与道路边界的横向距离。

可选地，按照如下方式构建目标函数：

其中，表示预测状态信息集合，其同时包含第一坐标系中的状态信息p_t ^c和第二坐标系中的状态信息p_t ^f，为状态信息阈值，为状态跟踪项，||Δu_t||_R为控制平顺项，ε²为软约束罚项，Q、R是正定的权重矩阵，记号||p||_Q表示二次型p^TQp，t为目标时刻。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

图4是根据一示例性实施例示出的一种电子设备700的框图。如图4所示，该电子设备700可以包括：处理器701，存储器702。该电子设备700还可以包括多媒体组件703，输入/输出(I/O)接口704，以及通信组件705中的一者或多者。

其中，处理器701用于控制该电子设备700的整体操作，以完成上述的轨迹规划方法中的全部或部分步骤。存储器702用于存储各种类型的数据以支持在该电子设备700的操作，这些数据例如可以包括用于在该电子设备700上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器702可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read-Only Memory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件703可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器702或通过通信组件705发送。音频组件还包括至少一个扬声器，用于输出音频信号。I/O接口704为处理器701和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件705用于该电子设备700与其他设备之间进行有线或无线通信。无线通信，例如Wi-Fi，蓝牙，近场通信(Near FieldCommunication，简称NFC)，2G、3G、4G、NB-IOT、eMTC、或其他5G等等，或它们中的一种或几种的组合，在此不做限定。因此相应的该通信组件705可以包括：Wi-Fi模块，蓝牙模块，NFC模块等等。

在一示例性实施例中，电子设备700可以被一个或多个应用专用集成电路(Application Specific Integrated Circuit，简称ASIC)、数字信号处理器(DigitalSignal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的轨迹规划方法。

在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的轨迹规划方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器702，上述程序指令可由电子设备700的处理器701执行以完成上述的轨迹规划方法。

在另一示例性实施例中，还提供一种计算机程序产品，该计算机程序产品包含能够由可编程的装置执行的计算机程序，该计算机程序具有当由该可编程的装置执行时用于执行上述的轨迹规划方法的代码部分。

以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，公开对各种可能的组合方式不再另行说明。

此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。