CN110193827B - 一种用于绳驱动连续体机器人的驱动补偿方法 - Google Patents

Abstract

本发明公开了一种用于绳驱动连续体机器人的驱动补偿方法，具体包括以下步骤：结合分段常曲率圆弧假设与几何分析法建立连续体机器人运动学模型；基于微分变换原理求解机器人的雅克比矩阵；基于虚功原理建立机器人静力学模型；基于库伦摩擦建立绳‑轮传动系统的力传递模型；基于胡克定律求解机器人驱动绳的伸长量；将驱动绳长补偿量反馈到控制单元中，实现对连续体机器人传动系统的误差补偿，提高机器人的运动控制精度。本发明的方法具有简单、高效、低成本及通用性好等特点，避免了使用昂贵的测量传感器，通过建立连续体机器人系统的驱动误差补偿模型，实现提高机器人运动控制精度的目的。

Description

一种用于绳驱动连续体机器人的驱动补偿方法

技术领域

本发明涉及一种用于绳驱动连续体机器人的驱动补偿方法，涉及连续体机器人控制技术领域。

背景技术

连续体机器人由于其独特的柔顺性，灵活性及安全性等特点被广泛地应用于微创介入手术领域。相比于传统的刚性关节式机器人而言，连续体机器人是一种模仿自然界生物的一种机器人，如象鼻、章鱼触角等，能够灵活的改变自身的形态以适应狭小非结构化的工作空间环境。基于连续体机器人的介入手术具有较高的安全性和高效性，不仅能够降低医生的工作强度和技术要求，减少医生和病患受辐射的程度，而且提高了介入手术的质量和效率，保证了介入手术的成功率和精确度。

由于连续体机器人系统的建模误差、驱动绳与导向轮间的摩擦误差及绳的回弹误差等影响，连续体机器人末端位置控制精度较低，将有可能导致组织和器官的损伤，严重时甚至威胁患者生命，需要对连续体机器人运动控制系统进行建模和补偿，以提高机器人本身的控制精度，保证微创介入手术的安全性和稳定性。

目前，通过外界昂贵的测量设备对机器人末端位姿进行实时跟踪反馈控制，从而提高机器人位置精度的方法成本较高，且用于血管介入手术的连续体机器人尺寸较小，传统的传感设备无法集成到机器人本体，通用性不佳。

发明内容

针对上述现有技术存在的不足，本发明提供一种用于绳驱动连续体机器人的驱动补偿方法，该方法简单、高效、成本较低且通用性好，通过机器人误差补偿模型来估算运动过程中驱动损失量，不需要昂贵的测量仪器，不仅提高了连续体机器人的控制精度，且降低了介入手术的技术难度和辐射伤害。

本发明的技术方案是：

1.一种用于绳驱动连续体机器人的驱动补偿方法，包括以下步骤：

(1)建立连续体机器人运动学模型

在分段常曲率假设下利用几何分析法建立连续体机器人的运动学模型，则关节坐标系i相对于关节坐标系i-1的齐次变换矩阵

为：

其中：l为第i节连续体弯曲单元的弧长；α_i为第i节连续体弯曲单元的弯曲方向角；θ_i第i节连续体弯曲单元的弯曲角；s表示sin；c表示cos；z表示z轴；y表示y轴；

根据几何分析法，则第i节连续体机器人各驱动绳的长度l_i1,l_i2,l_i3为：

l_i1＝l-rθ_icα_i

则n节连续体机器人末端在全局坐标系中的位姿

为：

其中：

为连续体机器人基坐标系在全局坐标系中的位姿；R_n为连续体机器人末端相对于全局坐标系的姿态；p_n为连续体机器人末端相对于全局坐标系的位置；

(2)机器人雅克比矩阵的求解

基于微分变换原理求解连续体机器人雅克比矩阵，则n节连续体机器人的雅克比矩阵为：

