基于Mindstorms的智能机器人控制系统开发*

程 翀 郭晓彬 陈祥磊 李超群 熊 辉

(武汉理工大学现代汽车零部件技术湖北省重点实验室1) 武汉 430070)(汽车零部件技术湖北省协同创新中心2) 武汉 430070)

0 引 言

智能机器人技术涵盖了机械工程、信息技术、电子技术和数学物理等多学科知识，能够完成重复乏味的工作，提高人们的工作效率和生活品质.具有代表性的乐高公司从1984年开始与美国麻省理工学院合作，研发出现代科技与传统玩具相互结合的智能机器人.所推出的最新的Mindstorms EV3机器人因其智能性、易操作性、开放性及算法可移植性，在机器人算法开发中得到了广泛的应用.

本文以Mindstorms EV3机器人为研究平台，利用超声波传感器、摄像头对智能机器人控制系统进行开发，所开发的智能机器人包括基于机器视觉的车道保持、换道避障、紧急制动，以及人机共驾等功能.其中，车道保持功能利用手机摄像头的机器视觉采集道路图像，并通过基于Python的PC图像处理程序提取车道线，将偏差等信号发送给机器人控制系统，从而保证机器人的车道保持功能.其次，机器人通过超声波传感器检测前方障碍物信息，并在控制器的控制下完成换道避障及紧急制动的功能.现如今，由于自动驾驶模式出现故障时无法快速人为操控，无人驾驶车辆事故频繁发生，因此，人为控制与自动控制模式之间控制权的快速转换尤为重要.当机器人自动控制模式出现故障时，能够快速启动人为控制模式，并借助人为操控转向盘、油门踏板和制动踏板来完成机器人的正常行驶.

1 机器人结构搭建

本研究的智能机器人为四轮移动机器人，在乐高EV3现有材料的种类与数量的基础上，充分运用各种材料的功能，设计出拥有前驱前转向和独立悬架系统的机器人结构.其中，转向系统由转向电机通过齿轮齿条方式控制，并在后期的转向测试中验证了转向系统的可行性、可靠性与快速响应性.驱动系统由驱动电机通过齿轮、差速器及万向轴传递给驱动轮，通过不断调整传动过程中的齿轮传动比，将转速与转矩调整到最佳模式.为了保证机器人快速检测到前方障碍物并作出反应，将超声波传感器安装于机器人最前方.为了充分检测到机器人前方道路信息，将手机安装于机器人前方较高处.最终，所搭建的智能机器人整体结构见图1.

图1 智能机器人整体结构图

2 底层模块开发

2.1 通信模块

转向盘、手机摄像头和小车主机的通信采用UDP协议实现，见图2，各硬件间的通信连接见图3.

图2 UDP原理图

图3 各硬件间的通信连接

2.2 底层控制模块

转向电机为一个中型电机，负责小车的横向运动；驱制动电机为两个大型电机，负责小车的纵向运动.底层控制模块与应用层的接口为小车前轮转角和小车速度.其中小车前轮转角由小车转向传动比和转向电机输出转角推算得到；小车速度由驱制动电机输出转角、采样时间,以及车轮半径推算得到.

小车的前轮转角控制和速度控制均采用PID控制器[1].小车前轮转角控制器的输入为转向电机的期望转角与实际转角差值，输出为转向电机占空比；小车速度控制器的输入为驱制动电机的期望转角和实际转角的差值、输出分别为驱制动电机占空比.其中转向电机和驱制动电机的期望转角分别由期望前轮转角和期望车速转换得到.电机转角控制框图见图4.上述控制器的控制策略见式(1)～(2).

图4 电机转角控制框图

(1)

e(t)=δdes(t)-δact(t)

(2)

式中：UPWM为电机占空比；δdes为电机期望转角；δact为电机实际转角；KP，KI，KD分别为PID控制器的比例、积分、微分系数.

3 控制策略开发

智能机器人的传感器主要由手机摄像头和超声波传感器组成，监测车道线和实时获取本机器人与前方障碍物之间的距离.整体控制系统见图5.

图5 机器人控制系统架构图

3.1 决策层控制策略

