(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210547901.7 (22)申请日 2022.05.18 (71)申请人 南京佗道医疗科技有限公司 地址 210012 江苏省南京市雨 花台区大周 路34号3幢 (72)发明人 陈云川　杨辉　桂凯　 (74)专利代理 机构 北京弘权知识产权代理有限 公司 11363 专利代理师 逯长明　许伟群 (51)Int.Cl. B25J 3/04(2006.01) A61B 34/37(2016.01) B25J 3/00(2006.01) B25J 9/04(2006.01) B25J 9/16(2006.01) (54)发明名称 一种主控制臂运动控制方法 (57)摘要 本申请实施例提供了一种主控制臂运动控 制方法， 包括： 一种主控制臂运动控制方法， 其特 征在于， 包括： 根据主控制臂各关节的当前速度 和当前位置得到各关节的第一期望速度； 根据成 本函数以及所述主控制臂各关节的当前位置得 到各关节的第二期望速度， 所述成本函数为所述 主控制臂的一个或多个关节的姿态变化量的函 数； 根据所述第一期望速度和第二期望速度计算 得到各关节的第三期望速度， 根据各关节的第三 期望速度得到各关节的期望位置； 根据各关节的 第三期望速度和期望位置控制所述主控制臂的 各关节进行运动。 本申请解决了主手容易达到 奇 异位姿的问题， 降低了主手的操作难度， 有利于 提高腹腔镜微创手术的安全性和手术效率。 权利要求书2页 说明书9页 附图4页 CN 114770459 A 2022.07.22 CN 114770459 A 1.一种主控制臂运动控制方法， 其特 征在于， 包括： 根据主控制臂各关节的当前速度和当前位置得到各关节的第一期望 速度； 根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第 二期望速度， 所述 成本函数为所述主控制臂的一个或多个关节的姿态变化 量的函数； 根据所述第 一期望速度和第 二期望速度计算得到各关节的第 三期望速度， 根据 各关节 的第三期望 速度得到各关节的期望位置； 根据各关节的第三期望 速度和期望位置控制所述主控制臂的各关节进行运动。 2.根据权利要求1所述的主控制臂运动控制方法， 其特征在于， 根据各关节的第 三期望 速度和期望位置控制所述主控制臂的各关节进行运动， 包括： 通过逆动力学 方法计算各关节的重力前馈； 根据各关节的第 三期望速度、 期望位置、 当前速度、 当前位置以及重力前馈计算目标力 矩； 根据所述目标力矩控制各关节进行运动。 3.根据权利要求1所述的主控制臂运动控制方法， 其特征在于， 所述成本函数的计算方 式为： 其中， i为所述各关节的关节序号， Ci(q)表示关节i的成本函数， qi(t)表示关节i的当前 位置， qimax表示所述关节i的关节最大限位， qimin表示所述关节i的关节最小限位。 4.根据权利要求1所述的主控制臂运动控制方法， 其特征在于， 根据成本函数以及所述 主控制臂各关节的当前位置得到各关节的第二期望 速度， 包括： 获取所述主控制臂各关节的当前位置， 根据所述当前位置计算 雅克比矩阵； 根据所述 雅克比矩阵及其伪逆矩阵， 计算 零空间映射矩阵； 构建成本函数， 计算得到所述成本函数的负梯度， 根据所述负梯度和零空间映射矩阵 得到各关节的第二期望速度， 其中， 所述成本函数为所述主控制臂的一个或多个关节的姿 态变化量的函数。 5.根据权利要求4所述的主控制臂运动控制方法， 其特征在于， 构建成本函数， 计算得 到所述成本函数的负梯度， 根据所述负梯度和零空间映射矩阵得到各关节的第二期望速 度， 包括： 构建成本函数， 计算得到所述成本函数的负梯度， 根据所述负梯度、 零空间 映射矩阵以 及步长得到各关节的第二期望 速度。 6.根据权利要求5所述的主控制臂运动控制方法， 其特征在于， 所述步长根据操作者的 操作速度实时计算得到， 计算公式包括： 其中， x1为所述步长， w为所述主控制臂对应的末端角速度， w0为预设角速度， x0为所述主控制臂对应的末端角速 度为w0时的步长 。 7.根据权利要求1所述的主控制臂运动控制方法， 其特征在于， 根据所述第 一期望速度 和第二期望 速度计算得到各关节的第三期望 速度， 包括： 对第一期望速度和第 二期望速度进行加权计算得到各关节的第 三期望速度， 计算公式权　利　要　求　书 1/2 页 2 CN 114770459 A 2包括： dqd＝k1*dq1+k2*dq2 其中， dqd为所述第三期望速度， k1为所述第一期望速度的权重系数， k2为所述第二期望 速度的权 重系数。 8.根据权利要求7 所述的主控制臂运动控制方法， 其特 征在于， k1、 k2均为常数。 9.根据权利要求7所述的主控制臂运动控制方法， 其特征在于， k2为常数， k1根据操作 者的操作速度实时计 算得到， 计 算公式包括： k1＝k0*(w/w0)， 其中， w为 所述主控制臂对应的 末端角速度， w0为预设角速度， k0为k1在所述主控制臂对应的末端角速度为 w0时的取值。 10.根据权利要求1所述的主控制臂运动控制方法， 其特征在于， 根据主控制臂各关节 的当前速度和当前位置得到各关节的第一期望 速度， 包括： 获取所述主控制臂各关节的当前位置和当前速度， 根据 所述当前位置计算雅克比矩阵 及其伪逆矩阵； 根据各关节的当前速度和雅克比矩阵， 计算各关节对应的末端角速度； 根据各关节的末端角速度和雅克比伪逆矩阵， 计算得到各关节的第一期望 速度。权　利　要　求　书 2/2 页 3 CN 114770459 A 3