ADAS/AD控制器模块开发13 - Feature开发之LDW&LKA

前言

General 模块还剩Power Model、Manufacturing、Packaging、Mounting、SPI等，暂时不想写这些了，毕竟哪个ECU基本都包含这些Building Blocks，准备往后放放。既然是在搞ADAS/AD开发，先讲讲一些与ADAS/AD相关的算法和功能吧！

本文先讲讲一个常见的ADAS功能：LDW/LKA。这个功能实际上就是ADAS层面的车辆基本横向控制功能，输出转向扭矩，通过与EPS通信，让EPS控制转向执行器（转向电机）来进行车辆的横向控制。

在ADAS/AD控制器模块开发06 - 高精地图与自动驾驶中大致讲过LDW/LKA功能的基本实现方式。车道线检测算法会用专门一篇文章来介绍，本文主要介绍下基于MATLAB/Simulink开发的LDW/LKA model。

正文

一、Feature 架构

图1 LDW/LKA Feature 架构

二、输入信号

LDW/LKA功能有两种输入信号类型，一个来自整车CAN网络的车辆信号（Vehicle signals），一个来自底层检测算法的视觉信号（Vision signals）。

车辆信号一般需要经过转换，才能传入Feature中。原因在于，ADAS领域被Tier1霸占，相关Feature一般也是在国外开发完成的。因此，模型中的信号都是某个具体项目、具体车型的信号。拿到国内后，为了提高开发效率，并不会修改模型中的具体信号，而是创建一个Translator In模块和一个Translator out模块，将国内项目中不同的车型的CAN信号，转化为模型内对应的（信号名不完全相同）信号。举个例子，国内某个车型上的Yawrate信号来自于安全气囊控制器的ACU1 CAN消息中，名字叫做DynYawRate；但是模型中的对应Yawrate信号名字来自与国外某车型的IMU的imu_msg2 CAN消息中，且信号名字为imu_yawrate。那么在，translator In中，就将这两个信号相等就OK了。这里的难点在于，会存在模型里有的信号，在国内某个具体车型中不存在，这个时候需要分析该信号在feature中所起的作用，是否可以通过赋值一个常量或者直接terminator掉。

视觉信号，一般在底层算法中，封装成结构化数据发送到上层feature中。在LKA feature中，视觉信号通常以三阶车道线模型（公式）存在，如图2

图2 三阶车道线模型

两种不同来源的信号进入到feature中，就会进入数据处理模块。

 

三、数据处理模块

1 处理车辆相关信号

数据处理一般就是滤波、积分、增益、根据原始信号计算一些用于做判断的Flag逻辑信号等处理方式。

a. 滤波：对Yaw Rate、Driver Applied Torque进行滤波；

b. 增益：对车速信号进行增益，计算出仪表盘显示车速；（一般为了安全起见，仪表显示的车速会故意在原始车速的基础上，乘以一个系数，例如1.1/1.08等，使车速看起来更快，来引导驾驶员开车速度稍慢于真实车速，避免车辆超速）

c. 计算Flag信号：

1. 计算显著转向FLag信号（显著转向，就是指LKA作用过程中，探测驾驶员主动转向的场景，一般的标定值为60度，即当驾驶员强行扭转方向盘，且幅度超过60度时，系统认为驾驶员有意进行转向，那么系统会自动退出，让出控制权，且保持10秒钟；这10秒钟之内不再探测新的驾驶员主动转向场景；这里的10秒钟，也是一个标定值）；

2. 计算Steer Angle Change Rate Inhibit Flag信号（即方向盘转角变化率抑制信号，如果方向盘转角变化率过快，就判定是驾驶员在主动快速介入转向，那么系统应该赶快退出。这里的转角变化率阈值，是一个二维表，随车速的增加、变化率阈值的标定会越来越小）

3. 计算Acceleration Pedal Inhibit Flag信号（同上逻辑类似，也是通过加速踏板位置、加速踏板位置变化率、车速信号、档位信号、纵向加速度信号等，通过一定判断，得出是否驾驶员主动踩油门踏板）

4. 计算Brake Pedal Inhibit Flag信号（同上逻辑）

该模块的常见输入信号如图2/3/4：

图2 数据处理模块输入信号1

图3 数据处理模块输入信号2

图4 数据处理模块输入信号3

 

四、决策模块和主状态机

决策模块（System Core）一般是将处理好的各种原始信号+可直接被决策使用的二次计算得出来的控制信号。决策模块中有大量标定相关的参数。

例如决策模块中的信号流，在进入状态机之前，会进入几个计算模块。包括：