小车包含自主控制模式和人为控制模式.两种工作模式可以通过转向盘上的按钮一键切换.当小车进入自主控制模式时，默认打开车道保持功能，此时小车将沿车道中心线以恒定速度行驶.若小车当前车道前方出现低速障碍物且满足自动换道功能的激活条件，则小车执行换道操作.而当小车前方突现障碍物或障碍物突然减速时，紧急制动功能被触发，小车将紧急制动，以避免碰撞或减轻碰撞伤害.各个功能的触发条件可见各功能的具体介绍.其中紧急避障拥有最高的优先级.

上述各个功能的切换通过Simulink中的Stateflow模块实现.决策模块的输入接口包括由环境感知和数据融合模块给出的信息，输出接口为希望小车下一时刻执行的前轮转角和速度指令.

3.2 车道保持控制策略

车道保持功能主要基于机器视觉和预瞄控制实现.

1) 图像处理 将传感器输出的图像由RGB转为灰度图，设定适当的阈值，二值化得到的灰度图，最后通过边缘检测算法，得到车道线信息.

2) 预瞄距离模糊控制 对输入的车辆速度和加速度进行模糊化，设定模糊规则，经模糊推理接解模糊后，得到输出量预瞄距离的精确值.

3) 方向盘角度模糊控制 对输入的车辆偏离中心线距离及其变化率进行模糊化，设定模糊规则，经模糊推理接解模糊后，得到输出量预瞄距离的精确值.

3.3 换道避障控制策略

1) 自主换道决策机制 实际场景中根据换道动机和影响结果的不同，换道行为可分为强制性换道和选择性换道两类.强制性换道指的是车辆在指定区域内必须进行换道操作，比如,交叉路口、匝道等.选择性换道指的是驾驶员为追求更宽阔的行驶空间和更高的行车速度而进行的换道行为.根据相关文献中数据，车辆换道的主要原因是当前车道前方车辆车速过低[2].本课题也主要针对此种情况开展研究.换道意图的产生主要依据期望间距和期望速度进行判断.

换道意图产生后还需要进行换道可行性分析，可行性分析往往需要综合考虑本车与相邻车道前后车的相对距离和相对速度两方面因素，如为满足本车的速度需求，相邻车道的前车车速应大于某一阈值；为保证安全，即当相邻车道前车出现特殊情况紧急刹车时，应保证足够的制动距离，因此,间距应大于最小安全间距.同时相邻车道后车也应与本车保持安全距离，以便后车有足够的距离紧急制动.不过由于本课题所用小车的车载传感器所能够获取的周边环境信息有限，本文假设换道意图产生后，换道总是可行的.

另外，根据换道请求发起人的不同，换道行为又可分为驾驶员指令下换道和自主换道.当驾驶员发出换道请求时，系统需要确定换道请求的可行性，若可行，则进行换道操作；若不可行，则保持在当前车道行驶.自主换道时，由系统自动实时判断是否进行换道操作.本课题可实现驾驶员指令下换道和自主换道，两者是类似的，本文以自主换道为例介绍说明.

车辆对当前车道的行车空间的期望一般是高于最小行车安全间距的，因此需要保证车辆安全行驶的最小车间距.本课题中期望间距Sdes为

Sdes=Sbrake+c

(3)

式中：最小车间距Sbrake与车辆制动性能有关；c为常数.

因此，车辆换道意图产生的一个条件为车辆间距S小于期望间距，且有减小趋势.

在实际行驶过程中，车辆总希望以期望车速行驶.若前方车辆持续低速，车辆的期望车速得不到满足，那么当不满持续到一定程度时换道意图产生.本课题借助车速不满积累度的概念衡量上述不满程度[3].车速不满积累度D(k)为

(4)

式中：udes为期望车速；upre为前车车速；T为采样周期.

因此，车辆换道意图产生的一个条件为车速不满积累度达到某一阈值DThr.

综上所述，车辆换道意图产生条件为

SDThr

(5)

2) 自主换道控制指令 一般而言，换道的目标车道确定后，将规划出一条期望的换道路径，并实时转换为转向盘转角、车速等控制指令由底层模块负责执行.换道的路径规划方法包括了基于搜索的、基于几何方法的、基于车辆模型的等.

考虑到本课题所用小车无法准确获知当前位置信息，也即无法完成闭环控制，因此,本课题的小车前轮转角指令采用开环控制.文献中，正弦变化的方向盘转角指令能够满足换道需求，且基本能够保证换道结束后车辆航向角保持不变[4].换道过程中，理想的前轮转角控制指令δ(t)变化见式(6).为方便底层模块良好跟踪转角指令，本课题对理想前轮转角控制指令进行平直化处理，处理后的前轮转角控制指令见图6.

