CN113147750B - 一种控制车辆行驶的安全决策系统及方法 - Google Patents

一种控制车辆行驶的安全决策系统及方法 Download PDF

Info

Publication number
CN113147750B
CN113147750B CN202110554703.9A CN202110554703A CN113147750B CN 113147750 B CN113147750 B CN 113147750B CN 202110554703 A CN202110554703 A CN 202110554703A CN 113147750 B CN113147750 B CN 113147750B
Authority
CN
China
Prior art keywords
obstacle
vehicle
obs
information
road surface
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110554703.9A
Other languages
English (en)
Other versions
CN113147750A (zh
Inventor
王红
于文浩
李骏
吴佳航
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN202110554703.9A priority Critical patent/CN113147750B/zh
Publication of CN113147750A publication Critical patent/CN113147750A/zh
Application granted granted Critical
Publication of CN113147750B publication Critical patent/CN113147750B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2552/00—Input parameters relating to infrastructure
    • B60W2552/50—Barriers

Abstract

本发明实施例公开一种控制车辆行驶的安全决策系统及方法，系统包括：感知子系统、决策子系统及控制子系统，其中，感知子系统用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；决策子系统用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；控制子系统用于根据得到的所述路径信息控制汽车自动行驶。本发明实施例充分考虑了路面凸起或凹陷障碍物的大小、形状和障碍物尖锐程度对车辆行驶的影响，提高了车辆按照所规划路径进行行驶的安全性。

Description

一种控制车辆行驶的安全决策系统及方法
技术领域
本发明涉及自动驾驶车辆的技术，特别涉及一种控制车辆行驶的安全决策系统及方法。
背景技术
在自动驾驶车辆的过程中，对智能汽车的路径规划和避障是反应智能汽车自主能力的关键要素之一，也是解决自动驾驶安全的问题之一。人工势场法路径规划是智能汽车的路径规划中较成熟的及实时性较好的规划技术。公告号“CN102591332A”的专利申请中，公开了一种用于无人驾驶汽车局部路径规划的装置以及方法，包括感知装置和基于人工势场的路径规划方法，通过环境传感器探测障碍物，建立道路边界和道路中心线模型，建立斥力函数和引力函数，解决了人工势场中的局部最优解问题。
但是，目前采用人工势场法进行路径规划时，没有考虑路面凸起或凹陷障碍物的大小、形状和障碍物尖锐程度对车辆行驶的影响，不能充分反应和解决路面障碍物的对车辆行驶的潜在威胁。
发明内容
有鉴于此，本发明实施例提供一种控制车辆行驶的安全决策系统，该系统能够在采用人工势场法进行路径规划时将路面障碍物对车辆行驶的影响因素引入，提高车辆基于所规划的路径行驶时的安全性。
本发明实施例还提供一种控制车辆行驶的安全决策方法，该方法能够在采用人工势场法进行路径规划时将路面障碍物对车辆行驶的影响因素引入，提高车辆基于所规划的路径行驶时的安全性。
本发明是这样实现的：
一种控制车辆行驶的安全决策系统，所述系统包括：感知子系统、决策子系统及控制子系统，其中，
感知子系统，用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；
决策子系统，用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；
控制子系统，用于根据得到的所述路径信息控制汽车自动行驶。
较佳地，所述路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)；所述车辆行驶信息包括：当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx,vy)、及当前时刻的车辆横向和纵向加速度(ax,ay)；
所述感知子系统还包括：激光雷达模块、全球定位系统GPS模块及惯性测量单元IMU模块和第一单片机模块，其中，
激光雷达模块，用于扫描路面信息，获得路面点云数据Q，发送给第一单片机单元；
第一单片机模块，用于从激光雷达模块接收路面点云数据Q，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，将路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)发送给决策子系统；
GPS模块，用于获取当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx,vy)，发送给决策子系统；
IMU模块，用于获取当前时刻的车辆横向和纵向加速度(ax,ay)，发送给决策子系统。
较佳地，所述决策子系统由第二单片机模块实现，还用于从第一单片机模块接收路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，从GPS模块接收当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx,vy)，从IMU模块接收当前时刻的车辆横向和纵向加速度(ax,ay)；
所述决策子系统，还用于采用路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)和车辆质心坐标(xcar,t,ycar,t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx,vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax,ay)计算车辆和障碍物之间的最小安全距离R0；
所述决策子系统，还用于对路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)进行均值计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度或深度hobs；根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物的尖锐程度S；根据障碍物高度或深度hobs和障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物对车辆的阻碍程度H；
所述决策子系统，还用于根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用模型预测控制MPC算法基于人工势场U求解得到车辆行驶的新的路径信息，发送给控制子系统。
较佳地，所述决策子系统，还用于判断障碍物是凸起障碍物或者凹陷障碍物为：当均值时，为凸起障碍物，障碍物的高度或深度hobs为max(zobs,i)；当均值时，为凹陷障碍物，障碍物的高度或深度hobs为min(zobs,i)。
较佳地，所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息；
所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息。
一种控制车辆行驶的安全决策方法，所述方法包括：
采集路面障碍物轮廓信息及车辆行驶信息；
根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息和路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息；
根据得到的所述路径信息控制汽车自动行驶。
较佳地，所述路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，所述采集路面障碍物轮廓信息包括：
由设置的激光雷达模块扫描路面信息，获得路面点云数据Q后，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)；
所述车辆行驶信息包括：当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx,vy)、及当前时刻的车辆横向和纵向加速度(ax,ay)，所述采集车辆行驶信息包括：
当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ及当前时刻的车辆横向和纵向速度(vx,vy)由设置的GPS模块采集；
当前时刻的车辆横向和纵向加速度(ax,ay)由设置的IMU模块采集。
较佳地，所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息；
所述计算得到路径异常状态信息包括：
对路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)进行均值计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度或深度hobs；根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物的尖锐程度S；根据障碍物高度或深度hobs和障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物对车辆的阻碍程度H；
所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息；
所述计算得到车辆与路面障碍物之间的关系信息包括：
采用路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)和车辆质心坐标(xcar,t,ycar,t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx,vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax,ay)计算车辆和障碍物之间的最小安全距离R0。
较佳地，所述判断障碍物是凸起障碍物或者凹陷障碍物包括：
当均值时，为凸起障碍物，障碍物的高度或深度hobs为max(zobs,i)；当均值/>时，为凹陷障碍物，障碍物的高度或深度hobs为min(zobs,i)。
较佳地，所述得到车辆行驶的新的路径信息包括：
根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用MPC算法基于人工势场U求解得到车辆行驶的新的路径信息。
如上所见，本发明实施例提供的系统包括：感知子系统、决策子系统及控制子系统，其中，感知子系统用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；决策子系统用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；控制子系统用于根据得到的所述路径信息控制汽车自动行驶。由于本发明实施例在采用人工势场法进行路径规划时，基于所采集的路面障碍物轮廓信息得到的路径异常状态信息进行，所以充分考虑了路面凸起或凹陷障碍物的大小、形状和障碍物尖锐程度对车辆行驶的影响，提高了车辆按照所规划路径进行行驶的安全性。
附图说明
图1为本发明实施例提供的一种控制车辆行驶的安全决策系统结构示意图；
图2为本发明实施例提供的一种控制车辆行驶的安全决策系统具体例子的结构示意图；
图3为本发明实施例提供的一种控制车辆行驶的安全决策方法流程图；
图4为本发明实施例提供的一种控制车辆行驶的安全决策方法具体例子流程图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本发明进一步详细说明。
从背景技术可以看出，造成车辆基于采用人工势场法所规划的路径进行行驶时，安全性不高的主要原因为：在路径规划过程中没有考虑路面凸起或凹陷障碍物的大小、形状和障碍物尖锐程度对车辆行驶的影响，不能充分反应和解决路面障碍物的对车辆行驶的潜在威胁。为了解决这个问题，本发明实施例提供了一种控制车辆行驶的安全决策系统，在进行路径规划时，不仅考虑到满足车辆行驶的安全性能要求，还考虑到路面障碍物的大小、形状和尖锐程度对车辆行驶的影响，采用人工势场法进行，使用模型预测控制(MPC，ModelPredictive Control)算法不断地规划最优的车辆行驶路径，解决了自动驾驶汽车的路径规划问题。
具体地说，本发明实施例提供的系统包括：感知子系统、决策子系统及控制子系统，其中，感知子系统用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；决策子系统用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；控制子系统用于根据得到的所述路径信息控制汽车自动行驶。由于本发明实施例在采用人工势场法进行路径规划时，基于所采集的路面障碍物轮廓信息得到的路径异常状态信息进行，所以充分考虑了路面凸起或凹陷障碍物的大小、形状和障碍物尖锐程度对车辆行驶的影响，提高了车辆按照所规划路径进行形式的安全性。
图1为本发明实施例提供的一种控制车辆行驶的安全决策系统结构示意图，所述系统包括：感知子系统、决策子系统及控制子系统，其中，
感知子系统，用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；
决策子系统，用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；
控制子系统，用于根据得到的所述路径信息控制汽车自动行驶。
参考图2所示的控制车辆行驶的安全决策系统具体例子的结构示意图，对本发明实施例提供的控制车辆行驶的安全决策系统进行详细说明。
在该系统中，感知子系统具体包括激光雷达模块、全球定位系统(GPS，GlobalPositioning System)模块及惯性测量单元(IMU，Inertial Measurement Unit)模块和第一单片机模块，其中，
激光雷达模块，用于扫描路面信息，获得路面点云数据Q，发送给第一单片机单元；
第一单片机模块，用于从激光雷达模块接收路面点云数据Q，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，将路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)发送给决策子系统；
GPS模块，用于获取当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx,vy)，发送给决策子系统；
IMU模块，用于获取当前时刻的车辆横向和纵向加速度(ax,ay)，发送给决策子系统。
可以看出，感知子系统中包括了各种类型的车辆行驶感知模块，采集得到的路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)；车辆行驶信息包括：当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx,vy)、及当前时刻的车辆横向和纵向加速度(ax,ay)。
在该系统中，决策子系统包括第二单片机模块，进行路径规划都是由第二单片机模块完成。
第二单片机模块，用于从第一单片机模块接收路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，从GPS模块接收当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx,vy)，从IMU模块接收当前时刻的车辆横向和纵向加速度(ax,ay)；采用路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)和车辆质心坐标(xcar,t,ycar,t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx,vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax,ay)计算车辆和障碍物之间的最小安全距离R0；对路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)进行均值/>计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度(深度)hobs；根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物的尖锐程度S；根据障碍物高度(深度)hobs和路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物对车辆的阻碍程度H；
根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用MPC算法基于人工势场U求解得到车辆行驶的新的路径信息，发送给控制子系统。
在这里，判断障碍物是凸起障碍物或者凹陷障碍物的过程包括：当时，为凸起障碍物，障碍物的高度(深度)hobs＝max(zobs,i)；当/>时，为凹陷障碍物，障碍物的高度(深度)hobs＝min(zobs,i)。
可以看出，所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息；所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息。
在该系统中，所述控制子系统为车辆原有系统，当接收到决策子系统发送的路径信息后，基于该路径信息对车辆的行驶进行控制，相应更改车辆的行驶状态或/和行驶轨迹。
图3为本发明实施例提供的一种控制车辆行驶的安全决策方法流程图，其具体步骤包括：
步骤301、采集路面障碍物轮廓信息及车辆行驶信息；
步骤302、根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息和路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息；
步骤303、根据得到的所述路径信息控制汽车自动行驶。
在该方法中，所述采集得到的路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，过程为：
由设置的激光雷达模块扫描路面信息，获得路面点云数据Q后，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)。
在该方法中，所述车辆行驶信息包括：当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx,vy)、及当前时刻的车辆横向和纵向加速度(ax,ay)，其中，当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ及当前时刻的车辆横向和纵向速度(vx,vy)由设置的GPS模块采集；当前时刻的车辆横向和纵向加速度(ax,ay)由设置的IMU模块采集。
在该方法中，所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息，所述计算得到路径异常状态信息包括：
对路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)进行均值计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度(深度)hobs；根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物的尖锐程度S；根据障碍物高度(深度)hobs和路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物对车辆的阻碍程度H。
进一步地，判断障碍物是凸起障碍物或者凹陷障碍物的过程包括：当时，为凸起障碍物，障碍物的高度(深度)hobs＝max(zobs,i)；当/>时，为凹陷障碍物，障碍物的高度(深度)hobs＝min(zobs,i)。
在该方法中，所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息，所述计算得到车辆与路面障碍物之间的关系信息包括：
采用路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)和车辆质心坐标(xcar,t,ycar,t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx,vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax,ay)计算车辆和障碍物之间的最小安全距离R0。
在该方法中，所述得到车辆行驶的新的路径信息包括：
根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用MPC算法基于人工势场U求解得到车辆行驶的新的路径信息。
举一个具体例子对本发明实施例提供的系统及方法进行详细说明。
图4为本发明实施例提供的一种控制车辆行驶的安全决策方法具体例子流程图，该方法基于本发明实施例提供的控制车辆行驶的安全决策系统实现，包括：
步骤401、所述系统通电，感知子系统中的激光雷达模块获取路面点云数据Q，将路面点云数据Q发向第一单片机模块；
感知子系统中的GPS模块获取当前时刻的车辆质心坐标(xcar,t,ycar,t)、当前时刻的车辆横摆角度θ、以及当前时刻的车辆横向和纵向速度(vx,vy)，并发向决策子系统中的第二单片机模块；
感知子系统中的IMU模块获取车辆横向和纵向加速度(ax,ay)，并发向决策子系统中的第二单片机模块。
步骤402、感知子系统中的第一单片机模块从激光雷达模块获取到路面点云数据Q后，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)，将路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)发送给决策子系统中的第二单片机模块。
在本步骤中，所述点云分割算法可采用2018年Mingyang Jiang等人在论文《PointSIFT:ASIFT-like Network Module for 3D Point Cloud Semantic》中提出的PointSIFT点云分割算法，计算路面障碍物情况，得到路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)。
步骤403、决策子系统中的第二单片机模块根据接收第一单片机模块发送的路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)和GPS模块发送的当前时刻的车辆质心坐标(xcar,t,ycar,t)计算车辆与障碍物的最短距离根据IMU模块发送的车辆横向和纵向加速度(ax,ay)及GPS模块发送的当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx,vy)计算得到安全距离R0。
在本步骤中，计算车辆与障碍物的最短距离的方式是：遍历车辆质心与路面障碍物边缘所有点之间的距离，取最小距离，采用的公式包括：
车辆与障碍物之间的安全距离R0为车辆当前的速度与当前加速度比值，公式如下：
步骤404、第二单片机模块根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物的高度(深度)hobs。
在本步骤中，障碍物的高度(深度)hobs具体计算公式如下：
其中zobs,i是障碍物轮廓z方向的坐标，N是障碍物轮廓空间坐标的总个数。
步骤405、第二单片机模块根据路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物尖锐程度S。
在本步骤中，计算障碍物尖锐程度S具体包括：
假设路面障碍物的尖锐程度与路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)中的xz平面内障碍物轮廓有关；
在路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)上任取一点，只考虑x坐标和z坐标，用Pi表示，其中i表示障碍物轮廓上的第i个点；以Pi为中心点，向前和向后分别取k个点，k的范围为1--2；以Pi，Pi-k，Pi+k为三角形顶点，角αi为和/>的夹角；由于Pi，Pi-k，Pi+k之间的间隔的点的数目很少，Pi-k到Pi的距离和Pi+k到Pi的距离很小，则假设|PiPi-k|＝|PiPi+k|；可算出/>公式如下：
当αi等于180时，
当αi趋近于0时，
定义一个角的尖锐度：
将轮廓上每个点在x轴和z轴组成的面上的尖锐度累加，尖锐程度S，障碍物的尖锐程度S计算公式如下：
0<αi≤180
步骤406、第二单片机模块根据障碍物高度(深度)hobs和路面障碍物轮廓空间坐标(xobs,i,yobs,i,zobs,i)计算障碍物对车辆的阻碍程度H。
在本步骤中，计算障碍物对车辆的阻碍程度H采用公式为：
其中，hmin是车辆最小离地距离，距离C1是远大于距离C2的系数。
距离C2的具体计算方式包括：
将障碍物截取(x,z)截面，从三维空间降至二维空间。假设凹陷障碍物边缘是以最低点与水平面的垂线对称的，则将左半凹陷障碍物的边缘拟合为直线f(x)＝mx+n，由于对称性，可求出圆心下降的距离C2。
距离C2具体公式如下：
zobs,i＝m·xobs,i+n
zobs,j＝m·xobs,j+n
C2＝|zobs,i+r|
其中，r是车辆的轮胎半径，xobs,min是凹陷障碍物轮廓最低点x方向坐标，A为当轮胎到达下降最大高度时切点到凹陷障碍物最低点的水平距离，β是此时两切点与轮胎圆心连线的夹角。
步骤407、第二单片机模块根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U。
在本步骤中，计算人工势场U的公式如下:
其中kr是人工势场系数，C＝HS。
步骤408、使用MPC算法求解人工势场U值最小的行驶路径信息，发向控制子系统，控制子系统根据接收到的路径信息对车辆的行驶进行控制。
至此一个工作循环完成。第二个工作循环从步骤401到步骤408，并依此循环一直工作到所述系统断电，也就是车辆停驶。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (6)

