基于光纤光栅的连续体机器人多接触点力感知装置及方法

本发明涉及绳机器人力感知，具体地，涉及一种基于光纤光栅的连续体机器人多接触点力感知装置及方法。

背景技术：

1、连续体机器人因其灵巧性和灵活性而在微创手术中引起了越来越多的关注，它的柔顺骨架可以自然地顺应环境障碍，使机器人能够通过曲折的路径到达人体腔道的深处。为了最大程度地减少机器人与人体腔道内侧摩擦和组织损伤，需要对机器人与环境的交互信息进行实时感知，以实现安全的体内导航。交互信息感知可以改善人机交互，以确保安全和直观的操作。增强连续体机器人的力感应对于提供详细的接触力反馈和安全操作至关重要，特别是对于被精细解剖结构包围的远端病变。然而，如何在保证机器人小尺度的情况下，实现对机器人的多接触点力感知，一直以来是研究的难题。

2、现有公告号为cn112917468b的中国专利申请文献，其公开了一种绳驱柔性机械臂的末端力感知方法及装置，其中的方法包括建立机械臂的杆件坐标系；建立末段臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力；建立倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算此时的末端作用力；建立倒数多段臂杆的动力学平衡方程与力矩平衡方程，迭代计算末端作用力；判断前后两个计算的末端作用力误差是否低于阈值，或者下一迭代次数是否大于臂杆数，如果是，则停止迭代。

3、现有技术中的绳驱柔性机械臂需要建立复杂的动力学平衡方程与力矩平衡方程，驱动丝和腔道的摩擦力需要通过模型来表征，过多理想模型的建立难以保证力估计的准确性，且该方法只能估计机器人末端的力，无法估计机器人其它位置的受力情况。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于光纤光栅的连续体机器人多接触点力感知装置及方法。

2、根据本发明提供的一种基于光纤光栅的连续体机器人多接触点力感知装置,包括多个依次连接的单元体，任一所述单元体均包括刚性段和可弯曲段；所述单元体的刚性段安装有定位块，所述定位块的中部形成有中心腔道，所述定位块的周侧形成有驱动腔道，所述驱动腔道沿定位块的周向间隔形成有多个；所述中心腔道内配置一根具有多个fbg传感器的多芯光纤，任一所述驱动腔道内均配置一根刻有多个fbg传感器的单芯光纤。

3、优选地，所述单元体为具有对称的切槽结构的镍钛管，所述单元体包括两个刚性段和两个可弯曲段，两个所述可弯曲段均设置在两个刚性段之间，且两个所述可弯曲段呈相对设置。

4、优选地，在单元体可弯曲段的横截面中，可弯曲段的梁与相邻的驱动腔道之间的夹角为45°。

5、优选地，所述定位块呈圆形片状，所述定位块同轴安装在单元体的刚性段内，所述驱动腔道由定位块和单元体的刚性段内壁配合形成，且所述驱动腔道沿定位块的轴向等间隔形成有四个。

6、优选地，所述多芯光纤的末端固定在机器人的末端，任一所述单芯光纤的末端均固定在机器人的末端。

7、根据本发明提供的一种基于光纤光栅的连续体机器人多接触点力感知方法,感知方法包括如下步骤：

8、步骤s1、根据多芯光纤上的fbg传感器的测量值和单芯光纤上的fbg传感器的测量值分别计算每个单元体上的每一根驱动丝的张力和弯曲曲率；

9、步骤s2、对步骤s1中计算得到的弯曲曲率进行插值，重建机器人的三维形状，并获得每个单元体的位姿；

10、步骤s3、根据弯曲曲率和三维形状初步估计接触点的个数和位置；

11、步骤s4、利用梁理论模型根据受力点个数、三维形状、单元体弯曲曲率以及单元体驱动丝张力构建单元体的力矩平衡方程；

12、和/或，利用梁理论模型根据受力点个数、三维形状、单元体弯曲曲率以及单元体驱动丝张力构建单元体的力矩平衡方程，若受到的外力个数大于一个，判断两个相邻力之间的距离，如果二者的距离大于单个单元体的长度，则利用二者间的单元体构建力矩平衡方程，提供额外约束方程；

13、步骤s5、根据步骤s4构建的方程求解接触点的位置和力的大小。

14、优选地，包括接触估计：

15、机器人第j个单元体相对于其自身局部坐标系{sj}沿y和z方向的曲率与弯矩的关系为：

16、

17、其中，ymj(sj)(j＝1,2,…,n)和zmj(sj)为弯矩，yκj和zκj为曲率，yeij和zeij为弯曲刚度，n为连续体机器人的单元体个数；

18、机器人基坐标系{r}和形状传感器基坐标系{s}位于机器人的根部，第j个单元体的局部坐标系位于该单元体的根部，{s1}表示第1个单元体的局部坐标系，第j个单元体相对于{sj}的驱动丝拉力和弯矩间的关系为：

19、

20、其中，和分别是第j个单元体沿y和z方向的力矩，由驱动丝拉力产生；tij(i＝1,2,3,4)是第j个单元体中第i根单芯光纤的拉力；r是机器人中心轴到驱动腔道的距离；

21、如果有一个外力作用在机器人本体上，则接触点的力f及其位置p可以从{r}变换到{sj}；

22、

23、

24、其中，f(sj)和p(sj)分别是接触力相对于{sj}的力和位置；和分别为{sj}相对于{s}的旋转矩阵和位置向量；是{s}相对于{r}的旋转矩阵；第j个单元体在外力作用下产生相对于{sj}的弯矩为：

25、

26、因此，第j个单元体沿{sj}的y方向总力矩ymj(sj)和z方向总力矩zmj(sj)为：