δ(t)=Asin (ωt)

(6)

式中：δ为前轮转角极值；t1，t2，t3为不同阶段的转角指令持续时间.图6中各参数需要根据车速进行标定，以保证小车换道后尽量保持在车道中心线附近.

图6 处理后前轮转角控制指令

考虑到小车车速较低，假定换道过程中车速恒定，车速指令设定为VALC.

3.4 紧急制动控制策略

1) 最小安全距离 在实际的行驶工况中，最小安全距离与前车的运动状态强相关，因此在建立最小安全距离模型时，根据前车的运动状态分为前车静止(CCRs)、前车匀速(CCRm)及汽车减速(CCRb)[5-6].

图7为车辆制动的四个阶段，当驾驶员发现前方危险时的反应时间，在驾驶员反应阶段，因为车辆的制动协调时间(包括消除各铰链和轴承间间隙的时间以及制动器摩擦片完全贴靠在制动盘上的时间t2和制动减速度从零增加到恒定值的时间t3)以上时间由车辆制动系统的性能决定，这一阶段制动器起作用需耗费t2+t3的时间；在制动力达到最大值之后，车辆会保持tc时间的最大制动，并且制动减速度保持不变；在放松制动阶段，即使驾驶员松开制动，由于液压或者气压制动力的消除需要一段时间t4，车辆也会继续受到制动力的减速.

图7 车辆制动距离分析

分别求出各个期间的行驶距离，即L1，L2，L3，Lc，L4.如果不考虑其他影响因素，则

L1=Vstartt1

(7)

在制动传递延迟时间t2内汽车以制动前的初速度匀速运动，因此所行驶的距离为

L2=Vstartt2

(8)

在t3时间内车辆做变减速运动，从图中可以得到每时刻所对应的加速度为

(9)

速度为

(10)

经积分变换，可求得该段时间内所行驶的距离为

(11)

假设tc段的初始速度为Vc，则在恒减速阶段的速度按为

(12)

又因为Vc也是t3阶段的末尾时的速度，则有

(13)

在tc阶段走过的路程

(14)

最终本车的制动行驶距离为

L=L1+L2+L3+Lc

(15)

将式

(16)

为了简化起见，令t=t1+t2+t3，因为车辆的制动协调时间仅为零点几秒，其1/2及二次方会更小，故等式中的后两项可忽略不计.这里称t0为制动操作反应时间，它包括驾驶人的制动反应时间和制动协调时间.此时，本车的制动行驶距离公式简化为

(17)

当前车静止时，此时前车的制动距离Lb=0，最危险的时刻发生在自车停止时与前车距离最近的地方.为保证自车的绝对安全，自车与前车之间存在一定的安全间距D0，则此时的最小安全间距为

(18)

当前车匀速时，当前车的速度高于自车的速度时，则两车之间的距离会越来越大，不存在危险的工况出现.但当自车的速度高于前车的初速度时，则在行驶的过程中会出现安全临界距离，此时就存在碰撞的危险.若驾驶员采取制动措施，直到与前车速度Vf相等，则此时自车行驶路过的距离为

(19)

自车从初速度减速到Vf，此过程经历的时间为

(20)

当前车静止时，即Vf=0，此时的最小安全距离即为Ls.

当前车减速时，自车与前车的初速存在三种情形：Vstart>Vf,Vstart=Vf,Vstart

2) AEB介入策略 当汽车在行驶过程中，AEB系统检测到有碰撞危险，但危险的程度较低，系统采用灯光和报警提醒驾驶员危险的存在.若驾驶员没有及时采取有效措施，随着危险程度的加深，灯光由原来的橙色提醒变为更为显眼的红色提醒，与此同时提醒的报警声更加急促.如果危险程度继续上升，AEB系统在发出预警的同时并采取了部分制动，随着危险程度的持续上升，碰撞即将发生或者无法避免时，AEB系统将采取完全制动的策略[8].

通过底层的传感器，测量本车与前车的实时距离dis_front,然后将该值与选定的最小安全距离Ls进行比较，当检测的距离小于安全阈值时，此时激活AEB的执行标志位，然后将该标志位，作为底层AEB激活的条件.

