基于光纤光栅传感器的软体机械臂三维形状检测方法

王 超

(上海交通大学电子信息与电气工程学院，上海 200240)

某些复杂而危险的工业过程，需要机械臂代替人手进行操作。传统的刚性工业机械臂在规则的环境中能达到很好的操作效果，但在某些杂乱而拥挤的环境中，如矿难现场、科学探测，由于刚性机械臂工作空间的限制，控制无法取得很好的效果。机械臂与环境的剧烈碰撞甚至可能破坏现场，引发灾难。软体机器人的出现[1]，在一定程度上缓解了上述问题。

对于应用在矿难营救、科学探测领域的软体机械臂而言，其优良的柔性能保证机械臂在进入狭小空间的同时不会与周围环境发生剧烈碰撞，从而避免造成二次矿难或者破坏探测现场。软体机械臂的柔性在保证安全性的同时，也带来了新的问题：软体机械臂的形状容易受到狭小工作环境的影响，无法根据关节变量计算其形状。当机械臂进入密闭环境后，操作者无法直接观测到机械臂的形状。这对操作者决策下一步动作来说，是极为不利的。因此，需要通过传感器将机械臂的形状信息反馈给操作者。然而，特殊的工作环境极大地限制了可使用的传感器的种类。光纤光栅传感器因其具有体积小、抗电磁干扰及无源等特性，广泛应用于建筑及医学等领域。Yi X H等提出一种基于光纤光栅的内窥镜形状检测方法[2]，但是该方法无法准确检测三维形状。狄海廷和付宜利提出了一种三维形状检测方法[3]，但是无法检测大扭转的软体机械臂，只适用于工程杆件。

为了能准确检测具有复杂空间构型的软体机械臂的形状，笔者设计了一套分布式光纤光栅传感网络来同时测量各个节点的曲率和挠率信息，同时提出了一种软体机械臂三维形状检测算法。最后，用仿真实验证明了该方法对于复杂的空间形状有很高的检测精度。

1 分布式光纤传感网络

1.1 传感网络结构

笔者所研究的软体机械臂是线驱硅胶软体臂，其实体模型如图1a所示，本体呈长细圆锥形，长度L为0.3m，最大半径rmax为0.013 5m，最小半径rmin为0.005 0m。软体机械臂内部嵌入4根驱动线，在驱动线拉力与重力的作用下，软体机械臂会弯曲成复杂的空间形状。

软体机械臂在弯曲、扭转时，其中心线长度保持恒定。因此，可以选择其中心线来表征软体机械臂的形状。当机械臂弯曲成复杂的空间形状时，其中心曲线的挠率不再为0，现有的测量曲率的传感网络不再适用。

为同时测量到中心曲线的曲率和挠率，设计分布式光纤光栅传感网络。整个传感网络包括3根光纤A、B、C，每根光纤上等距离刻着N个布拉格光栅，记为{gA,i，gB,i，gC,i}，编号i相同的光栅位于同一横截面。光纤沿着机械臂内部圆柱的母线分布，其横截面呈正三角形分布，如图1b所示，图中dA,i、dB,i、dC,i分别表示光栅{gA,i，gB,i，gC,i}到中心线的距离，约等于rmin；βC,i=2βB,i=240°；αi表示中心线的弯曲方向。

a. 软体机械臂与光纤分布

b. 光栅分布

1.2 测量算法

位于同一截面的光栅组合{gA,i,gB,i,gC,i}将机械臂划分成N段。假设每一段在弯曲过程中横截面不变形，而且其中心线具有分段常曲率、常挠率特性。3根光纤作为中心线的等距曲线，也具有同样的特性。用li表示第i段中心线的弧长，则中心线的参数方程可以表示为：

(1)

按上述分布的3根光纤作为其等距曲线，其长度可表示为：

i=1,2,…,N;J=A,B,C

(2)

其中βA,i=0。光栅可以测量到光栅处的应变，则3根光纤的实际长度可表示为：

(3)

其中εJ,i为光栅测量到的应变。计算的光纤长度应该等于实际的光纤长度，即lcal=lmeas，则根据式(2)、(3)解出参数ai、bi，从而计算出第i段中心线的曲率κi与挠率τi：

(4)

2 三维形状检测算法

本节介绍利用测量到的曲率-挠率信息重构软体机械臂三维形状的算法。

光栅截面将软体机械臂等距离分割成N段，笔者研究第i段和截面i的空间坐标与姿态，如图2所示。为分析中心线节点oi的坐标和截面i的姿态，建立局部坐标系{xi-1,yi-1,zi-1},其中zi-1为中心线在oi-1点的切线，yi-1为中心线在oi-1点的主法线；xi-1为中心线在oi-1点的副法线，即Frenet标架。

