基于柔性圆柱式电容传感器的伸缩式连续体机械臂本体感知方法

本发明涉及软体机械臂，具体而言，涉及一种基于柔性圆柱式电容传感器的伸缩式连续体机械臂本体感知方法。

背景技术：

1、柔性连续体机械臂具有可变形性和高灵活性的特点，其在工业、微创手术、航天等领域的应用越来广泛。柔性连续体机械臂的本体通常采用硅胶、橡胶等柔性材料制成，或者直接使用弹簧。应用柔性连续体机械臂时，将末端执行器安装在柔性连续体机械臂的前端，通过控制柔性连续体机械臂的弯曲变形来调整末端执行器在空间中的位置和姿态。为了更准确地使末端执行器达到预期的位置和姿态，采用闭环控制方式实现对柔性连续体机械臂的精确控制，反馈信号对闭环控制方式至关重要，目前主要通过视觉系统提供反馈信号，视觉系统的相机采集柔性连续体机械臂的图像，图像经过控制器处理进而识别出柔性连续体机械臂的形状、姿态。

2、然而，视觉系统的相机设置受到空间的限制，尤其是在飞机发动机维修、微创手术、灾后救援等非结构化、封闭的环境中，无法设置相机，不能进行图像采集。此外，图像处理的计算过程太复杂，相机的硬件成本还是比较高的。

技术实现思路

1、本发明就是为了解决现有技术通过相机采集机械臂的图像并经过图像处理识别柔性连续体机械臂的形状、姿态过程中，相机设置受到空间的限制，硬件成本较高，图像处理计算过程复杂的技术问题，提供一种不受空间限制，成本较低的基于柔性圆柱式电容传感器的伸缩式连续体机械臂本体感知方法。

2、本发明提供一种基于柔性圆柱式电容传感器的伸缩式连续体机械臂本体感知方法，包括以下步骤：

3、确定螺旋本体左旋直立状态的参数化方程为：

4、

5、参数化方程中，rse表示圆柱式电容传感器与螺旋本体之间的距离，θh表示螺旋极角，dh表示螺距，nh表示匝数；

6、根据以下公式(1)计算出圆柱式电容传感器本身的长度

7、

8、公式(1)中，表示圆柱式电容传感器在直线状态下的初始长度，表示圆柱式电容传感器的初始电容值，k表示圆柱式电容传感器的电容值和长度的线性关系系数；

9、接下来通过以下公式(2)计算长度lij：

10、

11、公式(2)中，rse表示圆柱式电容传感器与螺旋本体之间的距离，αi表示螺旋本体的每个单元绕螺旋本体自身中心线的扭转角度，αi的计算公式中k2表示扭转角度和长度之间的线性关系系数；

12、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元的曲率半径ρi：

13、

14、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元绕z轴的旋转角度

15、

16、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元绕y轴的旋转角度θi：

17、(0<θi≤π)

18、接下来，经平移与y，z轴的旋转变换可得：

19、

20、trans(ρi(cosθim-1)，0，ρisinθim-zs)rs

21、其中，

22、而柔性连续体机械臂分为下、中、上三个单元，相邻单元的坐标变换矩阵

23、

24、坐标变换矩阵中，p＝[ρi(1-cosθi)0ρisinθi]；

25、柔性连续体机械臂的中单元、上单元在全局坐标系的参数化方程是：

26、

27、

28、接下来，对整个柔性连续体机械臂建模。

29、优选地，对整个柔性连续体机械臂建模的过程是：将旋转角度θi分成大小相等的若干份，然后采用d-h齐次变化矩阵方法对整个柔性连续体机械臂建模。

30、本发明还提供一种伸缩式柔性连续体机械臂，包括螺旋本体、三根驱动线和三根圆柱式电容传感器，三根驱动线分别穿过螺旋本体的外围，三根驱动线在径向圆周方向上均布；

