带温度漂移补偿的软体机械臂形状估计方法与流程

本发明涉及光纤光栅传感领域和软体机器人领域，具体地，涉及一种带温度漂移补偿的软体机械臂形状估计方法。
背景技术：
：软体机械臂作为一种新型多自由度机械臂类型有着非常好的前景，受到了广泛关注，尤其在医疗领域具有非常大的使用空间，其在人体内脏的微创消融手术中有着高安全、低成本的特点。而如今于复杂手术中，对于软体机械臂的形状控制要求越来越高，更加精细的形状估计检测方法是目前急需攻克的技术难题。传统形状估计大体分为三类：其一为视觉控制，目前工作检测比较纯熟，但在内部环境中无法工作。其二为智能材料，能够在不破坏执行机构的情况下进行检测，但目前只能用于检测二维形状。其三是利用光纤光栅传感器，精度较高，对机械臂软体特性损伤小，但由于软体机械臂所处的工作环境温度具有多变性，且光纤光栅传感器对于温度敏感，将导致位置和形状检测有较大误差。目前没有发现同本发明类似技术的说明或报道，也尚未收集到国内外类似的资料。技术实现要素：针对现有技术中的缺陷，本发明的目的是提供一种带温度漂移补偿的软体机械臂形状估计方法。根据本发明提供的带温度漂移补偿的软体机械臂形状估计方法，包括如下步骤：形状检测步骤：建立软体机械臂的分析模型，在软体机械臂内部埋设第一组光纤光栅传感器，将软体机械臂分为多个均匀的分段，通过第一组光纤光栅传感器检测每个软体机械臂分段的相关参数，所述相关参数能够表征软体机械臂的空间姿态；温度漂移补偿步骤：在软体机械臂中轴位置埋设第二组光纤光栅传感器，用于检测中轴的温度变化对中心波长位移的影响量，结合形状检测步骤消除温度对软体机械臂形态的影响。优选地，所述形状检测步骤中的分析模型是指：将软体机械臂等效为由无数组初始情况下互相平行、距离相同且无法改变的刚性圆盘构成；其中，第一组光纤光栅传感器包括四根热膨胀系数、热光系数、有效弹光系数均相同的光纤光栅传感器，四根光纤光栅传感器沿软体机械臂的周向均匀放置在软体机械臂内部，且四根光纤光栅传感器与软体机械臂的中轴平行。优选地，所述形状检测步骤中的相关参数是指：软体机械臂分段的曲率、挠率、初始角度以及软体机械臂对应分段绕轴线扭转的角度。优选地，所述形状检测步骤包括：基于软体机械臂的分析模型，计算第一组光纤上光栅传感器穿过刚性圆盘位置处的应变值；具体地，将第一组光纤上光栅传感器穿过刚性圆盘的位置设为对应点，则第一组光纤光栅传感器中第i根光纤光栅传感器对应点的理论应变值εi′的计算公式如下：式中所有参数均为软体机械臂在考虑分段内的相关参数，其中：l为光纤光栅传感器光栅的自然长度，Δl表示软体机械臂该分段内体积微元模型中中心轴线的真实长度；表示软体机械臂该分段内体积微元模型中上截面相对于下截面绕中轴线旋转的角度；βi为考虑分段内第i根光纤上位于该分段的光栅处与所在截面圆心的连线相对于该分段建立的初始坐标系x轴的下平面的的连线和直角坐标系x轴的夹角，i＝1,2,3,4，其中下截面圆心为直角坐标系的原点O，上截面圆心为O’；r为光纤光栅传感器光纤分布处距软体机械臂轴线的距离，为在O’截面上位置为(d,α)的面元到O平面上对应面元的距离。优选地，所述温度漂移补偿步骤中的第二组光纤光栅传感器包括：两根中心波长相同，热膨胀系数、热光系数、有效弹光系数均不相同的光纤光栅传感器紧贴并排放置于软体机械臂的中轴处，相同中心波长光纤光栅传感器的光栅位置相同，且所述第二组光纤光栅传感器的光栅与第一组光纤光栅传感器相同中心波长的光栅对齐。优选地，第二组光纤光栅传感器的光栅处应变值随温度变化的计算公式如下：ΔλB1λB1=(1-Pe1)·ϵ+(α1+ξ1)·ΔTΔλB2λB2=(1-Pe2)·ϵ+(α2+ξ2)·ΔT]]>式中：ΔλB1表示考虑分段内第一根光纤光栅传感器的中心波长位移量，λB1表示在标准温度条件下考虑分段内第一根光纤光栅传感器处于自然拉伸状态下的中心波长，Pe1表示第一根光纤光栅传感器的光弹性系数，α1表示第一根光纤光栅传感器的热涨系数，ξ1表示第一根光纤光栅传感器的热光系数，ΔλB2表示第二根光纤光栅传感器的中心波长位移量，λB2表示在标准温度条件下第二根光纤光栅传感器处于自然拉伸状态下的中心波长，Pe2表示第二根光纤光栅传感器的光弹性系数，α2表示第二根光纤光栅传感器的热涨系数，ξ2表示第二根光纤光栅传感器的热光系数，ε表示光栅传感器处发生的应变，ΔT表示光栅传感器处温度变化量；将第一组光纤光栅传感器的光栅处应变值减去第二组光纤光栅传感器的光栅处由于温度变化引起的软体机械臂中轴处应变值的变化量，得到消除温度影响的绝对应变值。与现有技术相比，本发明具有如下的有益效果：1、本发明提供的带温度漂移补偿的软体机械臂形状检测方法，在对软体机械臂柔性特性不产生损害的前提下，能够精确地检测出软体机械臂轴线的形状参数。2、本发明中设置有温度补偿系统，将温度对于软体机械臂形状检测精度的影响有效地消除，使得其适应更多的工作环境。3、本发明整体结构简单，无需繁琐的连接线和控制器就能够实现对软体机械臂形状的实时检测。附图说明通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：图1为本发明使用的分析模型图；图2为本发明光纤传感器布置方式的示意图；图3为本发明应用在软体机械臂的整体结构示意图；图4为软体机械臂分段在直角坐标系下的结构示意图；图5为光栅的截面示意图。图中：1-用于温度漂移补偿的布拉格光纤光栅传感器；2-用于形状检测的布拉格光纤光栅传感器；3-刚性圆盘；4-软体机械臂主体。具体实施方式下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。根据本发明提供的带温度漂移补偿的软体机械臂形状估计方法，包含两个主要部分，一个是形状检测部分，一个是温度漂移补偿部分，其中：所述形状检测部分，由四根热膨胀系数、热光系数、有效弹光系数相同的光纤光栅传感器均匀地放置在机械臂内部，与机械臂的轴平行且到轴的距离相等，同时使得各光纤上相同中心波长的光栅对齐于同一垂直于机械臂轴线的平面内。具体检测原理如下：在软体机械臂分段为空间旋转螺旋线的假设前提下，机械臂的形变共分成两个方面：第一个方面是机械臂轴线的形状改变，可看成是无粗细刚性细线的形变问题；另一方面是机械臂自身绕中心轴线的扭转，这通常是由于线驱控制中拉伸作用造成的。检测这两个方面是本部分的主要目的。具体方法如下：选取其中一段进行微元，得到体积元如图4所示，体积元即横截面旋转了很小一个角度扫过的体积。由于材料的特性和线驱控制的拉扯，该段机械臂会绕其轴线旋转一个和Δθ相关的角度在该体积元中，都是一阶小量。在分段内，认为机械臂绕轴自身旋转的角度积累和轴线的长度是成正比的，因此在我们所给的微元中：K为常数，表现了该分段中机械臂绕其轴扭转的程度。根据空间解析几何定义，设机械臂其中一个分段的中心轴线OO’的参数方程为：x=a·[cos(θ+θ0)-cosθ0]y=a·[sin(θ+θ0)-sinθ0]z=bθ]]>对于所述分段，一共有四个未知量，即a，b，θ0和式中：x表示设定坐标轴上x方向上的分量，y表示设定坐标轴上y方向上的分量，z表示设定坐标轴上z方向上的分量，θ表示参数方程中的参变量，θ0表示轴线起始处对应的参变量θ的值，a表示空间螺旋线的旋转半径，b表示空间螺旋线z方向变化相对于参变量变化的比值大小；根据立体几何和空间解析几何的知识，这四个参数可以充分表现机械臂分段轴线形状特性。求取这四个未知量的方法原理如下：在机械臂发生了轴线形变以及绕轴旋转之后，分布在机械臂内部的光纤会随着机械臂形状的改变而产生相应形变，因此光栅处会产生相应的应变。这个应变值可以由空间解析几何的方法得到，所得值称为应变理论值。而通过四根光纤测得的中心波长平移，能够依据布拉格光纤光栅的中心波长平移特性方程求到对应光栅处的形变，称之为应变测量值。通过使应处的应变理论值和应变测量值相等，可以对于每一个分段列出四个方程，联立为一有唯一解的四元方程组。对方程组求解即可得到上述所求的四个未知量。求取应变理论值和应变测量值的具体方法如下：令θ＝Δθ，得到O’点的坐标：O’(a(cos(Δθ+θ0)-cosθ0),a(sin(Δθ+θ0)-sinθ0),bΔθ)以及中心轴线的空间长度Δl，设n为中心轴线在O’处的方向向量：(-asin(Δθ+θ0),acos(Δθ+θ0),b)设p为在O’平面内起点为O’且Z方向分量为0的方向向量：(cos(Δθ+θ0)，sin(Δθ+θ0)，0)在O’截面内，以p为极轴，位置为(d,α)处(α表示向量与p的夹角)的点R’坐标设为(x′,y′,z′)，则向量O’R’：(x′-a·(cos(Δθ+θ0)-cosθ0),y′-a(sin(Δθ+θ0)-sinθ0),z′-bΔθ)可以列出下列方程：[x′-a·(cos(Δθ+θ0)-cosθ0)]2+(y′-a·(sin(Δθ+θ0)-sinθ0))2+(z-bΔθ)2=d2[x′-a·(cos(Δθ+θ0)-cosθ0)]·cos(Δθ+θ0)+sin(Δθ+θ0)·(y′-(sin(Δθ+θ0)-sinθ0))=dcosα[x′-a·(cos(Δθ+θ0)-cosθ0)]·(-asin(Δθ+θ0))+(y′-a(sin(Δθ+θ0)-sinθ0))·acos(Δθ+θ0)+b·(z-bΔθ)=0]]>R’在O平面上的对应点R的坐标设为(x,y,z)，则有：令f(d,α)为在O’截面上位置为(d,α)的面元到O平面上对应面元的距离，即|OO’|：设第i根光纤上在考虑分段内的光栅传感器的理论应变值为εi’，测量应变值为εi，βi为第i根光纤上在考虑分段内的光栅底部与其所处截面的圆心连线与该分段的旋转中心到该截面圆心连线方向的夹角，其中，下标i表示所使用的光纤的序号，在本实施例中i＝1,2,3,4。设第j分段的βi为βi,j,则：βi,j=βi,j-1+Kj-1·Lj-1aj-1·bj-12+1]]>式中，Lj-1表示前一分段的长度，Kj-1表示前一段K常数，aj-1表示前一段的a常数，bj-1表示前一段的b常数。按照初始光纤的埋设情况，机械臂第一分段的β1＝0°，β2＝90°，β3＝180°，β4＝270°。之后每段通过求得前段相关参数用上述递推方法得到。同时，r为光纤光栅传感器光纤分布处距机械臂轴线的距离，l为光纤光栅传感器光栅的自然长度，如图5所示：ϵi′=limΔl→01Δl·f(r,βi+K·Δθ)-ll]]>由于光栅线度很小，可以认为Δl趋近于l，上式可退化为：ϵi′=f(r,βi+Ki·θ)-ll]]>其中：求出测量应变值εi：根据ΔλBiλBi=(1-Pe)·ϵi+(α+ξ)·ΔTi]]>其中，λBi、Pe、α、ξ分别表示标准温度条件下考虑分段内第i根光纤的光栅自然状态下的中心波长、光栅的光弹性系数、光栅的热涨系数、光栅的热光系数，且均由厂家提供，在不考虑温度影响时，ΔTi＝0，ΔTi表示光栅处温度相对于标准温度的变化量；ϵi=ΔλBλB·(1-Pe);]]>式中：ΔλBi表示考虑分段内第i根光纤的光栅的中心波长位移量，Pe表示光栅的光弹性系数；最后，联立下列方程，即可求出a、b、K、θ0。ϵ1′=ϵ1=f(r,β1+K·θ)-llϵ2′=ϵ2=f(r,β2+K·θ)-llϵ3′=ϵ3=f(r,β3+K.θ)-1lϵ4′=ϵ4=f(r,β4+K·θ)-ll]]>所述温度漂移补偿部分，由两根中心波长相同，热膨胀系数、热光系数、有效弹光均不相同的光纤光栅传感器紧贴并排放置于机械臂中轴处。其中，具有相同中心波长的光栅位置相同，且与形状检测部分相同中心波长的光栅对齐。具体工作原理如下：根据布拉格光纤光栅的原理，中心波长的偏移满足下列条件：ΔλBλB=(1-Pe)·ϵ+(α+ξ)·ΔT]]>其中，除ε和ΔT外，其他参数都是由厂家提供的。本例使用的方法是在机械臂的中心轴线处紧贴放置两根上述参数不相同的两根光纤光栅传感器。同一处的光栅处的应变和温度变化是相同的。列出下列方程：ΔλB1λB1=(1-Pe1)·ϵ+(α1+ξ1)·ΔTΔλB2λB2=(1-Pe2)·ϵ+(α2+ξ2)·ΔT]]>通过解上述二元方程，即可得到与中心轴线传感器上光栅对应的四个光栅处的温度变化。减去温度变化对中心波长位移的影响量，就能得到绝对的测得的应变量。所述的自带温度漂移补偿的软体机械臂形状估计方法，其两部分共同工作的具体方法是：通过对光纤光栅信号的调解，得到温度漂移补偿部分两根传感器在不同光栅处的对应中心波长。对两组处于同一位置的光栅的中心波长变化进行联立，根据光栅光纤温度和应变灵敏度特性方程，可以解得该处温度和应变分别对于光栅中心波长位移的影响。同时得到初始对齐的四个光栅中心波长的位移。根据前面步骤所得的各处温度对光栅中心波长的影响可认为在该平面内影响相同。将温度对于的测量值的偏移效果消除，即可得到各光栅的真实应变量。将这个消除温度影响后的测量应变量的求取过程替换形状估计部分对测量应变值的求取过程，即为本发明的完整方法步骤。套用形状检测部分的算法，即可得到较为精准的机械臂轴线形状以及绕轴扭转的角度。对机械臂进行多段划分，并以分布有多个不同中心波长的光栅的传感器匹配。光纤放入细软薄套管中后置入机械臂，可有效增强固定以及防止光纤损坏。可以置入多条光纤进行检测，通过多组测量值进行拟合，可提高精确度，防止因为实验器材突然损坏造成的实验错误。实施例1本实施例采用了一种带温度漂移补偿的软体机械臂形状估计方法，包含两个主要部分，一个是形状检测部分，一个是温度漂移补偿部分，其中：所述形状检测部分，由四根热膨胀系数、热光系数、有效弹光系数相同的光纤光栅传感器均匀地放置在机械臂内部，与机械臂的轴平行且到轴的距离相等，同时使得各光纤上相同中心波长的光栅对齐于一垂直于机械臂轴的平面内。所述温度漂移补偿部分，由两根中心波长相同，热膨胀系数、热光系数、有效弹光均不相同的光纤光栅传感器紧贴并排放置于机械臂中轴处。其中，具有相同中心波长的光栅位置相同，且与形状检测部分相同中心波长的光栅对齐。所述的带温度漂移补偿的软体机械臂形状估计方法，其两部分共同工作的具体方法是：通过对光纤光栅信号的调解，得到温度漂移补偿部分两根传感器在不同光栅处的对应中心波长。对两组处于同一位置的光栅的中心波长变化进行联立，根据光栅光纤温度和应变灵敏度特性方程，可以解得该处温度和应变分别对于光栅中心波长位移的影响。同时得到初始对齐的四个光栅中心波长的位移。根据前面步骤所得的各处温度对光栅中心波长的影响可认为在该平面内影响相同。将温度对于的测量值的偏移效果消除，即可得到各光栅的真实应变量。所述的具体工作方法，求得的轴线处应变量可以用于形状估计结果的自检。所述的具体工作方法，通过各光栅处真实形变量得到机械臂中心轴线的形状参数的方法是：将软体机械划分成若干段，段数与光纤光栅传感器上光栅个数相对应。每一段的轴线是一条空间螺旋线，其形状由四个参数决定，通过对应的一组四个光栅的应变量值即可对这四个参数分别求解。求解方法是：先建立分析模型，机械臂的组成是无数组初始情况下互相平行、距离相同且无法改变的刚性圆盘。形状检测部分的光纤穿过的位置设为对应点，光纤处的两相邻刚性圆盘对应点之间的距离减去刚性圆盘初始时的距离，就是绝对应变量的大小。以此算出的相对应变和光纤光栅所测的应变相等，一组四个应变量可列出四个方程，求解可得到四个参数的值。具体实施步骤：步骤S1：对机械臂进行多段划分，并以分布有多个不同中心波长的光栅的传感器匹配。步骤S2：光纤放入细软薄套管中后置入机械臂，可有效增强固定以及防止光纤损坏。步骤S3：将光纤的光口进行清理，而后与光纤调制仪相连。步骤S4：通过电机控制使得机械臂平直并进行调零工作。控制机械臂发生形变，通过本发明提供的方法即可进行检测。以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。当前第1页1 2 3