一种用于多节段可单独旋转连续体机器人的控制方法

1.本发明涉及柔性机器人控制技术领域，尤其涉及一种用于多节段可单独旋转连续体机器人的控制方法。


背景技术：

2.随着医疗技术的不断发展，外科手术正从传统多孔腹腔镜手术向经脐单孔腹腔镜手术乃至经自然腔道手术notes演变。notes术式通过口腔、尿道、阴道等人体自然腔道入路，将手术器械送入目标手术地点实施操作。对于妇科疾病而言，经阴道进行手术比其他入路更为适用。经阴道自然腔道内镜手术vnotes把经阴道手术与内镜技术相结合，为妇科医生提供了全新的手术方式，具有创伤小、无疤痕等优点。
3.目前常见的经自然腔道手术器械主要包括两类：一是纯柔性的导管和其他细长医疗装置，其优点在于柔性的结构能够将手术器械通过复杂的弯曲腔道送到病患组织处，但由于缺乏刚度，手术器械只能进行简单的操作，缺乏操作精确性和活动自由度，不适合剥离、缝合等复杂手术任务；另一类纯刚性器械有足够的刚度保证复杂手术操作的执行，但其器械活动空间局限，操作非常困难。进行经阴道自然腔道内镜手术时，手术器械从阴道进入，而手术操作区域在器械后部盆腔处，需要手术器械回转一定角度进行操作，且往往需要进行组织的分离等精细操作，因此上述纯刚性与纯柔性的器械都不适合于该类手术的需求。
4.多节段相互嵌套的连续体机器人具有多个相互嵌套的节段，每个节段可以单独弯曲和旋转，通过不同节段的弯曲旋转运动可以实现回转角度下进行复杂的手术操作。然而，该结构中嵌套在内层的节段的一部分需要遵从外层节段的曲率，因为这种曲率遵从性而带来的多节段耦合效应会让机器人的各个节段运动控制之间相互影响，使末端手术执行机构的运动造成误差，甚至对患者其他组织造成伤害，需要进行解耦。为解决上述问题，设计开发一种用于这类适合vnotes的多节段可单独旋转连续体机器人的控制方法，是非常有必要的。


技术实现要素：

