动态环境下利用K inect的移动机器人增强路径规划

郑为凑，于振中，惠 晶

(江南大学轻工过程先进控制教育部重点实验室，江苏无锡214122)

路径规划技术是智能移动机器人研究的核心内容之一，其中动态环境下移动机器人的路径规划问题是移动机器人研究领域的一个热点与难点。路径规划算法和环境感知手段是两个重要的研究方面。常用的路径规划算法主要有模板匹配路径规划技术、地图构建规划技术和人工智能路径规划技术［1-2］。模板匹配方法是将机器人当前状态与过去经历相比较，比较依赖机器人过去的经验;人工势场路径规划方法是将机器人在环境中的运动视为一种机器人在虚拟的人工受力场中的运动，存在局部极小点和计算量较大等特点;地图构建又分为路标法和栅格法，存在路径规划的实时性的问题。人工智能路径规划是将现代人工智能技术应用于移动机器人的路径规划中，包括人工神经网络、进化算法、模糊逻辑与信息融合等。但是人工智能存在学习样本难获取与学习滞后问题;传统的人工势场路径规划方法是将机器人在环境中的运动视为一种机器人在虚拟的人工受力场中的运动，存在局部极小点和目标不可达等缺陷［3-5］。文中针对人工势场在机器人中的应用特点，利用改进人工势场法并使用遗传信赖域算法求解改进人工势场的子目标点问题，由多个最小点构成全局最优路径，实现规划最优化路径。

Ravari等人研究了基于视频摄像机的移动机器人路径规划与导航［6］，但是需要将二维视频图像转换为3D模型，计算量大且误差也较大。基于无线传感网络的移动机器人定位导航技术，无线传感网与移动机器人结合可以扩展其感知范围，提供定位及路径规划，拓展机器人的导航能力［7-10］。Kinect是以影像辨识为核心技术的一种3D体感摄影机，可以每秒捕捉30次RGB图像，并且还有传感器负责探测力度和深度，结合2D平面影像摄影与3D深度影像摄影技术，具备即时动态捕捉、影像辨识、三维测量、颜色识别等多项功能。微软公司最近针对Kinect体感器提供了W indows平台的SDK，采用VC++编程工具可以方便地延伸Kinect的强大功能，为移动机器人系统基于Kinect实时创建三维地形模型提供了良好的技术支持。Benavidez等人研究基于Kinect的移动机器人的导航和目标跟踪系统，并利用实验验证了该系统的可靠性［11］。Smisek等人研究基于Kinect的实时3D建模和目标物体定位方法，并对建模和定位的精度进行了定量分析［12］。

文中采用Kinect体感器实时获取动态环境中的3D深度影像与RGB影像信息，为移动机器人实时探测收集局部环境信息，提高系统对复杂环境的感知能力。机器人根据实时环境信息的变化重新规划和调整路径，有效利用局部规划，得出更加优化的路径，及时处理所遇到的随机障碍信息，从而提高移动机器人整体的路径规划性能。其中，路径规划器采用基于遗传信赖域算法改进的人工势场法，可以有效提高移动机器人的路径规划效率。

1 机器人运动学模型

首先建立移动机器人的运动模型，如图1所示。机器人位置向量由一个三维向量来表示X=［x y θ］T，其中(x，y)为机器人在二维空间的位置坐标，θ为机器人相对于X轴的朝向，即速度方向与x轴的夹角。控制移动机器人的参数主要有2个:速度V与导向角ω。

图1 机器人运动学模型Fig.1 Robot kinematic model

由所建立的机器人运动学模型可得其运动学方程:

2 Kinect与路径规划

为实现移动机器人在未知动态环境中的安全移动，需完成以下任务:地形检测、障碍物识别、可移动区定义、路径规划与导航等。Kinect体感器通过内置的CMOS红外摄像机，采用光编码技术，无论周围环境的光照条件如何，均以每秒30帧速度生成景深图像流，实时3D地再现周围环境，可以为移动机器人完成地形检测与地图构建，障碍物与目标识别，并将这些信息传递给计算机，为移动机器人路径规划打下基础。Kinect与路径规划如图2所示。