1.一种控制车辆行驶的安全决策系统，其特征在于，所述系统包括：感知子系统、决策子系统及控制子系统，其中，
感知子系统，用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；
决策子系统，用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；
控制子系统，用于根据得到的所述路径信息控制汽车自动行驶；
所述路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)；所述车辆行驶信息包括：当前时刻的车辆质心坐标(xcar，t，ycar，t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx，vy)、及当前时刻的车辆横向和纵向加速度(ax，ay)；
所述感知子系统还包括：激光雷达模块、全球定位系统GPS模块及惯性测量单元IMU模块和第一单片机模块，其中，
激光雷达模块，用于扫描路面信息，获得路面点云数据Q，发送给第一单片机模块；
第一单片机模块，用于从激光雷达模块接收路面点云数据Q，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)，将路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)发送给决策子系统；
GPS模块，用于获取当前时刻的车辆质心坐标(xcar，t，ycar，t)、当前时刻的车辆横摆角度θ和当前时刻的车辆横向和纵向速度(vx，vy)，发送给决策子系统；
IMU模块，用于获取当前时刻的车辆横向和纵向加速度(ax，ay)，发送给决策子系统；
所述决策子系统由第二单片机模块实现，还用于从第一单片机模块接收路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)，从GPS模块接收当前时刻的车辆质心坐标(xcar，t，ycar，t)、当前时刻的车辆横摆角度0和当前时刻的车辆横向和纵向速度(vx，vy)，从IMU模块接收当前时刻的车辆横向和纵向加速度(ax，ay)；
所述决策子系统，还用于采用路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)和车辆质心坐标(xcar，t，ycar，t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx，vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax，ay)计算车辆和障碍物之间的最小安全距离R0；
所述决策子系统，还用于对路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)进行均值计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度或深度hobs，其中，zobs，i为第i个障碍物的高度或深度的坐标值；根据路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)计算障碍物的尖锐程度S；根据障碍物高度或深度hobs和障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)计算障碍物对车辆的阻碍程度H；
所述决策子系统，还用于根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用模型预测控制MPC算法基于人工势场U求解得到车辆行驶的新的路径信息，发送给控制子系统。
2.如权利要求1所述的系统，其特征在于，所述决策子系统，还用于判断障碍物是凸起障碍物或者凹陷障碍物为：当均值时，为凸起障碍物，障碍物的高度或深度hobs为max(zobs，i)；当均值/>时，为凹陷障碍物，障碍物的高度或深度hobs为min(zobs，i)。
3.如权利要求1所述的系统，其特征在于，所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息；
所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息。
4.一种控制车辆行驶的安全决策方法，其特征在于，所述方法包括：
采集路面障碍物轮廓信息及车辆行驶信息；
根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息和路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息；
根据得到的所述路径信息控制汽车自动行驶；
所述路面障碍物轮廓信息为路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)，所述采集路面障碍物轮廓信息包括：
由设置的激光雷达模块扫描路面信息，获得路面点云数据Q后，通过点云分割算法计算得到路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)；
所述车辆行驶信息包括：当前时刻的车辆质心坐标(xcar，t，ycar，t)、当前时刻的车辆横摆角度θ、当前时刻的车辆横向和纵向速度(vx，vy)、及当前时刻的车辆横向和纵向加速度(ax，ay)，所述采集车辆行驶信息包括：
当前时刻的车辆质心坐标(xcar，t，ycar，t)、当前时刻的车辆横摆角度θ及当前时刻的车辆横向和纵向速度(vx，vy)由设置的GPS模块采集；
当前时刻的车辆横向和纵向加速度(ax，ay)由设置的IMU模块采集；
所述路径异常状态信息包括：障碍物是凸起障碍物或凹陷障碍物信息、障碍物高度或深度信息、障碍物的尖锐程度信息、及障碍物对车辆的阻碍程度信息；
所述计算得到路径异常状态信息包括：
对路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)进行均值计算判断障碍物是凸起障碍物或者凹陷障碍物，及障碍物高度或深度hobs，其中，zobs，i为第i个障碍物的高度或深度的坐标值；根据路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)计算障碍物的尖锐程度S；根据障碍物高度或深度hobs和障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)计算障碍物对车辆的阻碍程度H；
所述车辆与路面障碍物之间的关系信息包括车辆和障碍物之间的最短距离信息、及车辆和障碍物之间的最小安全距离信息；
所述计算得到车辆与路面障碍物之间的关系信息包括：
采用路面障碍物轮廓空间坐标(xobs，i，yobs，i，zobs，i)和车辆质心坐标(xcar，t，ycar，t)计算障碍物和车辆之间的最短距离采用车辆横向和纵向速度(vx，vy)、车辆横摆角度θ和车辆横向和纵向加速度(ax，ay)计算车辆和障碍物之间的最小安全距离R0。
5.如权利要求4所述的方法，其特征在于，所述判断障碍物是凸起障碍物或者凹陷障碍物包括：
当均值时，为凸起障碍物，障碍物的高度或深度hobs为max(zobs，i)；当均值时，为凹陷障碍物，障碍物的高度或深度hobs为min(zobs，i)。
6.如权利要求4所述的方法，其特征在于，所述得到车辆行驶的新的路径信息包括：
根据车辆和障碍物之间的最短距离车辆和障碍物之间的最小安全距离R0、障碍物对车辆的阻碍程度H和障碍物的尖锐程度S建立人工势场U，采用MPC算法基于人工势场U求解得到车辆行驶的新的路径信息。
CN202110554703.9A 2021-05-21 2021-05-21 一种控制车辆行驶的安全决策系统及方法 Active CN113147750B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110554703.9A CN113147750B (zh) 2021-05-21 2021-05-21 一种控制车辆行驶的安全决策系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110554703.9A CN113147750B (zh) 2021-05-21 2021-05-21 一种控制车辆行驶的安全决策系统及方法

