未知环境下基于VFH+物理仿真耦合模型的 机器人避障

孙远敬，王 帅，李 鑫，郭 鹰

（辽宁工程技术大学 机械工程学院，辽宁 阜新 123000）

0 引言

机器人路径规划问题在消防救援、交通运输、航空航天、物流仓储等行业中有很大研究价值和发展潜力.路径规划作为构建起始点与目标点之间理论最优路径的策略问题，可分为对预设环境和未知环境的规划.基于这两种环境下的路径规划问题，为克服各种算法存在的局限性，相关学者对避障算法的优化改进和组合策略做了大量研究.庄宇辉[1]等基于向量场直方图法（Vector Field Histogram, VFH）提出考虑运动和环境特性，引入阈值评价函数对各行进方向进行综合评价，选取最合适的阈值范围通行.张亚兰[2]等将改进的双向A*算法与VFH算法联合优化应用在避障问题中，所提方法规划的路径长度短、耗时少，并能有效避免环境变化带来的碰撞风险.李卫硕[3]等提出将反向传播（Back Propagation，BP）神经网络应用在机器人避障中，选取合适的输入、得分选项，设置合适的条件概率训练神经网络模型.轨迹较人工势场法短，且被证明能够应用于较复杂的预设障碍物环境中.

本文对传统的机器人避障实验流程，针对某款移动机器人在矩阵实验室（Matlab）环境创建向量场直方图算法（Vector Field Histogram+）模型，在Matlab中验证算法的有效性后，再基于Simulink的机器人物理仿真耦合模型，进行耦合算法的仿真实验，实现避障机器人对障碍物的预警和规避.简化实验流程的同时解决了控制算法模型影响后期实验阶段机器人的路径精度问题，避免出现机器人偏离规划路径甚至发生碰撞风险.

1 改进向量场直方图算法

与VFH算法相比，VFH+算法由于其在迭代过程中将机器人的尺寸因素和最小安全距离考虑到了算法模型内，从而使路径具有更高的稳定性和平滑性.因此得到的路径轨迹比原始的VFH算法效果更好[4].VFH+算法使用栅格地图表示在此处存在障碍物的可信度（Certainty Value，CV）作为输入.高的CV值表示此处较大可能存在障碍物.活动窗口内每个栅格的CV值cij的大小可按经验方程确定.

1.1 主极坐标直方图的建立

初始阶段，将栅格地图的活动区域映射到主直方图.活动区域是随机器人移动的半径为ws的圆形窗口，栅格地图中的每个栅格的内容作为障碍物向量，障碍向量的向量方向βij由栅格到机器人中心的方向决定[5].

式中，x0、y0为机器人中心点的当前坐标；xj、yj为活动区域中障碍物栅格单元cij的坐标.

栅格单元中障碍向量模数为

式中，a，b为常数；dij为栅格单元cij到机器人中心的距离，m.根据建立的障碍向量建立主极坐标直方图Hp，Hp中可自由选择角分辨率α，活动窗口分区后得到的区间数s=360/α，本次实验中，将α设置为5，共72个区间.VFH方法未考虑机器人尺寸因素和最小安全距离的影响，而VFH+方法则通过低通滤波器来补偿机器人的尺寸因素可能会对实验造成的额外影响.另一方面，VFH+算法通过在构建主极坐标直方图时放大障碍物栅格范围，可以有效地实现宽度补偿保证了算法的稳定性.此外，VFH+算法还更新了所有与放大后的障碍物栅格相重合的直方图区间.图1显示了一个障碍物栅格单元被放大rr+s倍，在栅格地图中，被放大后的障碍物栅格区域表明机器人在该区域内为不能行走状态，实现宽度补偿，保证算法稳定性.此外，VFH+算法还会实时更新所有与放大后的障碍物栅相重合的直方图区间.

图1 放大后的障碍物栅格单元示例 Fig.1 example of an enlarged barrier grid unit

对于区间k在极坐标下的障碍密度为

式中，为滤波系数，为保证主极坐标直方图平顺性，该系数只考虑机器人视觉范围内障碍物；ijr为放大障碍物栅格以机器人质点为中心放大角，°.

1.2 二值极坐标直方图的建立

VFH+算法设置τlow和τhigh两个参数增加迟滞作用以改善VFH算法中机器人某些时刻与障碍物距离太近、安全区域太小等缺陷，本实验τlow取500，τhigh取1 400.根据主极坐标直方图Hp和τlow、τhigh以及二值极坐标直方图中的每个区间的状态值Hb，用来记录机器人在该时刻下可以自由通行的方向.状态值Hb分为自由通行和禁止通过两种情况，分别由0和1表示.当区域k内的障碍频次大于τhigh和τlow时，Hp通过以下规则更新二值极坐标直方图.

