电力线路巡检飞行机器人三维轨迹生成方法

柳长安,杨国田,吴 华,周 宏

(华北电力大学 控制与计算机工程学院, 北京102206)

轨迹生成作为电力线路巡检飞行机器人(flying robot for overhead powerline inspection,FROPI)自主飞行的重要保障, 是指依靠已知的地形(包括障碍)信息和威胁信息, 在某些约束条件下,寻找到从起点到目标点的可行飞行路线.目前,路径规划的研究方法很多[1-2],基于决策论的路径规划是近年来才出现在人工智能研究领域的[3],作为一种处理顺序决策问题的规划方法,特别适合于处理不确定情况下的轨迹生成问题.

马尔可夫决策过程(Markov decision process ,MDP)是其中最基本的应用模型.在这个理论框架下,可以把FROPI 的轨迹生成问题看作是在给定环境模型和奖惩原则的情况下, 寻求最优策略的问题.针对某型FROPI 的低空作业飞行的环境和自身运动特性,本文初步建立基于MDP 的全局路径规划模型[4] .对于FROPI 具有时空开销大、航向改变频繁的缺点,提出一种基于状态聚类方法的分层马尔可夫决策过程(hierarchical Markov decision process ,HMDP)模型,并结合FROPI 机载巡检设备(如摄像机)的控制标准,将其拓展到三维全局轨迹生成中.

1 FROPI 的MDP 模型

1 .1 MDP 模型定义

一个MDP 可以用一个四元组M=〈S,A,T,R〉描述.

S:包括所有环境状态的有限集合.定义80 km×80 km 范围的环境作为FROPI 路径规划的环境状态.规划时,基于栅格法以100 m(由实际FROPI运动约束决定)间隔进行二维离散化空间建模,得到6 .4 ×105个空间状态.

A:包括所有动作的有限集合.定义FROPI 有9个可行的动作, 分别为北、东北、东、东南、南、西南、西、西北和悬停.

T:S×A→Π(T)是状态转移函数,在给定目前状态和动作的情况下, 下一状态的概率分布将直接决定下一动作的输出.这里认为在没有目标时的初始状态转移概率是平均分配的(图1a).以目标点在正北方为例, 若给出动作北,状态s周围栅格的状态迁移概率分布如图1b 所示, 表示给定目前状态s和动作北的情况下,下一状态s′的概率分布, 已知状态分布后,再根据相应的报酬, 就可以得到最优策略.需要注意的是, 这只是概率分布的一种特殊情况,分布值会随着目标点、障碍情况而变化.

图1 状态迁移概率分配图Fig.1 Probability distribution figure of the state transition

R:S×A→Π(R)是报酬函数,表示在给定目前状态和动作的情况下所期望的立即报酬(一般用R(s,a)来表示在状态s下执行动作a所能得到的立即报酬).构造无模型的均匀表示的报酬函数模型Rm和Ra分别为正常飞行和遇到障碍时的报酬函数.

在这个模型中, 下一个状态和期望获得的立即报酬只和当前状态、所执行的动作有关,而与历史无关,这就是所谓的马尔可夫属性[5]:t+1 时刻的状态和报酬只依赖于t时刻的状态和在t时刻执行的动作.

1.2 搜索策略

FROPI 对动作策略进行搜索需要考虑因素有:①必须充分探索环境状态空间, 从而能够找到最优的或者次优的策略, 即探索问题;②要利用通过概率学习获得的经验选择动作,即利用问题.二者相互矛盾.如何合理地平衡二者从而有效选择动作, 即为搜索策略问题.这里采用动态规划, 使要搜索的那些评价函数最优, 即无限折扣报酬期望和最大的决策序列[5] ,则最优评价函数为

式中:λ为折扣因子, 本文取0 .8 ;E(·)为期望值;R t为t时刻的立即报酬.对于任意状态, 使评价函数最优的充要条件为[5]

式中,P为转移概率.式(4)为Bellman 方程.相应地,最优策略π*为

对于Bellman 方程的解, 采用函数迭代法,即直接对最优评价函数V*进行搜索.设在时间步t,系统的状态为s,V*(s)则按下式进行迭代:

比较两步连续迭代的评价函数的最大值, 如果差值小于指定的精度ε, 则结束迭代.

2 FROPI 的HMDP 模型描述

2 .1 状态聚类