Publications (2)

Publication Number Publication Date
CN113147750A CN113147750A (zh) 2021-07-23
CN113147750B true CN113147750B (zh) 2023-12-22

Family

ID=76876912

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110554703.9A Active CN113147750B (zh) 2021-05-21 2021-05-21 一种控制车辆行驶的安全决策系统及方法

Country Status (1)

Country Link
CN (1) CN113147750B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1062162A (ja) * 1996-08-13 1998-03-06 Nissan Motor Co Ltd 障害物検出装置
JP2000181541A (ja) * 1998-12-21 2000-06-30 Komatsu Ltd 自走式車輌
CN101549691A (zh) * 2009-04-23 2009-10-07 上海交通大学 自动识别道路深坑与障碍物的车辆智能装置
CN106080480A (zh) * 2016-06-29 2016-11-09 戴姆勒股份公司 汽车底盘防撞装置和方法及汽车
CN108444390A (zh) * 2018-02-08 2018-08-24 天津大学 一种无人驾驶汽车障碍物识别方法及装置
JP2019179495A (ja) * 2018-03-30 2019-10-17 パナソニックＩｐマネジメント株式会社 センサ処理システム、測距システム、移動体、センサ処理方法及びプログラム
CN112464812A (zh) * 2020-11-27 2021-03-09 西北工业大学 一种基于车辆的凹陷类障碍物检测方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1062162A (ja) * 1996-08-13 1998-03-06 Nissan Motor Co Ltd 障害物検出装置
JP2000181541A (ja) * 1998-12-21 2000-06-30 Komatsu Ltd 自走式車輌
CN101549691A (zh) * 2009-04-23 2009-10-07 上海交通大学 自动识别道路深坑与障碍物的车辆智能装置
CN106080480A (zh) * 2016-06-29 2016-11-09 戴姆勒股份公司 汽车底盘防撞装置和方法及汽车
CN108444390A (zh) * 2018-02-08 2018-08-24 天津大学 一种无人驾驶汽车障碍物识别方法及装置
JP2019179495A (ja) * 2018-03-30 2019-10-17 パナソニックＩｐマネジメント株式会社 センサ処理システム、測距システム、移動体、センサ処理方法及びプログラム
CN112464812A (zh) * 2020-11-27 2021-03-09 西北工业大学 一种基于车辆的凹陷类障碍物检测方法

