CN111289020A - 一种柔性体大变形空间位姿的感知方法 - Google Patents

Abstract

本发明涉及一种柔性体大变形空间位姿的感知方法，包括以下步骤：步骤1：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在每节均设置栅点；步骤2：计算柔性载体每节的拉伸应变、偏转角、曲率和弯曲角；步骤3：将步骤2中得到的偏转角、曲率、弯曲角代入旋转变换矩阵和平移变换矩阵，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵，得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角，本发明的感知方法精确高效，反应灵敏。

Description

一种柔性体大变形空间位姿的感知方法

技术领域

本发明涉及传感器技术领域，具体涉及一种柔性体大变形空间位姿的感知方法。

背景技术

这里的陈述仅提供与本发明相关的背景技术，而不必然地构成现有技术。

柔性体具有多自由度、大变形等特点，可实现弯曲、扭转、延伸、接触等操作。对柔性体进行运动控制和操作规划的前提是已知柔性体空间位姿与力学特性，进而进行闭环控制柔性体各种运动操作。由于缺乏合适的柔性体传感设备，大多数柔性体机器人采用开环控制，或基于P I D方法进行控制，发明人发现，采用传统传感器对柔性体的测量方法会影响柔性体变形和运动，仅基于视觉反馈的路径跟踪控制也无法检测柔性体内部的变形规律，无法达到高精度检测，而在视觉受限时，视觉反馈跟踪更是无法完成检测任务。光纤光栅具有体小质轻、抗电磁干扰、灵敏度高的优点，然而光纤光栅本身测量范围小，无法直接用于柔性体大变形的检测，因此研制一种柔性体大变形的空间位姿的测量方法至关重要。

发明内容

本发明的目的是为克服现有技术的不足，提供一种柔性体大变形空间位姿感知方法，能够对柔性体大变形空间位姿进行精确高效的测量，节省人力物力。

为实现上述目的，本发明采用如下技术方案：

第一方面，本发明的实施例提供了一种柔性体大变形空间位姿的感知方法，包括以下步骤：

步骤1：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在每节均设置栅点。

步骤2：计算柔性载体每节的拉伸应变、偏转角、曲率和弯曲角。

步骤3：将步骤2中得到的偏转角、曲率和弯曲角代入旋转变换矩阵和平移变换矩阵，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

结合第一方面，本发明实施例提供了第一方面的一种可能的实施方式，所述步骤2中，柔性载体第i节的拉伸应变ε_i总计算方法为：

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为柔性载体第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值；α为第一光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比,ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变；

其中：ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变。

第二方面，本发明实施例提供了一种柔性体大变形空间位姿的感知方法，包括以下步骤：

步骤a：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅及一根第二光纤光栅，第二光纤光栅的旋向与第一光纤光栅旋向相反，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在柔性载体的每节均设置栅点，其中与第二光纤光栅交叉的第一光纤光栅的栅点设置在交叉点处，第二光纤光栅的栅点设置在与第一光纤光栅的交叉点处。

步骤b：计算柔性载体每节的拉伸应变、偏转角、曲率、弯曲角和扭转角。

步骤c：将步骤b中得到的偏转角、曲率、弯曲角和扭转角代入旋转变换矩阵和平移变换矩阵，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

结合第二方面，本发明的实施例提供了第二方面的一种可能的实施方式，所述步骤(b)中，柔性载体第i节的拉伸应变ε_i总计算方法为：

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值，

为柔性载体第i节的扭转角，α为第一光纤光栅或第二光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比，ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变；

其中，ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变。

结合第一方面或第二方面，本发明实施例提供了第一方面或第二方面的一种可能实施方式，步骤2或步骤b中，柔性载体第i节的偏转角φ_i的计算方法为：

其中：

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，ε_i总为柔性载体第i节的拉伸应变，l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅栅点所在柔性载体第i节的母线长度。

结合第一方面或第二方面，本发明实施例提供了第一方面或第二方面的一种可能实施方式，柔性载体第i节的曲率k_i计算方法为：