5.有鉴于此，本发明提出了一种充分考虑手术机器人段间的耦合、通过分级驱动补偿进行解耦的、用于多节段可单独旋转连续体机器人的控制方法。
6.本发明的技术方案是这样实现的：本发明提供了一种用于多节段可单独旋转连续体机器人的控制方法，包括：
7.s1：建立连续体机器人的正运动学模型；
8.s2：根据连续体机器人的末端位姿求解构型参数；
9.s3：求解各节段驱动丝长度变化量；
10.s4：对结构耦合进行参数补偿；
11.s5：对驱动耦合进行参数补偿；
12.s6：得到驱动变量齿轮的目标位置。
13.在以上技术方案的基础上，优选的，所述连续体机器人包括两个可弯曲节段和末端执行器，第一个可弯曲节段为回转段，用于实现大角度的回转，使第二个可弯曲节段和末端执行器保持朝向病患方位；第二个可弯曲节段为运动段，用于在回转段定位的基础上，使末端执行器直接接触各个病患点，以便执行vnotes手术；各节段内分别设置有驱动丝；各节段的弯曲运动是由电机驱动齿轮带动丝杆传动机构将齿轮的转动转化为驱动丝的直线运动；各节段的旋转运动是电机驱动齿轮后直接通过齿轮啮合带动。
14.优选的，步骤s1所述建立连续体机器人的正运动学模型，是分别对连续体机器人的基准位置、回转段、回转段与运动段之间的刚性连接部分、运动段、运动段与末端执行器之间的刚性连接部分和末端执行器分别构建坐标系o0x0y0z0、o1x1y1z1、o2x2y2z2、o3x3y3z3、o4x4y4z4以及o5x5y5z5；令为第i+1个坐标系o
i+1
x
i+1yi+1zi+1
相对于第i个坐标系oixiyizi的变换矩阵，描述的是回转段相对于连续体机器人的基准位置变化；描述的是回转段与运动段之间的刚性连接部分相对于回转段的变换矩阵，描述的是运动段相对于回转段与运动段之间的刚性连接部分的变换矩阵，描述的是运动段与末端执行器之间的刚性连接部分与运动段的变换矩阵，描述的是末端执行器相对于运动段与末端执行器之间的刚性连接部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为示为
[0015][0016][0017]
上述表达式中的s表示正弦函数sin，c表示余弦函数cos，θ1表示回转段的弯曲角度；表示回转段绕坐标系o0x0y0z0的z0的旋转角度；l1为回转段的长度；lu1为回转段与运动段之间的刚性连接部分的长度；θ2表示运动段的弯曲角度；表示运动段绕坐标系o2x2y2z2的z2的旋转角度；l2为运动段的长度；lu2为运动段与末端执行器之间的刚性连接部分的长度；表示末端执行器绕坐标系o4x4y4z4的z4的旋转角度；l3为末端执行
器的长度。
[0018]
进一步优选的，步骤s2所述根据连续体机器人的末端位姿求解构型参数是令参数l1、l2、l3、lu1和lu2为已知；令参数θ1、θ2、和为未知的待求解的构型参数；将末端执行器相对于连续体机器人的基准位置的位姿t改写为如下齐次变换矩阵：其中p
x
、py和pz表示坐标系o5x5y5z5的原点在坐标系o0x0y0z0中的位置；n
x
、ny和nz表示坐标系o5x5y5z5的x5轴在坐标系o0x0y0z0中的单位矢量；a
x
、ay和az表示坐标系o5x5y5z5的y5轴在坐标系o0x0y0z0中的单位矢量；b
x
、by和bz表示坐标系o5x5y5z5的z5轴在坐标系o0x0y0z0中的单位矢量；对齐次变换矩阵求逆解，即可得到连续体机器人的末端位姿下的构型参数θ1、θ2、和
[0019]
进一步优选的，步骤s3所述求解各节段驱动丝长度变化量，令d1是回转段中驱动丝距离节段横截面圆心的偏移量；则在回转段中，驱动丝长度变化量δl
10
＝d1θ1；在运动段中，令d2为运动段中驱动丝距离节段横截面圆心的偏移量，驱动丝长度变化量δl
20
＝d2θ2。
[0020]
更进一步优选的，步骤s4所述对结构耦合进行参数补偿，是对于运动段其有一部分长度处于回转段内，回转段处于弯曲状态时，运动段的驱动丝在该部分的长度会发生相应变化，需要对该类型的耦合进行解耦，将变化量补偿在驱动丝的长度中，对结构耦合进行参数补偿后的驱动丝长度变化量为：参数补偿后的驱动丝长度变化量为：
[0021]
再进一步优选的，步骤s5所述对驱动耦合进行参数补偿，是为了消除驱动丝的直线运动张力导致对应节段的弯曲。当对应旋转的齿轮顺时针旋转，而对应弯曲的齿轮保持不动时，对于丝杆传动机构而言，相当于弯曲运动的齿轮发生了逆时针旋转驱动耦合影响，让对应弯曲的齿轮跟随对应旋转的齿轮同时运动进行解耦，对其进行参数补偿如下：其中k为丝杆传动机构的传动比。
[0022]
更进一步的优选的，步骤s6所述得到驱动变量齿轮的目标位置，是对于回转段，对应旋转的齿轮运动直接导致节段旋转；对于运动段，当回转段对应旋转的齿轮顺时针运动，而运动段对应旋转的齿轮静止时，相当于运动段对应旋转的齿轮逆时针运动。将以上弯曲运动与旋转运动的耦合效应逐一补偿后，得到一组驱动变量：上述驱动变量对应了一组齿轮的目标位置。
[0023]
本发明提供的一种用于多节段可单独旋转连续体机器人的控制方法，相对于现有技术，具有以下有益效果：
[0024]
(1)本方法通过分级驱动补偿进行解耦，实现了各节段的独立控制，通过多个驱动单元的协同配合以抵消其耦合效应；
[0025]
(2)目前常见的方案只能通过外界昂贵的测量设备或传感器对连续体机器人的末
的原点位置位于运动段远离末端执行器的端部中心；坐标系o4x4y4z4的原点位置位于运动段靠近末端执行器的端部中心；坐标系o5x5y5z5的原点位置位于末端执行器远离运动段的端部中心。
[0036]
令为第i+1个坐标系o
i+1
x
i+1yi+1zi+1
相对于第i个坐标系oixiyizi的变换矩阵，描述的是回转段相对于连续体机器人的基准位置变化；描述的是回转段与运动段之间的刚性连接部分相对于回转段的变换矩阵，描述的是运动段相对于回转段与运动段之间的刚性连接部分的变换矩阵，描述的是运动段与末端执行器之间的刚性连接部分与运动段的变换矩阵，描述的是末端执行器相对于运动段与末端执行器之间的刚性连接部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为
[0037][0038][0039][0040][0041]
上述表达式中的s表示正弦函数sin的简写，c表示余弦函数cos的简写，θ1表示回转段的弯曲角度；表示回转段绕坐标系o0x0y0z0的z0的旋转角度；l1为回转段的长度，即朝向运动段方向的轴向长度；lu1为回转段与运动段之间的刚性连接部分的长度；θ2表示运动段的弯曲角度；表示运动段绕坐标系o2x2y2z2的z2的旋转角度；l2为运动段的长度；lu2为运动段与末端执行器之间的刚性连接部分的长度；表示末端执行器绕坐标系o4x4y4z4的z4的旋转角度；l3为末端执行器的长度。
[0042]
s2：根据连续体机器人的末端位姿求解构型参数。
[0043]
此处根据连续体机器人的末端位姿求解构型参数的具体内容是，令参数l1、l2、l3、lu1和lu2为已知；令参数θ1、θ2、和为未知的待求解的构型参数；将末端执行器相
对于连续体机器人的基准位置的位姿t改写为如下齐次变换矩阵：其中p
x
、py和pz表示坐标系o5x5y5z5的原点在坐标系o0x0y0z0中的位置；n
x
、ny和nz表示坐标系o5x5y5z5的x5轴在坐标系o0x0y0z0中的单位矢量；a
x
、ay和az表示坐标系o5x5y5z5的y5轴在坐标系o0x0y0z0中的单位矢量；b
x
、by和bz表示坐标系o5x5y5z5的z5轴在坐标系o0x0y0z0中的单位矢量；对齐次变换矩阵求逆解，即可得到连续体机器人的末端位姿下的构型参数θ1、θ2、和
[0044]
s3：求解各节段驱动丝长度变化量。
[0045]
进一步优选的，步骤s3所述求解各节段驱动丝长度变化量，是令回转段内部的两根驱动丝的长度为：其中，在回转段中两根驱动丝的长度分别为l
1a
和l
1b
，令d1是回转段中驱动丝距离节段横截面圆心的偏移量；联立上面两等式可知在回转段中，驱动丝长度变化量δl
10
＝d1θ1。
[0046]
在运动段中，令其内部的两根驱动丝的长度为其中在运动段中的两根驱动丝的总长度分别为l
2a
和l
2b
，d2为运动段中驱动丝距离节段横截面圆心的偏移量，联立上面两等式可知在运动段中，驱动丝长度变化量δl
20
＝d2θ2。
[0047]
综上所述，在忽略耦合效应下，各节段的驱动丝长度变化量为：
[0048]
δl
10
＝d1θ1；δl
20
＝d2θ2。
[0049]
s4：对结构耦合进行参数补偿。
[0050]
对结构耦合进行参数补偿，是对于运动段其有一部分长度处于回转段内，驱动丝在该部分长度的曲率受回转段影响，需要对该类型的耦合进行解耦，将变化量补偿在驱动丝的长度中。
[0051]
如图4所示，运动段旋转
△
角度，运动段的驱动丝在该部分的长度会发生相应变化。将该类变化补偿于驱动丝长度内，运动段内部两根驱动丝的长度为：
[0052]
联立以上两式和步骤s3的结论，对结构耦合进行参数补偿后的驱动丝长度变化量为：δl
10
＝d1θ1；；
[0053]
s5：对驱动耦合进行参数补偿；
[0054]
本实施例中，各节段弯曲运动与旋转运动均由电机驱动齿轮传动，对应旋转的齿轮与其控制的对应节段是一体的，齿轮旋转直接导致对应节段旋转；对应弯曲的齿轮通过丝杆传动机构将齿轮旋转转化为驱动丝的直线运动，由驱动丝的直线运动张力导致对应节段的弯曲。当对应旋转的齿轮顺时针旋转，而对应弯曲的齿轮保持不动时，对于丝杆传动机构而言，相当于弯曲运动的齿轮发生了逆时针旋转；为了消除这种驱动耦合影响，应该让对应弯曲的齿轮跟随对应旋转的齿轮同时运动进行解耦，这样也会导致各节段驱动丝的变
化，对其进行参数补偿如下：
[0055]
其中k为丝杆传动机构的传动比。
[0056]
s6：得到驱动变量齿轮的目标位置。
[0057]
本实施例中，最终的驱动变量是一组齿轮的目标位置，为了实现最终对连续体机器人的控制，需要将补偿后的各节段驱动丝变化量转化为驱动变量。
[0058]
对于回转段，对应旋转的齿轮运动直接导致节段旋转；对于运动段，当回转段对应旋转的齿轮顺时针运动，而运动段对应旋转的齿轮静止时，相当于运动段对应旋转的齿轮逆时针运动。将以上弯曲运动与旋转运动的耦合效应逐一补偿后，得到一组驱动变量：
[0059]
得到一组驱动变量：上需要注意的是，上述得到的驱动变量是一组驱动单元位置的绝对值，并非每个驱动单元需要运动的量，即得到的是一组齿轮的目标位置，而非各个齿轮需要转动的量。驱动单元的运动量需要由该方法计算出的驱动参数与编码器的反馈值相减得到。
[0060]
本方案通过对连续体机器人的结构进行分析，在驱动参数的求解过程中对机械结构所导致的耦合进行补偿来实现解耦，使各节段之间可以独立控制，互不影响，实现在回转段固定时，运动段能够使执行器按照期望精准地达到目标位姿，避免手术中的误操作，提高了控制精度。
[0061]
通过对驱动结构进行分析，在驱动参数的求解过程中对驱动结构所导致的耦合进行补偿来实现解耦，使各个齿轮可以控制其对应的节段自由度而不对其他自由度产生非预期影响，提高了控制独立性与精度。
[0062]
以上所述仅为本发明的较佳实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，包括：s1：建立连续体机器人的正运动学模型；s2：根据连续体机器人的末端位姿求解构型参数；s3：求解各节段驱动丝长度变化量；s4：对结构耦合进行参数补偿；s5：对驱动耦合进行参数补偿；s6：得到驱动变量齿轮的目标位置。2.根据权利要求1所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，所述连续体机器人包括两个可弯曲节段和末端执行器，第一个可弯曲节段为回转段，用于实现大角度的回转，使第二个可弯曲节段和末端执行器保持朝向病患方位；第二个可弯曲节段为运动段，用于在回转段定位的基础上，使末端执行器直接接触各个病患点，以便执行vnotes手术；各节段内分别设置有驱动丝；各节段的弯曲运动是由电机驱动齿轮带动丝杆传动机构将齿轮的转动转化为驱动丝的直线运动；各节段的旋转运动是电机驱动齿轮后直接通过齿轮啮合带动。3.根据权利要求2所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s1所述建立连续体机器人的正运动学模型，是分别对连续体机器人的基准位置、回转段、回转段与运动段之间的刚性连接部分、运动段、运动段与末端执行器之间的刚性连接部分和末端执行器分别构建坐标系o0x0y0z0、o1x1y1z1、o2x2y2z2、o3x3y3z3、o4x4y4z4以及o5x5y5z5；令为第i+1个坐标系o
i+1
x
i+1
y
i+1
z
i+1
相对于第i个坐标系o
i
x
i
y
i
z
i
的变换矩阵，描述的是回转段相对于连续体机器人的基准位置变化；描述的是回转段与运动段之间的刚性连接部分相对于回转段的变换矩阵，描述的是运动段相对于回转段与运动段之间的刚性连接部分的变换矩阵，描述的是运动段与末端执行器之间的刚性连接部分与运动段的变换矩阵，描述的是末端执行器相对于运动段与末端执行器之间的刚性连接部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为部分的变换矩阵；则末端执行器相对于连续体机器人的基准位置的位姿t表示为
上述表达式中的s表示正弦函数sin，c表示余弦函数cos，θ1表示回转段的弯曲角度；表示回转段绕坐标系o0x0y0z0的z0的旋转角度；l1为回转段的长度；lu1为回转段与运动段之间的刚性连接部分的长度；θ2表示运动段的弯曲角度；表示运动段绕坐标系o2x2y2z2的z2的旋转角度；l2为运动段的长度；lu2为运动段与末端执行器之间的刚性连接部分的长度；表示末端执行器绕坐标系o4x4y4z4的z4的旋转角度；l3为末端执行器的长度。4.根据权利要求3所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s2所述根据连续体机器人的末端位姿求解构型参数是令参数l1、l2、l3、lu1和lu2为已知；令参数θ1、θ2、和为未知的待求解的构型参数；将末端执行器相对于连续体机器人的基准位置的位姿t改写为如下齐次变换矩阵：其中p
x
、p
y
和p
z
表示坐标系o5x5y5z5的原点在坐标系o0x0y0z0中的位置；n
x
、n
y
和n
z
表示坐标系o5x5y5z5的x5轴在坐标系o0x0y0z0中的单位矢量；a
x
、a
y
和a
z
表示坐标系o5x5y5z5的y5轴在坐标系o0x0y0z0中的单位矢量；b
x
、b
y
和b
z
表示坐标系o5x5y5z5的z5轴在坐标系o0x0y0z0中的单位矢量；对齐次变换矩阵求逆解，即可得到连续体机器人的末端位姿下的构型参数θ1、θ2、和5.根据权利要求4所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s3所述求解各节段驱动丝长度变化量，是令d1为回转段中驱动丝距离节段横截面圆心的偏移量；则在回转段中，驱动丝长度变化量δl
10
＝d1θ1；在运动段中，令d2为运动段中驱动丝距离节段横截面圆心的偏移量，驱动丝长度变化量δl
20
＝d2θ2。6.根据权利要求5所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s4所述对结构耦合进行参数补偿，是对于运动段其有一部分长度处于回转段内，回转段处于弯曲状态时，运动段的驱动丝在该部分的长度会发生相应变化，需要对该类型的耦合进行解耦，将变化量补偿在驱动丝的长度中，对结构耦合进行参数补偿后的驱动丝长度变化量为：丝长度变化量为：7.根据权利要求6所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s5所述对驱动耦合进行参数补偿，是为了消除驱动丝的直线运动张力导致对应节段的弯曲。当对应旋转的齿轮顺时针旋转，而对应弯曲的齿轮保持不动时，对于丝杆传动机构而言，相当于弯曲运动的齿轮发生了逆时针旋转驱动耦合影响，让对应弯曲的齿轮跟随对应旋转的齿轮同时运动进行解耦，对其进行参数补偿如下：其中k为丝杆传动机构的传动比。8.根据权利要求7所述的一种用于多节段可单独旋转连续体机器人的控制方法，其特征在于，步骤s6所述得到驱动变量齿轮的目标位置，是对于回转段，对应旋转的齿轮运动直
接导致节段旋转；对于运动段，当回转段对应旋转的齿轮顺时针运动，而运动段对应旋转的齿轮静止时，相当于运动段对应旋转的齿轮逆时针运动。将以上弯曲运动与旋转运动的耦合效应逐一补偿后，得到一组驱动变量：上述驱动变量对应了一组齿轮的目标位置。

技术总结
本发明提供了一种用于多节段可单独旋转连续体机器人的控制方法，包括S1：建立连续体机器人的正运动学模型；S2：根据连续体机器人的末端位姿求解构型参数；S3：求解各节段驱动丝长度变化量；S4：对结构耦合进行参数补偿；S5：对驱动耦合进行参数补偿；S6：得到驱动变量齿轮的目标位置。通过对连续体机器人进行分析，在驱动参数的求解过程中对机械结构所导致的耦合进行补偿来实现解耦，使各节段之间可以独立控制，互不影响，实现在回转段固定时，运动段能够使执行器按照期望精准地达到目标位姿，避免手术中的误操作，提高了控制精度。提高了控制精度。提高了控制精度。


技术研发人员：李存荣 程子俊 向港
受保护的技术使用者：武汉理工大学
技术研发日：2023.07.17
技术公布日：2023/10/11