通过上文构建状态空间的方法,可以看到,栅格的大小影响算法的时空复杂度.栅格的规格越小,划分环境后得到的小区域越多, 则数据所占的内存空间越多,搜索速度就越慢.但栅格太大又将影响路径的精确程度.本文构造的栅格平面状态为6 .4 ×105个,如果拓展到三维空间, 时空开销是很大的, 同时在规划中(如图2 所示)可以看到, FROPI 出现了航向频繁变化的问题, 规划的转向角度也无法在实际飞行中实现.因此,引入状态空间聚类的思路方法.

状态聚类的思路方法是通过把原始状态归并为较小的集结状态集合, 从而在更小维数的空间上规划路径.明确地说, 把环境状态空间S划分成m个子集S1,S2,…,Sm,S=S1∪S2∪… ∪Sm,把每个子集作为一个集结状态,进行规划.这样, 迭代的次数会明显减少,有效克服维数灾和节点过多的问题.依据标准的MDP 模型进行状态类聚,加入分层结构.定义由MDP 组成完整的分层系统,它们可以分别转化成标准的MDP .重新定义四元组Mn=〈Sn,An,Tn,Rn〉,其中n代表层数, 根据状态设定.定义M0为初始的平面MDP ,当n≥1 时,Mn由Mn-1通过聚类状态Sn-1得到,每类状态聚类后变为一个状态,在分层的过程中, 无形之间减少了空间状态数量, 大大加快了搜索速度.

每一层上的状态转移概率和报酬函数与平面MDP 模型中计算方法相同,只是范围已经局限在所在层的状态之间.

2.2 HMDP 模型的FROPI 路径规划

根据环境中的障碍信息, 进行状态聚类,参照文献[6-8] 中的八叉树方法,采用纵向划分状态层次的方法(如图2 所示).假设环境中有复杂形状的障碍物, 按照纵向划分标准把初始环境状态划分为9个子状态, 采用迭代策略,计算每一层次状态之间的转移概率, 在第1 次的搜索中, 规划的可能路径为1 →2 →4 →6 →7 →8 →9,当底层完成后再返回到上层,再次搜索时已经摒弃了一些无关的状态, 只在底层选定的状态中继续寻优.在图2 的例子中, 底层的路径规划实际上是路径的粗略选择, 顶层的规划其实是路径的细化和执行过程.对比平面MDP 规划附加多个障碍物的例子,进行相同条件下的仿真实验,结果如图3 所示, 将两种算法的性能比较由表1 列出.可以看出, 分层后的规划路径具有节点少、路径代价小、搜索时间快等优点.同时减少了FROPI 的航向变化, 因此规划更具有合理性.

图2 HMDP 模型的状态聚类过程Fig .2 Clustering process based on HMDP model

图3 附加不同障碍的HMDP 规划实验图Fig .3 Path planned by HMDP with different obstacles

表1 两种算法性能分析Tab.1 Performance of the two algorithms

3 基于HMDP 的FROPI 的三维轨迹生成方法

根据文献[9] 及实验测试, 现总结出以下关于FROPI 控制的基本标准:

(1)FROPI 不能太靠近障碍物, 否则会被电力线的磁场干扰, 或者机载设备如摄像机屏幕的大部分会被障碍物所占据.

(2)FROPI 的运动路径应尽量平滑, 不平滑的路径会让地面观察者觉得拍摄的画面不够自然.

(3)FROPI 机载摄像机的视角要稳, 屏幕中的地平线应当水平.这就意味着机载摄像机的固定要牢靠.

(4)FROPI 的各方向角不能剧烈变化, 如快速地左右摇摆、上下摇摆.

(5)当FROPI 需要转弯时应当减速, 否则, 机载设备的屏幕中的物体移动过快, 所成图像会模糊不清.

(6)FROPI 机载摄像机应该对路径的目标有所提示,尤其是转弯时,不能等到转弯结束才将镜头对着转弯后的方向, 而应该事先就将视角调整到转弯后的方向,提示FROPI 观看接下来的动作.

本文将问题求解过程分为场景预处理阶段、路径规划和执行阶段.在场景预处理阶段中, 利用层次细分方法将场景用八叉树表示, 之后构建连接图用于路径规划;在路径规划与执行阶段中, 使用IDA*[10]算法在连接图中找出较短的路径,经过三步优化后,得到一阶连续的曲线轨迹.机载摄像机沿此路径运动时,控制摄像机的速度与视角变化, 使结果符合上述的控制标准.