图2 K inect与路径规划Fig.2 Kinect and path planning

计算机根据构建的地图与障碍物位置信息，计算出移动机器人、目标、障碍物空间点的坐标。根据坐标结果，建立目标和障碍物的势场模型，利用遗传信赖域算法求解出子目标点，由多个子目标点最终构成全局的优化路径。

3 移动机器人的路径规划算法

3.1 人工势场法的改进措施

针对传统人工势场中的局部极小点和机器人与目标障碍物在一条直线上时反复运动，到达不了目标点的缺陷，做出以下改进:

1)将目标对机器人的吸引力以及障碍物对机器人的排斥力转化为一种势场强度，采用计算势场强度的方法来代替传统的矢量力控制。

2)在障碍物的斥力势中添加系数项‖XXg‖2，这样当机器人靠近目标点时，引力势减小的同时斥力势也在减小，直到达到目标点;引力势、斥力势同时减为0，从而解决了障碍物与目标点过近而导致的目标不可达问题。

3)对于局部极小点导致的“死锁”问题，引入“填平势场”来引导机器人走出局部极小点，即在局部极小点处增加一个附加势场Uadd。

3.2 改进人工势场的模型

根据以上措施可建立人工势场模型:

1)目标点对全方位移动车体的吸引力势模型如式(2)所示:

式中:ρ(X，Xg)为移动车体中心点当前位置与目标点之间的距离;k为正比例位置增益系数;X为机器人中心点在运动空间的位置［x，y］T;Xg为目标点位置［xgyg］T。

2)第i个静态障碍物对全方位移动车体的排斥力势模型如式(3)所示:

式中:i∈ (1，2，…，n)，n 为静态障碍物的总数;ρ(X，Xi)为移动车体中心当前位置与第i个障碍物间的最短距离;ρ0为障碍物的有效影响距离;η为正比例位置增益系数。

3)利用Kinect对动态环境的感知能力，由每秒30帧的景深图像流可以获得动态障碍物位置和运动的方向，计算机根据其位置与运动方向的解算，可以获得动态障碍物与机器人的相对运动速度。动态障碍物是运动的，仅考虑位置并不能完全反应环境信息，把动态障碍物与机器人的相对速度引入到势场函数中，得到第r个动态障碍物对全方位移动车体的排斥力势模型如式(4)所示:

式中:r∈ (1，2，…，m)，m 为移动障碍物的总数;ζ为正比例系数;V为移动车体的当前运动速度，V∈(2Vmax/3，Vmax);Vr为第r个动态障碍物的当前运动速度;φ为移动车体的当前运动方向;φ为第r个动态障碍物的当前运动方向。

4)当机器人处于局部极小点时，引入填平势解决局部极小点问题，填平势模型如式(5)所示:

式中:ρa为移动车体是否到达目标点的评判距离;s为正比例系数。

全方位移动车体总的势场强度如式(6)所示;当机器人处于局部极小点时由式(6)加上填平势作为总的势场强度值如式(7)所示:

基于以上模型，机器人运动过程中每个采样周期都以机器人可达范围内的势场强度之和最小的点作为子目标点，多个子目标点构成了全局优化路径。势场强度之和由式(6)表示。为了避免子目标点在局部极小点附近震荡的情况，采用矢量合成的方法判断机器人是否处于局部极小点，如果处于局部极小点，势场强度之和包含填平势，如式(7)表示。设机器人的最大速度为Vmax，采样周期为t0，则机器人在每个采样周期的可达范围就是以当前位置为中心，Vmaxt0为半径的一个圆。为了保证机器人运动的平稳性和执行的效率，机器人运动的速度不宜过大或过小，因此可以在R∈(2Vmaxt0/3，Vmaxt0)，θ∈(0，2π)的环形区域内选择子目标点。

