[发明专利]一种机器人末端器械的控制方法

说明书

本发明公开了一种机器人末端器械的控制方法，包括：根据更换前后器械的长度差确定更换后器械的目标位置；根据机器人动力学模型对器械进行重力矩和摩擦力矩补偿，控制器械运动；当更换后器械运动到距离目标位置为预设距离的位置时，采用具有固定重力补偿的PD控制模型对其进行控制。本发明可以实现器械拖动过程中平稳停止，且可以根据安装器械的不同调整记忆位置，实现器械末端位置保持一致。

技术领域

本发明涉及机器人技术领域，尤其涉及一种机器人末端器械的控制方法。

背景技术

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

医生通过操作主手控制从臂末端器械完成复杂的手术操作，但是手术过程中不可避免地会出现更换末端器械的情况，这时候就会存在以下问题：①更换器械后，医生无法准确记忆插拔器械前的目标位置，需要再次调整位置，浪费时间和精力；②重新安装器械后拖动位置离之前目标位置较远需要不断调整，或者拖动力过大导致器械运动超过目标位置，损伤器官；③不同器械的长度不同，更换器械后安装位置和之前有所不同，若根据记忆位置调整可能造成较大偏差。

现有的控制方案中，当拖动器械至目标位置时电机突然掉使能会让人产生电机故障的错觉，而且停止动作太过突然，影响医生操作体验感。另外，电机非使用状态后需要再次使能才可以完成后续操作，每次更换器械都需要重复此动作，电机重复使能无疑会削减电机使用寿命。

发明内容

发明目的：针对上述不足，本发明提出一种机器人末端器械的控制方法，可以实现器械拖动过程中平稳停止，运动和停止的连续动作更加连贯柔顺，且可以根据安装器械的不同调整记忆位置，实现器械末端位置保持一致。

技术方案：

一种机器人末端器械的控制方法，包括：

根据更换前后器械的长度差确定更换后器械的目标位置；

根据机器人动力学模型对器械进行重力矩和摩擦力矩补偿，控制器械运动；当更换后器械运动到距离目标位置为预设距离的位置时，采用具有固定重力补偿的PD控制模型对其进行控制。

所述具有固定重力补偿的PD控制模型具体为：

其中，τ为目标力矩；q_d为器械的期望位置，G(q_d)为器械在目标位置时的重力矩；为器械的期望速度和当前速度之间的偏差；e为器械的期望位置和当前位置之间的偏差；Kp、Kd分别为PD控制的位置变化量和速度变化量的权重系数。

Kp和Kd的计算方式如下：

Kp＝ω²h

Kd＝2ωζh

其中，h为末端器械惯性参数，根据器械的结构设计得到；ζ为系统阻尼系数；ω为自然频率，根据不同器械运动辨识得到。

根据不同器械运动辨识得到自然频率具体为：

当器械运动到距离目标位置为预设距离的位置时，控制器械以预设速度开始减速运动到目标位置，分别获取器械位于距离目标位置为预设距离的位置和目标位置的重力矩，代入PD控制模型求解得到器械的自然频率。

在求解得到的自然频率的数值中，选取较小的数值作为器械的自然频率。

所述预设距离为5mm以内。

在器械上设有RFID设备以供读取该器械的参数信息，进而得到更换前后器械的长度差。