用于驾驶辅助系统的路径规划方法及驾驶辅助系统与流程

1.本发明属于自动控制领域，特别涉及一种用于驾驶辅助系统的路径规划方法及驾驶辅助系统。


背景技术：

2.随着人工智能时代的到来，自动驾驶已经成为当今热门的研究方向之一。人们对于自动驾驶汽车最为关注的就是在行驶过程中的安全性问题。所以一般针对于自动驾驶的汽车都具有驾驶辅助系统。
3.驾驶辅助系统主要是协助汽车有车道保持辅助系统、自动泊车辅助系统、刹车辅助系统、倒车辅助系统和行车辅助系统。对于行车辅助主要是根据车辆当前的状态及其所属的环境规划路径，并控制车辆按照规划的路径进行行驶。
4.常规的路径规划控制是将横向控制与纵向控制拆分成两个独立模块运行，如常见的lka、acc等。这样规划处的横向路径与纵向路径是相互独立的，同时进行独立的控制。这样路径的规划与控制不能同时兼顾到横向和纵向的情况。


技术实现要素：

5.发明目的：本发明针对现有技术存在的问题，提出了一种能够通过考虑车辆横向和纵向情况的用于驾驶辅助系统的路径规划方法。
6.技术方案：为实现上述目的，本发明提供了一种用于驾驶辅助系统的行车路径调整规划方法，包括以下步骤：
7.步骤1：通过传感器采集当前车辆状态及车辆周围的障碍物状态；其中，车辆状态包括车辆的位置坐标、速度和加速度；车辆周围障碍物状态包括障碍物的位置坐标和速度信息；
8.步骤2：将步骤1采集得到的转换到当前车辆状态及车辆周围的障碍物状态均转换到frenet坐标系下；
9.步骤3：设定调整总时长t，分别在调整总时长t内进行独立的横向路径规划和纵向路径规划；
10.步骤4：将步骤3中规划的横向路径和纵向路径依次耦合；得到耦合后的路径轨迹；
11.步骤5：分别计算步骤4得到的每条耦合后的路径轨迹的平滑度g_grad，耦合后的路径轨迹到车道中心线的横向距离d_lat和耦合后的路径轨迹的航向角与车道航向角的差值yaw_diff；根据公式：
12.confidence＝x
t
px；
13.计算置信度confidence，选择置信度高的路径为最终的车辆行驶路径；其中，x＝[g_grad,d_lat,yaw_diff]，p为协方差，x
t
表示x的转置矩阵。
[0014]
进一步，将调整总时长t平均分成多个调整区间，根据每个调整时间点分别规划车辆的横向和纵向的状态，将每个调整时间点，车辆的横向状态和纵向状态分别形成横向路
径轨迹和纵向路径轨迹。采用这样的方式规划每个时间点上的车辆状态，再将每个时间点上车辆的位置连接起来形成轨迹；按照时间采样，符合车辆运动特征，便于将连续问题转换成离散问题，更加方便规划。
[0015]
进一步，根据每个调整时间点规划车辆的横向状态(di，v
横
，a
横
)的方法为：分别根据车辆在不同调整时间点回到车辆车道中心线行驶进行规划；根据公式dj＝d*(t
i-tj)/(t
i-t0)得到在第i个调整时间点，车辆到达车道中心线的情况下，在第j个调整时间点车辆在frenet坐标系中的横向位置dj；其中，i＝1、2、
…
、n；j＝1、2、
…
、i-1；在第i个调整时间点前，每个调整时间点时，di＝dj，车辆的横向状态为(dj，0，0)；在第i个调整时间点以后，每个调整时间点时，di＝0，车辆的横向状态为(0，0，0)；其中，di表示在第i个调整时间点车辆在frenet坐标系中的横向位置；v
横
表示在frenet坐标系中车辆的横向速度，a
横
表示在frenet坐标系中车辆的横向加速度。这样的规划方法更加的全面，促使后续找到最优的路径。
[0016]
进一步，根据每个调整时间点规划车辆的纵向状态的方法为：根据车辆前方障碍物的位置、速度信息，规划纵向路径中每个调整时间点时车辆的纵向状态(si，v
i纵
，a
纵
)，其中si表示第i个调整时间点的纵向距离，v
i纵
表示车辆在第i个调整时间点的纵向速度；
[0017]
当车辆前方不存在障碍物，则车辆以当前速度做匀速运动，对应每个调整时间点，车辆的纵向距离为si＝v
纵
*(t
i-t0)，i＝1、2、
…
、n；其中，si表示第i个调整时间点的纵向距离，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度；纵向路径中每个调整时间点对应的车辆的纵向状态为(v
纵
*(t
i-t0)，v
纵
，0)；
[0018]
当车辆前方存在障碍物，则比较障碍物与车辆之间的行驶速度：
[0019]
如果障碍物的行驶速度不小于车辆的行驶速度，则车辆以当前速度做匀速运动，对应每个调整时间点，车辆的纵向距离为si＝v
纵
*(t
i-t0)，i＝1、2、
…
、n；其中，si表示第i个调整时间点的纵向距离，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度；纵向路径中每个调整时间点对应的车辆的纵向状态为(v
纵
*(t
i-t0)，v
纵
，0)；
[0020]
如果障碍物的行驶速度小于车辆的行驶速度，则车辆做匀减速运动，其中其中，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度；v0表示在调整起始时刻t0时，障碍物的纵向速度；l表示在调整起始时刻t0时，车辆与障碍物之间的纵向距离；第i个调整时间点时，车辆的纵向距离为车辆的纵向速度为v
i纵
＝v-a
纵
(t
i-t0)，纵向路径中每个调整时间点对应的车辆的纵向状态为v-a
纵
(t
i-t0)，a
纵
)；其中，i＝1、2、
…
、n。这样的规划方法更加的全面，促使后续找到最优的路径。
[0021]
进一步，所述将步骤3中规划的横向路径和纵向路径依次耦合的方法为：将相同调整时间点的横向状态和纵向状态进行耦合，生成对应调整时间的综合状态；其中，综合状态包括耦合后的车辆位置，耦合后的车辆速度和耦合后的车辆的加速度。这样的耦合方法不仅简单，而且耦合后状态兼顾到车辆的横向状态和纵向状态。
[0022]
进一步，得到耦合后的路径轨迹耦合后的路径轨迹到车道中心线的横向距离d_lat为耦合后每个调整时间点对应的车辆所在位置的点到车道中心线的横向距离的平均
值；耦合后的路径轨迹的航向角与车道航向角的差值yaw_diff即为每个调整时间点对应的车辆的航向角与车道航向角的差值的平均值。这样得到的路径更优化。
[0023]
本发明还提供了一种驾驶辅助系统，包括数据采集模块、路径规划模块和控制模块，其中，
[0024]
数据采集模块采集当前车辆状态及车辆周围的障碍物状态；并将采集到的数据发送至路径规划模块；
[0025]
路径规划模块将接收到的数据转换到到frenet坐标系下，在调整总时长t内进行独立的横向路径规划和纵向路径规划；再将规划的横向路径和纵向路径依次耦合；得到耦合后的路径轨迹；根据公式confidence＝x
t
px计算每条耦合后的路径轨迹的置信度，将置信度最高的路径轨迹数据发送到控制模块；其中，x＝[g_grad,d_lat,yaw_diff]，p为协方差，x
t
表示x的转置矩阵；g_grad为耦合后的路径轨迹的平滑度；d_lat为耦合后的路径轨迹到车道中心线的横向距离；yaw_diff为耦合后的路径轨迹的航向角与车道航向角的差值。
[0026]
控制模块根据接收到的路径轨迹数据控制车辆沿着对应的路径行驶。
[0027]
进一步，将调整总时长t平均分成多个调整区间，根据每个调整时间点分别规划车辆的横向和纵向的状态，将每个调整时间点车辆的横向状态和纵向状态分别形成横向路径轨迹和纵向路径轨迹。
[0028]
进一步，还包括车道线识别模块，车道线识别模块实时将识别出的车道线坐标发送到路径规划模块；路劲规划模块根据接收到的车道线坐标可以实时获取车道中心线坐标。
[0029]
本发明还提供了一种存储软件的计算机可读介质，所述软件包括能通过一个或多个计算机执行的指令，所述指令通过这样的执行使得所述一个或多个计算机执行操作，所述操作包括如签署的用于驾驶辅助系统的行车路径调整规划方法的流程。
[0030]
有益效果：与现有技术相比，本发明规划的路径实现简单，计算量小，获取路径的速度快，鲁棒性强；将横向与纵向规划先解耦再耦合得到最优路径，能够辅助驾驶员在不变道的情况下，控制车辆沿着车道中心线行驶，并根据车辆前方障碍物状态，控制车辆速度；本发明提供的方法适用于干线物流等特定交通运输场景，保障交通安全。
附图说明
[0031]
图1为本发明提供的一种实施例的驾驶辅助系统结构示意图；
[0032]
图2为本发明提供的方法流程图；
[0033]
图3为纵向路径规划方法示意图；
[0034]
图4为本发明提供的另一种实施例的驾驶辅助系统结构示意图。
具体实施方式
[0035]
下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0036]
如图1所示，本实施例公开了一种驾驶辅助系统，主要包括数据采集模块、路径规
划模块和控制模块，其中，
[0037]
数据采集模块采集当前车辆状态及车辆周围的障碍物状态；并将采集到的数据发送至路径规划模块；
[0038]
路径规划模块将接收到的数据转换到到frenet坐标系下，在调整总时长t内进行独立的横向路径规划和纵向路径规划；再将规划的横向路径和纵向路径依次耦合；得到耦合后的路径轨迹；根据公式confidence＝x
t
px计算每条耦合后的路径轨迹的置信度，将置信度最高的路径轨迹数据发送到控制模块；其中，x＝[g_grad,d_lat,yaw_diff]，p为协方差，x
t
表示x的转置矩阵；g_grad为耦合后的路径轨迹的平滑度；d_lat为耦合后的路径轨迹到车道中心线的横向距离；yaw_diff为耦合后的路径轨迹的航向角与车道航向角的差值。
[0039]
控制模块根据接收到的路径轨迹数据控制车辆沿着对应的路径行驶。
[0040]
如图2所示，路径规划模块中采用的行车路径调整规划方法，主要包括以下步骤：
[0041]
步骤1：通过传感器采集当前车辆状态及车辆周围的障碍物状态；其中，车辆状态包括车辆的位置坐标、速度和加速度；车辆周围障碍物状态包括障碍物的位置坐标和速度信息；其中，通过传感器采集到的车辆的位置坐标和障碍物的位置坐标均在笛卡尔坐标系下。
[0042]
步骤2：将步骤1采集得到的转换到当前车辆状态及车辆周围的障碍物状态值均转换到frenet坐标系下；
[0043]
步骤3：设定调整总时长t，分别在调整总时长t内进行独立的横向路径规划和纵向路径规划；其中t优选为10秒，这样使系统有足够的反应和调整时间。
[0044]
其中，将调整总时长t平均分成n个调整区间，则按照时间发展顺序，调整时间点集合为[t0、t1、t2、
…
、ti、
…
tn]；其中，t0表示调整起始时刻，t1表示第一个调整时间点，以此类推，ti表示第i个调整时间点；i表示调整时间点的编号，tn表示第n个调整时间点，t
n-t0＝t；
[0045]
根据每个调整区间分别进行横向和纵向路径规划；
[0046]
横向路径规划方法为：在调整起始时刻t0时，车辆的横向状态为(d，v
横
，a
横
)，d表示在frenet坐标系中车辆的横向位置，即车辆距离车道中心线的距离；v
横
表示在frenet坐标系中车辆的横向速度，a
横
表示在frenet坐标系中车辆的横向加速度；其中，车辆横向的速度v
横
和加速度a
横
均为0。横向的调整目的是为了让车辆在车道线的中心线上行驶，所以横向路径规划即是将车辆的状态调整为(0，0，0)。
[0047]
调整总时长t中有n个调整时间点，一般n为100，这样可以确保精度满足控制需求。按照车辆在每个调整时间点都可能到达车道中心线进行规划，可以有n条路径轨迹，每条路径轨迹上每个调整时间点对应车辆横向状态为(di，0，0)。即如果第i个调整时间点车辆回到车道中心线，则根据公式dj＝d*(t
i-tj)/(t
i-t0)得到在第j个调整时间点车辆在frenet坐标系中的横向位置dj；j表示车辆没有回到车道中心线的调整时间点的编号；其中，i＝1、2、
…
、n；j＝1、2、
…
、i-1；从而在ti调整时间点前，每个调整时间点时，车辆的横向状态为(dj，v
横
，a
横
)；从第i调整点之后的所有调整时间点车辆在frenet坐标系中的横向位置d的值均为0，所以车辆的横向状态为(0，0，0)；所以在车辆没有回到车道中心线时，di＝dj，车辆回到车道中心线后，di＝0。从而根据i的取值，可以得到n条横向路径轨迹。
[0048]
如果在t1调整时间点，车辆到达车道中心线，则从t1时刻开始后面每个时刻车辆的横向状态为(0，0，0)，将每个调整时间点对应的位置点按照时间的顺序连接起来可以得到
第一条横向路径轨迹。
[0049]
如果在t2调整时间点，车辆到达车道中心线，则根据公式d1＝d*(t
2-t1)/(t
2-t0)得到在t1调整时间点，车辆距离车道中心线的距离d1，在t1调整时间点车辆的横向状态为(d1，0，0)；从t2时刻开始后面每个时刻车辆的横向状态为(0，0，0)，将每个调整时间点对应的位置点按照时间的顺序连接起来可以得到第二条横向路径轨迹。
[0050]
如果在t3调整时间点，车辆到达车道中心线，则根据公式d1＝d*(t
3-t1)/(t
3-t0)得到在t1调整时间点，车辆距离车道中心线的距离d1，在t1调整时间点车辆的横向状态为(d1，0，0)；则根据公式d2＝d*(t
3-t2)/(t
3-t0)得到在t2调整时间点，车辆距离车道中心线的距离d2，在t1调整时间点车辆的横向状态为(d2，0，0)；从t3时刻开始后面每个时刻车辆的横向状态为(0，0，0)，将每个调整时间点对应的位置点按照时间的顺序连接起来可以得到第三条横向路径轨迹。
[0051]
在ti调整时间点，车辆到达车道中心线，则根据公式dj＝d*(t
i-tj)/(t
i-t0)依次计算得到在第j个调整时间点，车辆距离车道中心线的距离dj，其中j＝1、2、
…
、i-1，且i《n。从ti时刻开始后面每个时刻车辆的横向状态为(0，0，0)，将每个调整时间点对应的位置点按照时间的顺序连接起来可以得到第i条横向路径轨迹。
[0052]
依次类推，直到在tn调整时间点，车辆到达车道中心线，通过每个调整时间点，车辆距离车道中心的距离计算，并将所有的位置点进行连接，得到第n条横向路径轨迹。
[0053]
从而完成横向路径规划。在横向路径规划中，根据平均分割的调整区间个数可以得到相同数量的横向路径轨迹。
[0054]
纵向路径规划方法为：根据车辆前方障碍物的位置、速度信息，进行纵向路径规划，得到纵向路径中每个调整时间点时车辆的纵向状态(si，v
i纵
，a
纵
)，其中si表示第i个调整时间点的纵向距离，v
i纵
表示车辆在第i个调整时间点的纵向速度，如图3所示，主要有以下几种状态：
[0055]
如果车辆前方不存在障碍物，则车辆以当前速度做匀速运动，对应每个调整时间点，车辆的纵向距离为si＝v
纵
*(t
i-t0)，i＝1、2、
…
、n；其中，si表示第i个调整时间点的纵向距离，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度。所以每个调整时间点，都对应一个轨迹点，将每个点按照时间的顺序连接起来则形成了规划的纵向路径，则该路径中每个调整时间点对应的车辆的纵向状态为(v
纵
*(t
i-t0)，v
纵
，0)。
[0056]
如果车辆前方存在障碍物，则比较障碍物与车辆之间的行驶速度：
[0057]
如果障碍物的行驶速度不小于车辆的行驶速度，则车辆以当前速度做匀速运动，对应每个调整时间点，车辆的纵向距离为si＝v
纵
*(t
i-t0)，i＝1、2、
…
、n；其中，si表示第i个调整时间点的纵向距离，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度。该路径中每个调整时间点对应的车辆的纵向状态为(v
纵
*(t
i-t0)，v
纵
，0)。
[0058]
如果障碍物的行驶速度小于车辆的行驶速度，则车辆做匀减速运动，其中其中，v
纵
表示在frenet坐标系中t0时刻车辆的纵向速度，v0表示在调整起始时刻t0时，障碍物的纵向速度，l表示在调整起始时刻t0时，车辆与障碍物之间的纵向距离。第i个调整时间点，v
i纵
＝v-a
纵
(t
i-t0)，该路径中每个调整时间点对应
的车辆的纵向状态为v-a
纵
(t
i-t0)，a
纵
)；其中，i＝1、2、
…
、n。
[0059]
步骤4：将步骤3中规划的横向路径和纵向路径依次耦合；得到n条耦合后的路径轨迹；其中，横向路径与纵向路径耦合的方法为：将第e条横向路径中和纵向路径中相同调整时间点的横向状态(d
e-i
，0，0)和纵向状态(si，v
i纵
，a
纵
)进行耦合，d
e-i
表示第e条横向路径中在第i个调整时间点，车辆距离车道中心线的距离，将每个相同调整时间点依次进行耦合，得到新的车辆位置点集合，将新的车辆位置点集合按照调整时间点进行连接，则得到耦合后的路径，其中，e＝1、2、
…
、n；依次类推，将每一条横向路径与纵向路径依次耦合，则得到n条耦合后的路径轨迹。
[0060]
步骤5：分别计算步骤4得到的每条耦合后的路径轨迹的平滑度g_grad，耦合后的路径轨迹到车道中心线的横向距离d_lat和耦合后的路径轨迹的航向角与车道航向角的差值yaw_diff；根据公式：
[0061]
confidence＝x
t
px；
[0062]
计算置信度confidence，选择置信度高的路径为最终的车辆行驶路径。其中，x＝[g_grad,d_lat,yaw_diff]，p为协方差，x
t
表示x的转置矩阵。耦合后的路径轨迹到车道中心线的横向距离d_lat即为耦合后的路径上所有轨迹点到车道中心线的横向距离的平均值；耦合后的路径轨迹的航向角与车道航向角的差值yaw_diff即为耦合后的路径上所有轨迹点的航向角与车道航向角的差值的平均值。
[0063]
如图4所示，在驾驶辅助系统中还可以包括车道线识别模块，车道线识别模块通过车载传感器采集车道线，并将采集到的车道线转换到以车辆为参考点的笛卡尔坐标系下，再转换到frenet坐标系，从而实时将得到的车道线在frenet坐标系下的坐标发送到路径规划模块；路径规划模块根据接收到的车道线坐标可以实时获取车道中心线坐标。这样能够方便路径规划模块更快的获取车辆与车道中心线之间的位置关系。
[0064]
本发明还提供了一种存储软件的计算机可读介质，所述软件包括能通过一个或多个计算机执行的指令，所述指令通过这样的执行使得所述一个或多个计算机执行操作，所述操作包括如前述外排型排土场的无人调度方法的流程。