一种受约束的机械臂零力控制系统及其控制方法与流程

1.本发明涉及机械臂控制方法，尤其涉及一种受约束的机械臂零力控制系统及其控制方法。


背景技术：

2.目前微创手术已基本取代开放手术成为外科医学领域发展的主要方向。相比于传统的开放式手术，微创手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，以达芬奇手术机器人为代表的基于机器人辅助系统的微创手术逐渐成熟，并被广泛应用。
3.腹腔手术机器人可适应不同科室的多种手术类型，但不同的手术类型需要不同的术前姿态。因此要求手术机器人的机械臂可实现灵活的空间拖动，从而方便医护人员实现术前摆位。同样地，对于腹腔手术机器人系统中的主操作手而言，也需要实现灵活的空间拖动，以使得医生可以轻松地完成手术操作。
4.不论是床旁手术机器人的术前预调整还是主操作手的操作，为实现机械臂的灵活拖动，常用的技术方案是，首先建立机械臂模型，再通过连杆运动学及动力学计算获取各个关节所需补偿的重力以及摩擦力等，最后由关节电机提供补偿力矩，从而可使得机械臂在关节空间内灵活拖动。
5.但是考虑到手术机器人应用场合的特殊性，在床旁手术机器人的术前预调整以及主操作手的操作过程中，如果拖动速度过快，容易出现机械臂不受控制的情况。而且，如果机械臂运动到笛卡尔空间以外，将与其他机械臂或设备发生碰撞。因此需要对其进行限速限位等处理，以确保术前摆位的安全性。


技术实现要素：

