基于D-S理论和改进势场法的路径规划算法
王随平1，桂卫华1，潘天国
(中南大学 信息科学与工程学院，湖南 长沙，410083)
摘 要：
理论和改进势场法相结合的路径规划算法。采用多个声纳传感器探测环境，将多个声纳传感器探测到的环境信息，利用不确定性证据推理D-S理论进行数据融合，得到较准确的环境信息。在此基础上，运用改进的势场法来实现集矿机导航与避障，克服人工势场法中目标点附近有障碍物时不可达的缺点。用VC开发平台对2种算法进行仿真，仿真结果验证了算法的有效性。
关键词：
中图分类号：TP391.9 文献标志码：A 文章编号：1672-7207(2011)S2-0341-06
Path planning algorithm based on D-S theory and improvements potential field
WANG Sui-ping, GUI Wei-hua, PAN Tian-guo
(School of Information Science and Engineering, Central South University, Changsha 410083, China)
Abstract: A path-planning algorithm based on D-S theory and improved artificial potenial field was presented. The environment information was detected by multi-sonar sensors and was fused by D-S theory for acquiring more precision environment information. The navigation and obstacle-avoidance were realized by using improved artificial potential field. The problem that the vehicle can not reach the planning goal position when some obstacles exist near it is solved. The simulation and experiment were carried out using the developing platform VC and experiment system. The effectiveness of these two methods was verified by the simulating results.
Key words: mining machine; path planning; D-S theory; improved potential field
深海底蕴藏着丰富的矿产资源，其开发手段的研究，对我国矿产资源的可持续利用及深海作业技术的发展具有重要的战略意义。深海集矿机行走于6 km 深海底“极稀软”沉积物底质，作业环境为无自然光、海底高压、未知复杂环境，其路径规划技术直接关系整个深海采矿系统的运行性能和采矿效率。
目前，国内外路径规划主要算法有人工势场法、栅格法及自由空间法等等。人工势场法是将机器人在环境中的运动视为一种虚拟的人工受力场中的运动。该算法结构简单，便于底层的实时控制，在实际避障和平滑的轨迹控制方面得到了广泛的应用，其不足在于存在局部最优解，容易产生死锁现象，可能使机器人在到达目标点之前就停留在局部最优点。为解决局部极小值问题，已经提出了一些改进算法，如Sato提出的Laplace势场法。改进算法是通过数学上合理定义势场方程，来保证势场中不存在局部极值。栅格法将移动机器人工作环境分解成一系列具有二值信息的网格单元来记录环境信息，有障碍物的地方累积值比较高，移动机器人就会采用优化算法避开。栅格法表现出良好的性能，但该方法存在环境分辨率与环境信息存储量的矛盾。自由空间法是一种经典的路径规划方法，它把机器人所在的环境空间分成两部分，即自由空间和障碍物空间。机器人在自由空间中找到一条按某种性能指标规划出来的安全路径。其优点是比较灵活，起始点和目标点的改变不会造成连通图的重构，缺点是复杂程度与障碍物的多少成正比，且有时无法获得最短路径。
本文作者提出基于D-S理论和改进势场法相结合的路径规划算法，将多个声纳传感器探测到的环境信息，利用不确定性证据推理D-S理论进行数据融合，得到较准确的环境信息。在此基础上，运用改进的势场法来实现集矿机导航与避障，克服了人工势场法中目标点附近有障碍物时不可达的缺点，通过仿真验证了算法的有效性。
1 基于D-S理论和改进势场法相结合的路径规划算法
D-S证据理论是Dempster[1]于1967年提出，后由Shafer[2]加以扩充和发展而形成的不确定性推理方法，广泛应用于信息融合及专家系统等领域。文献[1-3]中给出了具体定义和基本理论。D-S理论采用信任函数而不是概率作为度量，通过对一些事件的概率加以约束以建立信任函数而不必说明精确的难以获得的概率，当约束限制为严格的概率时，进而成为概率论。
1.1 Dempster合成及判定规则
由Dempster合成法则，可以用来计算n个证据共同作用下的基本可信度分配函数，因此，有多传感器的数据融合。
设由P个传感器构成一个多传感器系统，对所有传感器数据融合得到第s个命题As(s=1, 2, ···, K)，多传感器融合后，其基本可信度分配函数为：
(1)
“不确定”命题融合后，基本可信度分配函数为：
(2)
用证据理论组合证据后的判定与具体应用紧密关联。对于目标模式的分类决策，其判定规则为：
(1)判定的目标类型具有最大的信度函数值；
(2)判定的目标类型和其他类型的信度函数值之差大于某个门限；
(3)不确定信度函数值必须小于某个门限；
(4)判定目标类型的信度函数值大于不确定信度函数值。
上述规则可表示为：设
，满足
若有
则A1为判决结果。其中：ε1和ε2为预先设定的门限。
1.2 控制算法
1.2.1 坐标系变换
采用二维笛卡儿矩形栅格地图来描述机器人环境。栅格大小的选取直接关系到控制算法的精度，考虑到超声波传感器的特点以及机器人的物理尺寸，选择0.5 m×0.5 m 为一个栅格，建立环境坐标系来描述机器人环境，坐标系的选取如图1所示。
图1 机器人坐标系到环境坐标系的映射
Fig.1 Map of robot coordinate system to environment coordinate system
采用下式将传感器测得的障碍物信息映射到环境坐标系中：
(3)
式中： (x′e, y′e)为被测点在环境坐标系中的坐标；(xr, yr, αr)为机器人在环境坐标系中的位姿；d0为被测点离机器人原点的距离；αo为被测点矢量在车体坐标系中的矢量角。机器人坐标系的原点选在机器人前方车宽中点。采用下式将坐标(x′e, y′e)映射到环境坐标系中相应的栅格ceil(I, j)上。
(4)
式中：i和j为栅格序号；w为栅格宽度。
1.2.2 基于声纳传感器的环境描述
在集矿机模型车上距离地面0.28 m的同一水平150°圆弧上安装7个声纳传感器，每个传感器波束角为30°，有效测量范围为0.15~10.70 m，每两个传感器间隔角为23°，用以覆盖机器人周围约180°的空间。假定安装在机器人上的某一传感器的探测值为r，根据超声波传感器的声场特性和测距原理，障碍物上距离传感器最近的一点发生超声波反射，此点应该处在一段圆弧上，此圆弧半径为r，角度为波束角。为了简化计算，用弧对应的弦近似代替弧。设在超声波返回的距离处，传感器波束角对应的弧长范围内存在障碍的概率呈均匀分布，且概率总和为1，某一超声传感器在某一时刻对环境进行探测，第(i, j)个栅格有3种可能状态，即存在障碍物(full)、不存在障碍物(empty)或不确定，如图2所示。
定义在超声波返回的距离处，传感器波束角对 应弦上第( i, j) 个栅格存在障碍物的基本概率赋值 为：
(5)
其中：r为传感器探测到障碍物的距离；β为波束角的一半。不存在障碍物的基本概率赋值为：
(6)
而对于传感器波束角内的其他栅格，显然不存在障碍物，则
(7)
(8)
对于各栅格的状态，可定义一个有限集Θ={E, F}，E代表不存在障碍物，F代表存在障碍物，{E, F}代表状态不确定。则Θ的幂集
{E, F}}。设m为识别框架Γ上的基本可信度分配，
，m(A)称为A的基本可信数。对于各栅格，有
(9)
(10)
图2 声纳传感器环境探测概率赋值
Fig.2 Making probability value for environment detecting of sonar sensors
因此，只要知道mij(E)和mij(F)，即可求得mij({E, F})。根据传感器建模的分析，mij(E)和mij(F)在机器人第k个传感器的测量范围内的赋值如下：
cell(I, j)∈回声距离所在的弧，1≤ k ≤7，γ是调整参数。
cell(i, j)∈传感范围内的有效区域。
这样，机器人每走一步，其多个传感器获取一组传感范围内的环境信息数据，即得到一组栅格的基本可信度分配。对此信度与在此之前累计的同一栅格的基本可信度值采用D-S合成规则的证据推理方法进行融合，就可以得到传感范围内更新后的环境状态。融合公式如下：
(11)
同理可得：
(12)
其中：mp(g)表示机器人从初始到当前步之前在探测基础上经融合得到的某一栅格的基本可信数；mc(g)表示机器人传感器在当前步探测得到的同一栅格的基本可信数。采用基于基本概率赋值的决策方法[4]判定某一栅格是否存在障碍，判定规则如下：
满足
(13)
(14)
若有
(15)
则A1为判决结果。其中：ε1和ε2为预先设定的值。
1.2.3 用改进的势场法进行导航和避障
人工势场法是Khatib于1986年提出的，其基本思想是在运动空间中，将机器人视为一质点，使该质点受到人工势场力的作用，势场中包含引力极与斥力极。图3所示为建立的人工势场，其中：O为障碍物，G为目标。
(1) 斥力函数和引力函数的选取
在势场中，由于障碍物产生的势场对机器人产生排斥作用，且距离越近，排斥作用越大，反之就越小，这种势场与电势场和引力势场类似，与距离成反比，故斥力势函数为：
(16)
式中：ρ为机器人距障碍物的距离(m)；ρm为势场作用的最大范围(m)；Kr为斥力系数。
图3 人工势场
Fig.3 Artificial potential field
机器人受到的斥力Fr为：
(17)
当
时及
，即机器人与障碍物相碰时，受到的斥力为无穷大。为避免机器人与障碍物相碰，设定一个最小安全距离ρ0，当
时，，同时，为使Fr连续，Fr修改为：
(18)
ρ0及ρm的大小取决于机器人的尺寸、速度及环境中障碍物的稀疏程度。
目标G的势函数Uα同样也可基于距离的概念。取势函数为：
(19)
式中：Kα为引力系数，引力Fα为：
(20)
负号表示引力方向指向目标点。引力是一个常量，选取一个合适的值，使机器人在合力的作用下达到目标点。合力为：
(21)
(2) 斥力势函数的改进
实际上，当用这个合力来控制机器人时，如果目标在障碍物的影响范围之内，则机器人始终不能到达目标。因为当机器人向目标靠近时，距离障碍物也越来越近，这样斥力增加很快，机器人受到的合力为斥力方向，不能到达目标。这是因为目标点不是整个势场的全局最小点。如果在机器人向目标逼近时，斥力场趋向于零，那么目标点将是整个势场的全局最小点。因此，建立一个新的斥力场函数，这个斥力场函数考虑机器人与目标之间的相对距离，从而确保整个势场在目标点全局最小。改进的斥力势函数为：
(22)
为机器人到目标点的距离，斥力为：
(23)
将式(23)与式(18)相比，引入了机器人与目标之间的相对距离，随着机器人与目标的靠近，斥力逐渐减小，趋向于零，机器人在引力作用下达到目标。
2 仿真
根据以上算法，采用VC开发平台进行仿真。实际上，每个声纳能够覆盖一定角度范围的探测区域，所以，只使用7个声纳就可满足要求。但计算机仿真中由于使用的是模拟声纳探测方式，沿某一条直线 逐点检测颜色来探测障碍物的，它不能检测探测线周围一定角度的区域，所以，7条探测线是远远不够 的，因此，在仿真中采用13条探测线，其布置如图4所示。
在计算机仿真中，障碍物的颜色在屏幕上设定为红色，即Rightcolor = RGB(255, 0, 0)。检测程序开始后，即沿给定的方向逐点检测像素的颜色。当检测到某像素点的颜色为RGB(255, 0, 0)时，则记录该点的坐标，计算该点到机器人当前位置的距离并返回。声纳探测器从0号到6号(在仿真程序中是0号到12号探测线)依此对外界环境进行探测，如图5所示。图5(a) 为简单环境，图5(b)所示为复杂环境且目标点附近有障碍物。仿真结果表明，该算法能使机器人安全的从起始位置S通过未知环境达到目标位置G。
图4 仿真程序中探测线的布置图
Fig.4 Layout of probe cable in simulation experiment
图5 机器人在复杂环境中实现导航和避障
Fig.5 Robot navigation and avoid obstacles in complex environments
3 结论
采用D-S理论对7个声纳传感器探测到的环境信息进行融合，得到比较准确的环境信息。在此基础上，运用改进人工势场法来实现集矿机的导航和避障，克服了人工势场法中目标点附近有障碍物时不可达的缺点。仿真结果验证了该算法的有效性。
参考文献：
[1] Dempster A P. Upper and lower probabilities induced by a multivalued mapping[J]. The Annals of Mathematical Statistics, 1967, 38(1): 325-339.
[2] Shafer G. A mathematical theory of evidence[M]. Princeton: Princeton University Press, 1976: 77-85.
[3] 段新生. 证据理论与决策, 人工智能[M]. 北京: 中国人民大学出版社, 1993.
DUAN Xin-sheng. Evidence theory and decision, artificial intelligence[M]. Beijing: China Renmin University Press, 1993.
[4] 何友. 多传感器信息融合及应用[M]. 北京: 电子工业出版社, 2000: 20-97.
HE You. Multiple sensor information fusion and its application [M]. Beijing: Electronics Industry Press, 2000: 20-97.
(编辑 陈卫萍)
收稿日期：2011-06-15；修回日期：2011-07-15
基金项目：国际海底区域研究开发“十五”项目(DY105-03-02-06)；国家自然科学基金资助项目(60505018)
通信作者：王随平(1956-)，男，河南焦作人，博士，教授，从事人工智能、深海机器人、现场总线及计算机控制系统等研究；E-mail: wangsp@csu.edu.cn
摘要：提出基于D-S理论和改进势场法相结合的路径规划算法。采用多个声纳传感器探测环境，将多个声纳传感器探测到的环境信息，利用不确定性证据推理D-S理论进行数据融合，得到较准确的环境信息。在此基础上，运用改进的势场法来实现集矿机导航与避障，克服人工势场法中目标点附近有障碍物时不可达的缺点。用VC开发平台对2种算法进行仿真，仿真结果验证了算法的有效性。
