废墟搜救机器人形位检测系统

余见能， 沈林勇， 张 震， 章亚男

(上海大学机电工程与自动化学院，上海200072)

全球每年都会发生大量的自然灾害和人为破坏，巨大的灾难会造成大面积的建筑物坍塌和大量的人员伤亡.灾难发生之后最紧迫的事情就是搜救那些被困在废墟中的幸存者，并为其提供必要的医疗救助.实际经验表明，在48 h内拯救被困的幸存者可显著增大其存活的几率［1-2］.

2008年5月12日，我国四川省汶川县发生了里氏8.0级的大地震，遇难人数数以万计.在整个搜救过程中，数万受灾民众被困于废墟之中等待救援.这时，搜救人员要做的就是如何能在最短的时间内发现幸存者，然后定位幸存者，最后采取有效的营救措施.

由于灾难现场情况复杂，在救援人员自身安全得不到保证的情况下是很难进入现场开展救援工作的.此外，废墟中形成的狭小空间使搜救人员甚至搜救犬也无法进入，这时，废墟搜救机器人可以很好地解决上述问题.废墟搜救机器人可以在灾难发生后的第一时间进入灾难现场寻找幸存者，为被困人员提供基本的医疗救助服务，进入救援人员无法进入的现场搜集有关信息并反馈给救援指挥中心等.

本研究中涉及的废墟搜救机器人，就是能够进入废墟中的狭小空间对生命体进行搜救和定位的自动化设备.其本体是基于内窥镜的弹性软管结构，能够在废墟缝隙中自动转弯推进，遇到障碍物时可以基于视觉和触觉进行智能避障导航，找到幸存者后可以与之进行语音对话，并给予一定的营养补给.该机器人的总体组成结构如图1所示.

图1 废墟搜救机器人结构Fig.1 Search and rescue robot’s structure

本研究采用形位检测系统对机器人本体进行空间形状的重建显示和搜救目标的空间定位，以使废墟外的搜救人员知道废墟下幸存者的空间位置，从而进行有效的挖掘救治.

1 机器人形位检测的方法

当废墟搜救机器人进入废墟缝隙展开搜救行动时，废墟外的操作人员如果无法看到废墟内机器人本体的空间形状，就无法得知其爬行状况和内部环境，从而使得机器人的爬行控制操作具有一定的难度和盲目性.因此，需要设计一个形状重建子系统来获取机器人本体的空间形状信息，在形状重建子系统的协助下，操作人员可控制机器人在废墟里爬行.当机器人头部搜寻到被埋在废墟中的幸存者时(如图2中的A点位置)，搜救人员并不知道幸存者在废墟中的空间位置，即不知道A点的空间坐标(x，y，z)，因而无法立即开挖救人.为了解决这个问题，在形状重建子系统的基础上，引入目标定位子系统，其作用是检测机器人本体末端O点的空间姿态，进而实现对搜救目标A点的定位.

图2 形位检测方法Fig.2 Shape and position detecting method

1.1 基于离散点曲率信息的形状重建方法

根据微分几何原理可知，曲线除了其所在的位置以外，唯一地由其曲率和挠率所决定［3］.曲率度量了曲线在其两邻近点的切向量的夹角对弧长的变化率，就是说，曲率表征了曲线的弯曲度.挠率的绝对值度量了曲线在其两邻近点的法向量的夹角对弧长的变化率.根据弯曲信息来进行曲线拟合时，可以采用微分几何的方法，将所采集的曲率数据经过数次积分，得到曲线位置的表达式.具体计算可以采用数值积分方法或积分递推方法，这些方法在曲线拟合方面已取得了相应的研究成果［4］.文献［5］给出了基于弯曲、扭转的重建原理.本研究小组已经对基于曲率信息的重建方法进行了研究，并取得了一定的成果［6］.

解决空间曲线重构的基本思路如下：首先，在空间曲线上建立由曲线切线和曲率分量确定的运动坐标系，在运动坐标系中由曲率矢量确定密切平面;然后，在密切平面中进行曲线的弯曲计算和曲线拟合，再进行运动坐标系的运动分析，得到新的运动坐标系;接着，在新的运动坐标系中确定新的密切平面，依此类推，对空间曲线上的各个离散点进行逐点分析计算，并逐段拟合曲线;最后，通过计算机图形编程和数据接口来实现基于离散点曲率信息的空间形状重建.