6.发明目的：针对上述不足，本发明提出了一种受约束的机械臂零力控制系统及其控制方法，可避免拖动过程中速度过快不能及时制动，以及拖动过程中发生碰撞等问题，使得拖动过程更安全可靠。
7.技术方案：
8.一种受约束的机械臂零力控制系统，包括：
9.动力学补偿控制器，用于根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩；
10.速度限制控制器，用于根据笛卡尔空间及关节空间的速度限制控制各关节电机输出速度限制力矩；
11.位置限制控制器，用于根据笛卡尔空间及关节空间的位置限制控制各关节电机输出位置限制力矩；
12.机械臂各关节电机在前述三个控制器的控制下输出合力矩。
13.所述动力学补偿控制器根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩具体如下：
14.通过牛顿欧拉动力学或拉格朗日动力学计算各个关节所需的克服关节重力所需的补偿力矩mg和克服摩擦力所需的补偿力矩mf：
15.mg＝g(q)
[0016][0017]
其中，g(q)表示关节重力矩，表示关节的转动速度，表示关节转动的粘滞摩擦力矩，表示关节转动的库伦摩擦力矩。
[0018]
所述速度限制控制器为各关节提供速度限制力矩具体如下：
[0019]
假定笛卡尔空间速度为，其中，分别表示机械臂的笛卡尔空间速度和角速度；
[0020]
当分别表示设定的机械臂的笛卡尔空间速度最大值和角速度最大值；则提供笛卡尔空间限制力矩和角速度最大值；则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间得到根据笛卡尔空间的速度限制对参与运动的每个关节提供的反向力矩的反向力矩其中k1表示笛卡尔空间速度限制权重，j表示雅可比矩阵；
[0021]
假定某个关节实时速度为θ，当|θ|》θ
max
，提供关节空间限制力矩其中，θ
max
表示设定的关节速度最大值，k1表示关节空间速度限制权重；
[0022]
据此得到速度限制力矩为
[0023]
所述位置限制控制器为各关节提供位置限制力矩具体如下：
[0024]
假设在笛卡尔空间触发限制的边界位置为，当机械臂在笛卡尔空间的实际位置，且机械臂的运动方向依然有加大其在笛卡尔空间的实际位置超出边界位置的趋势时，则提供笛卡尔空间限制力矩位置的趋势时，则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩力矩其中，p
limx
、p
limy
、p
limz
分别表示笛卡尔空间x、y、z轴方向上的限制边界，(p
x
,py,pz)分别表示机械臂在笛卡尔空间的实际位置坐标，k2表示笛卡尔空间位置限制权重；j表示雅可比矩阵；
[0025]
假定某个关节实时位置为p，当某个关节的实时位置|p|》p
lim
且运动方向依然有加大其在关节空间内的实时位置超出其位置边界的趋势时，提供关节空间限制力矩其中，p
lim
表示在关节空间内某个关节的位置边界，k2表示关节空间位置限制权重；
[0026]
据此得到用于位置限制的合力矩为
[0027]
还包括力矩限制控制器，用于根据前述三个控制器控制各关节电机输出力矩的合力矩及对应关节电机的最大允许输出力矩控制各关节电机输出力矩。
[0028]
所述力矩限制控制器控制电机输出力矩具体为：设定电机最大允许输出力矩为τ
max
，三个控制器输出的合力矩为τ，若|τ|≤|τ
max
|，则电机输出力矩τ，若|τ|》|τ
max
|，则电机输出力矩|τ
max
|
·
sgn(τ)。
[0029]
还包括制动器控制器，其与各关节电机连接，并在各关节电机输出力矩持续达到其最大允许输出力矩设定时长时，对对应关节电机进行制动。
[0030]
在所述制动器控制器对对应关节电机进行制动后，若理论计算的电机输出力矩大于设定阈值δτ，且其力矩方向与所述制动器吸合前所述电机的输出力矩方向相反，则制动器控制器解除对所述关节的制动。
[0031]
一种采用前述机械臂零力控制系统的机械臂零力控制方法，通过所述动力学补偿控制器为各关节提供其所需的补偿力矩，并分别通过所述速度限制控制器和所述位置限制控制器为各关节提供速度限制力矩和位置限制力矩，机械臂各关节电机在前述三个控制器的控制下输出合力矩，以实现机械臂在笛卡尔空间以及关节空间内的速度约束和位置约束。
[0032]
通过力矩限制控制器根据前述三个控制器控制各关节电机输出力矩的合力矩及电机最大允许输出力矩控制电机的输出力矩。
[0033]
通过制动器控制器在各关节电机的输出力矩持续达到其最大允许输出力矩设定时长时对对应关节电机进行制动。
[0034]
有益效果：本发明在传统零力拖动的基础上，增加笛卡尔空间以及关节空间的速度和位置约束，可避免拖动过程中速度过快不能及时制动，以及拖动过程中发生碰撞等问题，使得拖动过程更安全可靠；而且可以限制电机的力矩输出，可避免电机出现因堵转而烧坏等故障，在电机本身出力有限或限制电机输出力矩的前提下，使用制动器作为辅助制动手段，确保速度过快以及位置超限等情况下可安全快速停止，进一步增加系统的安全性。
附图说明
[0035]
图1为本发明受约束的机械臂零力控制系统的逻辑框架图；
[0036]
图2为本发明的动力学补偿控制器逻辑图；
[0037]
图3为本发明的速度限制控制器逻辑图；
[0038]
图4为本发明的位置限制控制器逻辑图；
[0039]
图5为本发明的制动器辅助控制器逻辑图。
具体实施方式
[0040]
下面结合附图和具体实施例，进一步阐明本发明。
[0041]
图1为本发明受约束的机械臂零力控制系统的逻辑框架图，如图1所示，本发明实现安全的受约束的机械臂零力控制系统包括根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩的动力学补偿控制器、根据笛卡尔空间及关节空间的速度限制控制各关节电机输出速度限制力矩的速度限制控制器、根据笛卡尔空间及关节空间的位置限制控制各关节电机输出位置限制力矩的位置限制控制器、根据前述三个控制器控制各关节电机输出力矩的合力矩及对应关节电机的最大允许输出力矩控制各关节电机输出力矩的力矩限制控制器及与各关节电机连接并在各关节电机输出力矩持续达到其最大允许输出力矩设定时长时对对应关节电机进行制动的制动器控制器；
[0042]
其中，动力学补偿控制器根据机械臂动力学方程，通过牛顿欧拉动力学或拉格朗日动力学计算并控制各关节电机输出各个关节所需的补偿力矩，具体为：
[0043]
已知机械臂动力学方程：
[0044][0045]
其中，h(q)表示惯性矩阵，表示关节的转动加速度，表示关节的转动速度，表示关节转动的粘滞摩擦力矩，表示关节转动的库伦摩擦力矩，表示非线性耦合项，g(q)表示关节重力矩，ts表示关节受到的合力矩；
[0046]
为克服关节重力所需的补偿力矩为mg＝g(q)，为克服摩擦力所需的补偿力矩为动力学补偿控制器据此控制各关节电机输出其对应关节所需的补偿力矩mg+mf，那么得到操作者需要额外提供的操作力矩为如图2所示；
[0047]
速度限制包括笛卡尔空间速度限制以及关节空间速度限制，具体为：
[0048]
假定笛卡尔空间速度为，要求笛卡尔空间速度满足，其中，分别表示机械臂的笛卡尔空间速度和角速度，分别表示设定的机械臂的笛卡尔空间速度最大值和角速度最大值；
[0049]
当，则提供笛卡尔空间限制力矩，则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间得到根据笛卡尔空间的速度限制对参与运动的每个关节提供的反向力矩节提供的反向力矩以限制笛卡尔空间的速度过快；其中，k1表示笛卡尔空间速度限制权重，j表示雅可比矩阵；
[0050]
在拖动过程中机械臂的位姿实时变化，在某些特定的位姿下，其关节空间速度存在超限可能，因此，本发明对关节空间速度做以下限制，假定某个关节实时速度为θ，要求关节速度满足|θ|≤θ
max
，其中，θ
max
表示设定的关节速度最大值；当|θ|》θ
max
，提供关节空间限
制力矩其中，k1表示关节空间速度限制权重；
[0051]
据此得到用于速度限制的合力矩为速度限制控制器据此控制各关节电机输出该合力矩，如图3所示；
[0052]
位置限制也包括笛卡尔空间位置限制以及关节空间位置限制，具体为：
[0053]
笛卡尔空间的位置约束主要来源包括但不限于以下方面：
[0054]
a.相邻机械臂即将碰撞；
[0055]
b.机械臂即将与外部设备碰撞；
[0056]
c.机械臂即将到达笛卡尔空间的边界；
[0057]
假设在笛卡尔空间触发限制的边界位置为，其中，p
limx
、p
limy
、p
limz
分别表示笛卡尔空间x、y、z轴方向上的限制边界；要求机械臂在笛卡尔空间的实际位置满足，其中，(p
x
,py,pz)分别表示机械臂在笛卡尔空间的实际位置坐标；
[0058]
当机械臂在笛卡尔空间的实际位置，且机械臂的运动方向依然有加大其在笛卡尔空间的实际位置超出边界位置的趋势时，则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩个关节提供的反向力矩以限制笛卡尔空间的位置超限；其中，k2表示笛卡尔空间位置限制权重；
[0059]
此外在关节空间内，各个关节也需要对其位置进行限制；
[0060]
假定某个关节实时位置为p，要求在拖动过程中关节位置满足|p|≤p
lim
，其中，p
lim
表示在关节空间内某个关节的位置边界；当某个关节的实时位置|p|》p
lim
且运动方向依然有加大其在关节空间内的实时位置超出其位置边界的趋势时，提供关节空间限制力矩其中，k2表示关节空间位置限制权重；
[0061]
据此得到用于位置限制的合力矩为位置限制控制器据此控制各关节电机输出该合力矩，如图4所示。
[0062]
当速度误差以及位置误差比较大时，理论上需要提供的补偿力矩也会较大，可能超过电机能提供的额定力矩，甚至达到峰值力矩，而电机若在堵转状态持续一段时间，将导致电机损坏；为此，有必要对电机的输出力矩进行限制，据此本发明设计了力矩限制控制器；为安
全起见设定电机最大允许输出力矩为τ
max
，三个控制器的输出合力矩为力矩限制控制器判断输出合力矩的值与电机最大允许输出力矩之间的大小，若|τ|≤|τ
max
|，则控制电机输出力矩τ，若|τ|》|τ
max
|，则控制电机输出力矩|τ
max
|
·
sgn(τ)。
[0063]
由于电机本身出力有限且对电机提供的最大力矩进行了限制，因此实际上电机提供的约束力可能无法对机械臂的拖动速度和位置进行理想的约束；
[0064]
为保证约束尽可能达到理想效果，本发明在电机侧设置了制动器，并设计了对应的制动器控制器对其进行控制，从而使用制动器作为辅助约束手段；制动器控制器实时获取各关节电机出力，当检测到对应关节电机出力持续达到其电机最大允许输出力矩τ
max
超过设定时长t时，制动器控制器控制对应的制动器吸合，对对应电机进行制动，从而将对应电机的实际出力清零，此时对应的关节被锁定在当前位置；
[0065]
之后若理论计算的电机出力大于设定阈值δτ，且其力矩方向与制动器吸合前电机输出力矩的方向相反，则制动器控制器控制制动器打开，通过对应电机提供拖动过程中的补偿力。
[0066]
本发明还提供了一种基于前述受约束的机械臂零力控制系统的机械臂零力控制方法包括步骤：
[0067]
通过前述动力学补偿控制器为各关节提供其所需的补偿力矩，并分别通过前述速度限制控制器和前述位置限制控制器为各关节提供速度限制力矩和位置限制力矩，机械臂各关节电机在前述三个控制器的控制下输出合力矩，以实现机械臂在笛卡尔空间以及关节空间内的速度约束和位置约束，通过力矩限制控制器根据前述三个控制器控制各关节电机输出力矩的合力矩及电机最大允许输出力矩控制电机的输出力矩，并在各关节电机的输出力矩持续达到其最大允许输出力矩设定时长时通过制动器控制器控制对应关节电机制动。
[0068]
本发明在传统零力拖动的基础上，增加笛卡尔空间以及关节空间的速度和位置约束，可避免拖动过程中速度过快不能及时制动，以及拖动过程中发生碰撞等问题，使得拖动过程更安全可靠。
[0069]
本发明可以限制电机的力矩输出，可避免电机出现因堵转而烧坏等故障。在电机本身出力有限或限制电机输出力矩的前提下，使用制动器作为辅助制动手段，确保速度过快以及位置超限等情况下可安全快速停止，进一步增加系统的安全性。
[0070]
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