某一时刻，Hp通过以下规则更新二值极坐标为

假设机器人右转向半径为rr，左转为rl.在障碍物栅格地图中，机器人的左右转向圆圆心与当前机器人中心的相对距离关系为

式中，Δx为转向圆圆心在栅格地图中相对机器人中心在x轴方向的偏移距离，m；Δy为转向圆圆心在栅格地图中相对机器人中在y轴方向的偏移距离，m；θ为机器人的轨迹转角，°.

当左转向圆与将栅格地图中的障碍物栅格A放大了rr+s倍后的区域有部分重叠时，此时满足

式中，dl为栅格单元cij到左转向圆中心的距离，m；Δx（j）、Δy（i）为栅格单元cij到机器人中心的相对x方向、y方向的距离，m.

二者覆盖的区域此时都被Hb认为是禁止通过区域，相反，由于障碍栅格B与右转向圆无重叠区域，除放大后的栅格B区域外，其他区域都被认为是自由通行区域[6]为

图2中，放大的障碍物栅格A禁止机器人向左侧方向行进.而障碍物B没有阻挡右侧方向.若点s在图示位置，VFH算法由于没有考虑机器人尺寸、障碍物尺寸以及最小安全距离等，会将机器人引导至左侧并可能会使机器人与障碍物A相撞.而在VFH +算法中，由于转向圆区域与放大后的障碍物栅格区域重叠，使得机器人在A前右转直行，不再向左转向，直至两区域不再有重叠部分.

图2 基于VFH+算法的转向原理示意 Fig.2 schematic diagram of steering principle based on VFH+algorithm

1.3 转向方向的选择

根据Hp和Hb中的值行进可以最大限度避免障碍物碰撞，但可能会偏离实验的目标方向.因此，建立一个在行进代价和目标方向中做出取舍的平衡函数.由该函数选择全局最合适的行进方向[7].

本次VFH+算法模型的平衡函数为

式中，μ1，μ2，μ3为权衡系数决定了算法的优劣性，一般情况下应满足μ1＞μ2+μ3；Δ（c,kt）表示候选方向与目标方向间的差角；Δ（c,θi/α）为候选方向与行进方向之间的差角；Δ（c,kn,i-1）为当前候选方向与前一次选择方向的角度变化值.

2 基于Matlab的VFH+避障算法模型

利用Matlab软件建立基于VFH+算法的机器人避障算法模型，障碍物地图模型范围为10 m× 10 m环境，A（1，4）为出发点，D（9，7）为目标点.N1时刻，机器人从出发点A到达点B（3，5）见图3.

图3 避障机器人到达点B示意 Fig.3 schematic of obstacle avoidance robot reaching point B

N1时刻下的二值极坐标直方图见图4.此时区间0～8，28～42的状态值Hb为0，表明该区域发生碰撞的可能性极低.此时代表两个候选宽域区间内的任意方向均可自由通行[8].

图4 机器人到达点B时的二值极坐标直方图 Fig. 4 binary polar coordinate histogram when the robot reaches point B

二值坐标直方图中的Hb规划了两个候选宽域区间的方向范围之后，再根据主极坐标图中的Hp和平衡函数确定最终的转角大小.Hp中不仅记录了每个候选区间自由通行和禁止通过两种状态值，还记录了图3中点B到达目标点D的目标方向，根据图5中Hp标注的目标方向和平衡函数计算出下一时刻机器人行进的转角大小和方向，此时机器人在点B选择沿初始前进方向偏左11°行进.

图5 机器人到达点B时的主极坐标直方图 Fig.5 histogram of main polar coordinates when the robot reaches point B

N2时刻，机器人从点B移动到点C，见图6.达到点C时，图7中的二值极坐标直方图会实时记录72个区间的自由通行和禁止通过的状态值.在到达点C时，区间0～16，28～47，51～72的状态值Hb等于0，表明这三个宽域区间内的任意方向均可自由通行.

图6 避障机器人到达点C示意 Fig.6 schematic of obstacle avoidance robot reaching point C

图7 机器人到达点C时的二值极坐标直方图 Fig.7 binary polar coordinate histogram when the robot reaches point C

