CN113959359B - 一种管道状连续机器人的形状测量装置和方法 - Google Patents

Abstract

本发明公开了一种管道状连续机器人的形状测量装置和方法，该形状测量装置包括长度测量单元、软轴单元和角度测量单元，其中所述长度测量单元连接在所述软轴单元的第一端，所述角度测量单元包括前关节、后关节和多根光纤光栅，所述后关节的后端连接在所述软轴单元的第二端，多根所述光纤光栅并列连接在所述后关节的前端与所述前关节的后端之间。本发明依靠光纤的精确传感能力和高灵敏度，可以改善传统连续型机器人姿态估计误差的问题。

Description

一种管道状连续机器人的形状测量装置和方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种管道状连续机器人的形状测量装置和方法。

背景技术

连续型机器人是一种多关节串联的多自由度机器人，它仿照象鼻或章鱼足的特点，可以在自身多处向任意方向弯曲。连续型机器人灵活多变，工作半径大，适合在管道、狭缝、复杂结构、易损伤结构中工作，对多障碍物环境适应性非常强，一些典型的应用场景如复杂机械结构的检查探伤、医疗内窥镜等。

连续型机器人是近几年开始发展的一种新型机器人结构，它不同于传统离散型刚性机器人，后者的结构通常是刚性杆与离散关节构成，在作业时往往存在工作空间小、易与环境碰撞等问题，连续型机器人因其强大的自由弯曲变形能力，对多变复杂的空间适应性非常强，并且可以有多种工作方式，不仅可以通过末端的执行器进行探测、抓取等工作，也可以通过自身的弯曲变形对物体进行卷绕抓取。

连续型机器人的最大问题是姿态误差较大，通常连续型柔性机器人没有固定的关节点，常用方法难以对其形态进行准确测量，并且由于这种类型机器人的关节是串联结构，前一级的误差会叠加到下一级上并通过臂长放大，因此机器人末端位置误差最大，另一方面由于连续型机器人结构往往为长圆柱形，内部被骨架、筋腱等占据，无法放置关节测量元件，因此通常臂形姿态测量在机器人根部或通过外界光学测量，一方面造成误差另一方面也限制了机器人的工作条件。

如图1a和图1b所示公开的是一种采用驱动绳末端测量的方法，该多自由度连续机器人结构是通过多段刚性杆101连接，关节处通过三根或以上低弹性驱动绳102拉动驱动，驱动绳102一直保持拉紧状态，当关节103向不同方向弯曲时，三根驱动绳102各自的伸缩量不同，通过测量驱动绳102伸缩量就可以得到关节的弯曲状态。另一种方式是关节向不同方向弯曲时，驱动器104的力矩不同，通过驱动器104力矩反馈得到关节的弯曲状态。这种测量方式存在以下问题：(1)当关节数较多时驱动绳102往往产生明显弹性，伸缩量与关节实际姿态不同；(2)驱动绳102与上基盘105、下基盘106之间会产生较大摩擦力，干扰驱动器104的力矩测量；(3)测量驱动绳102长度往往采用编码器等器件，关节数较多时测量系统的复杂度增加，数据采集受限。

如图2所示公开的是一种采用外部视觉测量的方法，该连续机器人201本身并不含有测量系统，连续机器人201仅由驱动器、骨架、驱动筋腱等组成，在机器人外部通过摄像机、结构光或激光雷达等进行视觉捕捉(例如图2中包括水平摄像头202、竖直摄像头203和识别靶盘204)，并通过图像处理等方式估计机器人的位姿。这种测量方式存在以下问题：(1)连续机器人201周边不能有遮挡，大大限制了连续机器人201在复杂环境中的应用；(2)图像处理的速度不高，采样率往往在10Hz量级；(3)图像处理的精度不高，受限于环境的光学噪声和采集器的像素精度。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

为解决上述技术问题，本发明一种管道状连续机器人的形状测量装置和方法，依靠光纤的精确传感能力和高灵敏度，可以改善传统连续型机器人姿态估计误差的问题。

为了达到上述目的，本发明采用以下技术方案：

本发明公开了一种管道状连续机器人的形状测量装置，包括长度测量单元、软轴单元和角度测量单元，其中所述长度测量单元连接在所述软轴单元的第一端，所述角度测量单元包括前关节、后关节和多根光纤光栅，所述后关节的后端连接在所述软轴单元的第二端，多根所述光纤光栅并列连接在所述后关节的前端与所述前关节的后端之间。

优选地，所述角度测量单元还包括镍钛合金丝，多根所述光纤光栅沿着所述镍钛合金丝的轴向并列连接在所述镍钛合金丝的外圆周上，所述镍钛合金丝连接在所述后关节的前端与所述前关节的后端之间。

优选地，所述光纤光栅的数量为三根，三根所述光纤光栅分别连接在所述后关节的前端与所述前关节的后端之间沿圆周方向的三等分位置处。

优选地，所述形状测量装置还包括多个卡环，多个所述卡环沿着所述软轴单元的轴向方向上排列以使得所述软轴单元能够卡合设置在待测的管道状连续机器人的管道内。

本发明还公开了一种管道状连续机器人的形状测量方法，采用上述的形状测量装置进行测量，包括以下步骤：

将所述形状测量装置伸入到所述管道状连续机器人的管道内进行测量，设定初始测量点后，每次测量后前进预设距离进行多次测量，根据所述形状测量装置的多次测量得到每次测量时的伸入长度和关节角度值，再根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标，以得到所述管道的形状。

优选地，根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标包括：根据当前次测量时的伸入长度和关节角度值以及计算得到的前一次测量时的位置坐标，来计算得到当前次测量时的位置坐标。

优选地，根据当前次测量时的伸入长度和关节角度值以及计算得到的前一次测量时的位置坐标，来计算得到当前次测量时的位置坐标具体采用下式：

P_n＝P_n-1+ΔP_n＝[x_n-1+Δx_n,y_n-1+Δy_n,z_n-1+Δz_n]

式中，P_n、P_n-1分别是第n、n-1次测量时的位置坐标，ΔP_n为第n次测量相比第n-1次测量的位移量，P_n-1＝[x_n-1,y_n-1,z_n-1]，ΔP_n＝[Δx_n,Δy_n,Δz_n]。

优选地，其中第n次测量相比第n-1次测量的位移量ΔP_n根据下式计算：

[s,Δx_n,Δy_n,Δz_n]＝q*[0,Δx_n-1,Δy_n-1,Δz_n-1]*q^-1

式中，s为四元数的实数部分，q^-1为q的共轭，q、q^-1表达式分别为：

式中，ψ为关节实际角度，γ＝[γ_η,γ_μ,γ_ν]为所述角度测量单元转动围绕的旋转轴的方向向量。

优选地，其中关节实际角度ψ是根据当前次测量得到的关节角度值计算得到：

式中，θ为横向角度，为纵向角度，θ、/>均为根据当前次测量得到的关节角度值。

优选地，所述角度测量单元转动围绕的旋转轴的方向向量γ＝[γ_η,γ_μ,γ_ν]是根据当前次测量得到的关节角度值计算得到：

式中，θ为横向角度，为纵向角度，θ、/>均为根据当前次测量得到的关节角度值。

与现有技术相比，本发明的有益效果在于：本发明提出的管道状连续机器人的形状测量装置和方法，依靠光纤的精确传感能力和高灵敏度，可以改善传统连续型机器人姿态估计误差的问题，是一种适合连续型机器人的连续性测量方法，同时本发明提出的结构可应用较少的光纤光栅，采集数据处理较简单，成本低易制作。本发明具有高精度、结构简单、体积小等优点，非常适合在特种机器人领域、医疗机器人领域、遥操作机器人领域推广。

附图说明

图1a和图1b是现有技术的驱动绳末端测量法示意图；

图2是现有技术的外部视觉测量方法；

图3a是本发明优选实施例的基于光纤光栅的管道状连续机器人的形状测量装置示意图；

图3b是图3a中角度测量单元的结构示意图；

图3c是图3b的角度测量单元在管道状连续机器人的管道内的两个状态示意图；

图4是本发明优选实施例的光纤光栅角度测量示意图；

图5是本发明优选实施例的测量过程示意图。

具体实施方式

以下对本发明的实施方式作详细说明。应该强调的是，下述说明仅仅是示例性的，而不是为了限制本发明的范围及其应用。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路/信号连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

如图3a至图3c所示，本发明优选实施例公开的管道状连续机器人的形状测量装置包括长度测量装置10、钢丝软轴20和基于光纤光栅的角度测量关节30，长度测量装置10连接在钢丝软轴20的第一端，角度测量关节30包括前关节31、后关节32和三根光纤光栅33，后关节32的后端连接在钢丝软轴20的第二端，三根光纤光栅33并列连接在后关节32的前端和前关节31的后端之间，通过光纤光栅33可以测量当前点的曲率和方向。

钢丝软轴20不可压缩、拉伸与扭转，并传递角度测量关节30的位置，长度测量装置10则测量当前钢丝软轴20的伸出长度。可用手动或自动将角度测量关节30从管道状连续机器人的管道50中伸入，随角度测量关节30的进入，该点弯曲曲率和弯曲方向被测量，结合进入长度可获得管道50的形状。

其中，如图3c，管道50为连续机器人内部的管道或内置的柔性软管，测量时须保证角度测量关节30的前关节31和后关节32卡住管道50的内壁，以保证其角度即为管道50的当前点角度(或者曲率)。因此，在本实施中，结合图5，该形状测量装置还包括多个卡环40，多个卡环40沿着钢丝软轴20的轴向方向排列以使得钢丝软轴20能够卡合设置在管道50内。另外，卡环40处可设置轮、润滑剂等，以保证在管道50中通过时摩擦较小且能卡住管道50。

具体地，结合图4，角度测量单元还包括镍钛合金丝34，三根光纤光栅33分别间隔120°并列粘贴于镍钛合金丝34的外圆周上，镍钛合金丝34连接在后关节32的前端和前关节31的后端之间。其中，镍钛合金34可以超弹性弯曲，并且不会传递扭转，保证前关节31和后关节32较好地卡在待测量的管道50内部。

本发明优选实施例提出的管道状连续机器人的形状测量装置，通过内置传感器，没有视场限制，在机器人操作过程中也可进行测量，适合作为机器人实际使用中的传感器；而且所用光纤光栅较少，采集数据处理较简单，成本低易制作。

本发明的另一优选实施例还提供了一种管道状连续机器人的形状测量方法，采用上述形状测量装置进行测量，包括以下步骤：将形状测量装置伸入到管道状连续机器人的管道内进行测量，设定初始测量点后，每次测量后前进预设距离进行多次测量，根据形状测量装置的多次测量得到每次测量时的伸入长度和关节角度值，并根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标，以得到管道的形状。

如图4所示，假设镍钛合金丝根部为原点P(坐标系为[η,μ,ν])，通过将形状测量装置伸入到管道内，可在每次测量时测得当前的横向角度与纵向角度θ，并根据下式可计算得到关节实际角度ψ：

此时，角度测量关节30指向的方向向量δ和转动围绕的旋转轴的方向向量γ可分别表示为：

下面通过四元数法表示测量点坐标与关节角度θ,ψ的关系。

当测量时，角度测量关节从管道(软管)中通过，每次测量后前进距离Δl，则第n次测量相比第n-1次测量的位移向量ΔP_n为：

ΔP_n＝[Δx_n,Δy_n,Δz_n]

并有：

[s,Δx_n,Δy_n,Δz_n]＝q*[0,Δx_n-1,Δy_n-1,Δz_n-1]*q^-1

其中，q^-1为q的共轭，

得到第n次测量的位置坐标P_n为：

P_n＝P_n-1+ΔP_n

＝[x_n-1+Δx_n,y_n-1+Δy_n,z_n-1+Δz_n]

通过上述多个公式即可根据测量形状测量装置的多次测量得到每次测量时的伸入长度和关节角度值，并根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标，以得到管道的形状。

