基于PanoSim仿真开发平台LDW验证以及LDP的构想

一、PanoSim简介
PanoSim是一款面向汽车自动驾驶技术与产品研发的一体化仿真与测试平台，集高精度车辆动力学模型、高逼真汽车行驶环境与交通模型、车载环境传感器模型和丰富的测试场景于一体，支持与Matlab/Simulink联合无缝仿真，提供包括离线仿真、实时硬件在环仿真（MIL/SIL/HIL/VIL)和驾驶模拟器等在内的一体化解决方案；支持包括ADAS和自动驾驶环境感知、决策规划与控制执行等在内的算法研发与测试。PanoSim具有很强的开放性与拓展性，支持定制化开发，操作简便友好，已在美国通用汽车、德国戴姆勒汽车、上汽集团和东风汽车等企业和科研院所广泛使用。

PanoSim汽车智能驾驶一体化仿真测试平台具备功能强大且齐全的模型库和仿真测试工具链，结合灵活开放的云端一体软件架构，为自动驾驶行业测试机构和产业链正向开发提供全流程实践支撑。

二、LDW简介
汽车车道偏移预警系统,LDW英文全称为：Lane Departure Warning，缩写为LDW。车道偏离预警LDW通过摄像头监测前方道路车道线，当判断发生驾驶员无意识状态下、车辆偏离车道线情况，系统将通过声音、振动等方式向驾驶员发出预警。 这是继车道偏离预警(LDW)旨在帮助驾驶员在高速公路、快速道路等类似主干道上减少因为车身偏移而造成各种危险事故，当车速大于或等于60km/h且道路标线清晰可见时，车道偏离预警功能启用，车道偏离预警(LDW)将通过视觉、听觉和触觉的方式警告驾驶员。如果转向信号指示灯处于开启状态或驾驶员有明显的转向意图（如快速转动方向盘等），车道偏离预警（LDW）将不会发出警示。

这种系统的主要作用，就是当汽车偏离原车道时，能够迅速判断该动作是否属于驾驶者的无意识行为，从而在0.5秒内作出反应。在对驾驶者进行明确警示的基础上，提醒驾驶者尽快纠正错误的驾驶行为，从而减少汽车因车道偏离而发生交通事故。

三、LDW开发背景
据交通部统计，约有50%的汽车交通事故是因为汽车偏离正常的行驶车道引起的。究其原因，主要有驾驶员情绪不稳定、注意力不集中或驾驶疲劳等几种情形。另据统计，23%的汽车驾驶者一个月内至少在转向盘上睡着一次；66%的卡车驾驶者在驾驶过程中会有打瞌睡的经历；而28%的卡车驾驶者在一个月内会有在转向盘上睡着的经历。如此惊人的比例足以证明防止车道偏离的重要意义。

另外，根据美国联邦公路局的估计，美国2002年所有致命的交通事故中44%是跟车道偏离有关的，同时车道偏离也被看成车辆侧翻事故的主要原因。四个驾驶员中就有一个驾驶员经历过车道偏离引起的伤亡事故。

这些数据表明，搭载车道偏离预警系统（LDW）后，由于减少了非正常的车道偏离，汽车行驶的安全性将大大提高，交通事故的发生率也将显著下降。故此，LDW系统正逐渐成为汽车安全技术的重要组成部分。

四、LDW控制系统接口
下图为PanoSim传感器配置SensorBuilderUI界面，能够直接将所需要的模块直接拖至图中即可搭载对应的传感器，并且在Simulink中完成链接，LDW主要需要用到车道传感器，完成链接后，即可在总线中读取相应数据（车道线数，车道线ID，左右车道线距车距离，车道线曲率以及多次项的系数）用以拟合车道多项式的参数，并且将其所得数据做为数组通过numpy的计算进行运算和判断，从而优化左右车道线距离车辆的距离最终用于算法的计算去做判断。

而主要的控制信号同样封装于Simulink当中，需要从总线中读出，主要包括有throttle（油门），brake（刹车），steer（方向盘），mode（挡位），gear（齿轮）等多种控制方式，用于控制车本身的状态，并且在设计过程中考虑到部分变量需要做为全局变量用以调用，所以PanoSim在开发时候预留了八个全局变量（GlobalVariable）用于参数调用，在LDW开发过程中就是将第八个全局变量做为了报警信号交互的通道

五、LDW车道线拟合

MonoDetection传感器 ：Type、 Distance 、C0、 C1、 C2、 C3
车辆信息：Vx(纵向车速)、Ax(纵向加速度)、Vy(横向车速)、Ay(横向加速度)
disToLeft_0 (左侧车道线距离)
disToRight_0 (右侧车道线距离)
VX (转向车速)

摄像头识别，4条车道线，三条车道。2、3分别是靠近主车左、右两侧的车道线，该算法封装于车道线传感器内部，用拟合多项式去计算出最近车道离车重心点的距离用于汽车偏向的判断，当然在行驶过程中自身的速度，姿态，也是LDW开发过程中的一个重要节点，所以在设计过程中也需要将车身的状态读取下来，用以下部分算法的开发当中去。