二值极坐标直方图中Hb规划了三个候选区域后，主极坐标直方图中也记录了从当前位置C到达目标点D的目标方向，见图8.此时区间0～3的障碍频次Hp在820至980左右，这是由于如果继续按照左转11°的方向行进，右侧放大的障碍物栅格会与机器人的右转向圆重合，导致安全距离太小产生碰撞风险.因此在平衡函数和障碍密度的指导下，并考虑目标方向.在区间0～16候选区域中，机器人选择初始前进方向偏左15°的方向继续向点D行进.

图8 机器人到达点C时的主极坐标直方图 Fig.8 histogram of main polar coordinates when the robot reaches point C

在行进过程的每个时刻，区域状态值Hb都能够实时计算并记录下行进过程中符合条件的所有候选方向，基于VFH+算法的频次Hp结合目标方向和平衡函数实时计算出机器人在不同方向上的行进代价，选择最优的行进路线，最终避障机器人成功到达目标点.

3 避障机器人模型搭建及实验

3.1 Simulink模块下的动力学模型搭建

Simulink模块下的Simscape提供了不同模块来代表机械系统中的实体零件、载荷约束、空间关系等.用户可通过这些模块定义机械系统内部的运动关系、约束及受力情况，从而建立物理仿真模型[9].

避障试验中使用的是基于 Arduino UNO 主板和L298N驱动模块为基础的自制机器人.搭载HC-SR04超声波测距模块用于测量障碍物位置信息.该机器人的相关参数见表1.

表1 避障机器人的相关尺寸及参数 Tab.1 related dimensions and motion parameters of obstacle avoidance robots

机器人的物理仿真模型主要由机器人左右轮与机器人主体之间的约束关系、机器人与地面环境的相对位置关系和Stateflow对机器人转向系统和电机状态的控制逻辑等组成，将建立的VFH+算法模型导入到物理仿真模型中实现模型耦合.在Multibody进行动力学仿真时根据VFH+算法实时计算的结果，stateflow可以实现对机器人运动方向和速度的实时控制，解决复杂的控制逻辑问题.将stateflow控制逻辑模块放入物理仿真耦合模型中[9].基于Simulink建立的物理耦合仿真模型见图9.

图9 避障机器人的物理仿真耦合模型示意 Fig.9 coupling of physical simulation for obstacle avoidance robot

3.2 模型性能验证

基于Matlab和Simulink建立物理仿真耦合模型后，进行实际环境下的耦合模型有效性验证的避障实验.Simulink不仅可以创建机器人系统的物理仿真模型，还直接提供了硬件接口实现对各主流机器人主板系统的连接和控制，包括树莓派、乐高等平台[10].实现了对目标问题的动力学物理仿真模型搭建、基于算法的逻辑模型的建立和实现对硬件系统的直接控制，耦合模型对机器人运动状态的直接控制，避免了将大量实验时间耗费在编写代码指令上，简化了实验流程.对基于Simulink模块下的耦合仿真模型直接控制的机器人系统进行基于VFH与VFH+算法的实地避障实验测试耦合模型的有效性和机器人的避障性能.

由图10两种算法模型的路径对比可以看出，基于VFH算法的蓝色路径轨迹的距离与基于VFH+算法绿色轨迹的距离相比差别不大，但是在行进途中与障碍物的安全距离过小，极易发生碰撞，而考虑了机器人尺寸因素与VFH+算法耦合模型的机器人,在相同的障碍物地图中，其规划路径距离更短，安全距离更大.综上，基于Simulink物理仿真耦合模型的有效性得到了验证，VFH+算法性能在全局路径规划和局部实时避障方面均优于VFH算法.

图10 两种算法的耦合模型路径对比 Fig.10 comparison of the coupling model path of two algorithms

4 结论

本文基于MATLAB创建了VFH+算法模型，基于Simulink下的Simscape模块建立了某款机器人的物理模型.实现耦合模型对避障过程中的机器人运动状态直接控制.可以得到以下结论：

（1）基于Matlab建立的VFH+算法模型可以实现对机器人避障实验的前期仿真实验，检验避障算法的性能是否符合实验要求，提高了避障实验效率.

（2）采用Stateflow对机器人转向系统和电机状态实现控制，简化了传统的代码编写过程，降低了代码错误造成的实验风险.

（3）利用Simulink提供的机器人主板系统平台的硬件支持包，实现了耦合模型对机器人运动状态的直接控制，避免了代码移植过程中代码缺失等因素对避障试验的影响，简化了避障试验的实验流程，为机器人避障实验研究提供了新的思路与方法.