CN117742316A - 一种基于带挂车模型的最优轨迹规划方法 - Google Patents

Abstract

本发明公开一种基于带挂车模型的最优轨迹规划方法，创新的边界解析方法，能够便捷、高效处理结构化道路的复杂障碍物信息，结合考虑了带挂车运动学模型的最优控制问题的构建、求解，极大程度地提高轨迹规划在极端狭窄空间和复杂障碍物避让情况下的性能，使带挂车在复杂场景下的算力高效性和可靠性上得到保证。

Description

一种基于带挂车模型的最优轨迹规划方法

技术领域

本发明属于自动驾驶技术领域，涉及到车辆的轨迹规划，具体涉及到一种基于带挂车模型的最优轨迹规划方法。

背景技术

自动驾驶带挂车的运动规划，在结构化道路环境中驾驶是车辆的常见情况，其运动规划需要更高阶的平滑路径，同时需要满足避障的需求，优化算法可以很好的保证这两点需求，所以是该场景轨迹规划中最合适的方法。

对于这种复杂的道路环境和障碍物避让情况，带挂车辆的自动驾驶，相比非带挂的，要更具挑战。由于挂车以铰接的方式连接到牵引车，这相当于增加了一个被动自由度，而系统的输入数量保持不变，这使得系统更加复杂，将此系统用于优化算法中会增加求解成功的难度。

现有技术，在带挂车辆的运动规划中，通常只使用了质点模型，由于模型没有考虑到挂车的影响，为了避免挂车与障碍物的碰撞，只能通过类似增加安全距离的方式进行运动规划。但该规划方式较粗糙，对于一些狭窄场景无法精细化操作，要么安全距离给的太大导致无解，要么安全距离给的太小，虽然有解，但可能会产生碰撞。这使得车辆无法很好的处理多种多样的障碍物环境。

还有些现有技术虽然使用了带挂车运动学模型，在障碍物处理的时候使用增加碰撞势能或者采用多级走廊算法(multi stage corridors)虽然能满足避障要求，但在处理障碍物数量、种类变多后的复杂场景，就显得捉襟见肘，比如不同障碍物需要保持不同的安全距离。有些边界是绝对禁止跨越的，而有些边界在特殊情况可以跨越，比如车道线。传统方法在算力高效性和可靠性上都难以保证。

发明内容

针对上述问题，本发明的主要目的在于设计一种基于带挂车模型的最优轨迹规划方法，解决带挂车在极端狭窄空间和复杂障碍物避让情况下，路径规划和控制困难，以及性能差的技术问题，满足带挂车在结构化道路环境中的驾驶需求。

为了实现上述目的本发明采用如下技术方案：

一种基于带挂车模型的最优轨迹规划方法，包括如下步骤：

步骤1：以车道的中心线为参考线，根据车道上障碍物的情况，计算车辆在车道上的可行驶横向距离并生成横向边界；

步骤2：基于步骤1生成的横向边界构建最优控制问题，所述最优控制问题的构建包括：

S21：构建带挂车运动学模型；

S22：计算检测点相对于参考线的横向距离，所述检测点包括牵引车后轴中心、牵引车前端点、挂车后端点；

S23：构建微分方程，计算检测点与参考线上对应的弧长；

S24：构建回调函数，根据步骤S23计算出不同检测点的弧长值，再根据步骤1中解析的横向边界，查询对应的横向边界值；

S25：针对横向边界构建约束；

S26：构建目标函数；

步骤3：求解步骤2中的最优控制问题，得到横向轨迹。

作为本发明进一步的描述，所述步骤1中，横向边界的生成包括如下步骤：

S101：将参考线分成连续若干段；

S102：根据车道边界，计算每段参考线所对应的左右边界[ll_min，ll_max]；

S103：根据车道上的障碍物位置更新左右边界；

S104：根据不同的绕行需求和障碍物安全距离，将每段的左右边界分为硬边界[ll_min，ll_max]和软边界[sll_min，sll_max]。

作为本发明进一步的描述，所述步骤S21中，构建带挂车运动学模型，表示为：

