[发明专利]一种主控制臂运动控制方法

说明书

本申请实施例提供了一种主控制臂运动控制方法，包括：一种主控制臂运动控制方法，其特征在于，包括：根据主控制臂各关节的当前速度和当前位置得到各关节的第一期望速度；根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第二期望速度，所述成本函数为所述主控制臂的一个或多个关节的姿态变化量的函数；根据所述第一期望速度和第二期望速度计算得到各关节的第三期望速度，根据各关节的第三期望速度得到各关节的期望位置；根据各关节的第三期望速度和期望位置控制所述主控制臂的各关节进行运动。本申请解决了主手容易达到奇异位姿的问题，降低了主手的操作难度，有利于提高腹腔镜微创手术的安全性和手术效率。

技术领域

本申请涉及主控制臂控制技术领域，尤其涉及一种主控制臂运动控制方法。

背景技术

微创外科手术指利用腹腔镜、胸腔镜等内窥镜及相关设备进行的手术。相较于传统的开放式手术，微创外科手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，腹腔镜手术机器人系统在微创外科手术中应用越来越广泛。

腹腔镜手术机器人系统包括主手和从手，主手设置有主控制臂和连接在主控制臂上的夹持端，主控制臂包括多个关节以及连杆，从手设置有操纵臂和安装在操纵臂上的内窥镜和手术器械。腹腔镜手术机器人系统为主从式操作系统，操作者通过主手的夹持端可控制从手上的手术器械和腹腔镜进行运动。在操作主手的过程中，当夹持端的多个连杆接近平行时，主手会到达奇异位姿，操作者难以进行俯仰操作。通常此时需要断开主手和从手之间的主从连接，将主手复位后再重新建立主从连接，然而，该方法会导致手术暂停，降低了手术的连续性，增加了手术风险。

发明内容

为解决主手控制效果不佳的技术问题，本申请提供了一种主控制臂运动控制方法。

本申请提供了一种主控制臂运动控制方法，该方法包括：

根据主控制臂各关节的当前速度和当前位置得到各关节的第一期望速度；

根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第二期望速度，所述成本函数为所述主控制臂的一个或多个关节的姿态变化量的函数；

根据所述第一期望速度和第二期望速度计算得到各关节的第三期望速度，根据各关节的第三期望速度得到各关节的期望位置；

根据各关节的第三期望速度和期望位置控制所述主控制臂的各关节进行运动。

在一些实施例中，根据各关节的第三期望速度和期望位置控制所述主控制臂的各关节进行运动，包括：

通过逆动力学方法计算各关节的重力前馈；

根据各关节的第三期望速度、期望位置、当前速度、当前位置以及重力前馈计算目标力矩；

根据所述目标力矩控制各关节进行运动。

在一些实施例中，所述成本函数的计算方式为：

其中，i为所述各关节的关节序号，C_i(q)表示关节i的成本函数，q_i(t)表示关节i的当前位置，q_imax表示所述关节i的关节最大限位，q_imin表示所述关节i的关节最小限位。

在一些实施例中，根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第二期望速度，包括：

获取所述主控制臂各关节的当前位置，根据所述当前位置计算雅克比矩阵；

根据所述雅克比矩阵及其伪逆矩阵，计算零空间映射矩阵；

构建成本函数，计算得到所述成本函数的负梯度，根据所述负梯度和零空间映射矩阵得到各关节的第二期望速度，其中，所述成本函数为所述主控制臂的一个或多个关节的姿态变化量的函数。