27、

28、其中，和是在y和z方向上的值，二者可以通过式(1)、(2)和(6)求得；如果三个方向的值都为0，则意味着没任何外力作用于机器人本体；如果连续体机器人的接触点个数大于1，则根据式(6)可得机器人根部到近端第一个接触点之间所有单元体的力矩为：

29、

30、其中，q是接触点的个数，和分别第j个单元在第k个外力作用下产生的沿y和z方向的力矩；

31、如果对机器人本体某一位置施加一个外力，则力施加位置前后的曲率变化是不同的；接触点的数量及其相关位置可事先估计，如下：

32、利用插值技术计算机器人本体上连续的曲率和驱动丝张力，沿着机器人身体长度取m个等距离的点，并为每个点设置一个局部坐标系，第i个点的局部坐标系为{csi}，则第i个点沿{csi}的y和z方向的力矩分别为和计算每两个相邻点之间的力矩差由于力矩与力和距离呈线性关系，因此和的变化可用于估计外力的数量及其相关位置；

33、使用bishop算法框架估计机器人的三维位姿，当机器人的形状重建完成后，使用三个二维的三次样条分别拟合机器人在xoy、xoz和yoz平面上的形状，由于外力对机器人的作用点在机器人本体上，因此这三个二维三次样条函数可以用来约束所估计的接触点位置，使优化过程更加鲁棒，如下所示：

34、

35、其中，p＝[x,y,z]为外力的位置，tij(i＝1,2,3；j＝1,2,3,4)为三次样条函数的拟合参数。

36、优选地，包括驱动丝拉力计算：

37、以机器人的末端为参考，逐渐往根部推导求得：

38、

39、nij＝m-round(lij/df)                  (10)

40、其中，lij是第i根单芯光纤从机器人末端到第j个单元体的长度，p是两个相邻单元体间离散点的数量，m是每根单芯光纤的fbg传感器的数量，df是单芯光纤两个相邻fbg传感器之间的距离，nij是第i根单芯光纤的fbg传感器的序列号，最后，将序列号为nij的fbg传感器分配给第j个单元体以计算该单元驱动丝的张力；当一根光纤被拉动时，第j个单元体的第i根单芯光纤的拉力tij为：

41、tij＝εijefaf                        (11)

42、其中，εij是第nij个fbg传感器的应变；ef和af分别是光纤的杨氏模量和横截面积；如果温度变化可以忽略不计，那么tij可以计算为：

43、

44、其中，λ(nij)和λ0(nij)分别是光纤的第nij个fbg传感器的波长和初始波长，sε为光纤应变敏感系数。

45、优选地，包括标定方法：标定是通过在连续体机器人上施加一个已知的三维力来完成，机器人的刚度可以通过式(1)和(6)来标定，如下式：

46、

47、其中，和依赖于四根单芯光纤的波长读数，yκj和zκj依赖于mcf的波长读数，和可根据已知的三维力求得，单芯光纤的参数标定过程主要是在光纤末端固定不同质量的砝码，并将光纤fbg传感器的波长变化转化为拉力，如果在标定过程中温度变化可以忽略不计，则光纤的参数为：

48、

49、其中,t是砝码的重力,δλ和λ0为fbg传感器的波长变化量和初始波长。

50、优选地，包括外力计算：根据式(7)，3q个的单元体可以建立6q个方程，每两个相邻受力点力之间的单元体用来构造求解方程，根据机器人的曲率，可以估计外力的接触位置，如果两个相邻外力沿连续体机器人本体的距离大于ζ，则这两个相邻力之间的单元体用于构造方程(7)，如果连续体机器人受到q个外力，力求解的优化目标函数可写为：

51、

52、其中，

53、

54、

55、1g(k)＝t11xk3+t12xk2+t13xk+t14-yk

56、2g(k)＝t21xk3+t22xk2+t23xk+t24-zk

57、3g(k)＝t31yk3+t32yk2+t33yk+t34-zk

58、

59、

60、h是从机器人根部到最近的第一接触点的单元体总数，如果h大于3q，则h设置为3q；如果任意两个相邻接触点之间的距离大于ζ，则ψ包括两个相邻接触点之间的所有单元体；u表示从连续体机器人的根部到末端的第u外力；pk＝[xk,yk,zk]t为第k个外力相对于{r}的位置；fk是相对于{r}的第k个外力；η1,η2,η3为权重系数；和为第i个单元体受第k个外力作用产生的沿y和z方向的力矩；优化过程的约束条件为外力和接触位置的最小值和最大值，记为pmin<pk<pmax，fmin<fk<fmax；pmin和pmax分别为接触位置的最小值和最大值；fmin和fmax分别为外力的最小值和最大值；最后可以利用优化算法来求解上述优化问题，计算出和在优化求解过程中，将利用曲率计算得到的接触位置将作为的初始值。

61、与现有技术相比，本发明具有如下的有益效果：

62、1、本发明通过单芯光纤上fbg传感器波长的变化情况实时重建沿整个驱动丝的张力分布，建立考虑分段差异、多驱动丝和外力相互作用的基于梁理论的力学模型，同时对连续体机器人上多个接触力的位置和大小进行感知，利用fbg传感器重建沿整个驱动丝的张力，无需对驱动丝和驱动通道之间的摩擦进行显式建模，克服了因建模所带来的误差和计算资源耗费过大的问题，该方法能够同时估计连续体机器人本体上多个接触点和接触力大小，为手术机器人的安全介入提供了极大的保障。

63、2、本发明在保证机器人小尺度的情况下，实现了具有空间主动弯曲能力的连续体机器人多接触点的力感知，简化了力估计过程并进一步增加估计的准确性。