技术特征：
1.一种受约束的机械臂零力控制系统，其特征在于：包括：动力学补偿控制器，用于根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩；速度限制控制器，用于根据笛卡尔空间及关节空间的速度限制控制各关节电机输出速度限制力矩；位置限制控制器，用于根据笛卡尔空间及关节空间的位置限制控制各关节电机输出位置限制力矩；机械臂各关节电机在前述三个控制器的控制下输出合力矩。2.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述动力学补偿控制器根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩具体如下：通过牛顿欧拉动力学或拉格朗日动力学计算各个关节所需的克服关节重力所需的补偿力矩mg和克服摩擦力所需的补偿力矩mf：mg＝g(q)其中，g(q)表示关节重力矩，表示关节的转动速度，表示关节转动的粘滞摩擦力矩，表示关节转动的库伦摩擦力矩。3.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述速度限制控制器为各关节提供速度限制力矩具体如下：假定笛卡尔空间速度为其中，分别表示机械臂的笛卡尔空间速度和角速度；当当分别表示设定的机械臂的笛卡尔空间速度最大值和角速度最大值；则提供笛卡尔空间限制力矩速度最大值；则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间得到根据笛卡尔空间的速度限制对参与运动的每个关节提供的反向力矩向力矩其中k1表示笛卡尔空间速度限制权重，j表示雅可比矩阵；假定某个关节实时速度为θ，当|θ|>θ
max
，提供关节空间限制力矩其中，θ
max
表示设定的关节速度最大值，k1表示关节空间速度限制权重；据此得到速度限制力矩为4.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述位置限制控制器为各关节提供位置限制力矩具体如下：假设在笛卡尔空间触发限制的边界位置为当机械臂在笛卡尔空间的实际位置
且机械臂的运动方向依然有加大其在笛卡尔空间的实际位置超出边界位置的趋势时，则提供笛卡尔空间限制力矩的趋势时，则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩力矩映射到关节空间根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩其中，p
limx
、p
limy
、p
limz
分别表示笛卡尔空间x、y、z轴方向上的限制边界，(p
x
,p
y
,p
z
)分别表示机械臂在笛卡尔空间的实际位置坐标，k2表示笛卡尔空间位置限制权重；j表示雅可比矩阵；假定某个关节实时位置为p，当某个关节的实时位置|p|>p
lim
且运动方向依然有加大其在关节空间内的实时位置超出其位置边界的趋势时，提供关节空间限制力矩其中，p
lim
表示在关节空间内某个关节的位置边界，k2表示关节空间位置限制权重；据此得到用于位置限制的合力矩为5.根据权利要求1所述的机械臂零力控制系统，其特征在于：还包括力矩限制控制器，用于根据前述三个控制器控制各关节电机输出力矩的合力矩及对应关节电机的最大允许输出力矩控制各关节电机输出力矩。6.根据权利要求5所述的机械臂零力控制系统，其特征在于：力矩限制控制器控制电机输出力矩具体为：设定电机最大允许输出力矩为τ
max
，三个控制器输出的合力矩为τ，若|τ|≤|τ
max
|，则电机输出力矩τ，若|τ|>|τ
max
|，则电机输出力矩|τ
max
|
·
sgn(τ)。7.根据权利要求5所述的机械臂零力控制系统，其特征在于：还包括制动器控制器，其与各关节电机连接，并在各关节电机输出力矩持续达到其最大允许输出力矩设定时长时，对对应关节电机进行制动。8.根据权利要求7所述的机械臂零力控制系统，其特征在于：在所述制动器控制器对对应关节电机进行制动后，若理论计算的电机输出力矩大于设定阈值δτ，且其力矩方向与所述制动器吸合前所述电机的输出力矩方向相反，则制动器控制器解除对所述关节的制动。9.一种采用权利要求1～8任一所述的机械臂零力控制系统的机械臂零力控制方法，其特征在于：通过所述动力学补偿控制器为各关节提供其所需的补偿力矩，并分别通过所述速度限制控制器和所述位置限制控制器为各关节提供速度限制力矩和位置限制力矩，机械臂各关节电机在前述三个控制器的控制下输出合力矩，以实现机械臂在笛卡尔空间以及关节空间内的速度约束和位置约束。10.根据权利要求9所述的机械臂零力控制方法，其特征在于：通过力矩限制控制器根据前述三个控制器控制各关节电机输出力矩的合力矩及电机最大允许输出力矩控制电机的输出力矩。11.根据权利要求10所述的机械臂零力控制方法，其特征在于：通过制动器控制器在各关节电机的输出力矩持续达到其最大允许输出力矩设定时长时对对应关节电机进行制动。

技术总结
本发明公开了一种受约束的机械臂零力控制系统及其控制方法，包括：动力学补偿控制器，用于根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩；速度限制控制器，用于根据笛卡尔空间及关节空间的速度限制控制各关节电机输出速度限制力矩；位置限制控制器，用于根据笛卡尔空间及关节空间的位置限制控制各关节电机输出位置限制力矩；机械臂各关节电机在前述三个控制器的控制下输出合力矩。本发明在传统零力拖动的基础上，增加笛卡尔空间以及关节空间的速度和位置约束，可避免拖动过程中速度过快不能及时制动，以及拖动过程中发生碰撞等问题，使得拖动过程更安全可靠。使得拖动过程更安全可靠。使得拖动过程更安全可靠。


技术研发人员：程敏 杨辉 陈云川 袁文 申登伟
受保护的技术使用者：南京佗道医疗科技有限公司
技术研发日：2021.12.09
技术公布日：2022/3/8