图2 光纤截面的空间坐标和姿态

在弧长li足够小的情况下，可以认为曲线oi-1oi是一条圆柱螺旋线。为了便于分析，假设扭转角全部集中在点oi处，扭转角约等于τili。局部坐标系{xi-1，yi-1，zi-1}到坐标系{xi，yi，zi}的变换可以分为4个旋转变换和一个平移变换。旋转变换为：

Φi=τili-αi

(5)

其中θi为圆弧oi-1oi对应的圆心角，其大小为κili。坐标系{xi-1,yi-1,zi-1}到坐标系{xi,yi,zi}的平移向量为oi-1oi，其在{xi-1,yi-1,zi-1}坐标系下可以表示为[4]：

(6)

则截面i相对于全局坐标系的姿态和节点oi的全局位置坐标可以表示为：

(7)

节点oi的全局位置向量Pi的集合{P1,P2，…，PN}描述了软体机械臂中心线的形状。

由于技术的限制，每根光纤上光栅的数目有限。这说明每一段的长度li不会足够小。直接使用测量的节点的曲率-挠率信息计算{P1,P2，…，PN}会存在较大的误差，计算出的节点不会落在真实的曲线上。为获得准确的节点坐标，应尽可能减小li的值。

软体机械臂在受力扭曲时，其中心线是一条光顺曲线[5]，即中心线有着二阶的光滑性，而且其曲率和挠率的变化都比较均匀，不存在突变。由于曲率-弧长曲线、挠率-弧长曲线变化很均匀，可以对测量到的离散{(κi,si)}和{(τi,si)}进行三次B样条插值，得到连续的曲率曲线κ(s)和挠率曲线τ(s)。取得足够多的曲率、挠率信息，再根据式(5)～(7)计算出准确的节点位置信息，最后根据节点位置坐标描绘出准确的软体机械臂的形状。

3 仿真实验

3.1 平面形状检测

软体机械臂中心线弧长L=0.3m。光栅数目为10个，则每一段机械臂中心线长度li=0.03m。为验证算法对平面形状检测的有效性，令机械臂弯曲成平面形状，其中心线的参数方程为：

(8)

插值得到的曲率曲线如图3a所示，检测到的形状与参考形状如图3b所示。可以发现，检测形状与参考形状基本完全重合，末端误差为7mm。其相对于整个机械臂长度的百分比误差约为2.3%。

a. 曲率曲线

b. 参考形状与检测形状

3.2 三维形状检测

为验证算法对复杂空间形状的有效性，令软体机械臂弯曲成空间三维曲线，其中心线的参数方程如下：

(9)

按上述方法测量到的弧长、曲率和挠率数据见表1。

表1 软体机械臂的弧长、曲率和挠率

对表1的数据进行三次B样条插值，得到完整的曲率曲线与挠率曲线，如图4a所示。在曲率曲线和挠率曲线上对弧长s等距划分，选择1 200个样本对{(s,κ,τ)}，代入式(7)，计算出每个样本点的实际坐标，描绘出软体机械臂的形状，如图4b所示。可以发现，检测形状与参考形状基本重合，最大位置误差不超过1cm。其相对于整个软体机械臂长度的百分比误差约为3.3%。

a. 曲率曲线与挠率曲线

b. 参考形状与检测形状

4 结束语

软体机械臂在微创手术、矿难营救和科学探测领域有广阔的应用前景，但存在形状检测困难的问题。特殊的工作环境对传感器提出了极为苛刻的要求。光纤光栅传感器作为一种新兴的传感器，广泛应用于建筑及医学等领域。笔者以光纤光栅传感器为基础，设计了一套分布式光纤光栅传感网络和软体机械臂三维形状检测算法。二维形状检测和三维形状检测的两个仿真实验表明：该方法对于复杂的三维形状，能达到较高的检测精度。

[1] Trivedi D,Rahn C D,Kier W M, et al.Soft Robotics:Biological Inspiration, State of the Art, and Future Research[J].Applied Bionics and Biomechanics,2008,5(3):99～117.

[2] Yi X H, Qian J W, Shen L Y,et al. An Innovative 3D Colonoscope Shape Sensing Sensor Based on FBG Sensor Array[C].Proceedings of the 2007 International Conference on Information Acquisition.Seogwipo-si：IEEE,2007:227～232.

[3] 狄海廷,付宜利.利用光纤曲率传感器重建三维曲面结构[J].光学精密工程,2010,18(5):1092～1098.

[4] Wang H S, Chen W D, Yu X J, et al.Visual Servo Control of Cable-driven Soft Robotic Manipulator[C].Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Tokyo：IEEE,2013:57～62.

[5] 刘保嘉，徐宗俊，王巍，等.曲线及曲面的光顺性研究[J].机械与电子,2001,(5):49～52.