深海履带机器车的实时导航和避障
王随平,熊光辉
(中南大学 信息科学与工程学院,湖南 长沙,410083)
摘要:针对深海底履带机器车的未知、复杂工作环境,提出一种履带机器车导航和避障算法。该算法先用声纳传感器实时检测障碍物的位置,再用D-S理论推算出障碍物的更准确位置,然后运用改进势场法进行行走方向决策,规划出履带机器车的作业路径,实现对深海履带机器车的导航和避障。仿真和试验结果表明此算法适应于动态未知复杂环境下的深海履带机器车实时导航和避障,且在目标附近存在障碍物时也能达到目标。
关键词:深海底;履带机器车;避障;Dempster-Shafer理论;改进人工势场法
中图分类号:TP273 文献标识码:A 文章编号:1672-7207(2007)01-0128-05
Real-time navigation and obstacle avoidance for
deep-seabed tracked vehicle
WANG Sui-ping, XIONG Guang-hui
(School of Information Science and Engineering, Central South University, Changsha 410083, China)
Abstract: A real-time navigation and obstacle avoidance method was presented for deep-seabed tracked vehicle in unknown and complex environment. The position of obstacles was firstly detected by sonar sensors for this method, then Dempster-Shafer theory was used to calculate more accuracy position of obstacles. Lastly, the moving direction of tracked vehicle was decided by improved artificial potential field and the working path was planned in time. So, navigation and obstacle avoidance for tracked vehicle was realized. This method is valid in complex and dynamic environment. It also can overcome the problem of unreachable goals near the obstacles. The effectiveness of this method is verified by simulation and experiment results.
Key words: deep-seabed; tracked vehicle; obstacle avoidance; Dempster-Shafer theory; improved potential field
在深海5 km处采矿是一项复杂的高技术。1991年3月,中国成为继印度、原苏联、法国、日本之后第5个国际海底开发先驱投资者,最终拥有对7.5万km2海域矿产资源的优先开采权[1]。海洋多金属结核多分布在水深5 km的海底表层,而海底环境复杂多变,表现为复杂的地表特征及海底洋流等多种形式的未知扰动。深海极限工况和采矿系统串式级联特征,使深海底机器人-履带机器车成为采矿系统的重要环节,其行走控制性能的优劣直接决定着整个采矿系统能否正常作业和作业效率的高低。履带机器车控制要求是:当在履带机器车前进路径上出现无法逾越的障碍物时,应当具有提前辨障,安全绕障的自主能力。这就需要根据海底的特殊环境设计履带机器车避障路径规划算法。由于海水介质是一种导电介质,电磁波会被海水介质屏蔽,声波传播速度虽远小于电磁波传播速度,但在传播中的损耗却远小于电磁波的损耗,所以,履带机器车环境探测采用声纳传感器[2-3],而对环境的表示采用栅格法[4-6]。声纳传感器信息处理简单、快速,但存在一定的发射角,方向分辨率差,不能发现其他方向上的障碍物,且单个声纳传感器存在幻影等不足。
为此,本文作者采用多个声纳传感器探测环境,将多个声纳传感器探测到的环境信息用不确定性证据推理Dempster-Shafer[7-9]理论进行数据融合,得到较准确的环境信息,在此基础上,运用改进的势场法[10-12],实现履带机器车导航与避障。
1 避障控制系统结构
避障控制系统结构如图1所示。由下位机采集7个声纳传感器信息,算出距离,然后由串口送往上位机,避障算法根据距离信息和履带机器车的位姿信息得出其行走方向,由上位机发送行走控制信号,使履带机器车避开障碍物达到目标点。
图1 深海履带机器车避障控制系统结构
Fig.1 Structure of control system for obstacle avoidance
for deep-seabed tracked vehicle
2 控制算法
2.1 坐标系变换
选取0.5 m×0.5 m的栅格,建立环境坐标系来描述机器人环境。在这个坐标系中,栅格不断被机器人传感器采样环境所更新,环境坐标系是绝对的表示环境,主要用于建立机器人环境地图,坐标系的选取如图2所示。
将传感器测得的障碍物信息映射到环境坐标系中: (1)
(xr,yr,αr)为机器人在环境坐标系中的位姿;d0为被测点离机器人原点的距离;αo为被测点矢量在车体坐标系中的矢量角。机器人坐标系的原点选在机器人前方车宽中点。
图2 机器人坐标系到环境坐标系的映射
Fig.2 Transition of robot coordinate to environment
coordinate
将坐标(,)映射到环境坐标系中相应的栅格 (i,j)上:
2.2 基于超声传感器的环境描述
在履带机器车上做试验极不方便,因此,研制履带机器车模型车,并且用超声波传感器代替水下声纳传感器。在车上距离地面0.28 m的同一水平150?圆弧上安装7个超声波传感器,每个传感器波束角为30?,测量范围为0.15~10.70 m,每2个传感器间隔角为25?。假定安装在机器人上的某一传感器的探测距离为r,根据超声波传感器的声场特性和测距原理,障碍物上距离传感器最近的1点发生超声波反射,此点应该处在一段圆弧上,此圆弧半径为r,角度为波束角。由于超声波的方向分辨率低,无法确定此点在此段弧的具体位置。事实上,从导航和避障的角度来看,并不需要知道障碍物的精确位置,只要知道障碍物在环境中的大概分布情况就可以实现机器人导航和避障。因此,采用概率方法Dempster-Shafer理论描述障碍物分布。为了简化计算,用弧对应的弦近似代替弧。设在超声波返回的距离处,传感器波束角对应的弧长范围内存在障碍的概率呈均匀分布,且概率总和为1,某一超声传感器在某一时刻对环境进行探测,第(i,j)个栅格有3种可能状态,即,存在障碍物(full)、不存在障碍物(empty)或不确定(如图3所示),则定义在超声波返回的距离处, 传感器波束角对应的弦上第(i,j)个栅格存在障碍物的基本概率赋值为:
不存在障碍物的基本概率赋值为:
(4)
而对于传感器波束角内的其他栅格, 显然不存在障碍物,故
(5)
(6)
(a) 障碍物的探测;(b) 障碍背境的栅格表示
图3 声纳传感器环境探测概率赋值
Fig.3 Make probability value for environment detecting
of sonar sensors
对于各栅格的状态, 可定义一个有限集Θ={E,F},E代表不存在障碍物,F代表存在障碍物,{E,F}代表状态不确定。则Θ的幂集Γ=2Θ={,E,F,{E,F}}。设m为识别框架Γ上的基本可信度分配,,m(A)称为A的基本可信度。对于各栅格,有
(7)
(8)
因此,只要知道mij(E)和mij(F)即可求得mij(E,F)。根据传感器建模的分析,mij(E)和mij(F)在机器人第k个传感器的测量范围内的赋值如下:
回声距离所在的弧,1≤k≤7,γ为调整参数,有
传感范围内的有效区域,有
这样,机器人每走1步,其多个传感器获取一组传感范围内的环境信息数据,即得到一组栅格的基本可信度分配,将此可信度与在此之前累计的同一栅格的基本可信度值采用Dempster-Shafer合成规则的证据推理方法进行融合,就可以得到传感范围内更新后的环境状态。融合公式如下:
(9)
同理可得:
(10)
其中:mp(·)表示机器人从初始到当前步之前在探测基础上经融合得到的某一栅格的基本可信度;mc(·)表示机器人传感器在当前步探测得到的同一栅格的基本可信度。采用基于基本概率赋值的决策方法[13]判定某一栅格是否存在障碍,判定规则如下:
a. 判定的目标类型应具有最大的信度函数值;
b. 判定的目标类型和其他类型的信度函数值之差要大于某个门限;
c. 不确定信度函数值必须小于某个门限;
d. 判定目标类型的信度函数值要大于不确定信度函数值。即,满足
; (11)
。 (12)
若有
(13)
则A1即为判决结果。其中,ε1和ε2为预先设定的值。
2.3 用改进的势场法进行导航和避障
在机器人前面开出一个以R(R为传感器测量距离)为半径的半圆形假想窗口,称为活动窗口,传感器不断检测活动窗口内环境,用D-S理论得到环境信息,即各障碍物在环境坐标系中的位置,然后,用势场法进行方向决策。势场法的基本思想是在移动机器人的工作环境中构造一个人工势场,在这种势场中,目标对机器人产生引力,障碍物对机器人产生斥力,机器人所受的合力方向即为机器人前进方向。
2.3.1 斥力函数的选取
在势场中,由障碍物产生的势场对机器人产生排斥作用,且距离越近,排斥作用越大,反之就越小,这种势场与电势场和引力势场类似。同时,为了使目标附近在有障碍物时机器人具有可达性,斥力势函数取为:
≤
(14)
其中:ρ为机器人距障碍物的距离;ρm为势场作用的最大范围;K0为加权系数;||qgoal-q||为机器人到目标的距离。机器人受到的斥力(Fr)为:
(15)
斥力的方向背离障碍物。当ρ→0时,Fr→∞,即当机器人与障碍物相碰时,受到的斥力为无穷大。为避免机器人与障碍物相碰,设定一个最小安全距离ρ0,当ρ→ρ0时,Fr→∞,同时为使Fr连续,Fr修改为:
≤
(16)
其中:ρ0和ρm取决于机器人的尺寸、速度及环境中障碍物的稀疏程度。
2.3.2 引力势函数的选取
目标对机器人产生吸引作用,且距离越远,吸引作用越大,反之越小。当距离为零时,机器人到达目标,引力为零。这种势能和重力势能相似,故取势函数为
(17)
其中:ρ为机器人距目标的距离,引力为:
(18)
引力是一个常量,选取一个合适的值,使机器人在合力的作用下达到目标。合力为:
(19)
3 仿真与试验
根据以上算法,用VC[14-16]进行仿真,结果如图4所示。仿真结果表明该算法能使机器人安全地从起点S通过未知环境达到目标位置G。在履带机器车模型车上实现此算法。模型车采用液压驱动,履带行走,前面装有7个Polaroid智能型超声波传感器,相互间隔为25?,用单片机控制7个声纳传感器的发射与接收,并把采集到的数据通过串口送到上位工控机。试验环境模拟海底环境,试验结果验证了该算法能指导机器人有效地避开障碍物到达目标位置。
(a) 简单环境;(b) 复杂环境
图4 履带机器车在复杂环境中实现导航和避障
Fig.4 Navigation and obstacle avoidance in complex
environment for tracked vehicle
4 结 论
a. 用D-S理论对7个声纳传感器探测到的环境信息进行触合,得到了比较准确的环境信息。
b. 运用改进的人工势场法实现履带机器的导航和避障,克服了人工势场法中目标附近有障碍物时不可达的缺点。
c. 用VC进行仿真,仿真结果表明在简单和复杂环境下,深海底履带机器车都能实现导航和避障。
参考文献:
[1] 中国大洋协会办公室. 国际海底区域研究开发“十五”总体设计[R] . 北京: 中国大洋协会, 2001.
Bureau of China Ocean Mineral Resource R&D Association (COMRA). Design of mining system of international deepseabed area research and development in “the 10th Five Year”[R]. Beijing: COMRA, 2001.
[2] 李贻斌, 刘 明, 周风余. 移动机器人多超声传感器信息融合方法[J]. 系统工程与电子技术, 1999, 21(9): 55-57.
LI Yi-bing, LIU Ming, ZHOU Feng-yu. Multi-Ultrasonic sensor fusion method for mobile robot[J]. Systems Engineering and Electronics, 1999, 21(9): 55-57.
[3] ZOU Yi, CHUA Chin-seng, ZHOU Xiao-wei, et al. Multi- ultrasonic sensor fusion for autonomous mobile robots[C]// Dasarathy B V, ed. Proc SPIE, Sensor Fusion: Architectures, Algorithms, and Applications IV. 1991(4051): 314-321.
[4] Borenstein J, Koren Y. Real-time obstacle avoidance for a fast mobile robots[J]. IEEE Transactions on Systems, Man and Cybernetics, 1989, 19(5): 1179-1187.
[5] 马兆清, 袁曾任. 基于栅格法的移动机器人实时导航和避障[J]. 机器人, 1996, 18(6): 344-348.
MA Zhao-qing, YUAN Ceng-ren. Real-time navigation and obstacle avoidance based on grids method for fast mobile robot[J]. Robot, 1996, 18(6): 344-348.
[6] Korean Y, Borenstein J. Potential field methods and their inherent limitations for mobile robot navigation[C]//Proceedings of the 1991 IEEE International Conference on Robotics and Automation.Sacramento, 1991: 1398-1404.
[7] Dempster A P. Upper and lower probabilities induced by a multivalue mapping[J]. The Annals of Mathematical Statistics, 1967, 38(1): 325-339.
[8] 杨 莹, 刘大有. 对一种基于证据理论的不确定性处理模型的重要扩充[J]. 计算机学报, 1990, 13(10): 772-778.
YANG Ying, LIU Da-you. The extension of an uncertainty reasoning model based evidence theory[J]. Chinese Journal of Computers, 1990, 13(10): 772-778.
[9] 叶 涛, 陈尔奎, 杨国胜, 等. 全局环境未知时机器人导航和避障的一种新方法[J]. 机器人, 2003, 25(6): 516-520.
YE Tao, CHEN Er-kui, YANG Guo-sheng, et al. A novel method of navigation and obstacle avoidance for mobile robot in global unknown environment[J].Robot, 2003, 25(6): 516-520.
[10] KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[C]//IEEE Int Conf on Robotics and Automation. St Louis, Missouri, 1985: 500-505.
[11] Kim J H, Kim K C. Path planning and role selection mechanism for soccer robots[C]//Proceedings of the 1998 IEEE International Conference on Robotics & Automation. Leuven, 1998: 3216-3221.
[12] ZHANG Qi, YANG Yi-min, LI Ying. Soccer robot anti-collision control based on modified artificial potential method[J]. Robot, 2002, 24(1): 13-15.
[13] 何 友, 等. 多传感器信息融合及应用[M]. 北京: 电子工业出版社, 2000.
HE You, et al. Multi-sensor fusion and applications[M]. Beijing: Electronics Industry Press, 2000.
[14] 张 菁, 王立权, 张铭钧. 基于模糊决策法的水下机器人路径规划方法[J]. 哈尔滨工程大学学报, 2001, 22(1): 49-54.
ZHANG Jing, WANG Li-quan, ZHANG Ming-jun. Path planning for underwater robot by fuzzy decision making method[J]. Journal of Harbin Engineering University, 2001, 22(1): 49-54.
[15] Kruglinski D J, Wingo S. Visual C++6.0技术内幕[M]. 北京: 北京希望电子出版社, 1999.
Kruglinski D J, Wingo S. Inside visual C++[M]. Beijing: Beijing Hope Electronic Press, 1999.
[16] 陈 峰, 王随平, 韩晓英. 深海集矿机器人的自修正专家模糊控制[J]. 中南大学学报: 自然科学版, 2005, 36(6): 1059-1062.
CHEN Feng, WANG Sui-ping, HAN Xiao-ying. Self-adjustable expert fuzzy control of deep ocean mining robot[J]. Journal of Central South University: Science and Technology, 2005, 36(6): 1059-1062.
收稿日期:2006-09-25
基金项目:国际海底区域研究开发“十五”项目(DY105-03-02-06);国家重点基础研究发展规划项目(2002CB312203);国家自然科学基金资助项目(60505018)
作者简介:王随平(1956-),湖南长沙人,教授,从事人工智能、深海机器人、现场总线以及计算机控制系统研究
通讯作者:王随平,男,教授;电话:0731-8836475(O);E-mail: wangsp@mail.csu.edu.cn