如图3所示，图中的环形阴影部分即为子目标点的可选区域。图3中阴影部分的点可以表示为x′=x+R cosθ，y′=y+R sinθ。因此，式(6)和式(7)是关于变量R和θ的函数，R∈(2Vmaxt0/3，Vmaxt0)，θ∈(0，2π)。设 z1=x+R cosθ，z2=y+R sinθ，z=(z1，z2)。由式(5)和式(6)可以得出，使用遗传信赖域算法求解子目标点的目标函数如式(8)所示，即求解一类线性约束最优化问题，当用矢量合成方法判断出机器人处于局部极小点时式(8)加上填平势Uadd(z)作为目标函数:

图3 子目标点的选择范围Fig.3 Choice of the sub-target point

使用二次逼近，构造约束信赖域子问题:

式中:gk=▽U(zk);Δk为信赖域半径;Gk=▽2U(zk)，求解Gk非常复杂，用拟牛顿法的BFGS公式构造Hession矩阵Bk来近似Gk;dk为下降试探步;Ω为R和θ的取值范围。

算法所涉及符号说明:z∈ R2，zk=(zk1，zk2)，gk+1－ gk，:= 表示赋值BkdkdTkBk/dTkBkdk，实际下降量与预估下降量的比值为其中在求解用min U)时，应用遗传算法快速求解min U()得到一个优于当前点的迭代点。从而最终求解出子目标点得到变量R和θ。计算机根据变量R和θ计算出最终的移动机器人的线速度V和导向角ω，从而达到移动机器人的路径规划。

4 实验与分析

4.1 基于K inect的移动机器人路径规划体系结构

基于Kinect的移动机器人控制体系结构主要由Kinect体感器、计算机、移动机器人3部分组成。路径规划体系结构如图4所示。

图4 基于Kinect的移动机器人控制框架Fig.4 Framework of K inect-based mobile robot control

利用Kinect体感器产生的RGB图像和3D图像实时探测获取移动机器人的周边动态环境，实现对动态环境中障碍物和地形的感知。计算机将Kinect传递来的3D深度影像、距离传感器、RGB图像的原始数据流进行分析处理，实现对目标和障碍物的识别与定位。RGB图像与3D深度图像提供了移动机器人周边动态环境的特征，包括障碍物的大小、远近、颜色等特征。利用SLAM算法或者VFH/VFH+算法［5］实现机器人在未知环境下的自我定位和目标定位。通过对目标与机器人的自身定位，获得目标、障碍物及机器人的位置坐标，构建改进的人工势场，并利用遗传信赖域算法来求解子目标点，通过多个子目标点构成全局的最优路径。移动机器人从计算机中获得运动控制量:运动速度V和导向角速度ω，来调整自己的运动状态及运动姿势，实现在动态环境下的无碰运动。

4.2 实验验证

为了验证该体系结构的有效性，利用现有条件:IRobot机器人，Microsoft Kinect体感器，计算机，动态障碍物，静态障碍物，构建有效的实验系统，设计路径规划实验。实验场景如图5所示。

选取算法的初始参数为:Δ0=0.5U(z0)，ε1=0.1，ε2=0.03，ε3=0.05，a=b=0.5，M=1.5，η1=0.15，η2=0.3，B0=I2×2，β1=0.35，β2=0.75，β3=1.25，N=20，Pc=0.99，Pm=0.05，Tmax=100，κ =0.1，编 码 长 度 l=32。Vmax=0.3 m/s，t0=3 s，z0=(5Vmaxt0/6，0)，k=1，η =2，ζ =0.1，n=2，m=2，s=0.3，φ = π/2，ρ0=1 m，ρa=0.15 m，机器人起始点(1.00，0.25)，目标点(1.35，3.65)，单位为 m。根据建立的坐标系，计算机记录移动机器人在运动过程中的多个典型点，多个点拟合的路径曲线如图6所示。运动轨迹上典型点的势场强度和算法执行时间如表1所示。

图5 实验场景Fig.5 Experimental scene

图6 移动机器人的运动轨迹Fig.6 Trajectory of themobile robot

在实验中从移动机器人的移动轨迹分析可见，移动机器人的行为具备了良好的稳定性、一致性与连续性。改进人工势场模型与遗传算法的结合应用可以克服传统人工势场法的缺点，并且在求解子目标点时具有较快的收敛速度。从实验中可以看出，移动机器人可以快速避开静态与动态的障碍物，无碰撞地快速到达目标位置。该体系结构能够较好地实现移动机器人在动态环境下的路径规划任务。