为了进行避障检测, 往往需要对模型进行简化,使用至少包含模型的基本形体作为模型的包围盒来进行相交测试.简化所带来的不精确性是一般的碰撞检测所要避免的, 但过于精确的避障检测模型反而容易使FROPI 接近障碍.本算法使用边长为L的立方体来网格化模型, 若网格中存在物体或其中存在一部分物体, 统称此类网格为障碍网格;否则称为自由网格.将每一个原障碍网格周围的26 个网格都视为障碍网格, 经过这样的处理后, 只要FROPI 在自由网格间移动,与障碍物的距离就不会小于L,这样可以通过调整边长L来满足不同的安全标准.

对场景简化之后, 事实上已经可以把每个网格作为连接图的节点, 根据网格的邻接关系构建连接图用于路径规划.但是根据文献[11] ,在启发函数相同的情况下,降低连接图中节点数对于A*算法效率的提高非常重要, 所以用八叉树来表示这个简化了的场景.把八叉树的单位分为障碍单元与自由单元,FROPI 只能在自由单元中移动, 并将八叉树所有自由单元的中心点作为连接图的节点.将计算出相邻单元连线的长度作为连接图的边的权值.连接图的边由粗线表示, 连接了相邻的自由单元.路径规划即在此连接图中找到一条路径, 使得路径经过的边的权值之和最小.

带权图求解最短路径问题最早由Dijkstra 提出的算法解决,之后Hart 等在1968 年提出了A*算法.由于标准A*算法空间复杂度太高, Korf 又将其改进为IDA*[10],其他的改进方法详见文献[11] .笔者使用IDA*算法,将当前位置与目标的直线距离作为启发函数,从得到的连接图中, 找到自起点所在单元中心点到终点所在单元中心点的最短路径, 可以得到一条自起点到终点的连通路径.

由于这条路径以单元中心作为节点, 忽略了从一单元行至另一单元的其他路线, 得到的路径并不是最短的, 甚至不能算较短的.故借鉴了可见图算法中的一些思想, 做如下改进:

步骤一,对路径P1,每次从一个单元行进至另一单元而必然通过的2 个单元的正方形邻接面,按穿越顺序编号为G1,G2,…,Gn,将Gi(i=1, …,n)的4 个顶点与4 条边的中点组成的点集称为PGi.

步骤二,i从1 循环至n-1,将PGi的每个点作为起点,PGi+1的每个点作为终点, 两两相连, 得到一张有向连接图.再将起点Q与终点G加入到此连接图,然后以Q为起点,P G1中每个点作为终点,形成的连线加入到连接图中;同理, 将PSn到G的每条连线加入到连接图中.选择这些点, 是因为通过计算,在路径规划中要绕过某障碍, 最短的路径一定经过这个障碍的边界.

步骤三,同样用IDA*方法, 在此有向连接图中搜索最短路径, 得到路径P2 ,由于连接图是有向的,且节点限制在与P1相关的这些点,而不是来自整个飞行空间, 所以计算P2所需要的时间很少.

步骤四,把每个路径点寻找可见的最远路径点作为下一目标.设P2 上的路径点为N1 ,N2,…,N m.i从1 开始,从N m到N i+2中寻找能与N i直接相连的路径点, 即它们的连线不与任何障碍单元相交.若找到, 则将N i与此点连接起来,删除中间的路径点,再从此点开始继续此算法;否则,i+1 后继续此算法,直至N m-2,得到路径P3.

4 基于HMDP 的FROPI 三维轨迹生成仿真实验

通过前面的基于MDP 规划算法的分析与研究,已经能够很好地求解FROPI 的二维全局路径规划问题.但是,在FROPI 的实际飞行中,必须有高度方向的运动,且FROPI 的使命要素中可能包含不同高度的规划信息, 即要求FROPI 具有三维规划的能力.现将HMDP 模型拓展到三维环境中(为某型FROPI 的低空飞行的环境20 km ×20 km ×1 km),三维的空间状态并不是立体的栅格形式, 考虑FROPI 的最大爬升角, 定义高度方向的栅格尺寸为20 m ,三维HMDP 模型中的转移概率和报酬函数与平面中的相同.

如图4 所示,在三维环境坐标系(OX YZ)下,据障碍的高度(h1～h2),按照前面的方法,向聚类垂直面内的状态,由于进行规则化描述, 障碍物均表示为长方体形式.

