基于各向异性空间投影的地铁列车底部巡检机器人避障研究.pdf

第4 1卷 第8期2 0 2 3年8月MA CH I N E R Y&E L E C T R ON I C SV o l.4 1 N o.8A u g.2 0 2 3收稿日期:2 0 2 2 1 1 2 8作者简介:毛成林(1 9 8 7-),男,安徽巢湖人,博士,博士后,研究方向为机器视觉机械臂运动规划;于瑞强(1 9 8 6-),男,江苏南京人,工程师,研究方向为特种机器人;宋爱国(1 9 6 8-),男,安徽黄山人,博士,教授,博士研究生导师,研究方向为机器人传感与遥操作机器人技术、信号处理及仿生智能计算等。基于各向异性空间投影的地铁列车底部巡检机器人避障研究毛成林1,2,于瑞强2,宋爱国1(1.东南大学仪器科学与工程学院,江苏 南京 2 1 0 0 9 6;2.亿嘉和科技股份有限公司,江苏 南京 2 1 0 0 1 2)摘 要:针对地铁列车底部的非示教场景,提出一种基于各向异性的空间投影的点云分割新算法。算法针对使用立体相机获取的三维点云数据,将点云数据投影到机器人坐标系下的不同平面上,然后利用聚类算法获得目标的分割区域,再映射回点云空间计算出相应的包围盒。在整列地铁车厢底部进行的多次实验验证取得了预期的效果,证明了算法的有效性,可以实现地铁列车底部复杂环境下的高效规划避障。关键词:机械臂控制;运动规划避障;点云分割;各向异性空间投影;包围盒中图分类号:T P 2 4 2.6;T P 2 4 1 文献标志码:A 文章编号:1 0 0 1 2 2 5 7(2 0 2 3)0 8 0 0 7 6 0 5R e s e a r c h o n O b s t a c l e A v o i d a n c e o f R o b o t A r m f o r S u b w a y T r a i nB o t t o m I n s p e c t i o n B a s e d o n A n i s o t r o p i c S p a t i a l P r o j e c t i o nM A O C h e n g l i n1,2,Y U R u i q i a n g2,S O N G A i g u o1(1.S c h o o l o f I n s t r u m e n t S c i e n c e a n d E n g i n e e r i n g,S o u t h e a s t U n i v e r s i t y,N a n j i n g 2 1 0 0 9 6,C h i n a;2.Y i j i a h e T e c h n o l o g y C o.,L t d.,N a n j i n g 2 1 0 0 1 2,C h i n a)A b s t r a c t:A n e w p o i n t c l o u d s e g m e n t a t i o n a l g o r i t h m b a s e d o n a n i s o t r o p i c s p a t i a l p r o j e c t i o n f o r n o n t e a c h i n g s c e n e s a t t h e b o t t o m o f t h e s u b w a y t r a i n i s p r o p o s e d.T h i s a l g o r i t h m a i m s a t t h e 3 D p o i n t c l o u d d a t a o b t a i n e d b y s t e r e o c a m e r a,p r o j e c t s t h e p o i n t c l o u d d a t a t o d i f f e r e n t p l a n e s u n d e r t h e r o b o t c o o r d i n a t e s y s t e m,a n d t h e n t h e s e g m e n t a t i o n a r e a o f t h e t a r g e t i s o b t a i n e d b y u s i n g t h e c l u s t e r i n g a l g o r i t h m,a n d t h e n t h e c o r r e s p o n d i n g b o u n d i n g b o x i s c a l c u l a t e d b y m a p p i n g b a c k t o t h e p o i n t c l o u d s p a c e.M a n y e x p e r i m e n t s h a v e p r o v e d t h e e f f e c t i v e n e s s o f t h e a l g o r i t h m,w h i c h c a n a c h i e v e e f f i c i e n t o b s t a c l e a v o i d a n c e u n d e r t h e c o m p l e x e n v i r o n m e n t o f t h e b o t t o m o f t h e s u b w a y t r a i n.K e y w o r d s:r o b o t a r m c o n t r o l;o b s t a c l e a v o i d a n c e i n m o t i o n p l a n n i n g;p o i n t c l o u d s e g m e n t a t i o n;a n i s o-t r o p i c s p a t i a l p r o j e c t i o n;b o u n d i n g b o x0 引言机械臂正在被应用于大量非标准化工业场景,地铁车底智能巡检机器人系统也是其中一类。基于机器视觉处理的地铁车底智能巡检机器人系统,通常被用于对地铁车底进行巡检,采集车底拍摄图像,进行视觉分析,检测异常情况。机器人系统通常装备12只机械臂,并在机械臂末端搭载高分辨可见光相机和立体相机进行拍摄。为解决机械臂在执行任务过程中的避障问题,需要机器人具有感知障碍物的能力。当前工业界在机器人上搭载三维传感器,开发基于点云信息的机械臂避障方法。其中激光雷达和立体相机是获取点云的主流设备1。以立体成像技术为核心的立67毛成林等:基于各向异性空间投影的地铁列车底部巡检机器人避障研究机电一体化体相机已经实现了多样化发展,其获得的深度图像(R G B D)是在R G B数据基础上融合了深度数据,应用相机内参可将深度图像转化为点云数据2。业界获取点云数据后进行避障的主流处理方法是构建O c t o M a p3。O c t o M a p是一种基于八叉树的高效概率三维建图框架。但是地铁车底智能巡检机器人对巡检效率要求较高,通常是要求整个列车在3 0 m i n内巡 检完毕。使用O c t o M a p虽然足够 精细,但分割的子节点过多导致后续运动规划计算速度较慢,不符合对巡检效率的要求。点云分割是将点云数据分割成若干个互不相交的子集。目前的点云分割方法基于处理点与点之间拓扑关系4,包括基于密度聚类的D B S C A N算法5、基于超体聚类的过分割的L C C P算法6和C P C算法7等。传统的三维点云分割算法通常聚焦于语义分割、实例分割等方向,没有考虑避障任务的特殊性。避障任务只需要给出空间位置是否存在障碍物,即使不属于同一物体,只要其所占用的空间位置能够拼接,在避障任务中也是推荐执行的。因此,传统的三维点云分割算法也不适合这一应用场景。本文针对地铁车底智能巡检机器人这一特种行业应用场景,为解决机械臂运动规划高效避障的问题,采用立体相机对地铁列车底部进行环境建模,并设计了一种算法对采集的三维点云数据进行分割以支撑高效避障规划。所提算法生成较少的分块(在具体实施中,对于3.0 m2.0 m2.6 m的空间,分块数为4 07 0,明显少于O c t o M a p和传统三维点云分割算法),并构成包围盒来表达该分块,从而保证执行机械臂规划任务时速度更快,同时满足机械臂的运动避障需求。1 列车底部环境的点云数据获取任务开始于采集列车底部的环境数据。地铁车底智能巡检机器人采用机械臂和安装在机械臂末端的立体相机来执行这一任务。初始的障碍物三维点云图是以立体相机坐标系为参考采集的。立体相机捕获的障碍物三维点云图以数组的形式进行存储和处理,数组中的每个元素相当于外部物体在相机坐标系下的坐标点。这一三维点云图需要通过机械臂手眼标定转换到机械臂基坐标系下再转换到机器人世界坐标系下。转换公式为gw o=gw bgb ege c(1)gw b为机械臂坐标系和世界坐标系的转换关系;gb e为立体相机在机械臂坐标系下的位姿;ge c为立体相机拍摄的外部物体在相机坐标系下的位姿;gw o为外部物体在世界坐标系下的位姿。对于地铁列车底部巡检任务,进行巡检的流程如下:列车停靠在检修站台铁轨上,铁轨是架空挑高的,下方挖有地沟(称为股道)(图1),人或机器人在股道中运动,对设备进行观测。机器人世界坐标系的定义如下:将平行于铁轨的方向定义为世界坐标系的x轴,x轴正方向定义为机器人行进方向,将垂直于地面向上的方向定义为世界坐标系的z轴正方向,由右手定则确定y轴(图2)。图1 车底巡检机器人运行环境base link_yzx图2 车底巡检机器人的世界坐标系定义环境建模的整体流程如图3所示。步骤如下:a.首先操作机械臂到达一个预设位姿。b.使用末端安装的立体相机进行拍照。c.将立体相机生成的点云数据结合机械臂位姿转换到世界坐标系。d.调整机械臂末端位姿,重复步骤b。e.最后将全部转换后的点云数据合并起来(直接拼合)。?,?a?,bcd?,e?，?(?)4?,c?,?d?,b?图3 车底环境建模流程772 0 2 3(8)2 基于空间投影的深度图点云分割由于人也是在股道内部对地铁底部的设备进行巡检,列车在制造时为方便人操作和巡检,具有如下特性:当人抬头向上观测时,可以看到设备基本是垂直于地面方向安装的;当人转头向两侧车轮方向观测时,也可以发现设备基本是垂直于车轮平面方向安装的。因此对于抬头向上观测的设备,可将点云数据沿着z轴正方向将z值数据投影到坐标系x o y平面上,然后对二维图像采用聚类方法获得目标分割区域,然后根据二维图像像素对应的点云数据,再映射回点云空间计算包围盒。同理,对于两侧车轮方向观测的设备,可将点云数据分别沿y轴正负方向将y值数据投影到坐标系y o z平面上,然后 进行处理,因 此算法是各 向异性的。算法总体流程如图4所示。?，?图4 基于空间投影的深度图点云分割流程具体步骤如下:a.三维点云对不同方向(z轴正方向、y轴正方向和y轴负方向)进行投影,以固定分辨率生成栅格图,每个栅格图的像素值由不同方向来决定选取对应轴上的最大值或最小值代表。以z轴正方向为例,计算公式为:Viv o x e l=(xi,yi,zi)(2)Ciz=Viv o x e l(x)Rr e s o l u t i o n,Viv o x e l(y)Rr e s o l u t i o n (3)Pz+(Ciz)=m a x(Pz+(Ciz),Viv o x e l(z)(4)Viv o x e l为三维点云中第i个点的三维坐标;Ciz为三维点云向z轴的正方向投影,点云第i个点落在投影图上的坐标值;Rr e s o l u t i o n为投影分辨率。由于分辨率的存在,有多个三维点云中的点投影到投影图上同一坐标,因此要选出是哪个点代表这个坐标的像素值。Pz+代表投影图是z轴正方向时生成的栅格图。此时,像素坐标Ciz的像素值为选取投影到此像素坐标的若干个值中的最大值。同理可得其他方向的投影图计算方法。b.对投影生成的栅格图进行聚类分类。本文采用km e a n s算法8进行分类。km e a n s算法需要手动指定分类的个数,在所提算法中,分类的个数由该栅格图中像素值的最大值和最小值之间的间隔决定,对应的分段区间的中点被选择为聚类中心。以z轴正方向为例,计算公式为K=m a x(Pz+)-m i n(Pz+)I(5)K为分类个数;m a x(Pz+)为投影图的最大值;m i n(Pz+)为投影图的最小值;I为采用的间隔长度。c.对分类结果计算连通区域,进行形态学处理。km e a n s算法输出的每个分类可能对应多个连通区域,使用腐蚀去除过小的单独区域,使用膨胀填补大区域中的小空洞。d.映射回三维点云数据计算包围盒。将点云以步骤a的方式进行投影,如果投影后落在连通区域内,则认为点云中的这个点属于此连通区域,计算每个连通区域对应的三维点云集合。考虑前述此场景的特性,对点云集合计算AA B B包围盒。e.对包围盒进行合并。对包围盒计算密度,密度为该包围盒对应的点云中点的个数除以包围盒体积。如果2个包围盒合并后的密度大于这2个包围盒各自的密度,则认为这2个包围盒可以合并。考虑所提出算法的流程,在点云投影步骤中,同一高度(宽度)的设备投影的像素值是相同的,因此聚类后会被分割到同一分类中,这符合避障要求的分块尽可能少的特性,而传统分割算法可能将其分割到多个分类中。3 机器人作业实验为验证所提算法的效果,将部署有算法的机器人在地铁基地部署试运行。所研发的机器人采用国产六自由度机械臂,立体相机为I n t e l r e a l s e n s e d 4 3 5,如图5所示。图5 硬件系统示意87毛成林等:基于各向异性空间投影的地铁列车底部巡检机器人避障研究机电一体化3.1 算法分割效果针对本文所提算法,以如图2所示姿态开始采集点云数据。此姿态六关节机械臂对应关节角为-9 0.0,-5 1.6,-1 5 7.0,-6 0.0,-9 0.0,-1 8 0.0 。然后机械臂第6关节每4 5 调整1次,末端执行器关节调节俯仰角为1 5 和7 5,共计拍摄照片1 6张,最终合成完整的三维点云数据。对合成的点云数据进行投影操作,选取如图1所示的地铁底部场景,以沿z轴正方向投影为例,r e s o l u t i o n采用0.0 1 m,i n t e r v e l采用0.0 5 m,第2节中步骤a和步骤c分别获得的投影和聚类分割如图6所示,可见包括最突出的P T I支架(图7中央)都被分割出来。最终生成的包围盒如图8所示,可见包围盒将点云均包络在内,且分割简单实用。（)?a z（)?b z图6 图1场景z轴正方向投影图和分割图图7 图1场景侧面图base link_图8 算法生成包围盒示意3.2 实际规划效果为测试本文算法在机械臂避障方面的应用效果,本文使用M o v e I t软件9进行仿真实验并在实际场景中加以测试。在图1所示的场景中,将手臂末端立体相机从铁块左侧向左观测规划移动到铁块右侧向右观测,六自由度机械臂起始位姿对应的6个关节角为-3 7.2,-7 4.5,-8 6.0,-1 0 7.7,-9 0.5,-2 3 2.6 ,目标位姿对应的6个关节角为-1 1 8.0,-8 4.2,-7 8.5,-1 0 5.4,-8 8.8,-1 5 1.3 。机械臂的避障运动效果如图9所示,图中展示了机械臂的运动轨迹,可知机械臂规划运动成功避开了P T I支架,运动轨迹平滑。()?a(?)b?base link_base link_?图9 机械臂避障运动测试972 0 2 3(8)在实际场景中,整列地铁总长约1 2 0 m,平均每1.5 m建模1次,巡检整列地铁机械臂总共需运动1 0 0 0次左右。机器人持续运行2周,总计机械臂运动8万余次,避障效果均达到预期。3.3 和其他算法实验对比及分析为进一步验证本文算法效果,将本文算法与M o v e I t中 自 带 的O c t o M a p算 法 和P C L库1 0的C P C点云分割算法进行对比测试。针对3.1节所述相同场景,O c t o M a p算法和C P C算法的效果分别如图1 0和图1 1所示,分块个数分别为8 1 8 9 5和7 8 2个,而所提出的算法为5 9个。图9所示路径规划重复规划了1 0 0 0次,总耗时分别为O c t o M a p算法4 5 2 s,C P C算法7 6 9 s,本文算法2 1 4 s,可见所提算法在效率上高于对比算法,同时可知错误的分割反而会导致规划耗时的增加,如表1所示。base link_图1 0 O c t o M a p算法效果示意base link_图1 1 C P C算法效果示意表1 3种算法对比对比项目O c t o M a p算法C P C算法本文算法分块个数8 1 8 9 57 8 25 9平均规划耗时/s0.4 5 20.7 6 90.2 1 44 结束语本文针对地铁列车底部巡检这一特定行业应用场景,利用使用立体相机获取的三维点云数据,设计了一种基于各向异性的空间投影的点云分割算法。所提算法将点云数据投影到机器人坐标系下的不同平面上,将直接针对三维数据的处理转化为对二维数据的处理,再映射回三维点云空间获得最终结果。对比O c t o M a p算法和C P C算法,结果表明采用各向异性空间投影得到的点云分块数量较少,质量较佳。本文算法在实际场景中表现良好,可以实现地铁列车底部复杂环境下的高效规划避障,为机器人的部署提供了保障。参考文献:1 周恒旭,何志敏,周燕面向机械臂避障的非结构场景感知研究J 机械与电子,2 0 2 0,3 8(9):7 4 8 0.2 郭 清 达,全 燕 鸣 采 用 空 间 投 影 的 深 度 图 像 点 云 分割J.光学学报,2 0 2 0,4 0(1 8):1 4 0 1 4 8.3 HOR NUNG A,WURM K M,B E NN EW I T Z M,e t a l.O c t o M a p:a n e f f i c i e n t p r o b a b i l i s t i c 3 D m a p p i n g f r a m e-w o r k b a s e d o n o c t r e e sJ.A u t o n o m o u s r o b o t s,2 0 1 3,3 4(3):1 8 9 2 0 6.4 NGUY E N A,L E B.3 D p o i n t c l o u d s e g m e n t a t i o n:A s u r v e yC 2 0 1 3 6 t h I n t e r n a t i o n a l C o n f e r e n c e o n R o-b o t i c s,A u t o m a t i o n a n d M e c h a t r o n i c s(R AM).M a n i l a:I E E E,2 0 1 3:2 2 5 2 3 0.5 E S T E R M,K R OG E R P,S AN D E R J,e t a l.A d e n s i t yb a s e d a l g o r i t h m f o r d i s c o v e r i n g c l u s t e r s i n l a r g e s p a t i a l d a t a b a s e s w i t h n o i s eC I n t e r n a t i o n a l C o n f e r e n c e o n K n o w l e d g D i s c o v e r y a n d D a t a M i n i n g,1 9 9 6:2 2 6 2 3 1.6 S T E I N S C,S CHO E L E R M,P A P ON J,e t a l.O b j e c t p a r t i t i o n i n g u s i n g l o c a l c o n v e x i t yC 2 0 1 4 I E E E C o n-f e r e n c e o n C o m p u t e r V i s i o n a n d P a t t e r n R e c o g n i t i o n.N e w Y o r k:I E E E,2 0 1 4:3 0 4 3 1 1.7 S C HO E L E R M,P A P O N J,WO R G O T T E R F.C o n s t r a i n-e d p l a n a r c u t s o b j e c t p a r t i t i o n i n g f o r p o i n t c l o u d sC2 0 1 5 I E E E C o n f e r e n c e o n C o m p u t e r V i s i o n a n d P a t t e r n R e c o g n i t i o n.B o s t o n:I E E E,2 0 1 5:5 2 0 7 5 2 1 5.8 A R THU R D,V A S S I L V I T S K I I S.Km e a n s+:T h e a d v a n t a g e s o f c a r e f u l s e e d i n gCP r o c e e d i n g s o f t h e E i g h t e e n t h A n n u a l A CMS I AM S y m p o s i u m o n D i s c r e t e A l g o r i t h m s,2 0 0 7:1 0 2 7 1 0 3 5.9 C H I T T A S,S U C A N I,C O U S I N S S.M o v e i t!r o s t o p-i c s J.I E E E R o b o t i c s a n d a u t o m a t i o n m a g a z i n e,2 0 1 2,1 9(1):1 8 1 9.1 0 R U S U R B,C O U S I N S S.3 D i s h e r e:p o i n t c l o u d l i b r a r y(P C L)C/2 0 1 1 I E E E I n t e r n a t i o n a l C o n f e r e n c e o n R o-b o t i c s a n d A u t o m a t i o n.S h a n g h a i:I E E E,2 0 1 1:1 4.08