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

本发明涉及自动驾驶车辆的技术，特别涉及一种控制车辆行驶的安全决策系统及方法。

背景技术：

在自动驾驶车辆的过程中，对智能汽车的路径规划和避障是反应智能汽车自主能力的关键要素之一，也是解决自动驾驶安全的问题之一。人工势场法路径规划是智能汽车的路径规划中较成熟的及实时性较好的规划技术。公告号“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，modelpredictivecontrol)算法不断地规划最优的车辆行驶路径，解决了自动驾驶汽车的路径规划问题。

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

图1为本发明实施例提供的一种控制车辆行驶的安全决策系统结构示意图，所述系统包括：感知子系统、决策子系统及控制子系统，其中，

感知子系统，用于采集路面障碍物轮廓信息及车辆行驶信息，发送给决策子系统；

决策子系统，用于根据路面障碍物轮廓信息计算得到路径异常状态信息，根据车辆行驶信息及路面障碍物轮廓信息计算得到车辆与路面障碍物之间的关系信息，根据路径异常状态信息及车辆与路面障碍物之间的关系信息采用人工势场法进行路径规划，得到车辆行驶的新的路径信息，发送给控制子系统；

控制子系统，用于根据得到的所述路径信息控制汽车自动行驶。

参考图2所示的控制车辆行驶的安全决策系统具体例子的结构示意图，对本发明实施例提供的控制车辆行驶的安全决策系统进行详细说明。

在该系统中，感知子系统具体包括激光雷达模块、全球定位系统(gps，globalpositioningsystem)模块及惯性测量单元(imu，inertialmeasurementunit)模块和第一单片机模块，其中，

激光雷达模块，用于扫描路面信息，获得路面点云数据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年mingyangjiang等人在论文《pointsift:asift-likenetworkmodulefor3dpointcloudsemantic》中提出的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，并依此循环一直工作到所述系统断电，也就是车辆停驶。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。