[发明专利]一种受约束的机械臂零力控制系统及其控制方法

说明书

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

技术领域

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

背景技术

目前微创手术已基本取代开放手术成为外科医学领域发展的主要方向。相比于传统的开放式手术，微创手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，以达芬奇手术机器人为代表的基于机器人辅助系统的微创手术逐渐成熟，并被广泛应用。

腹腔手术机器人可适应不同科室的多种手术类型，但不同的手术类型需要不同的术前姿态。因此要求手术机器人的机械臂可实现灵活的空间拖动，从而方便医护人员实现术前摆位。同样地，对于腹腔手术机器人系统中的主操作手而言，也需要实现灵活的空间拖动，以使得医生可以轻松地完成手术操作。

不论是床旁手术机器人的术前预调整还是主操作手的操作，为实现机械臂的灵活拖动，常用的技术方案是，首先建立机械臂模型，再通过连杆运动学及动力学计算获取各个关节所需补偿的重力以及摩擦力等，最后由关节电机提供补偿力矩，从而可使得机械臂在关节空间内灵活拖动。

但是考虑到手术机器人应用场合的特殊性，在床旁手术机器人的术前预调整以及主操作手的操作过程中，如果拖动速度过快，容易出现机械臂不受控制的情况。而且，如果机械臂运动到笛卡尔空间以外，将与其他机械臂或设备发生碰撞。因此需要对其进行限速限位等处理，以确保术前摆位的安全性。

发明内容

发明目的：针对上述不足，本发明提出了一种受约束的机械臂零力控制系统及其控制方法，可避免拖动过程中速度过快不能及时制动，以及拖动过程中发生碰撞等问题，使得拖动过程更安全可靠。

技术方案：

一种受约束的机械臂零力控制系统，包括：

动力学补偿控制器，用于根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩；

速度限制控制器，用于根据笛卡尔空间及关节空间的速度限制控制各关节电机输出速度限制力矩；

位置限制控制器，用于根据笛卡尔空间及关节空间的位置限制控制各关节电机输出位置限制力矩；

机械臂各关节电机在前述三个控制器的控制下输出合力矩。

所述动力学补偿控制器根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩具体如下：

通过牛顿欧拉动力学或拉格朗日动力学计算各个关节所需的克服关节重力所需的补偿力矩Mg和克服摩擦力所需的补偿力矩Mf：

Mg＝g(q)

其中，g(q)表示关节重力矩，表示关节的转动速度，表示关节转动的粘滞摩擦力矩，表示关节转动的库伦摩擦力矩。

所述速度限制控制器为各关节提供速度限制力矩具体如下：

假定笛卡尔空间速度为，其中，分别表示机械臂的笛卡尔空间速度和角速度；