J＝[J₁ (Ad₁J₂) ··· (Ad_n-1J_n)]

其中：J为连续体机器人末端速度相对于初始坐标系的雅克比矩阵；J_i为第i节连续体弯曲单元在当前坐标系i下的雅克比矩阵，

为第i节弯曲单元对应的伴随矩阵；R_i为第i节弯曲单元在初始坐标系的姿态，

为第i节弯曲单元在初始坐标系下位置向量的反对称矩阵；

对步骤(1)中的驱动绳长模型进行求导，得：

Δq_i＝J_iqψΔψ_i

其中：Δq_i为第i节连续体机器人驱动绳长的变化量；J_iqψ为关节参数到驱动绳长的雅克比矩阵；Δψ_i＝[Δα_i,Δθ_i]为第i节连续体机器人的关节参数；

(3)建立机器人静力学模型

基于虚功原理，建立连续体机器人的静力学模型为：

其中：

为连续体机器人末端所受的外力及外力矩；Δx为在外力作用下机器人末端的位移偏差，

为使连续体机器人弯曲变形所需的理论驱动力，Δq_i为驱动绳长变化量，ΔU_i为机器人弯曲变形后的势能；

则连续体机器人弯曲变形时，驱动绳理论张力τ_i为：

其中：I为单位向量，η为用于分配作用到中心骨架上力矢量，常取η＝[1 1 1]^T；

(4)建立绳-轮传动系统力传递模型

基于库伦摩擦原理建立绳-轮传动系统力传递模型，则经过绳-轮传动系统后，输入力与输出力间的关系如下：

其中：τ_i,τ_in分别为通过步骤(3)中的静力学模型计算的理论绳张力和输入绳-轮传动系统的绳张力；

μ,φ分别为驱动绳相对于导向轮的运动速度，绳-轮接触面的摩擦系数和包角；t^-1表示前一时刻的状态；

(5)求解机器人驱动绳的伸长量

根据胡克定律并结合步骤(4)中的绳-轮传动系统力传递模型，则驱动绳的伸长模型为：

其中：δ_i为驱动绳的伸长量，τ₀为绳的初始预张力，r为导向轮的半径，φ_i为绳轮接触面的包角；E_i,A_i分别为驱动绳的弹性模量和横截面积；

(6)连续体机器人驱动误差补偿

将由步骤(5)计算得到的各驱动绳长伸长量补偿到机器人控制器中，然后与步骤(1)计算得出的理论值进行代数叠加，从而对连续体机器人驱动绳长进行误差补偿；

(7)补偿效果验证

重新控制连续体机器人运动若干个点，对比补偿前后机器人末端位置精度，若不满足精度要求，则继续步骤(3)、步骤(4)、步骤(5)，步骤(6)，直到满足精度要求。

2.所述步骤(1)中，弯曲角度为0时，相邻坐标系间的齐次变换矩阵为：

3.所述步骤(4)中，驱动绳一端固定于伺服电机转轴上，一端经过导向轮转向后固定于连续体机器人末端连接盘上。

4.所述步骤(6)中，连续体机器人的驱动误差补偿模型为

式中

为第i节连续体机器人补偿后的驱动量，q_i为由步骤(1)计算得到的理论绳长，δ_i为由步骤(5)求解得到的驱动绳长补偿量。

本发明的有益效果是：将连续体机器人应用于微创介入手术领域，通过建立连续体机器人的驱动误差补偿模型，从而得到变形运动过程中驱动绳长补偿量，然后将其修正到控制器中以提高连续体机器人的运动控制精度。该方法具有简单、高效、低成本及通用性好等优点，不需要安装昂贵的测量传感器，提高了连续体机器人的控制精度，且降低了介入手术的技术难度和伤害。

附图说明

下面结合附图对本发明的具体实施方式作进一步的说明：

图1是本发明的流程图；

图2是单节连续体机器人结构示意图；

图3是单节连续体机器人坐标系示意图；

图4是绳-轮传动系统间的力传递示意图；

其中，1-连接盘，2-中心骨架，3-驱动绳。

具体实施方式

结合附图1、2、3对本发明提出的技术路线进行具体的解释和说明，此处的应用实例并不只是适用于本例，适用于各种结构类型的绳驱动连续体机器人。

参考图1，本发明的一种用于绳驱动连续体机器人的驱动补偿方法包括以下步骤：

(1)建立连续体机器人运动学模型

连续体机器人运动学模型主要包括两个部分，一是建立驱动空间与关节空间的映射关系，二是关节空间与操作空间的映射关系，通过两者之间的叠加从而推导出导管机器人的运动学模型。单节弯曲单元结构如图2所示，在分段常曲率假设下利用几何分析法建立连续体机器人的运动学模型，相邻坐标示意图如下图3(a-c)所示，则关节坐标系i相对于关节坐标系i-1的齐次变换矩阵

其中：l为第i节连续体弯曲单元的弧长；α_i为第i节连续体弯曲单元的弯曲方向角；θ_i第i节连续体弯曲单元的弯曲角；s表示sin；c表示cos；z表示z轴；y表示y轴；

若弯曲角度θ_i为0时，为避免齐次变换矩阵陷入奇异性，则第i节连续体机器人的变换矩阵为：

在连续体机器人弯曲变形时，根据几何分析法得出第i节连续体机器人各驱动绳长l_i1,l_i2,l_i3为：

l_i1＝l-rθ_ic(α_i)

根据坐标系间的齐次变换原理，可以求解出第n节连续体机器人末端在全局坐标系中的位姿

为：

其中：

为连续体机器人基坐标系在全局坐标系中的位姿；R_n为连续体机器人末端相对于全局坐标系的姿态；p_n为连续体机器人末端相对于全局坐标系的位置；

(2)机器人雅克比矩阵的求解

机器人雅克比矩阵表述的是机器人关节参数变化量与末端位姿偏差间的映射关系，基于微分变换原理对机器人末端位姿分别进行求导，可求解连续体机器人相邻坐标系间的雅克比矩阵，将其转化到基坐标系下表示，则n节连续体机器人的雅克比矩阵为：

J＝[J₁ (Ad₁J₂) ··· (Ad_n-1J_n)]

其中：J为连续体机器人末端速度相对于初始坐标系的雅克比矩阵；J_i为第i节连续体弯曲单元在当前坐标系i下的雅克比矩阵，

为第i节弯曲单元对应的伴随矩阵；R_i为第i节弯曲单元在初始坐标系的姿态，

为第i节弯曲单元在初始坐标系下位置向量的反对称矩阵；

对步骤(1)中的驱动绳长模型进行求导，得：

Δq_i＝J_iqψΔψ_i

其中：Δq_i为第i节连续体机器人驱动绳长的变化量；J_iqψ为关节参数到驱动绳长的雅克比矩阵；Δψ_i＝[Δα_i,Δθ_i]为第i节连续体机器人的关节参数；

(3)建立机器人静力学模型

绳驱动连续体机器人的变形运动是通过外置的伺服电机模组将力/力矩施加在机器人末端而实现的，驱动绳的一端固定在连续体机器人的末端，而另一端与驱动电机转轴相连。对连续体机器人进行静力学建模，分析机器人变形运动与其所受力/力矩间的关系，从而较为准确的建立运动模型，实现精准的控制。基于虚功原理，建立连续体机器人的静力学模型为：

其中：

连续体机器人末端所受的外力及外力矩；Δx为在外力作用下机器人末端的位移偏差，

为连续体机器人弯曲变形时所需的理论驱动力，Δq_i为驱动绳长变化量，ΔU_i为机器人弯曲变形后的势能；

根据所建的机器人静力学模型并结合步骤(2)中的雅克比矩阵，则连续体机器人弯曲变形时，驱动绳理论张力τ_i为：

其中：I为单位向量，η为用于分配作用到中心骨架上力矢量，常取η＝[1 1 1]^T；

(4)建立绳-轮传动系统力传递模型

由于驱动绳与导向轮间摩擦力的存在，则输入到连续体机器人本体的绳张力必然造成损失，影响机器人的控制精度。绳-轮传动系统接触面示意图如图4，根据经典的库伦摩擦模型对绳-轮传动系统的运动特性进行分析，建立绳-轮传动系统力传递模型，则输入和输出绳-轮传动系统后绳张力的关系如下：

其中：τ_i,τ_in分别为通过步骤(3)中的静力学模型计算的理论绳张力和输入绳-轮传动系统的绳张力；

μ,φ分别为驱动绳相对于导向轮的运动速度，绳-轮接触面的摩擦系数和包角；t^-1表示前一时刻的状态；

(5)求解机器人驱动绳的伸长量

由于驱动绳本身材料的特性及张力的作用，将不可避免的造成绳的伸长，影响机器人的控制精度，故需要求解连续体机器人运动过程中绳的伸长模型。根据胡克定律并结合步骤(4)中的绳-轮传动系统力传递模型，则驱动绳的伸长模型为：

其中：δ_i为驱动绳的伸长量，τ₀为绳的初始预张力，r为导向轮的半径，φ_i为绳轮接触面的包角；E_i,A_i分别为驱动绳的弹性模量和横截面积；

(6)连续体机器人驱动误差补偿

将由步骤(5)计算得到的各驱动绳长伸长量补偿到机器人控制器中，然后与步骤(1)计算得出的理论值进行代数叠加，从而对连续体机器人驱动绳长进行误差补偿，连续体机器人具体的驱动误差补偿模型为

式中

为第i节连续体机器人补偿后的驱动量，q_i为由步骤(1)计算得到的理论绳长，δ_i为由步骤(5)求解得到的驱动绳长补偿量。

(7)补偿效果验证

重新控制连续体机器人运动若干个点，对比补偿前后机器人末端位置精度，若不满足精度要求，则继续步骤(3)、(4)、(5)，(6)，直到满足精度要求。

本发明的一种绳驱动连续体机器人应用于微创介入手术领域，能够灵活、安全地主动改变自身形状以到达患者的病变部位并实现诊断和治疗，减少了医生的劳动强度和辐射程度，保证了介入手术的质量和效率。

其次，针对连续体机器人传动系统由于摩擦力、建模误差等影响下末端定位精度较低，研究一种基于连续体机器人驱动误差模型的补偿方法，通过模型估算驱动控制中的补偿量实现误差补偿，避免了测量传感设备的集成化难度，减少了计算的复杂程度和外来误差引入的可能性，保证了实时控制的质量和效率。

最后本文提出的一种用于绳驱动连续体机器人的驱动补偿方法具有简单、高效、低成本及通用性好等优点，不需要安装昂贵的测量传感器，不仅提高了连续体机器人的控制精度，且降低了介入手术的技术难度和伤害。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种用于绳驱动连续体机器人的驱动补偿方法，其特征在于，包括以下步骤：

(1)建立连续体机器人运动学模型

在分段常曲率假设下利用几何分析法建立连续体机器人的运动学模型，则关节坐标系i相对于关节坐标系i-1的齐次变换矩阵

为：

其中：l为第i节连续体弯曲单元的弧长；α_i为第i节连续体弯曲单元的弯曲方向角；θ_i第i节连续体弯曲单元的弯曲角；s表示sin；c表示cos；z表示z轴；y表示y轴；

根据几何分析法，则第i节连续体机器人各驱动绳长l_i,1,l_i,2,l_i,3为：

l_i1＝l-rθ_icα_i

则n节连续体机器人末端在全局坐标系中的位姿

为：

其中：