图4 HMDP 模型的三维状态聚类过程Fig .4 Clustering process of the three dim ensional states based on HMDP model

分层后得到的顶层规划状态为s1,s2,…,s7,计算这些状态的转移概率, 并按照R进行V*(s)的计算,搜索最大报酬动作, 初步得到可行状态为s1→s3→s7.在这3 个状态上最后确定最优的细化路径.分层后将问题分解, 避免产生由于数据量太大造成的维数灾.为了较真实地描述实际环境中规划问题,按照前文所述的模型, 利用Matlab 生成网格图, 并附加了2 个和3 个障碍.图5a, 5b 即为三维规划图,可以看出,这种简单的HMDP 模型是合理、有效的.

图5 基于HMDP 的三维轨迹生成图Fig .5 Three dimensional trajectory based on HMDP

5 结论

带有层次的HMDP 模型的总体思想其实仍旧是一种规划策略,一种从分层状态的概率分布到FROPI采取最佳动作的优化匹配.划分层次后, 顶层的MDP 直观地给出了到达目标点所应走的总体路线,并且包含了FROPI 的当前状态.通过这种方法, 摒弃了与规划无关的状态.顶层规划完成后, 再在每一层状态中搜索最优路径.优化后得到的路径仍可能不是最优的, 但是要在三维的形态空间中寻找到最短的路径,完备的方法具有指数级别的复杂度.基于效率考虑, 使用本算法可以得到令人满意的较短路径, 并且最终路径的转角不多, 非常适合作为FROPI 的运动轨迹.同时, 结合高度分层, 实现了三维轨迹生成,为FROPI 在实际飞行中的局部规划奠定了基础.

[1] 张建英, 刘暾.基于人工势场法的移动机器人最优路径规划[J] .航空学报, 2007, 28(S1):183.ZH ANG Jianying, LIU Tun.Optimized path planning of mobile robot based on artificial potential field [J] .Acta Aeronau tica Et Astronau tica Sinica, 2007, 28(S1):183.

[2] 孙汉昌, 朱华勇.基于概率地图方法的无人机路径规划研究[J] .系统仿真学报, 2006(11):3050.SUN Hanchang,ZH U Huayong .Study on path planning for UAV based on probabilistic roadmap method [J] .Acta Simulata Systematica Sinica, 2006(11):3050.

[3] Foka A F,Trahanias P E .Predictive au tonom ous robot navigation[C] ∥Proceedings of the IEEE/ RSJ International Conference on Intelligen t Robots and Systems.Piscataway ,NJ:IE EE,2002:490-495.

[4] 洪晔, 房建成.基于H MDP 的无人机三维路径规划[J] .北京航空航天大学学报, 2009, 35(1):100.H ONG Ye, FANG Jiancheng .Hierarchical Markov decision processes based path planning for UAV in three-dim ensional environment[J] .Jou rnal of Beijing University of Aeronautics and Astronau tics, 2009, 35(1):100.

[5] Baker B,Zivkovic Z,Krose B,et al.Hierarchical dynamic prog ramming for robot path planning[C] ∥Proceedings of the 2001 IEEE International C onference on Robotics &Automation.Orleans:IEE E,2002:46-50.

[6] 史红兵, 张毅彬, 童若锋, 等.虚拟场景自动漫游的路径规划算法[J] .计算机辅助设计与图形学学报, 2006, 4(18):592.SHI Hongbing,ZHANG Yibin, TONG Ruofeng,et al.Path planning for automated navigation in virtual environment[J] .Journal of Computer-Aided Design & Computer Graphics,2006, 4(18):592.

[7] Salom on B,Garber M.Interactive navigation in complex environment using path planning [C] ∥Proceeding s of Symposium on Interactive 3D Graphics.Monterey :A ssociation for Computing Machinery,2003:41-50.

[8] Peters B, Dziugys A .Numerical simulation of the motion of granular material using object oriented techniques [J] .Computer Methods in Applied Mechanics and Engineering ,2002, 5(3):193.

[9] Nieuw enhuisen D,Overmars M.Motion planning for camera movements in virtual environments [M] .Utrecht :Utrecht University,UUCS220032004, 2003.

[10] Korf R E .Depth2first iterative deepening :an optimal admissible tree search [J] .Artificial Intelligence,1985, 27(1):97.

[11] Russell Stuart,Norvig Peter .Artificial intelligence :a modern approach[M] .Englew ood Cliffs, NJ:P rentice Hall, 1995.92-110.