其中，x，y是牵引车后轴中心在世界坐标系的平面坐标位置，κ是曲率，dκ是曲率变化率，α是挂车与牵引车的角度，l_i是挂车铰接点距离挂车后轴的距离，θ是牵引车的航向角。

作为本发明进一步的描述，所述步骤S22中，计算三个检测点相对于参考线的横向距离，包括如下步骤：

S2201：计算牵引车后轴中心相对于参考线的横向距离l，表示为：

l＝-sinθ_r(x-x_r)+cosθ_r(y-y_r)，

其中，x_r，y_r是牵引车后轴中心在参考线上对应点的坐标值，θ_r是对应的航线角；

S2202：计算牵引车前端点相对于参考线的横向距离l_c，表示为：

l_c＝-sinθ_cr(x_c-x_cr)+cosθ_cr(y_c-y_cr)，

其中，x_cr，y_cr是牵引车前端点在参考线上对应点的坐标值，x_c，y_c是牵引车前端点的世界坐标值，θ_cr是对应的航线角；

S2203：计算挂车后端点相对于参考线的横向距离l_i，表示为：

l_i＝-sinθ_ir(x_i-x_ir)+cosθ_ir(y_i-y_ir)，

其中，x_ir，y_ir是挂车后端点在参考线上对应点的坐标值，x_i，y_i是挂车后端点的世界坐标值，θ_r是对应的航线角。

作为本发明进一步的描述，所述步骤S23中，构建微分方程，计算带挂车运动模型上检测点与参考线上对应的弧长(s)，表示为：

其中，κ_r是对应点的曲率。

作为本发明进一步的描述，所述步骤S24中，构建回调函数，根据不同检测点的s值，查询并更新步骤1中的横向边界；

得到牵引车后轴中心的横向硬边界[l_min，l_max]和横向软边界[sl_min，sl_max]；

得到牵引车前端点的横向硬边界[l_cmin，l_cmax]和横向软边界[sl_cmin，sl_cmax]；

得到挂车后端点的横向硬边界[l_imin，l_imax]和横向软边界[sl_imin，sl_imax]。

作为本发明进一步的描述，所述步骤S25中，构建的约束包括横向硬约束、横向软约束与其他约束，具体包括如下：

S2501：构建横向硬约束，表示为：

l_min≤l≤l_max，

l_cmin≤l_c≤l_cmax，

l_imin≤l_i≤l_imax，

S2502：构建横向软约束，表示为：

sl_min-slack≤l≤sl_max+slack，

sl_cmin-slack_c≤l_c≤sl_cmax+slack_c，

sl_imin-slack_i≤l_i≤sl_imax+slack_i，

其中，slack，slack_c，slack_i分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点对应性横向约束松弛变量；

S2503：其他约束包括曲率κ，曲率变化率dκ，挂车与牵引车的角度α的约束，表示为：

-κ_max≤κ≤κ_max，

-dκ_max≤dκ≤dκ_max，

-α_max≤α≤α_max.

其中，κ_max是曲率最大值，dκ_max是曲率变化率最大值，α_max是挂车转角最大值。

作为本发明进一步的描述，所述步骤S26中，构建目标函数包括状态目标函数、输入目标函数、松弛目标函数，表示为：

J＝J_x+J_u+J_s，

S2601：构建状态目标函数，表示为：

其中，w_l是横向距离权重，w_κ是曲率权重；

S2602：构建输入目标函数，表示为：

其中，w_dκ是曲率变化权重；

S2603：构建松弛目标函数，表示为：

其中，w_slack，w_slackc，w_slacki分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点的松弛变量权重。

作为本发明进一步的描述，所述步骤3中，通过求解器进行最优控制问题的求解，包括如下步骤：

S301：设置求解器时间点个数和时间间隔；

S302：求解步骤2中的最优控制问题，将最优解中所有时间点的x，y，θ，κ组合成一条横向轨迹。

相对于现有技术，本发明的技术效果为：

本发明提供了一种基于带挂车模型的最优轨迹规划方法，创新的边界解析方法，能够便捷、高效处理结构化道路的复杂障碍物信息，结合考虑了带挂车运动学模型的最优控制问题的构建、求解，极大程度地提高轨迹规划在极端狭窄空间和复杂障碍物避让情况下的性能，使带挂车在复杂场景下的算力高效性和可靠性上得到保证。

附图说明

图1为本发明的轨迹规划方法整体流程示意图；

图2为本发明的横向边界生成示意图；

图3为本发明中带挂车运动学模型顶视图。

具体实施方式

下面结合附图对本发明进行详细描述：

在本发明的一种实施例中，公开一种基于带挂车模型的最优轨迹规划方法，参考图1-3所示，该方法包括如下步骤：

步骤1：以车道的中心线为参考线，根据车道上障碍物的情况，计算车辆在车道上的可行驶横向距离并生成横向边界；

步骤2：基于步骤1生成的横向边界构建最优控制问题，所述最优控制问题的构建包括：

S21：构建带挂车运动学模型；

S22：计算检测点相对于参考线的横向距离，所述检测点包括牵引车后轴中心、牵引车前端点、挂车后端点；

S23：构建微分方程，计算检测点与参考线上对应的弧长；

S24：构建回调函数，根据步骤S23计算出不同检测点的弧长值，再根据步骤1中解析的横向边界，查询对应的横向边界值；

S25：针对横向边界构建约束；

S26：构建目标函数；

步骤3：求解步骤2中的最优控制问题，得到横向轨迹。

具体的，本实施例，针对上述步骤进行具体分析，内容如下：

如图2所示，本实施例，上述步骤1中，具体横向边界的生成步骤如下：

S101：将参考线分成连续若干段，

S102：根据车道边界，计算每段参考线所对应的左右边界[ll_min，ll_max]；

S103：根据车道上的障碍物位置更新左右边界；

图2中，根据车道上的障碍物A，更新s₂到s₅的左边界，根据车道上的障碍物B，更新s₆到s₈的右边界；

S104：根据步骤S103的结果构成硬边界[ll_min，ll_max]。假如障碍物的安全距离为sd，则将s₂到s₅的左边界往右推sd，将s₆到s₈的右边界往左推sd，最终得到软边界[sll_min，sll_max]。

如图3所示，本实施例，上述步骤2中，基于步骤1生成的横向边界构建最优控制问题，所述最优控制问题的构建详细内容如下：

所述步骤S21中，构建带挂车运动学模型，表示为：

其中，x，y是牵引车后轴中心在世界坐标系的平面坐标位置，κ是曲率，dκ是曲率变化率，α是挂车与牵引车的角度，l_i是挂车铰接点距离挂车后轴的距离，本实施例的l_i＝8.47m，θ是牵引车的航向角。

所述步骤S22中，计算三个检测点相对于参考线的横向距离，包括如下步骤：

S2201：计算牵引车后轴中心相对于参考线的横向距离l，表示为：

l＝-sinθ_r(x-x_r)+cosθ_r(y-y_r)，

其中，x_r，y_r是牵引车后轴中心在参考线上对应点的坐标值，θ_r是对应的航线角；

S2202：计算牵引车前端点相对于参考线的横向距离l_c，表示为：

l_c＝-sinθ_cr(x_c-x_cr)+cosθ_cr(y_c-y_cr)，

其中，x_cr，y_cr是牵引车前端点在参考线上对应点的坐标值，x_c，y_c是牵引车前端点的世界坐标值，θ_cr是对应的航线角；

S2203：计算挂车后端点相对于参考线的横向距离l_i，表示为：

l_i＝-sinθ_ir(x_i-x_ir)+cosθ_ir(y_i-y_ir)，

其中，x_ir，y_ir是挂车后端点在参考线上对应点的坐标值，x_i，y_i是挂车后端点的世界坐标值，θ_r是对应的航线角。

所述步骤S23中，构建微分方程，计算带挂车运动模型上检测点与参考线上对应的弧长(s)，表示为：

其中，κ_r是对应点的曲率。

所述步骤S24中，构建回调函数，根据不同检测点的s值，查询并更新步骤1中的横向边界；

得到牵引车后轴中心的横向硬边界[l_min，l_max]和横向软边界[sl_min，sl_max]；

得到牵引车前端点的横向硬边界[l_cmin，l_cmax]和横向软边界[sl_cmin，sl_cmax]；