其中：

r′＝r(1-με_i总)；

μ为柔性载体的泊松比，r为柔性载体初始状态下的半径；r‘为柔性载体轴向长度发生变化后的半径；

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，ε_i总为柔性载体第i节的拉伸应变；l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅所在柔性载体母线长度。

结合第一方面或第二方面，本发明的实施例提供了一种第一方面或第二方面可能的实施方式，步骤2或步骤b中，柔性载体第i节弯曲角θ_i的计算方法为：

θ_i＝k_il_i0(1+ε_i总)；

其中，k_i为柔性载体第i节的曲率，l_i0为柔性载体第i节的初始长度，ε_i总为柔性载体第i节的拉伸应变。

本发明的有益效果：

本发明的柔性体大变形空间位姿的感知方法，通过设置能够随带检测柔性体运动的柔性载体，柔性载体上设置光纤光栅，能够利用光纤光栅检测柔性载体多节的曲率、偏转角、扭转角、拉伸应变等信息，然后通过转换矩阵转换为柔性载体末端坐标和位姿，进而对待检测柔性体的大变形和空间位姿进行感知，而且曲率、偏转角等信息的计算带入了柔性体每节的拉伸应变，适用于柔性体轴长发生变化时的检测，精确高效，反应灵敏，节省了人力物力。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的限定。

图1为本发明实施例1检测方法流程示意图；

图2为本发明实施例1柔性载体与待检测柔性体装配示意图；

图3为本发明图2的爆炸示意图；

图4为本发明实施例2检测方法流程示意图；

其中，1.柔性载体，2.待检测的柔性体，3.法兰盘，4.运动部，5.螺杆，6.螺母，7.第一光纤光栅，8.固定部。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

为了方便叙述，本发明中如果出现“上”、“下”、“左”“右”字样，仅表示与附图本身的上、下、左、右方向一致，并不对结构起限定作用,仅仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位，以特定的方位构造和操作，因此不能理解为对本发明的限制。

正如背景技术所介绍的，现有的光纤光栅检测技术测量范围小，无法直接用于柔性体大变形的检测，针对上述问题，本申请提出了一种柔性体大变形空间位姿的感知方法。

本申请的一种典型实施方式实施例1中，如图1-3所示，一种柔性体大变形空间位姿的感知方法，柔性体只受到拉力和弯力，包括以下步骤：

步骤1：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅后，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布。两个第一光纤光栅中心点与柔性载体截面圆周圆心连线夹角为120°。

所述柔性载体1为由橡胶材料制成的圆柱形结构，待检测的柔性体2为管状结构，其两端能够通过螺栓连接法兰盘3，柔性载体的外周面上开设有三个左旋螺旋线槽和三个右旋螺旋线槽，采用3D打印一体化制作，三根第一光纤光栅7分别设置在三个左旋螺旋线槽内，柔性载体的两端分别设置有固定部8和运动部4，固定部和运动部由树脂材料3D打印而成，所述固定部和运动部上均设置有螺杆5，螺杆能够穿过法兰盘，所述螺杆上能够旋紧螺母6，将柔性载体同轴置入待检测的柔性体内部，然后将法兰盘与待检测柔性件固定连接，利用螺母将柔性载体的固定部和运动部分别与待检测的柔性体的两端的法兰盘固定连接，三条第一光纤光栅穿出柔性体与外部设备连接。

所述柔性载体划分为多节，光纤光栅在柔性载体的每节均设置一个栅点，栅点处利用柔性胶水进行封装，控制柔性胶水的厚度、长度等，设计光栅和胶水之间的应变传递系数，使光纤光栅承担一小部分应变，初步增大光纤光栅的测量范围。光纤光栅螺旋分布弱化光纤光栅轴向应变的灵敏度，进一步增大光纤光栅的测量范围。

步骤2：待检测柔性体的运动端在空间内运动，柔性载体随待检测柔性体产生形变，利用第一光纤光栅波长的变化计算柔性载体形变之后的空间位姿，实现待检测柔性体大变形空间位姿的感知。

计算柔性载体每节的拉伸应变、偏转角、曲率和弯曲角；

柔性载体第i节的拉伸应变ε_i总计算方法为：

其中：ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变，可通过下述公式(2)、(3)、(4)求得。

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值，检测人员能够通过三条第一光纤光栅连接的外部设备直接读取得到；α为第一光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比。

第i节的偏转角φ_i的计算方法为：

其中：

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，可通过公式(2)、(3)、(4)求得，ε_i总为柔性载体第i节的拉伸应变。

l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅栅点所在柔性载体第i节的母线长度。

第i节的曲率k_i计算方法为：

其中：

r＝r(1-με_i总) (8)

μ为柔性载体的泊松比，r为柔性载体初始状态下的半径，r‘为柔性载体轴向长度发生变化后的半径；

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，ε_i总为柔性载体第i节的拉伸应变。

l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅栅点所在柔性载体第i节的母线长度。

柔性载体第i节的弯曲角θ_i的计算方法为：

θ_i＝A_il_i0(1+ε_i总) (9)

其中，k_i为柔性载体第i节的曲率，l_i0为柔性载体第i节的初始长度，ε_i总为柔性载体第i节的拉伸应变。

步骤3：将步骤2中得到的偏转角、曲率和弯曲角代入旋转变换矩阵

和平移变换矩阵P_i ^i-1，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵T，其中，固定端坐标系为基坐标系，运动端坐标系为待检测柔性载体的末端坐标系，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

变换矩阵T的计算方法为：

其中：

n为柔性载体划分的节数，φ_i为第i节的偏转角，θ_i为第i节的弯曲角，d_x、d_y、d_z为柔性载体运动端中心点在固定端坐标系中的坐标值，

为柔性载体第i节和第i-1节坐标系的旋转变换矩阵，P_i ^i-1为柔性载体第i节和第i-1节坐标系平移变换矩阵，i＝1、2…n

柔性载体运动端端面的欧拉角α、β、γ的计算方法为：

将公式(11)、公式(12)代入公式(10)后，经过矩阵计算得到一个四行四列的矩阵，其中，a_x、b_x、c_x、d_x、a_y、b_y、c_y、d_y、a_z、b_z、c_z、d_z为该四行四列矩阵对应位置的数值。

本申请的另一种典型实施方式实施例2中，如图4所示，待检测的柔性体受到拉力、弯力和扭力，柔性体的运动信息检测步骤包括以下具体步骤：

步骤a：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅及一根第二光纤光栅，第二光纤光栅的旋向与第一光纤光栅旋向相反，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在柔性载体的每节均设置栅点，其中与第二光纤光纤交叉的第一光纤光栅的栅点设置在交叉点处，第二光纤光栅的栅点设置在与第一光纤光栅的交叉点处。

柔性载体及第一光纤光栅的布置方式与实施例1相同，在此不进行详细叙述，所述的第二光纤光栅设置在其中一个右旋螺旋槽内，第二光纤光栅在柔性载体的每节中均与第一光纤光栅交叉，交叉的第一光纤光栅和第二光纤光栅均在交叉点处设置栅点，将设置好第一光纤光栅和第二光纤光栅的柔性载体与待检测的柔性体连接，连接方式与实施例1相同，在此不进行详细叙述。

步骤b：计算柔性载体每节的拉伸应变、偏转角、曲率、弯曲角和扭转角。

柔性载体第i节的扭转角的计算方法为：

且ε_i1＝ε_i4

ε_i1FBG、ε_i4FBG分别为交叉的第一光纤光栅和第二光纤光栅栅点测量得到的总应变值，由操作人员在与第一光纤光栅和第二光纤光栅连接的外部设备读取得到，ε_i1为与第二光纤光栅交叉的第一光纤光栅栅点所在柔性载体母线的总应变，ε_i4为第二光栅栅点所在柔性载体母线总应变，

为柔性载体第i节的扭转角，α为第一光纤光栅或第二光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比。

柔性载体第i节的拉伸应变ε_i总计算方法为：

其中，ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变，可通过下述公式(17)、(18)、(19)求得。

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值，

为柔性载体第i节的扭转角，α为第一光纤光栅或第二光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比。

柔性载体第i节的偏转角、曲率及弯曲角的计算方法与实施例1相同，在此不进行详细叙述。

偏转角、曲率及弯曲角的计算过程中，带入了柔性体每节的拉伸应变，考虑到了柔性体轴向长度发生变化的情况，计算结果更加精确。

步骤c，将步骤b中得到的偏转角、曲率、弯曲角和扭转角代入旋转变换矩阵

和平移变换矩阵P_i ^i-1，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵T，其中，固定端坐标系为基坐标系，运动端坐标系为待检测柔性载体的末端坐标系，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

变化矩阵T的计算方法为：

其中，

φ_i为第i节的偏转角，θ_i为第i节的弯曲角，k_i为第i节的曲率，

为第i节的扭转角，d_x、d_y、d_z为柔性载体运动端中心点在固定端坐标系中的坐标值，

为柔性载体第i节和第i-1节坐标系的旋转变换矩阵，P_i ^i-1为柔性载体第i节和第i-1节坐标系平移变换矩阵，i＝1、2…n。

将公式(21)、公式(22)代入公式(20)后，经过矩阵计算得到一个四行四列的矩阵，其中，a_x、b_x、c_x、d_x、a_y、b_y、c_y、d_y、a_z、b_z、c_z、d_z为该四行四列矩阵对应位置的数值。

柔性载体运动端端面的欧拉角α、β、γ的计算方法与实施例1相同，在此不进行详细叙述。

采用实施例1或实施例2的检测方法，适用于柔性体轴线方向长度发生变化时的位移信息检测，精确高效，反应灵敏，节省了人力物力。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种柔性体大变形空间位姿的感知方法，其特征在于，包括以下步骤：

步骤1：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在每节均设置栅点；

步骤2：计算柔性载体每节的拉伸应变、偏转角、曲率和弯曲角；

步骤3：将步骤2中得到的偏转角、曲率和弯曲角代入旋转变换矩阵和平移变换矩阵，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

2.如权利要求1所述的一种柔性体大变形空间位姿的感知方法，其特征在于，所述步骤2中，柔性载体第i节的拉伸应变ε_i总计算方法为：

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为柔性载体第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值；α为第一光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比,ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变；

其中：ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变。

3.如权利要求1所述的一种柔性体大变形空间位姿的感知方法，其特征在于，所述步骤3中，转变矩阵T的计算方法为：

其中：

φ_i为柔性载体第i节的偏转角，θ_i为柔性载体第i节的弯曲角，k_i为柔性载体第i节的曲率，d_x、d_y、d_z为柔性载体运动端中心点在固定端坐标系中的坐标值，n为柔性载体划分的节数，

为柔性载体第i节和第i-1节坐标系的旋转变换矩阵，P_i ^i-1为柔性载体第i节和第i-1节坐标系平移变换矩阵；

柔性载体运动端端面的欧拉角α、β、γ的计算方法为：

其中，a_x、b_x、c_x、d_x、a_y、b_y、c_y、d_y、a_z、b_z、c_z、d_z为变换矩阵计算后对应位置处的数值。

4.一种柔性体大变形空间位姿的感知方法，其特征在于，包括以下步骤：

步骤a：在柔性载体的外周面沿螺旋线布置三根第一光纤光栅及一根第二光纤光栅，第二光纤光栅的旋向与第一光纤光栅旋向相反，将柔性载体的两端分别与待检测柔性体的运动端和固定端固定连接，柔性载体同一截面上，三根第一光纤光栅沿截面圆周均匀分布，将柔性载体划分为多节，第一光纤光栅在柔性载体的每节均设置栅点，其中与第二光纤光栅交叉的第一光纤光栅的栅点设置在交叉点处，第二光纤光栅的栅点设置在与第一光纤光栅的交叉点处；