3.5 人机交互界面开发

基于python开发GUI界面，该界面中分为两大显示区域，其中参数端口区，主要是启用/关闭IP Camera、其中Position error端口的作用是显示车辆与当前车道中心线的偏差值，同时也是用于车道保持最重要的参数.

4 仿真结果分析

4.1 车辆底层控制模块仿真分析

转向电机和驱制动电机PID控制器的输入为期望转角和实际转角的差值，输出为电机的占空比.图8为转向电机(中型电机)和驱制动电机(大型电机)转角的控制效果，图中虚线为电机期望转角、实线为电机实际转角.由图8可知，电机响应的上升时间在0.15～0.2 s，且几乎不超调，电机转角的跟随控制效果良好，可保证上层控制指令的执行效果.

图8 电机转角

4.2 ALC功能仿真分析

小车自动换道过程中的前轮转角采用开环控制，这里以小车向左换道为例说明.向左换道时，小车前轮转角变化见图9，其中虚线表示期望前轮转角，实线表示实际前轮转角(由转向电机转角转换得到).由图9可见，小车实际前轮转角能够较好地跟随期望前轮转角.图10为Simulink中二自由度车辆模型向左换道的仿真结果.仿真开始0.4 s后换道功能触发，换道过程中车辆始终以0.5 m/s的速度行驶.图中黑色直线为车道线，车道宽度约为0.3 m，黑色曲线为车辆换道轨迹.由图10可知，向左换道后车辆基本处于车道中心线上并且车辆航向角几乎保持不变.因此，良好的前轮转角跟随性能和稳定的车速保持性能能够保证较好的小车自动换道效果.另外，实际场景中，即便换道后小车位置或航向角与目标值存在偏差，也可由小车车道保持功能予以弥补修正.

图9 小车期望与实际前轮转角对比

图10 小车自动向左换道轨迹

4.3 AEB功能仿真分析

小车以0.65 m/s的速度，匀速在直路上行驶，当检测到与前方障碍物的距离小于两者之间的最小安全距离时，AEB系统被触发，首先是预警系统作用，提醒司机危险的存在，若司机并未采取措施，随着危险的进一步加重，制动系统开始介入工作，见图11.在3.3 s时刻开始，采取完全制动的方式，小车的速度由原来的0.65 m/s迅速下降到0，从而避免了碰撞的发生.其中虚线代表的是期望车速，实线代表的是实际的车速，采用PID控制，从图中可以看出，实际车速与期望车速之间的偏差较小，较好地满足了设计的要求.小车超声波传感器从检测到目标到发现危险存在，到最后制动停车，与前方障碍物之间的距离关系见图12.

图11 小车紧急制动车速曲线

图12 小车紧急制动与障碍车距离曲线

5 结 束 语

以Mindstorms为平台搭建了智能四轮移动机器人，为使其结构更接近真车，使用了前驱差速前转向机构.通过自带的超声波传感器和外加的手机摄像头，在Simulink中开发出智能车辆辅助系统控制算法，可实现基本的车道保持、自主换道和AEB等.由于该智能机器人与实际车辆存在一定的差距，故在智能辅助系统控制算法移植性还存在一定的问题，接下来要重点对Simulink被控对象模型进行验证.

[1] ZHAN J. An automotive longitudinal dynamic model building for adaptive cruise control[J]. Journal of Jilin University (Engineering Science Edition),2006,36(2):157-160.

[2] 朱愿.基于视觉和雷达的智能车辆自主换道决策机制与控制研究[D].北京：中国人民解放军军事医学科学院,2006.

[3] 王政.智能车辆自主换道方法的研究[D].长春:吉林大学,2016.

[4] ZHU X C, LIU Z C, LI L. Design and implementation of vehicle tracking controller for unmanned vehicles[J]. Journal of Xi’an University of Technology,2015(1):55-59.

[5] 李霖,朱西产,董小飞,等.自主紧急制动系统避撞策略的研究[J].汽车工程,2015,37(2):168-174.

[6] 吕章洁.汽车AEB仿真控制算法优化及验证[D].重庆:重庆理工大学,2016.

[7] 李文娜.汽车主动防撞预警系统的安全策略研究[D]. 长春：吉林大学,2016.

[8] 柯振宇.基于毫米波雷达的车辆纵向碰撞预警系统设计[D].武汉:武汉理工大学,2014.