得到挂车后端点的横向硬边界[l_imin，l_imax]和横向软边界[sl_imin，sl_imax]。

所述步骤S25中，构建的约束包括横向硬约束、横向软约束与其他约束，具体包括如下：

S2501：构建横向硬约束，表示为：

l_min≤l≤l_max，

l_cmin≤l_c≤l_cmax，

l_imin≤l_i≤l_imax，

S2502：构建横向软约束，表示为：

sl_min-slack≤l≤sl_max+slack，

sl_cmin-slack_c≤l_c≤sl_cmax+slack_c，

sl_imin-slack_i≤l_i≤sl_imax+slack_i，

其中，slack，slack_c，slack_i分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点对应性横向约束松弛变量；

S2503：其他约束包括曲率κ，曲率变化率dκ，挂车与牵引车的角度α的约束，表示为：

-κ_max≤κ≤κ_max，

-dκ_max≤dκ≤dκ_max，

-α_max≤α≤α_max.

其中，κ_max是曲率最大值，dκ_max是曲率变化率最大值，α_max是挂车转角最大值。

本实施例，在具体的实施中反应了实车物体上的限制，则构建约束，常数取值如下：

k_max取值为0.1，dk_max取值为0.1，α_max取值为π/2。

所述步骤S26中，构建目标函数包括状态目标函数、输入目标函数、松弛目标函数，表示为：

J＝J_x+J_u+J_s，

S2601：构建状态目标函数，表示为：

其中，w_l是横向距离权重，w_κ是曲率权重；

S2602：构建输入目标函数，表示为：

其中，w_dκ是曲率变化权重；

S2603：构建松弛目标函数，表示为：

其中，w_slack，w_slackc，w_slacki分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点的松弛变量权重。

本实施例，在具体的实施中反应了实车物体上的限制，构建目标函数，各权重值如下：

w_l取值为0.01，w_κ取值为10.0，w_dκ取值为1000.0，w_slack取值为0.1，w_slacki取值为10.0，w_slackc取值为1.0。

需要说明的是，在步骤2构建的最优控制问题，通常需要求解器进行求解，市面上的求解器包括多种，求解器的配置参数包括时间点个数和时间间隔。

具体的，本实施例，上述步骤3中，通过求解器进行最优控制问题的求解，包括如下步骤：

S301：设置求解器的时间点个数和时间间隔；其中，一个时间点代表一个轨迹点，时间间隔代表相邻轨迹点之间的距离；

S302：求解步骤2中的最优控制问题，将最优解中所有时间点的x，y，θ，κ组合成一条横向轨迹。

通过上述实施例公开了本发明的技术内容，相对于现有技术，本发明的优势在于以下几点：

1.本发明的边界解析方法能够便捷高效的处理结构化道路的复杂障碍物信息；

2.本发明的最优控制问题构建与求解，极大提高自动驾驶带挂车的轨迹规划，在极端狭窄空间和复杂障碍物避让情况的性能。

以上实施例仅用以说明本发明的技术方案而非限制，本领域普通技术人员对本发明的技术方案所做的其他修改或者等同替换，只要不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种基于带挂车模型的最优轨迹规划方法，其特征在于，包括如下步骤：

步骤1：以车道的中心线为参考线，根据车道上障碍物的情况，计算车辆在车道上的可行驶横向距离并生成横向边界；

步骤2：基于步骤1生成的横向边界构建最优控制问题，所述最优控制问题的构建包括：

S21：构建带挂车运动学模型；

S22：计算检测点相对于参考线的横向距离，所述检测点包括牵引车后轴中心、牵引车前端点、挂车后端点；

S23：构建微分方程，计算检测点与参考线上对应的弧长；

S24：构建回调函数，根据步骤S23计算出不同检测点的弧长值，再根据步骤1中解析的横向边界，查询对应的横向边界值；

S25：针对横向边界构建约束；

S26：构建目标函数；

步骤3：求解步骤2中的最优控制问题，得到横向轨迹。

2.根据权利要求1所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤1中，横向边界的生成包括如下步骤：

S101：将参考线分成连续若干段；

S102：根据车道边界，计算每段参考线所对应的左右边界[ll_min,ll_max]；

S103：根据车道上的障碍物位置更新左右边界；