六、偏移速度计算

在LDW的开发过程中，经典的算法是通过车道线多项式拟合后所得到车重心距离最近车道线的左右距离做为判断变量去做决策的，但是这样的算法可能会略微简单。规范的LDW算法需要考虑车身姿态角（航偏角yaw），自身车速Vx以及在左右转向时候的变化率综合其三最终做出决策。

姿态角主要用于判断用户是否是主动转向还是无意识车头的偏移，所以在设计时候需要将车道做为y轴，而垂直于车道向右延申的直线做为x轴通过计算，如果其车身航偏角在85-95°表示是由于驾驶员无意识的偏转，而如果航偏角不在该范围内，意为用户主动控制方向盘而导致角度变化，该状态下不予以触发。

自身车速是用以判断驾驶员驾驶过程中是否达到速度阈值，如果车速小于60km/h即为16.7m/s，在安全车速中不会触发LDW，如果大于该车速下，完成其他判断条件即会触发。

转向的变化率，在原始的LDW判断当中，通过去计算车身重心距离最近车道线的左右距离去做判断，如果小于固定阈值即会触发报警，该算法应用可行性较高，触发较为精确，但是在应用中也应该考虑到车身重心距离最近车道线的变化率，将该值用于判断能够更好更灵敏的表现出其变化的效果，并且考虑到转向过程中重心的转移会导致变化率会出现负值等各种情况，在设计该部分时候，需要考虑到往某一方向偏移时候，该方向车道线的偏移量会变小，但是另一方向的偏移量会变大，因为车身呈现出一种圆形旋转的姿态，外向车轮的变化率必定大于内向车轮的变化率。

在该试验当中每次仿真回调周期Ts是10ms，该时间需要针对不同模型进行改进。其基本思想也是较为简单的通过设计disToLeft_1和disToRight_1去读取上一回调周期的偏移量，而disToLeft_0和disToRight_0读取该回调周期的偏移量，两者做差除以回调周期即可得到该回调周期的变化率通过该变化率去做判断工作。这里用到了高数中简单的导数定义式的方法去计算，其大致数学公式如下。