1.2 搜救目标点定位方法

如图2所示，搜救机器人进入废墟后，形状重建子系统可以重建出能够代表机器人本体空间形状的空间曲线.形状重建的起始点为图中所示的O点，且形状重建是在基准坐标系X'Y'Z'下完成的，该基准坐标系可随着机器人本体末端摆放方位的变化而变化.由于废墟现场一般很难找到平坦的地面来摆放机器人装置，导致基准坐标系不同于操作人员所处的大地坐标系XYZ，因此，若要实现搜救目标的定位，必须检测出大地坐标系与基准坐标系之间的相对空间姿态关系.

在形状重建中以O点作为形状重建的起始点，且假设O点的坐标值为(0，0，0).空间曲线的重建采用离散点逐点拟合的方法，在曲线的拟合过程中可以计算出A点相对于O点的坐标值(x'，y'，z')，A点为机器人本体头部搜救目标定位点.A点的坐标参数(x'，y'，z')是在基准坐标系下的坐标值，但现场搜救操作人员需要的是在大地坐标系下的绝对空间坐标值(x，y，z)，因此，需要采用一些方法来测量O点处机器人本体相对于大地坐标系的空间姿态信息.利用这些信息可将A点在基准坐标系下的坐标值(x'，y'，z')转换到大地坐标系下的坐标值(x，y，z)，这将由目标定位子系统来实现.操作人员得到A点的坐标值(x，y，z)，也就知道了幸存者在废墟中的具体空间位置，就可以开展救援工作了.

只要测量得到O点处机器人本体相对于大地坐标系的空间姿态信息，即机器人本体分别绕大地坐标系的X轴、Y轴、Z轴旋转的3个角度(横滚角、俯仰角、航向角)，就可以利用这3个角度将A点在基准坐标系下的坐标值(x'，y'，z')通过旋转矩阵转换为A点在大地坐标系下的绝对空间坐标值(x，y，z).

由旋转变换矩阵公式可知：

式(1)～(3)分别表示绕X轴、Y轴、Z轴旋转角度为α，β，γ的旋转变换矩阵，α，β，γ值由三轴电子罗盘测得，因此，A点在大地坐标系下的空间坐标值(x，y，z)的计算公式为

2 机器人形位检测系统设计［7-12］

2.1 系统组成

为了实现上述机器人本体头部的空间方位检测，废墟搜救机器人的形位检测系统由两部分组成：一部分是以光纤光栅曲率传感器为主的形状检测与重建系统，传感器的信号经调制解调仪解调后由网线传输给数据处理主机;另一部分是以三轴电子罗盘为主的空间姿态检测系统，罗盘采集的数据通过串口传输给主机进行分析处理.图3所示为机器人本体形位检测系统的组成图，包括废墟搜救机器人本体、光纤光栅曲率传感器、三轴电子罗盘、四通道光纤光栅调制解调器、数据分析处理主机及显示器.光纤光栅曲率传感器内部有4根光纤，引出的4个光通道连接到4通道的光纤光栅调制解调仪.该调制解调仪是基于局域网的光信号采集系统，其采样频率为250 Hz，较高的采样频率为实时显示提供了很好的采集采样速度.

图3 机器人的形位检测系统的组成图Fig.3 Diagram of robot’s shape and position detecting system

定位过程首先由光纤光栅调制解调仪对光纤光栅曲率传感器进行数据采集，将数据上传给笔记本电脑进行数据分析并实现形状的重建与显示，从而完成机器人本体空间形状的检测;然后，将三轴电子罗盘采集的姿态数据通过串口RS232传输给笔记本电脑进行数据处理，最终实现目标定位.

2.2 光纤光栅曲率传感器的设计与制作

光纤光栅曲率传感器的设计制作过程分为3个部分：①确定光栅点数和布局方式;②封装基材的选择和封装辅助装置的设计;③封装传感器.

光栅点数的确定需要考虑以下一些因素：解调设备光源的谱宽、传感器的探测范围和缓冲区，以及传感器的长度和光栅点间的距离等.这些因素并非完全独立，有时是相互制约的，很难同时满足上述所有要求，因此必须对光栅点数进行优化.所谓的优化就是根据实际应用场合的具体情况来权衡上述各种因素，使其尽量满足要求，若各因素间有冲突则取折中值.

已知光栅点数量n的估算公式为

式中，

W为解调设备的波长解调范围值，Sb为应变系数，r为传感器封装后的有效半径，ρmin为基材的最小弯曲曲率半径，Pi是为避免相邻波长重叠而预留的余量.

已知实验室现有的光纤光栅解调仪的光源谱宽为1 510～1 590 nm(要求的传感器长度为3 m)，故W=80 nm;通过对搜救机器人的各种弯曲状态进行最小曲率半径的测量，取ρmin=300 mm;取r=1 mm，Pi=0.5 nm，Sb=1.209×10-3.把所有参数代入式(5)可得n≈10，这就说明每根光纤上布置10个等间距的光栅点是比较合适的.

本研究选用横截面为正方形的超弹性合金丝作为封装基材，把4根刻有光栅点的光纤粘贴在基材上，光纤光栅的阵列分布如图4所示.从图中可以看出，4根光纤中，a与b组成一对光纤组，c与d组成另一对光纤组，每一组光纤上的光栅点也是成对的，2个光栅点之间的圆周角度为90°，这样就能够实现对空间曲率的检测.

图4 光纤光栅阵列分布图Fig.4 Distribution of FBG array

由于每根光纤上刻有10个等间距的光栅点，而2对光纤组的光栅点在基材轴线方向上又是交错分布的，因此该曲率传感器可以检测20个离散点的空间曲率.通过调整设定这20个离散检测点的分布间距，可以满足各种检测长度的要求.

确定采用横截面为正方形的超弹性合金丝作为封装基材后，为与之匹配，本研究设计了一种定位套来实现4根光纤在封装过程中的定位要求，并借助一些辅助封装工具完成涂覆封装.涂覆封装后的光栅点如图5所示.

图5 涂覆封装后的光栅点Fig.5 Grating after packaging

2.3 传感器的标定［13］

完成传感器的封装后，还要进行标定实验，标定的目的是计算出传感器的波长变化量与曲率之间的线性关系比例系数，即曲率灵敏度K.由图4可知，每一个检测点的空间曲率检测需要在相互垂直的2个平面上进行标定.标定时，利用标定块进行合理夹持，确保光纤光栅传感头部所在的母线位置不发生偏转.柔性杆向上、下、左、右4个方向(即z+，z-，y+，y-)进行弯曲，得到4组K值.在每组K值标定的过程中，分别测出内窥镜镜体在曲率半径为无穷大(直线)以及其他情况下输出的光纤光栅中心波长，通过数据处理获得标定系数K值.

将光纤光栅传感器摆放在标定板平面上进行多组拉伸和压缩标定实验.本研究中的光纤光栅曲率传感器共有20个检测点，因此需要对每一个点进行z+，z-，y+，y-这4个方向上的拉压标定，得到每个点的4组K值.每一点的空间曲率检测都是由2根光纤实现的，它们在基材的圆周方向上成90°夹角，在此命名为1号光纤和2号光纤.对实验数据进行线性拟合后，得到2根光纤在拉压时中心波长的变化量与曲率的线性关系(见图6).

由图6可以看出，1号和2号光纤光栅在受到拉伸和压缩时，曲率与波长变化量线性较好.图中曲线的斜率即为曲率灵敏度K值，1号和2号光纤的K值分别为717.437和717.523 nm·mm.

3 机器人本体头部空间方位测量实验验证

完成光纤光栅传感器的标定后，将传感器插入机器人本体内腔，安装好电子罗盘，即可显示出重建的机器人本体的空间曲线形状，并在此基础上得到机器人本体头部目标点的空间坐标值.机器人本体的长度为3.5 m，头部的直径为3 cm，从机器人的应用场合和项目指标要求考虑，机器人搜救目标定位的误差允许在20 cm以下.本研究对机器人本体的多种不同空间形状进行了头部定位误差分析实验，以及多组本体头部目标点空间坐标的测量实验，并通过这些实验数据分析了头部目标定位的误差.

图6 1号和2号光纤受拉压时中心波长随曲率的变化关系Fig.6 Relations between wavelength and curvature when No.1 and No.2 fibers under tension and compression

这里只列举2组空间形状的定位误差分析实验.第一组实验的机器人柔性本体实际摆放实物如图7(a)所示，图7(b)为重建得到的曲线图像.第二组实验机器人实物及重建图像如图8所示.表1和表2中的数据分别为2组机器人本体头部端点的实际坐标值和重建坐标值.将实际值与重建值的差值作为头部端点定位的误差值，其中实际坐标值是借助米尺等测量工具人工测量得到的，重建坐标值是由重建程序计算得到的.由于坐标数值是多组重复实验测量数据的平均值，因此，这在一定程度上消除了人工测量误差.

图7 第一种空间形状的对比图Fig.7 Comparison of the first spatial shape

表1 第一种空间形状的定位误差数据Table 1 Error data of the first spatial shape cm

图8 第二种空间形状的对比图Fig.8 Comparison of the second spatial shape

表2 第二种空间形状的定位误差数据Table 2 Error data of the second spatial shape cm

本研究共进行了6组不同空间形状的实验(包括上述2种空间形状)，对得到的定位误差值进行平均处理，得到机器人形位检测系统目标定位在X，Y，Z轴上的平均误差值分别为10.20，7.07，6.00 cm.

通过对多种不同空间形状下多组实验数据的分析可知，机器人本体头部目标点的实际空间坐标值与重建空间坐标值之间的差值在10 cm左右.从机器人的应用场合和项目指标要求考虑，基本满足废墟中搜救机器人对搜救目标的定位要求.

4 结束语

本研究将光纤光栅的曲率检测技术引入到废墟搜救机器人的形状重建领域，自主设计了一根大长度的光纤光栅曲率传感器.由于长度的增加必定影响形状重建的精度，因此，本研究采用了新的封装方法，这在一定程度上保证了精度.为了实现废墟搜救机器人的搜救目标定位，采用三轴电子罗盘来检测形状重建基准点的空间姿态.实验数据表明，这种方法可基本满足废墟搜救机器人的搜救定位要求.

［1］ CASPERJ，MURPHYR，MICIREM.Issues in intelligent robots for search and rescue［C］∥SPIE Ground Vehicle TechnologyⅡ.2000：292-302.

［2］ 李斌.蛇形机器人的研究及在灾难救援中的应用［J］.机器人技术与应用，2003，24(3)：22-26.

［3］ 梅向明，黄敬之.微分几何［M］.北京：人民教育出版社，1981：178-194.

［4］ 陈大鹏，周文伟.空间弹性曲杆在三维变形中的曲率-位移关系［J］.西南交通大学学报：自然科学版，1997，32(2)：123-129.

［5］ LEED，KEVINE，ANDREWT.Spatially continuous six degree of freedom position and orientation sensor［J］.Sensors Review，1999，19(2)：106-112.

［6］ 郑晋，钱晋武，沈林勇，等.基于空间曲率信息的三维面型重构［J］.上海大学学报：自然科学版，2009，15 (3)：235-237.

［7］ 张震.医疗内窥镜视觉导航技术研究［D］.上海：上海大学，2006：36-45.

［8］ 吴家麒，杨东英，钱晋武，等.基于曲率数据的曲线拟合方法研究［J］.应用科学学报，2003，21(3)：258-262.

［9］ 易新华，钱晋武，章亚男，等.基于光纤光栅传感阵列及双目视觉的内窥镜三维显形系统的研究［J］.生命科学仪器，2007，5(8)：23-27.

［10］ 易新华，钱晋武，张伦伟，等.一种新型的内窥镜三维形状重构与定位算法［J］.仪器仪表学报，2008，29 (1)：55-60.

［11］ 易新华，钱晋武，张震，等.基于视觉的内窥镜端部位姿模型分析［J］.高技术通讯，2008，7(12)：42-47.

［12］ 马成瑶.智能内窥镜形状感知系统的基准空间姿态实时检测研究［D］.上海：上海大学，2008：24-28.

［13］ 张伦伟.基于光纤光栅传感的智能内窥镜形状感知系统［D］.上海：上海大学，2005：42-46.