S104：根据不同的绕行需求和障碍物安全距离，将每段的左右边界分为硬边界[ll_min,ll_maxx]和软边界[sll_min,sll_max]。

3.根据权利要求1所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S21中，构建带挂车运动学模型，表示为：

其中，x，y是牵引车后轴中心在世界坐标系的平面坐标位置，κ是曲率，dκ是曲率变化率，α是挂车与牵引车的角度，l_i是挂车铰接点距离挂车后轴的距离，θ是牵引车的航向角。

4.根据权利要求3所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S22中，计算三个检测点相对于参考线的横向距离，包括如下步骤：

S2201：计算牵引车后轴中心相对于参考线的横向距离l，表示为：

l＝-sinθ_r(x-x_r)+cosθ_r(y-y_r)，

其中，x_r，y_r是牵引车后轴中心在参考线上对应点的坐标值，θ_r是对应的航线角；

S2202：计算牵引车前端点相对于参考线的横向距离l_c，表示为：

l_c＝-sinθ_cr(x_c-x_cr)+cosθ_cr(y_c-y_cr)，

其中，x_cr，y_cr是牵引车前端点在参考线上对应点的坐标值，x_c，y_c是牵引车前端点的世界坐标值，θ_cr是对应的航线角；

S2203：计算挂车后端点相对于参考线的横向距离l_i，表示为：

l_i＝-sinθ_ir(x_i-x_ir)+cosθ_ir(y_i-y_ir)，

其中，x_ir，y_ir是挂车后端点在参考线上对应点的坐标值，x_i，y_i是挂车后端点的世界坐标值，θ_r是对应的航线角。

5.根据权利要求4所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S23中，构建微分方程，计算带挂车运动模型上检测点与参考线上对应的弧长(s)，表示为：

其中，κ_r是对应点的曲率。

6.根据权利要求5所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S24中，构建回调函数，根据不同检测点的s值，查询并更新步骤1中的横向边界；得到牵引车后轴中心的横向硬边界[l_min，l_max]和横向软边界[sl_min，sl_max]；

得到牵引车前端点的横向硬边界[l_cmin，l_cmax]和横向软边界[sl_cmin，sl_cmax]；

得到挂车后端点的横向硬边界[l_imin，l_irmax]和横向软边界[sl_imin，sl_irmax]。

7.根据权利要求6所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S25中，构建的约束包括横向硬约束、横向软约束与其他约束，具体包括如下：

S2501：构建横向硬约束，表示为：

l_min≤l≤l_max，

l_cmin≤l_c≤l_cmax，

l_imin≤l_i≤l_imax，

S2502：构建横向软约束，表示为：

sl_min-slack≤l≤sl_max+slack，

sl_cmin-slack_c≤l_c≤sl_cmax+slack_c，

sl_imin-slack_i≤l_i≤sl_imax+slack_i，

其中，slack，slack_c，slack_i分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点对应性横向约束松弛变量；

S2503：其他约束包括曲率κ，曲率变化率dκ，挂车与牵引车的角度α的约束，表示为：

-κ_max≤κ≤κ_max，

-dκ_max≤dκ≤dκ_max，

-α_max≤α≤α_max.

其中，κ_max是曲率最大值，dκ_max是曲率变化率最大值，α_max是挂车转角最大值。

8.根据权利要求7所述的一一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤S26中，构建目标函数包括状态目标函数、输入目标函数、松弛目标函数，表示为：

J＝J_x+J_u+J_s，

S2601：构建状态目标函数，表示为：

其中，w_l是横向距离权重，w_κ是曲率权重；

S2602：构建输入目标函数，表示为：

其中，w_dκ是曲率变化权重；

S2603：构建松弛目标函数，表示为：

其中，w_slack，w_slackc，w_slacki分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点的松弛变量权重。

9.根据权利要求8所述的一种基于带挂车模型的最优轨迹规划方法，其特征在于：所述步骤3中，通过求解器进行最优控制问题的求解，包括如下步骤：

S301：设置求解器时间点个数和时间间隔；

S302：求解步骤2中的最优控制问题，将最优解中所有时间点的x，y，θ，κ组合成一条横向轨迹。