步骤b：计算柔性载体每节的拉伸应变、偏转角、曲率、弯曲角和扭转角；

步骤c：将步骤b中得到的偏转角、曲率、弯曲角和扭转角代入旋转变换矩阵和平移变换矩阵，根据旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵，根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角。

5.如权利要求4所述的一种柔性体大变形空间位姿的感知方法，其特征在于，所述步骤b中，

柔性载体第i节扭转角的计算方法为：

ε_i1FBG、ε_i4FBG分别为交叉的第一光纤光栅和第二光纤光栅栅点测量得到的总应变值，ε_i1为与第二光纤光栅交叉的第一光纤光栅栅点所在柔性载体母线的总应变，ε_i4为第二光栅栅点所在柔性载体母线总应变，ε_i1＝ε_i4，

为扭转角，α为第一光纤光栅或第二光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比。

6.如权利要求5所述的一种柔性体大变形空间位姿的感知方法，其特征在于，所述步骤b中，柔性载体第i节的拉伸应变ε_i总计算方法为：

ε_i1FBG、ε_i2FBG、ε_i3FBG分别为第i节处的三个第一光纤光栅的栅点分别测量得到的总应变值，

为柔性载体第i节的扭转角，α为第一光纤光栅或第二光纤光栅所在螺旋线的螺旋角，μ为柔性载体的泊松比，ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变；

其中，ε_i1、ε_i2、ε_i3分别为柔性载体第i节中，三条第一光纤光栅的栅点所在的柔性载体的母线总应变。

7.如权利要求4所述的一种柔性体大变形空间位姿的感知方法，其特征在于，所述步骤c中，变化矩阵T的计算方法为：

其中，

φ_i为柔性载体第i节的偏转角，θ_i为柔性载体第i节的弯曲角，

为柔性载体第i节的扭转角，k_i为柔性载体第i节的曲率，d_x、d_y、d_z为柔性载体运动端中心点在固定端坐标系中的坐标值，n为柔性载体划分的节数，

为柔性载体第i节和第i-1节坐标系的旋转变换矩阵，P_i ^i-1为柔性载体第i节和第i-1节坐标系平移变换矩阵；

柔性载体运动端端面的欧拉角α、β、γ的计算方法为：

其中，a_x、b_x、c_x、d_x、a_y、b_y、c_y、d_y、a_z、b_z、c_z、d_z为变换矩阵计算后对应位置处的数值。

8.如权利要求1或4所述的一种柔性体大变形空间位姿的感知方法，其特征在于，柔性载体第i节的偏转角φ_i的计算方法为：

其中：

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，ε_i总为柔性载体第i节的拉伸应变，l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅栅点所在柔性载体第i节的母线长度。

9.如权利要求1或4所述的一种柔性体大变形空间位姿的感知方法，其特征在于，柔性载体第i节的曲率k_i计算方法为：

其中：

r′＝r(1-με_i总)；

μ为柔性载体的泊松比，r为柔性载体初始状态下的半径；r‘为柔性载体轴向长度发生变化后的半径；

l_i0为柔性载体每节的初始长度，ε_i1、ε_i2、ε_i3为柔性载体第i节中，三条第一光纤光栅的栅点所在柔性载体母线的总应变，ε_i总为柔性载体第i节的拉伸应变；l_i1′、l_i2′、l_i3′分别为柔性载体第i节发生轴向长度变化后，三根第一光纤光栅所在柔性载体母线长度。

10.如权利要求1或4所述的一种柔性体大变形空间位姿的感知方法，其特征在于，柔性载体第i节弯曲角θ_i的计算方法为：

θ_i＝k_il_i0(1+ε_i总)；

其中，k_i为柔性载体第i节的曲率，l_i0为柔性载体第i节的初始长度，ε_i总为柔性载体第i节的拉伸应变。

Images