Also Published As

Publication number Publication date
CN113147750A (zh) 2021-07-23

Similar Documents

Publication Publication Date Title
CN110341711B (zh) 一种基于码头环境的行驶轨迹生成系统及方法
CN111309013B (zh) 一种碰撞距离确定方法及系统、车辆、存储介质
US20180150081A1 (en) Systems and methods for path planning in autonomous vehicles
CN108536149A (zh) 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法
US11260857B2 (en) Polyline contour representations for autonomous vehicles
CN105160356B (zh) 一种车辆主动安全系统传感器数据融合方法及系统
CN112577506B (zh) 一种自动驾驶局部路径规划方法和系统
CN112558608A (zh) 一种基于无人机辅助的车机协同控制及路径优化方法
CN112068574A (zh) 一种无人车在动态复杂环境中的控制方法及系统
US11541890B2 (en) Vehicle control device and storage medium which stores a computer program for vehicle control
CN106371436A (zh) 一种无人驾驶汽车的避障方法及系统
CN116118780A (zh) 一种车辆避障轨迹规划方法、系统、车辆及存储介质
CN111746537B (zh) 基于路面平整度的自适应巡航车速控制系统、方法及车辆
CN111966089A (zh) 在移动机器人中使用代价地图估计动态障碍物速度的方法
CN115933648A (zh) 一种机器人动态避障方法及系统
CN112674646A (zh) 基于多算法融合的自适应贴边作业方法及机器人
CN116811916A (zh) 一种基于5g车路协同的自动驾驶系统
CN115562281A (zh) 一种复杂环境下自动泊车系统动态路径规划方法
CN113147750B (zh) 一种控制车辆行驶的安全决策系统及方法
CN113887276A (zh) 基于图像的前向主目标检测方法
CN113459852A (zh) 一种路径规划方法、装置以及移动工具
CN116991104A (zh) 一种无人驾驶车辆自动化驱动的装置
CN208061025U (zh) 一种基于Dubins路径的无人驾驶车辆避障控制装置
CN116331264A (zh) 一种未知障碍物分布的避障路径鲁棒规划方法及系统
KR20150061445A (ko) 3차원 도로 지도 작성 및 차선인식 장치

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant