CN106137400A - 用于机械臂的控制系统、控制方法及一种手术机器人 - Google Patents

Abstract

本发明涉及一种用于机械臂的控制系统、控制方法及一种手术机器人，其中的方法提供一个与各关节位置误差和速度误差相关的终端滑模面，计算机械臂的关节位置在状态空间中到终端滑模面的距离，所述状态空间的状态变量包括所述位置误差和速度误差；针对关节位置到终端滑模面的距离，设计一个快速趋近律；根据机械臂的动力学模型以及所述快速趋近律得出滑模控制律的完整表达；根据所述滑块控制律输出力矩指令；以及对所述力矩指令进行饱和限制，得到最终的力矩输出指令，用于控制各关节的电机动作，以驱动机械臂的各个关节达到设计位置。该方法可以提高该机械臂的控制性能。

Description

用于机械臂的控制系统、控制方法及一种手术机器人

技术领域

本发明涉及机器人系统的机械臂的控制系统、控制方法及一种手术机器人。

背景技术

微创伤外科手术机器人系统中，各个关节的精度决定着整个机器人系统的运动精度和工作性能，而在整个系统中，主操作手与从机械臂的跟随性能是保证系统精度的关键。主操作手是医生手持的操作器，也可以简称“主操作手”或“主手”。从机械臂是病人端机器人安装的机械臂，手术时会插入人体内部执行具体手术操作，可以简称为从手。主从机械臂之间的跟随效果是通过主从控制来实现的，其目的是通过坐标变换和运动映射，采取特定的控制策略，保证主操作手在人眼坐标系下的运动与从机械臂在应用场合坐标系下的运动精确对应。

在手术过程中，主从控制的中断操作是不可避免的，例如更换从机械臂上的手术器械。然而，连接中断后，由于主操作手的位置或者姿态被调整，其在人眼坐标系下的坐标不再与从机械臂在内窥镜坐标系下的坐标一致，因此在重新建立与从机械臂的连接时，主从机械臂之间的运动映射发生了变化。为了保证跟随效果，主手的位置和姿态需要根据从机械臂的位置和姿态进行轻微的调整，从而确保主从两端运动映射的准确性和一致性。否则，从机械臂为了跟随主手的运动和姿态，其姿态会跟随主手的姿态发生突变。

目前，基于前馈控制器和PD控制器的主操作手在主动控制状态下的控制系统，由于PD控制本质上是对偏差进行操作，当偏差为零时，控制作用也为零，在有摩擦和重力等非线性项作用的情况下，偏差不会为0，同时主操作手的伺服刚度一般较小，因此并不能够很好地消除静态误差。而且从机械臂开始跟随主操作手姿态时，由于静态误差的作用，从机械臂会发生位置和姿态的突变，在手术中容易造成危险的后果。

发明内容

本发明的目的在于提供一种机械臂的控制系统及其控制方法，以提高其控制性能。本发明还提供一种手术机器人包括机械臂和该控制系统，该控制系统用于控制该机械臂。

一种用于机械臂的控制系统，包括滑模控制器，所述滑模控制器用于接收机械臂上各关节位置误差信号e和速度误差信号并根据滑模控制律输出力矩指令，用于控制各关节的电机动作，以驱动机械臂的各个关节达到设计位置，其中，

所述滑模控制律由根据机械臂的动力学模型以及一快速趋近律得出，所述快速趋近律为根据所述机械臂上各关节的关节位置到终端滑模面的距离设定，所述终端滑模面与各关节位置误差和速度误差相关，所述距离是机械臂的各关节位置在状态空间中到该终端滑模面的距离，所述状态空间的状态变量包括所述位置误差和速度误差。

在一实施例中，所述控制系统还包括第一叠加器和第二叠加器，所述第一叠加器的输入端接收机械臂上各关节的实际位置信号q和理想位置信号q_d，并输出位置误差信号e；

所述第二叠加器的输入端接收机械臂上各关节的实际速度信号v和理想速度信号v_d，并输出输出误差信号

在一实施例中，所述控制系统还包括微分器，所述微分器的输入端接入机械臂各关节上的位置传感器发送的位置信号q，并输出实际速度信号v。

在一实施例中，所述控制系统还包括限制器，所述限制器的输入端与滑模控制器的输出端连接，对控制滑模控制器的力矩指令进行饱和限制，输出最终的力矩指令。

在一实施例中，所述控制系统还包括前馈补偿装置和第三叠加器，所述前馈补偿装置的输入端接收机械臂上各关节的实际位置信号q和实际速度信号v，前馈补偿装置的输出端、滑模控制器的输出端与第三叠加器输入端连接，第三叠加器的输出端与限制器的输入端连接，所述限制器对第三叠加器输出的力矩指令进行饱和限制，并输出最终的力矩指令。

在一实施例中，所述距离设定为：

$D=e+\mathrm{\λ}\stackrel{\·}{e}+{\mathrm{\μe}}^{\mathrm{\γ}},$

其中，D为所述距离，e为位置误差，为速度误差，λ,μ和γ是相应于机械臂的系统要求的可选择性参数。

在一实施例中，λ>0,μ>0,1<γ<2

在一实施例中，λ＝2,μ＝1,γ＝1.5

在一实施例中，所述快速趋近律选择为：

u＝-ηsign(D)-ρD，

其中，η和ρ是相应于机械臂系统要求的可选择性参数。

在一实施例中，所述快速趋近律选择为：

u＝-ηsat(D)-ρD，其中，

$sat\left(D\right)=\left\{\begin{array}{cc}+1,& D>\mathrm{\&delta;}\\ \frac{D}{\mathrm{\&delta;}},& \left|D\right|\&le;\mathrm{\&delta;}\\ -1,& D<-\mathrm{\&delta;}\end{array}\right.,$

δ>0是切换边界，η和ρ是相应于机械臂系统要求的可选择性参数。。

在一实施例中，η>0，ρ>0。

在一实施例中，η＝3，ρ＝1。

在一实施例中，所述动力学模型表达为：

$H\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)=\mathrm{\&tau;},$

其中，H是n×n的对称正定矩阵，为关节空间惯性矩阵，是关节位置q相关的函数；C是n×n的矩阵，与离心力和科氏力相关，是关节实际位置q与关节实际速度的函数；G是重力项的矢量；τ是控制律的输出，即控制律的操作对象的输入；n为机械臂的关节数。

一种机械臂的控制方法包括：

提供一个与机械臂上各关节位置误差和速度误差相关的终端滑模面，计算机械臂的各关节位置在状态空间中到终端滑模面的距离，所述状态空间的状态变量包括所述位置误差和速度误差；

针对各关节位置到终端滑模面的距离，设计一个快速趋近律；

根据机械臂的动力学模型以及所述快速趋近律得出滑模控制律的完整表达；

根据所述滑块控制律输出力矩指令，用于控制各关节的电机动作，以驱动机械臂的各个关节达到设计位置。

一种手术机器人，包括医生端机械臂和所述的控制系统，所述的控制系统用于控制所述医生端机械臂。

一种手术机器人，包括病人端机械臂和所述的控制系统，所述的控制系统用于控制所述病人端机械臂。

本发明的控制方法，结构简单，响应速度快，并且可以解决传统PD控制跟随精度不能满足要求的问题，具有消除稳态误差的控制效果，避免了重连时误差较大时产生的姿态突变问题。

本发明的优选实施例基于滑模面距离函数和快速趋近律提高了控制器跟踪精度，其中的快速趋近律，用饱和函数代替符号函数，可以有效地消除或减弱系统处于滑动模态时的震颤。

本发明的优选实施例中滑模面距离函数采用了幂函数和线性函数，提高了系统在滑模面运行时的平滑性。

附图说明

本发明的上述的以及其他的特征、性质和优势将通过下面结合附图和实施例的描述而变得更加明显，其中：

图1为本发明一比较例的机械臂的控制系统的结构框图；

图2为本发明一实施例中的机械臂的控制系统的结构框图；

图3为本发明一实施例中机械臂的滑模控制器的结构框图；

图4为本发明一实施例中机械臂的滑模控制方法的流程图。

具体实施方式

本发明人通过对手术机器人系统的控制系统的研究，发现在手术过程中，主从控制的中断操作是不可避免的，具体应用场景主要有三种：情况一是当主操作手已到达动作极限，而从机械臂并未到达极限时，医生需要采取中断操作，主动断开主操作手与从机械臂的关联，调整主操作手位置后，与从机械臂重新建立连接；情况二是当手术中需要完成更换器械的动作前后，需要采取中断和重连的操作；情况三为当需要调整内窥镜位置时，也要求医生采取与从机械臂的中断操作。然而，连接中断后，由于主操作手的位置或者姿态被人手调整，其在人眼坐标系下的坐标不再与从机械臂在内窥镜坐标系下的坐标一致，因此在重新建立与从机械臂的连接时，主从机械臂之间的运动映射发生了变化，为了保证跟随效果，主操作手的位置和姿态需要根据从机械臂的位置和姿态进行轻微的调整，从而确保主从两端运动映射的准确性和一致性。如果缺少这一调整环节，当重新建立连接时，从机械臂为了跟随主操作手的运动和姿态，其姿态会跟随主操作手的姿态发生突变。而此时从机械臂的末端器械仍处于病人的腹腔内，这样的突变很可能会造成手术失误，引起严重的后果。

由于该问题很难用精确的机器人动力学模型来描述，并且具有非线性特性，加上微创伤手术中对从机械臂末端器械的动作要求极其精微，传统的PD关节控制器很难满足其高精度的要求。

例如医生调整主手后与从机械臂重新建立连接，此时主手在人眼坐标系下的末端姿态为：

$R_M=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

而从机械臂在内窥镜坐标系下的末端姿态为：

$R_s=\left[\begin{array}{ccc}{r^{\&prime;}}_{11}& {r^{\&prime;}}_{12}& {r^{\&prime;}}_{13}\\ {r^{\&prime;}}_{21}& {r^{\&prime;}}_{22}& {r^{\&prime;}}_{23}\\ {r^{\&prime;}}_{31}& {r^{\&prime;}}_{32}& {r^{\&prime;}}_{33}\end{array}\right]$

中断重连时主操作手与从机械臂姿态不对应会引发的从机械臂姿态突变问题。

如图1所示的控制系统，为基于前馈控制器和PD控制器的主操作手在主动控制状态下的控制系统结构框图。主操作手的前馈补偿环节1-1是对摩擦补偿和惯性力补偿，主操作手的控制环节采用PD控制器1-2，对位置偏差e及速度偏差进行负反馈控制，保证主从跟随效果，饱和限制环节1-3是限制输出力矩不大于允许的最大力矩。而在中断重连时，主操作手需要根据从机械臂的姿态进行主动跟随调整，由于PD控制本质上是对偏差进行操作，当偏差为零时，控制作用也为零，在有摩擦和重力等非线性项作用的情况下，偏差不会为0，同时主操作手的伺服刚度一般较小，因此并不能够很好地消除静态误差。而且当重建连接，从机械臂开始跟随主操作手姿态时，由于静态误差的作用，从机械臂会发生位置和姿态的突变，在手术中容易造成危险的后果。

为了更好地解决这个问题，避免手术安全事故的出现，本发明提出基于滑模控制的机械臂控制系统，由于该控制系统无需精确的动力学模型，并且具有响应快、精度高、鲁棒性好的特点，因此可以有效地提高调整过程的系统性能，保证主操作手姿态能够精准的跟随从机械臂姿态，从而避免机械臂产生大的运动或者抖动，对整个机器人控制安全有非常重要的意义。后述的机械臂以主从机器人系统的主手为例，但本发明也适合于其他应用场合中的机械臂及其控制方法，例如主从手术机器人病人端的机械臂(又称“从手”)。

下面结合具体实施例和附图对本发明作进一步说明，在以下的描述中阐述了更多的细节以便于充分理解本发明，但是本发明显然能够以多种不同于此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下根据实际应用情况作类似推广、演绎，因此不应以此具体实施例的内容限制本发明的保护范围。

图2为本发明一实施例中的机械臂的控制系统的结构框图，主操作手基于前馈控制器和滑模控制器处于被动调整状态下。图1、图2所示的结构相似，区别在于在正常的主从控制状态下，图2所示的结构采用滑模控制器2-2来提高主操作手的性能，达到高精度跟随的控制目的，最终保证手术安全。在图2中，前馈补偿环节(前馈补偿装置)2-1利用位置信息q_d和速度信息v_d，来对受控主操作手的非线性耦合系统提供摩擦补偿和惯性力补偿，使控制器的控制效果更精确。本发明对前馈补偿环节2-1没有特别的要求，采用本领域现有技术皆可。系统检测到的实际位置信号q和计算得到的理想位置信号q_d通过第一叠加器求得该时刻的位置误差信号e，由位置信号微分得来的实际速度信号v和理想速度信号v_d通过第二叠加器求得该时刻的速度误差信号两个偏差信号输入至滑模控制器2-2中进行处理生成滑模控制信号，与前馈补偿2-1输出的补偿信号作叠加，即通过第三叠加器进行叠加，随后输入到饱和限制环节(限制器)2-3中。饱和限制环节2‐3是用来对控制器输出的力矩命令进行限制，比如限制其输出力矩不超出电机可提供的最大力矩，或限制其输出力矩不大于该关节允许的最大力矩等。

如图3所示的是图2中控制系统中滑模控制器的结构框图。在模块3‐1中建立主操作手的动力学模型：

$H\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)=\mathrm{\&tau;}---\left(1\right)$

其中，H是n×n的对称正定矩阵，称为关节空间惯性矩阵，是关节位置q相关的函数；C是n×n的矩阵，与离心力和科氏力相关，是关节位置q与关节速度的函数；G是重力项的矢量；τ是控制律的输出，即控制律的操作对象的输入；n为机械臂的关节数。

在模块3‐2中，建立滑模面环节3-2，其是用于接收输入的位置误差信号e和速度误差信号并设计一个与e和相关的函数D，来计算关节信息在状态空间中到滑模面的距离，状态空间的定义是以n个状态变量作为基底组成的n维空间。其中，状态变量是指一组能确定系统在时间域中的行为或运动信息的变量，在本发明的实施例中可以用位置偏差e和速度偏差作为两个状态变量，由此就可定义一个二维的状态空间。而控制的目的是消除偏差，使位置偏差e和速度偏差均为零，也就是让该状态空间的两个状态变量在滑模控制器的作用下到达次此状态空间的原点。

选取到滑模面的距离函数D如下：

$D=e+\mathrm{\&lambda;}\stackrel{\&CenterDot;}{e}+{\mathrm{\&mu;e}}^{\mathrm{\&gamma;}}---\left(2\right)$

其中，λ,μ和γ都是可根据相应机械臂的系统要求进行选择的参数，一般可让λ>0,μ>0,1<γ<2，e是位置偏差，在本发明的一实施例中，λ＝2,μ＝1,γ＝1.5。

在模块3‐3中的快速趋近律环节用于接收到滑模面的距离函数D，并提供一个快速趋近律u，使得状态空间中的两个状态变量到滑模面的距离，即关节的位置偏差和速度偏差均趋向于零。这是通过设计控制函数，保证系统在滑模面上运行。

快速趋近律u是一个与距离函数D的绝对值相关的，适用于主操作手的单调函数，可选择为：

u＝-ηsign(D)-ρD (3)

其中，η和ρ是可根据相应机械臂的系统要求进行选择的参数，可让η>0，ρ>0。在本发明的一实施例中，η＝3，ρ＝1。为了消除或减弱系统处于滑动模态时的震颤，可以用如下的饱和函数sat(D)替换上式中的符号函数sign(D)：

$sat\left(D\right)=\left\{\begin{array}{cc}+1,& D>\mathrm{\&delta;}\\ \frac{D}{\mathrm{\&delta;}},& \left|D\right|\&le;\mathrm{\&delta;}\\ -1,& D<-\mathrm{\&delta;}\end{array}\right.---\left(4\right)$

其中δ>0是切换边界。

在模块3‐4中的滑模控制器环节是利用模块3‐1中的动力学模型和模块3‐3中的快速趋近律得到的控制函数，计算出可应用于该主操作手的完整的滑模控制律：

$\mathrm{\&tau;}=H\left(q\right)\&lsqb;-\mathrm{\&eta;}sign\left(D\right)-\mathrm{\&rho;}D\&rsqb;+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)---\left(5\right)$

在模块3‐5中限制环节对应于图2中的饱和限制环节2-3，是对模块3-4输出的力矩进行饱和限制，目的是限制其输出力矩不超出电机可提供的最大力矩，或限制其输出力矩不大于该关节允许的最大力矩等。该环节的输出信号τ_sat，为最终的输出力矩指令，输出信号至作用于机器人主操作手各个关节的伺服电机，驱动各关节达到理想位置。

图4所示为本发明的一实施例中，用于主操作手的滑模控制方法的整体流程图。如图所示，首先，在步骤4-1中建立主操作手的动力学模型，具体动力学方程见公式(1)，可由刚性机械臂的欧拉‐拉格朗日方程推导得出。在步骤4-2中，关节在状态空间中的位置到滑模面的距离函数D被计算得出，此距离函数的不是唯一的，其中一种见公式(2)。在步骤4-3中，根据步骤4-2中的距离函数D，可计算得出使主操作手运行至滑模面快速趋近律，其设计方法可是应用的机器人系统而定，其中一种方案见公式(3)。随后在步骤4-4中，设计得到的快速趋近律被带入动力学方程，从而得到应用于机器人系统完整的滑模控制律，即公式(5)，求得系统为达到理想控制效果所需要的输出力矩。由于该结果只由计算得出，可能不完全适合当前系统采用的电机、负载或应用场景等，因此在步骤4‐5中对上一步输出的力矩加以限制，防止其超出系统期望的力矩。最后，在步骤4-6中，控制系统的最终的力矩指令被输出，作用于主操作手的各个关节，达到期望的控制目的。通过以上控制策略，主操作手的控制系统的跟踪性能得到提升，中断重连过程中从机械臂姿态突变的情况可以被有效地避免，手术的安全性得到了提高。

本发明虽然以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以做出可能的变动和修改。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何修改、等同变化及修饰，均落入本发明权利要求所界定的保护范围之内。

Claims (16)

1.一种用于机械臂的控制系统，包括滑模控制器，其特征在于，所述滑模控制器用于接收机械臂上各关节位置误差信号e和速度误差信号并根据滑模控制律输出力矩指令，用于控制各关节的电机动作，以驱动机械臂的各个关节达到设计位置，其中，

所述滑模控制律由根据机械臂的动力学模型以及一快速趋近律得出，所述快速趋近律为根据所述机械臂上各关节的关节位置到终端滑模面的距离设定，所述终端滑模面与各关节位置误差和速度误差相关，所述距离是机械臂的各关节位置在状态空间中到该终端滑模面的距离，所述状态空间的状态变量包括所述位置误差和速度误差。

2.如权利要求1所述的控制系统，其特征在于，所述控制系统还包括第一叠加器和第二叠加器，所述第一叠加器的输入端接收机械臂上各关节的实际位置信号q和理想位置信号q_d，并输出位置误差信号e；

所述第二叠加器的输入端接收机械臂上各关节的实际速度信号v和理想速度信号v_d，并输出输出误差信号

3.如权利要求2所述的控制系统，其特征在于，所述控制系统还包括微分器，所述微分器的输入端接入机械臂各关节上的位置传感器发送的位置信号q，并输出实际速度信号v。

4.如权利要求1所述的控制系统，其特征在于，所述控制系统还包括限制器，所述限制器的输入端与滑模控制器的输出端连接，对控制滑模控制器的力矩指令进行饱和限制，输出最终的力矩指令。

5.如权利要求4所述的控制系统，其特征在于，所述控制系统还包括前馈补偿装置和第三叠加器，所述前馈补偿装置的输入端接收机械臂上各关节的实际位置信号q和实际速度信号v，前馈补偿装置的输出端、滑模控制器的输出端与第三叠 加器输入端连接，第三叠加器的输出端与限制器的输入端连接，所述限制器对第三叠加器输出的力矩指令进行饱和限制，并输出最终的力矩指令。

6.如权利要求5所述的控制系统，其特征在于，所述距离设定为：

其中，D为所述距离，e为位置误差，为速度误差，λ,μ和γ是相应于机械臂的系统要求的可选择性参数。

7.如权利要求6所述的控制系统，其特征在于，λ>0,μ>0,1<γ<2。

8.如权利要求6所述的控制系统，其特征在于，λ＝2,μ＝1,γ＝1.5。

9.如权利要求6所述的控制系统，其特征在于，所述快速趋近律选择为：

u＝-ηsign(D)-ρD，

其中，η和ρ是相应于机械臂系统要求的可选择性参数。

10.如权利要求6所述的控制系统，其特征在于，所述快速趋近律选择为：

u＝-ηsat(D)-ρD，其中，

δ>0是切换边界，η和ρ是相应于机械臂系统要求的可选择性参数。

11.如权利要求9或10所述的控制系统，其特征在于，η>0，ρ>0。

12.如权利要求11所述的控制系统，其特征在于，η＝3，ρ＝1。

13.如权利要求6所述的控制系统，其特征在于，所述动力学模型表达为：

其中，H是n×n的对称正定矩阵，为关节空间惯性矩阵，是关节位置q相关的函数；C是n×n的矩阵，与离心力和科氏力相关，是关节实际位置q与关节实 际速度的函数；G是重力项的矢量；τ是控制律的输出，即控制律的操作对象的输入；n为机械臂的关节数。

14.一种机械臂的控制方法，其特征在于，包括：

提供一个与机械臂上各关节位置误差和速度误差相关的终端滑模面，计算机械臂的各关节位置在状态空间中到终端滑模面的距离，所述状态空间的状态变量包括所述位置误差和速度误差；

针对各关节位置到终端滑模面的距离，设计一个快速趋近律；

根据机械臂的动力学模型以及所述快速趋近律得出滑模控制律的完整表达；

根据所述滑块控制律输出力矩指令，用于控制各关节的电机动作，以驱动机械臂的各个关节达到设计位置。

15.一种手术机器人，包括医生端机械臂和权1-13任一项所述的控制系统，所述的控制系统用于控制所述医生端机械臂。

16.一种手术机器人，包括病人端机械臂和权1-13任一项所述的控制系统，所述的控制系统用于控制所述病人端机械臂。