5 结语

文中提出了一种基于最新体视觉传感器Kinect的移动机器人动态环境下的路径规划方法。利用Kinect体感器实时精准获取当前的三维地形信息，可以高效感知移动机器人周边的动态环境信息，采用基于遗传信赖域的改进人工势场法，克服传统人工势场法的缺陷，实现优化的路径规划。由实验验证了基于Kinect的移动机器人系统具有良好的稳定性与实用性，能够较好地满足实时控制，完成移动机器人在动态环境下的路径规划任务，为移动机器人的路径规划提出了一条新的思路。

表1 运动轨迹上典型点执行时间和势场强度Tab.1 Typical point on the trajectory execution time and potential field strength

［1］朱大奇，颜明重.移动机器人路径规划技术综述［J］.控制与决策，2010，25(7):961-967.ZHU Daqi，YAN Mingzhong.Mobile robot path planning technology review［J］.Control and Decision，2010，25(7):961-967.(in Chinese)

［2］周兰凤，徐芳.一种考虑不确定性的移动机器人路径规划方法［J］.微电子学与计算机，2010，27(7):86-89.ZHOU Lanfang，XU Fang.Account of uncertaintymobile robot path planning［J］.Microelectronics and Computer，2010，27(7):86-89.(in Chinese)

［3］陈立彬，尤波.基于改进人工势场法的机器人动态追踪与避障［J］.控制理论与应用，2007，26(4):8-10.CHEN Libin，YOU Bo.Dynamic tracking and obstacle avoidance robot based improved artificial potential field method［J］.Control Theory and Applications，2007，26(4):8-10.(in Chinese)

［4］况菲，王耀南.基于混合人工势场－遗传算法的移动机器人路径规划仿真研究［J］.系统仿真学报，2006，18(3):774-777.KUANG Fei，WANG Yaonan.Hybrid artificial potential field-simulation of mobile robot path planning GA［J］.System Simulation，2006，18(3):774-777.(in Chinese)

［5］Park M，Jeon J，Lee M.Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing［C］//Proceedings of the 2001 IEEE International Symposium on Industrial Electronics.Pusan:Institute of Electrical and Electronics Engineers，2001:1530-1535.

［6］Ravari A R N，Taghirad H D，Tam jidi A H.vision-based fuzzy navigation of mobile robots in grassland environments［C］//Proceeding of International Coference on Advanced IntelligentMechatronics.［S.l.］:IEEE/ASME Press，2009:1441-1446.

［7］薛晗，陶溢，马宏绪.基于无线传感器网络的未知环境下移动机器人实时路径规划［J］.计算机应用研究，2008，25(7):2029-2032.XUE Han，TAO Yi，MA Hongxu.Real-time path planning formobile robotbased on wireless sensor networks in unknown environment［J］.Application Research of Computers，2008，25(7):2029-2032.(in Chinese)

［8］朱齐丹，吴叶斌.基于无线传感器网络的移动机器人路径规划［J］.华中科技大学学报:自然科学版，2010，38(12):113-116.ZHU Qidan，WU Yebin.Mobile robot path planning based on wireless sensor networks［J］.Journal of Huazhong University of Science and Technology:Natural Science Edition，2010，38(12):113-116.(in Chinese)

［9］李文锋.无线传感器网络与移动机器人控制［M］.北京:科学出版社，2009.

［10］Jang-Ping Sheu，Prasan Kumar Sahoo.Efficient path planning and data gathering protocols for the wireless sensor network［J］.Computer Communication，2010，33(5):398-408.

［11］Patrick Benavidez.Mobile robot navigation and target tracking system［C］//International Conference on Systems Engineering.Albuquerque，New Mexico:［s.n.］，2011:299-304.

［12］Jan Smisek，Michal Jancosek，Tomas Pajdla.3D with kinect［C］//IEEE International Conference on Computer Vision Workshops.Barcelona，Spain:［s.n.］，2011:1154-1160.