31、圆柱式电容传感器由平面介电弹性体传感器卷制而成，平面介电弹性体传感器包括介电层、两个电极层和两个保护层，平面介电弹性体传感器被卷成圆柱体状后两端用铜套固定；

32、三根圆柱式电容传感器分别与螺旋本体的外围连接，三根圆柱式电容传感器在径向圆周方向上均布，每根圆柱式电容传感器的上端、下端分别固定在螺旋本体上；当螺旋本体弯曲、收缩或伸展时，每根圆柱式电容传感器的中部能够沿着螺旋本体的外围滑动。

33、优选地，圆柱式电容传感器的上端、下端分别通过固定块固定在螺旋本体上，螺旋本体的边缘设有嵌入槽，嵌入槽设有半圆形凹槽，固定块的内侧设有半圆形凹槽，圆柱式电容传感器上端的铜套位于螺旋本体边缘的一个嵌入槽的半圆形凹槽中，圆柱式电容传感器下端的铜套位于螺旋本体边缘的一个嵌入槽的半圆形凹槽中，用一个固定块放入圆柱式电容传感器上端所在的嵌入槽中并固定，再用一个固定块放入圆柱式电容传感器下端所在的嵌入槽中并固定；圆柱式电容传感器的中部位于螺旋本体边缘的两个嵌入槽中，该两个嵌入槽分别与相应的固定块配合固定连接，该两个嵌入槽与相应固定块配合形成两个圆孔，圆柱式电容传感器的中部穿过两个圆孔并可以滑动。

34、优选地，螺旋本体设有上、中、下三个单元，上、中、下三个单元分别连接有三根圆柱式电容传感器。

35、本发明还提供一种坐标变换方法，包括以下步骤：

36、确定螺旋本体左旋直立状态的参数化方程为：

37、

38、参数化方程中，rse表示圆柱式电容传感器与螺旋本体之间的距离，θh表示螺旋极角，dh表示螺距，nh表示匝数；

39、根据以下公式(1)计算出圆柱式电容传感器本身的长度

40、

41、公式(1)中，表示圆柱式电容传感器在直线状态下的初始长度，表示圆柱式电容传感器的初始电容值，k表示圆柱式电容传感器的电容值和长度的线性关系系数；

42、接下来通过以下公式(2)计算长度lij：

43、

44、公式(2)中，rse表示圆柱式电容传感器与螺旋本体之间的距离，αi表示螺旋本体的每个单元绕螺旋本体自身中心线的扭转角度，αi的计算公式中k2表示扭转角度和长度之间的线性关系系数；

45、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元的曲率半径ρi：

46、

47、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元绕z轴的旋转角度

48、

49、通过以下公式计算出柔性连续体机械臂的下、中、上三个单元绕y轴的旋转角度θi：

50、(0<θi≤π)

51、接下来，经平移与y，z轴的旋转变换可得：

52、

53、trans(ρi(cosθim-1)，0，ρisinθim-zs)rs

54、其中，

55、而柔性连续体机械臂分为下、中、上三个单元，相邻单元的坐标变换矩阵

56、

57、坐标变换矩阵中，p＝[ρi(1-cosθi)0ρisinθi]。

58、本发明还提供一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时，实现上述方法的各个步骤。

59、本发明还提供一种装置，包括处理器和存储介质，存储器用于存储程序；处理器用于执行程序，进而实现上述方法的各个步骤。

60、本发明的有益效果是：设置圆柱式电容传感器不受应用场景的空间限制，集成在机械臂本体内，不通过摄像头等外部传感器来感知，不会损坏本体的自然柔韧性。

61、建模方法具有优异的位置精度，可以实现实时形状重建，为可伸缩式连续体机械臂的智能控制提供有力支持。将可伸缩式连续统机械臂分割为三个单元，并采用恒定曲率模型来表征每个细分的形状，实现了准确的臂形检测和高精度的端点定位。

62、本发明进一步的特征和方面，将在以下参考附图的具体实施方式的描述中，得以清楚地记载。