为连续体机器人基坐标系在全局坐标系中的位姿；R_n为连续体机器人末端相对于全局坐标系的姿态和位置；p_n为连续体机器人末端相对于全局坐标系的位置；

(2)机器人雅克比矩阵的求解

基于微分变换原理求解连续体机器人雅克比矩阵，则n节连续体机器人的雅克比矩阵为：

J＝[J₁ (Ad₁J₂) … (Ad_n-1J_n)]

其中：J为连续体机器人末端速度相对于初始坐标系的雅克比矩阵；J_i为第i节连续体弯曲单元在当前坐标系i下的雅克比矩阵，

为第i节弯曲单元对应的伴随矩阵；R_i为第i节弯曲单元在初始坐标系的姿态，

为第i节弯曲单元在初始坐标系下位置向量的反对称矩阵；

对步骤(1)中的驱动绳长模型进行求导，得：

△q_i＝J_iqψ△ψ_i

其中：△q_i为第i节连续体机器人驱动绳长的变化量；J_iqψ为关节参数到驱动绳长的雅克比矩阵；△ψ_i＝[△α_i,△θ_i]为第i节连续体机器人的关节参数；

(3)建立机器人静力学模型

基于虚功原理，建立连续体机器人的静力学模型为：

其中：

为连续体机器人末端所受的外力及外力矩；△x为在外力作用下机器人末端的位移偏差，

为使连续体机器人弯曲变形所需的理论驱动力，△q_i为驱动绳长变化量，△U_i为机器人弯曲变形后的势能；

则连续体机器人弯曲变形时，驱动绳理论张力τ_i为：

其中:I为单位向量，η为用于分配作用到中心骨架上力矢量，常取η＝[1 1 1]^T；

(4)建立绳-轮传动系统力传递模型

基于库伦摩擦原理建立绳-轮传动系统力传递模型，则经过绳-轮传动系统后，输入力与输出力间的关系如下：

其中：τ_i,τ_in分别为通过步骤(3)中的静力学模型计算的理论绳张力和输入绳-轮传动系统的绳张力；

μ,φ分别为驱动绳相对于导向轮的运动速度，绳-轮接触面的摩擦系数和包角；t^-1表示前一时刻的状态；

(5)求解机器人驱动绳的伸长量

根据胡克定律并结合步骤(4)中的绳-轮传动系统力传递模型，则驱动绳的伸长模型为：

其中：δ_i为驱动绳的伸长量，τ₀为绳的初始预张力，r为导向轮的半径，φ_i为绳轮接触面的包角；E_i,A_i分别为驱动绳的弹性模量和横截面积；

(6)连续体机器人驱动误差补偿

将由步骤(5)计算得到的各驱动绳长伸长量补偿到机器人控制器中，然后与步骤(1)计算得出的理论值进行代数叠加，从而对连续体机器人驱动绳长进行误差补偿；

(7)补偿效果验证

重新控制连续体机器人运动若干个点，对比补偿前后机器人末端位置精度，若不满足精度要求，则继续步骤(3)、步骤(4)、步骤(5)，步骤(6)，直到满足精度要求。

2.根据权利要求1所述的一种用于绳驱动连续体机器人的驱动补偿方法，其特征在于：所述步骤(1)中，弯曲角度为0时，相邻坐标系间的齐次变换矩阵为：

3.根据权利要求1所述的一种用于绳驱动连续体机器人的驱动补偿方法，其特征在于：所述步骤(4)中，驱动绳一端固定于伺服电机转轴上，一端经过导向轮转向后固定于连续体机器人末端连接盘上。

4.根据权利要求1所述的一种用于绳驱动连续体机器人的驱动补偿方法，其特征在于：所述步骤(6)中，连续体机器人的驱动误差补偿模型为

式中

为第i节连续体机器人补偿后的驱动量，q_i为由步骤(1)计算得到的理论绳长，δ_i为由步骤(5)求解得到的驱动绳长补偿量。

Images