进一步地，可以通过增大测量点n的数目同时减小测量间隔Δl可增加测量精度，以进一步增强对连续型机器人或管道形状的连续测量的精度。

本发明优选实施例提出的管道状连续机器人的形状测量方法，通过尽量少的光纤光栅即可实现对整个柔性机器人的连续型姿态测量，该形状测量方法不受传统测量方法中(传统方法是每隔一段距离测量该点坐标后拟合出整体形状，拟合精度会造成测量误差)测量点间隔的限制；而且可进行任意长度的管道测量。

本发明的背景部分可以包含关于本发明的问题或环境的背景信息，而不是由其他人描述现有技术。因此，在背景技术部分中包含的内容并不是申请人对现有技术的承认。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (6)

1.一种管道状连续机器人的形状测量方法，其特征在于，采用形状测量装置进行测量，所述形状测量装置包括长度测量单元、软轴单元和角度测量单元，其中所述长度测量单元连接在所述软轴单元的第一端，所述角度测量单元包括前关节、后关节和多根光纤光栅，所述后关节的后端连接在所述软轴单元的第二端，多根所述光纤光栅并列连接在所述后关节的前端与所述前关节的后端之间；

所述形状测量方法包括以下步骤：

将所述形状测量装置伸入到所述管道状连续机器人的管道内进行测量，设定初始测量点后，每次测量后前进预设距离进行多次测量，根据所述形状测量装置的多次测量得到每次测量时的伸入长度和关节角度值，再根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标，以得到所述管道的形状；

其中，根据每次测量时的伸入长度和关节角度值计算得到每次测量时的位置坐标包括：根据当前次测量时的伸入长度和关节角度值以及计算得到的前一次测量时的位置坐标，来计算得到当前次测量时的位置坐标具体采用下式：

P_n＝P_n-1+ΔP_n＝[x_n-1+Δx_n,y_n-1+Δy_n,z_n-1+Δz_n]

式中，P_n、P_n-1分别是第n、n-1次测量时的位置坐标，ΔP_n为第n次测量相比第n-1次测量的位移量，P_n-1＝[x_n-1,y_n-1,z_n-1]，ΔP_n＝[Δx_n,Δy_n,Δz_n]；

其中第n次测量相比第n-1次测量的位移量ΔP_n根据下式计算：

[s,Δx_n,Δy_n,Δz_n]＝q*[0,Δx_n-1,Δy_n-1,Δz_n-1]*q^-1

式中，s为四元数的实数部分，q^-1为q的共轭，q、q^-1表达式分别为：

式中，ψ为关节实际角度，γ＝[γ_η,γ_μ,γ_ν]为所述角度测量单元转动围绕的旋转轴的方向向量。

2.根据权利要求1所述的形状测量方法，其特征在于，所述角度测量单元还包括镍钛合金丝，多根所述光纤光栅沿着所述镍钛合金丝的轴向并列连接在所述镍钛合金丝的外圆周上，所述镍钛合金丝连接在所述后关节的前端与所述前关节的后端之间。

3.根据权利要求1所述的形状测量方法，其特征在于，所述光纤光栅的数量为三根，三根所述光纤光栅分别连接在所述后关节的前端与所述前关节的后端之间沿圆周方向的三等分位置处。

4.根据权利要求1所述的形状测量方法，其特征在于，所述形状测量装置还包括多个卡环，多个所述卡环沿着所述软轴单元的轴向方向上排列以使得所述软轴单元能够卡合设置在待测的管道状连续机器人的管道内。

5.根据权利要求1所述的形状测量方法，其特征在于，其中关节实际角度ψ是根据当前次测量得到的关节角度值计算得到：

式中，θ为横向角度，为纵向角度，θ、/>均为根据当前次测量得到的关节角度值。

6.根据权利要求1所述的形状测量方法，其特征在于，所述角度测量单元转动围绕的旋转轴的方向向量γ＝[γ_η,γ_μ,γ_ν]是根据当前次测量得到的关节角度值计算得到：

式中，θ为横向角度，为纵向角度，θ、/>均为根据当前次测量得到的关节角度值。