1. 驾驶员抑制模块（Driver Suppression）；即在某些情况下，抑制LKA的作用。例如：左右转向灯起作用时（来自Vehicle CAN上的Turn Switch Status）要抑制LKA（让其从active状态回到standby状态，并不是变成default（关闭）状态）；最近5秒钟时间内的驾驶员刹车踏板向下踩的位置变化率超过一定百分比时；最近5秒钟时间内加速踏板向下踩的位置变化率超过一定百分比时；最近2秒钟时间内方向盘转角变化率超过一定程度时。注：最近几秒钟的这个参数都是标定值，一定百分比或者一定程度这种表述，也是标定值。
2. 驾驶员方向盘脱手检测模块（Driver HandsOff Detection）。有些EPS不提供现成的HandsOff接口（偷懒，自己不愿意做这个功能；垃圾装逼供应商太多，比如博世华裕？鄙视！），所以LKA功能会实现相关模块，如果EPS提供，就通过Config参数关闭该模块。该模块的输入信号是驾驶员申请扭矩（Driver Applied Torque）、方向盘转角（Steer Angle）、方向盘转角变化率（Steer Angle Change Rate），再加上一些标定值辅助判断（创建两个标定阈值，当驾驶员申请扭矩大于多少（例如0.45）时，触发某个计数器开始计数，时间超过一定程度（例如3秒），就判定驾驶员手一直在方向盘上放着；当驾驶员申请扭矩小于多少时（例如0.4），触发另一个计时器计数，如果时间超过一定程度，就判定驾驶员脱手），可以实现脱手检测。同时也会检查HandsOffState状态，如果EPS提供相关脱手接口，那么HandsOffState参数会map到接口，通过HandsOffState直接屏蔽整个Driver HandsOff Detection模块。（顺便吐槽B公司，虽然我也在B公司干了很多年，但是到OEM后，跟B公司合作，一旦什么开发更改牵涉到B公司，那个恶心、那个难办啊！真想让人骂娘）。
3. 车道纠偏区域检测模块（Lane Intervention Detection）。LDP车道保持与Lane Centering车道保持的区别有两个，最主要的不同除了在主状态机不同外，就是Lane Intervention Detection & Lane Alignment了。实质上，这个模块是整个LDP类型LKA的核心模块！该模块一般都是将Vision Signals（TTLC、Current Distance to Lane Marker、Lateral Rate横向速度、Lane Heading航向角、车道线置信度、路面曲率等参数或信号）、车速信号、HandsOff信号、以及来自主状态机上一运行周期的LKA状态（Standby？左纠偏1阶段？右纠偏1阶段？左纠偏2阶段？右纠偏2阶段？等等）。通过以上信号或参数，主要是考虑横向速度、Current Distance to Lane Marker、航向角、车道线置信度、路面曲率都满足对应条件时（全是标定值，自己想象下车辆慢慢偏离车道时各个状态参数应该的逻辑状态值，进行标定值的限制。例如Distance to Lane Marker应该是-a到b之间的一个值，-a表征一个标定值，意味着车辆冲出车道线最远的位置比如-20cm, 就是车轮穿过车道线并距离车道线20cm；b表征在车道内的某一个距离车道线的距离，比如35cm...），就认为车辆进入了需要纠偏的区域，LKA需要对车辆进行横向控制。同样，也会有一些纠偏退出条件。核心也是设置一些标定值，对控制参数进行监控和分类，触发想要的逻辑参数（Flag）。
4. 主状态机。主状态机实际上是最没什么可讲的模块，它更多是Simulink环境中对Stateflow模块的应用和搭建，该创建的参数要创建，用于接收传入的信号值...巴拉巴拉...输出的LKA_state信号封装了状态机中的各种状态，0表示fault、1表示stangby、...
5. Fault Check模块。LKA_state信号和Vision信号等最终会进入LKA Feature Diag模块中进行Flag输出前的系统完整性检查，如果有错误就报错，同时disable掉LKA。

 

五、前馈PID Controller 扭矩控制器

1. PID扭矩控制器就一个目标，即根据横向速度、车道线曲率、距离车道中心线距离等原始物理模型数据，辅以各种增益标定值，生成P比例、I积分和D微分等各项参数计算生成的扭矩值，最后三项扭矩值相加，叠加出最终的PID控制值。需要说明的是，1. 最先生成的PID三项是D项微分项，该微分项是一个增益标定值乘以路面曲率标定值形成的，然后该D值再乘以横向速度，转化成了初始控制扭矩TD1；2. P项生成过程中，需要考虑当前和未来某个时间（T时间，这个T就是三阶车道线模型方程式中的Preview-Time，向前看的时间）的物理量（所谓前馈），乘以距离车道中心线位置和曲率的乘积，最终形成前馈P+当前P形成的扭矩量TP; 3. I项最后引入，也是根据一个物理量和一个可标定的增益形成，这个物理量是Yawrate。
2. 最终，以上各个项扭矩叠加，形成最终Torque （D+P前馈+P当前+I）；
3. 最终的Torque还要经过一个安全模块，比如最大扭矩限定值之类的，才能把扭矩最终释放出来，给VCAN模块，封装成CAN消息，发送给EPS，用于转向扭矩控制。

 

（完） comment by 小阿狸 in 2018.12.04

PS：终于有时间更新了...