# 版本1.1 用距离车道偏移的距离去计算变化率
# 通过每次回调前后回调后的距离差除以回调时间就可以得到一个变化速度，该速度即可类似于转弯速度
disToLeft_1 = disToLeft_0
disToRight_1 = disToRight_0
disToLeft_0 = lane_DistanceTo[0][0]
disToLeft_d=disToLeft_0-disToLeft_1
disToLeft_v=abs(disToLeft_d//Ts)
disToRight_0 = lane_DistanceTo[0][1]
disToRight_d=disToRight_0-disToRight_1
disToRight_v=abs(disToRight_d//Ts)

设函数y=f（x）在点x0的某个邻域内有定义，当自变量x在x0处有增量Δx，（x0+Δx）也在该邻域内时，相应地函数取得增量Δy=f（x0+Δx）-f（x0）；如果Δy与Δx之比当Δx→0时极限存在，则称函数y=f（x）在点x0处可导，并称这个极限为函数y=f（x）在点x0处的导数，即

或者也可以等价为

disToRight_v是个相对距离的变化率，如果汽车朝着左边开，右边的距离变化就大，所以该值就大，故只需要反向判断就可以较好第得到汽车的运动状态，如果汽车右转左边的相对变化量较大，所以同一回调周期内disToLeft_V就会较大说明在右转。在判断时候只需要该变化率大于某个固定值，并且上速度VX大于60km/h即(16.6m/s)，同时车身航偏角（即车头与水平线的距离）在1.5弧度到1.7弧度内，即86°至97.4°内表示车身是用户无意识偏移，非主动偏移，是驾驶员未注意到的转向信号，该时刻需要给总线发出警告信号。

if  disToRight_v>100 and VX>16.6 and (ego_yaw>1.5 and ego_yaw<1.7) : #这是个相对距离的变化率，如果汽车朝着左边开，右边的距离变化就大，所以该值就大，故反向判断
    userData["global8"].writeHeader(*(userData["time"], 101))
elif   disToLeft_v>100 and VX>16.6 and (ego_yaw>1.5 and ego_yaw<1.7) : #and (ego_yaw>1.5 and ego_yaw<1.7) #可以用来规定转向角度在86.4 - 97.2之间
    userData["global8"].writeHeader(*(userData["time"], 102))
else:
    userData["global8"].writeHeader(*(userData["time"], 0))

通过文章简介中可以看到LDW触发的位置调整至HUD抬头数字显示，并且在HUD显示中还包括有车速，车轮转速和挡位等多种信息非常有利于交互和体验，并且在该场景下我们加入了随机交通流，该算法集成于PanoSim内部，可以通过在PanoEXP中UI界面中打开或者关闭交通流也可以通过在WordlBuilder中设置合适的交通信息，包括有车辆密度，行人密度，车辆类型，驾驶员驾驶情况等多个参数设置为静态参数，当然在PanoEXP中也可以直接放置车辆和行人等各种干扰物做为动态参数，在图中的行人和正前方车辆就是自己放置的。

七、LDP算法简介以及开发
LDP算法功能定义车道偏离预防LDP通过摄像头实时监测车辆与车道边线的相对位置，在判断车辆即将发生车道偏离时控制车辆横向运动，辅助驾驶员将车辆保持在原车道内行驶。其算法的开发依赖于上述的LDW算法，当车辆在车道内行驶，如果有偏离出本车道的趋势以及已经偏出本车道一部分时，LDP系统会触发并使方向盘自动响应，使车辆驶回车道中心附近。
方向盘期望转角计算模块方向盘期望转角是根据车辆与当前车道的相对位置关系得到的，具体是根据车辆阿克曼运动模型计算得到。车辆以圆弧曲线运动来确定前轮中点转角的方法是选取车辆前方一定距离d处期望路径上的坐标点（x_t，y_t）。

确定前轮转角假设以固定转角θ进行圆弧转弯运动会刚好到达那个目标点，那么这个前轮转角θ就是当前时刻应该发出的期望转角，并在下一个采用时间用同样的方法进行更新。确定圆弧半径R的方法为：如果车身朝向正对着预瞄点，那么R为无穷大，期望前轮转角为0；如果车身没有正对着预瞄点，则定义车辆第二轴的中点坐标为（x_0，y_0），预瞄点坐标为（x_t，y_t），这两对坐标值为已知，并且圆弧中心的坐标（x_c，y_c）一定是在第二轴两轮连线的延长线上，那么圆心坐标应满足：

上式中，k和b由第二轴两轮的坐标来决定。
当得到圆心坐标后，圆弧半径为：

则此时前轮中点期望转角为：

将上式计算结果变成角度值，再考虑方向盘转角与前轮转角之间的传动比，即可得到方向盘的期望转角。
在开发过程中选择用一个较为简单的方法去模拟LDP的算法进行，通过在LDW的判断过程后，加入油门和方向盘的控制，即可模拟达到LDP算法的控制功能，最终达到较好的控制效果。首先转弯时候会有速度损失，并且在LDW的设计中速度要大于60km/h才能进入判断加速，所以在每次LDP控制需要对速度进行判断，如果速度小于68左右，即需要加速，在命令中使throttle置1，而在方向控制中如果出现了左偏或者右偏的情况，需要进行右转或者左转，就比如说disToRight_V大于80时候说明右边偏移率很大，说明在左偏，这时候即要控制方向盘右转，只需要对steer赋值然后将命令发给总线即可。

if  disToRight_v>80  and (ego_yaw>1.5 and ego_yaw<1.7) and VX>16.6: #这个感觉是个相对距离的变化率，如果汽车朝着左边开，右边的距离变化就大，所以该值就大，故反向判断，
    userData["global8"].writeHeader(*(userData["time"], 101))
    # 转弯时候会有速度损失，并且本书速度要大于60km/h才能进入判断加速，最终会使速度保持70-60
    if VX<19:
        throttle = 1
    else:
        throttle = 0
    brake = 0
    if ego_erro>-0.1:
        ego_erro=-0.1
    steer = -ego_erro*10
    valid = 1
    userData['ego_control'].writeHeader(*(userData['time'], valid, throttle, brake, steer, mode, gear))
elif   disToLeft_v>80  and (ego_yaw>1.5 and ego_yaw<1.7) and VX>16.6: # #可以用来规定转向角度在86.4 - 97.2之间
    userData["global8"].writeHeader(*(userData["time"], 102))
    if VX<19:
        throttle = 1
    else:
        throttle = 0
    brake = 0
    if abs(ego_erro)<0.1:
        ego_erro=0.1
    steer = -ego_erro*10
    valid = 1
    userData['ego_control'].writeHeader(*(userData['time'], valid, throttle, brake, steer, mode, gear))
else:
    userData["global8"].writeHeader(*(userData["time"], 0))
    throttle = 0
    brake = 0
    steer = 0
    valid = 1
    userData['ego_control'].writeHeader(*(userData['time'], valid, throttle, brake, steer, mode, gear))
print('steer', steer)

在上述的分析过程中主要介绍了LDW的原理，LDW简介,LDW应用背景，软硬件开发所需，以及对应LDW算法的验证，能够一定程度上优化该算法的实际应用能力，而为了能够更好顺应ADAS开发的进程，也对LDP算法进行了尝试，希望能够在将来能够将更多的算法进行应用与开发，并且非常感谢PanoSim便利的自动驾驶仿真平台，能够将所设计的算法快速应用于实地场景以检验算法的可靠性以及可行性，对于自动驾驶算法开发来说是一个举足轻重的平台。