CN112975987A - 一种基于动力学模型的骨科手术机器人控制方法 - Google Patents

Abstract

本发明公开了一种基于动力学模型的骨科手术机器人控制方法，涉及机器人领域，包括：通过坐标变化关系和重力矢量确定待校准关节受到的总重力矩；将stribeck模型中与摩擦力相关的系数作为待辨识参数、构建得到待辨识矩阵；通过LM算法迭代求解得到待辨识矩阵的初始解并作为遗传算法的初始化种群；基于遗传算法得到待辨识矩阵的迭代结果；将迭代结果代入stribeck模型中得到待校准关节的总摩擦力矩；得到待校准关节的力矩合；通过力矩合得到待校准关节的对应电机的位置信号值，根据位置信号值驱动待校准关节。通过计算力矩合避免外力对骨科手术机器人的定位准确性的影响，有效提高了稳定性和安全性。

Description

一种基于动力学模型的骨科手术机器人控制方法

技术领域

本发明涉及机器人领域，尤其是一种基于动力学模型的骨科手术机器人控制方法。

背景技术

随着科学技术的不断提高，越来越多的机器人进入到医疗领域中，而骨科手术机器人通过其微创和快捷等特点正成为医生的得力助手。

骨科手术机器人主要目标是在骨科手术中辅助医生完成特定的精细操作，例如，骨科手术机器人能够完成截骨切削、磨削钻孔和夹持固定等动作，这些操作由机器人完成具有很大的优势，一方面是由于这些小手术动作较为简单，骨科手术机器人能够胜任，同时这些操作需要一定的稳定性，通过骨科手术机器人能够大大减少医生的作业强度，节约手术时间，避免医生在长时间的手术过程中保持相同姿势而带来的肌肉劳损，从而提高手术的安全性。

目前的骨科手术机器人的基本工作方式有手动模式和自动模式，手动模式需要在手术过程中配备一名操作员，操作员通过医生的口述指令完成相关的动作操作，过程较为繁琐，实际使用中也不如人工来的简便；而自动模式相比于手动模式优势明显，骨科手术机器人能够接受手术前就设置好的移动指令来进行操作，但现有的骨科手术机器人在移动过程中不能主动对自身进行位置校准，存在一定的误差，从而无法确保手术的标准化和安全性。

发明内容

本发明人针对上述问题及技术需求，提出了一种基于动力学模型的骨科手术机器人控制方法，本发明的技术方案如下：

一种基于动力学模型的骨科手术机器人控制方法，所述骨科手术机器人包括底座和多个关节，每个所述关节通过对应设置的电机驱动，其特征在于，所述方法包括：

确定待校准关节的待校准关节坐标系和其他关节的关节坐标系在底座的基准坐标系中的坐标变化关系，并确定待校准关节在所述待校准关节坐标系中的重力矢量；

通过所述坐标变化关系和所述重力矢量确定每个关节对所述待校准关节的重力矩并累加得到所述待校准关节受到的总重力矩；

将所述骨科手术机器人的关于摩擦力与摩擦力矩的stribeck模型中与摩擦力相关的系数作为待辨识参数、构建得到待辨识矩阵；

通过LM算法迭代求解得到所述待辨识矩阵的初始解并作为遗传算法的初始化种群；

基于遗传算法得到所述待辨识矩阵的迭代结果；

将所述迭代结果代入所述stribeck模型中得到所述待校准关节的总摩擦力矩；

基于动力学模型根据所述总重力矩和所述总摩擦力矩得到所述待校准关节的力矩合；

通过所述力矩合得到所述待校准关节的对应电机的位置信号值，根据所述位置信号值驱动所述待校准关节。

其进一步的技术方案为，所述重力矢量g_i的计算公式为：

其中，g₀为待校准关节在基座坐标系的重力矢量，g₀＝[0,0,-9.81]^T，

表示所述基座坐标系变换至所述待校准关节坐标系中的旋转矩阵。

其进一步的技术方案为，所述确定每个关节对所述待校准关节的重力矩，包括对于任意第j个关节：

根据所述待校准关节在所述待校准关节坐标系中的重力矢量确定所述第j个关节在所述待校准关节坐标系下的重力转化值

计算公式为：

其中，mj表示所述第j个关节的质量，g_i表示待校准关节在所述待校准关节坐标系中的重力矢量；

根据所述待校准关节坐标系与所述第j个关节的关节坐标系之间坐标变化关系确定所述第j个关节在所述待校准关节坐标系下的重力坐标

计算公式为：

其中，

表示待校准关节坐标系和第j个关节坐标系的坐标变化关系，rj表示第j个关节在第j个关节坐标系中的坐标；

根据所述第j个关节在所述待校准关节坐标系下的重力转化值和所述第j个关节在所述待校准关节坐标系下的重力坐标得到所述第j个关节对所述待校准关节的重力矩。

其进一步的技术方案为，所述重力矩

的计算公式为：

其中，

表示第j个关节在所述待校准关节坐标系下的重力转化值，

表示第j个关节在所述待校准关节坐标系下的重力坐标

的Z轴坐标。

其进一步的技术方案为，所述总重力矩的计算公式为：

其中，X表示所述骨科手术机器人的自由度，

表示第j个关节对所述待校准关节的重力矩。

其进一步的技术方案为，所述stribeck模型的表达式为：

其中，τ_f表示摩擦力矩；f_s表示静摩擦力；w表示角速度；w_s表示stribeck速度；f_v表示粘性摩擦力，其计算公式为：

f_c表示库伦摩擦力，其计算公式为：

其中，μ_v表示粘性摩擦系数，μ_c表示库伦摩擦系数，

表示关节转角的一阶导数，则所述待辨识矩阵包括粘性摩擦系数μ_v、库伦摩擦系数μ_c、静摩擦力f_s和stribeck速度w_s。

其进一步的技术方案为，所述通过LM算法迭代求解得到所述待辨识矩阵的初始解并作为遗传算法的初始化种群，包括：

对所述stribeck模型进行预定采样次数的模拟训练得到关于所述待辨识矩阵的模拟预测值，并确定每次采样的所述模拟预测值与所述待辨识矩阵之间的关系得到偏差关系式；

求解

得到最小二乘结果作为LM算法的初始值：

其中，Y表示待辨识矩阵，D_n表示偏差关系式，N表示预定采样次数，n为参数，n表示第n次采样；

通过LM算法迭代求解所述初始值得到所述待辨识矩阵的初始解。

其进一步的技术方案为，所述基于遗传算法得到所述待辨识矩阵的迭代结果，包括：

通过适应度函数得到每个个体的适应度，每个所述个体对应每个所述待辨识参数；

确定当前迭代次数对应的每个个体的适应度，若所述当前迭代次数未达到预定迭代次数，则对每个所述个体进行选择操作、交叉操作和变异操作，令迭代次数加一并重复所述通过适应度函数得到每个个体的适应度的步骤；

若当前迭代次数达到预定迭代次数时，则得到所述待辨识矩阵的迭代结果。

其进一步的技术方案为，所述动力学模型的表达式为：

其中，q表示关节转角，

和

分别表示关节转角的一阶和二阶导数，

表示惯性项，

表示非线性耦合力矩项，g(q)表示总重力矩，τ_ext表示外界环境与待校准关节接触产生的外力矩，τf表示总摩擦力矩，τ表示力矩合。

其进一步的技术方案为，所述通过所述力矩合得到所述待校准关节的对应电机的位置信号值，包括：

通过力矩合与电机的位置信号值的关系式得到所述待校准关节的对应电机的位置信号值，其表达式为：

其中，K_m表示位置环增益矩阵，K_v表示速度环增益矩阵，K_p表示电机转矩灵敏度，q_d表示位置信号值，q表示关节转角，

表示关节转角的一阶导数，τ表示力矩合。

本发明的有益技术效果是：通过计算力矩合避免外力对骨科手术机器人的定位准确性的影响，有效提高了稳定性和安全性；同时stribeck模型的参数较少，同时对零速度下的摩擦特性具有很好的描述，是一种能够完整描述初始状态下的摩擦模型；通过LM算法能够保证函数不会陷入局部最小值，从而使得初始解较大程度的靠近最优解，便于后续的计算过程。

附图说明

图1是本申请的控制方法的处理流程图。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步说明。

一种基于动力学模型的骨科手术机器人控制方法，处理流程如图1所示，骨科手术机器人包括底座和安装在底座上的多个驱动骨科手术机器人运行的关节，每个关节由自身的电机进行驱动，同时每个关节可以看成一个杆件，因此每个关节都可以得到关节坐标系，不同的关节在不同的关节坐标系中的坐标是不同的，由此确定待校准关节的待校准关节坐标系和其他关节的关节坐标系在底座的基准坐标系中的坐标变化关系，关节坐标系的建立是机器人领域的一种常规方法，在本申请中使用D-H方法进行建立，同时也可以利用其它方法进行建立，本申请不做过多限定。

关节在移动过程中，主要受到外力和内力作用，内力作用主要包括惯性力矩、离心力和科里奥利力，内力作用对骨科手术机器人的影响较小，而往往是外力会对骨科手术机器人的误差产生较大影响，外力作用主要包括其他关节的作用下的重力矩、关节的摩擦力矩和与外界环境发生碰撞产生的力矩，其中与外界环境发生碰撞产生的力矩可以通过在关节上安装传感器来获得，而其他两个力矩则难以获取，因此这两个力矩的计算是本申请的重点。

步骤一：关节在移动过程中，会受到其他关节的重力作用，因此基于各关节的质心参数，从而确定待校准关节受其他关节重力作用的总重力矩：

确定待校准关节i在待校准关节坐标系中的重力矢量g_i为：

其中，g₀为待校准关节在基座坐标系的重力矢量，是一个已知量，即g₀＝[0,0,-9.81]^T

，

表示基座坐标系变换至待校准关节坐标系中的旋转矩阵；

确定第j个关节在待校准关节坐标系下的重力矩转换值

其计算公式为：

其中，m_j表示第j个关节的质量；

根据待校准关节坐标系与第j个关节的关节坐标系之间坐标变化关系确定第j个关节在待校准关节坐标系下的重力坐标

计算公式为：

其中，

表示待校准关节i在第j个关节坐标系的坐标变化关系，rj表示第j个关节在第j个关节坐标系中的坐标；

则第j个关节对待校准关节i的重力矩

的计算公式为：

其中，

表示重力坐标

的Z轴坐标。

因此将每个关节对待校准关节i的重力矩大小累加得到待校准关节受到的重力矩g(q)，其计算公式为：

其中，X表示骨科手术机器人的自由度，对于六轴自由度的骨科手术机器人来说此时X取6。

步骤二：建立stribeck模型用于计算摩擦力矩。

骨科手术机器人的关节采用一体化结构，一体化结构包括谐波减速器和直流无刷伺服电机，因此关节的摩擦力主要来源于电机轴承上的摩擦力和谐波减速器中钢轮柔轮啮合摩擦。

由此根据关节的结构特点建立关于摩擦力与摩擦力矩的stribeck模型：

其中，τ_f表示摩擦力矩；f_s表示静摩擦力；w表示角速度；w_s表示stribeck速度；f_v表示粘性摩擦力，其计算公式为：

f_c表示库伦摩擦力，其计算公式为：

其中，μ_v表示粘性摩擦系数，μ_c表示库伦摩擦系数，

表示关节转角的一阶导数。

stribeck模型的参数较少，同时对零速度下的摩擦特性具有很好的描述，是一种能够完整描述初始状态下的摩擦模型。

随后将stribeck模型的表达式更新为：

基于stribeck模型确定待校准关节的待辨识矩阵Y，待辨识矩阵包括多个待辨识参数，待辨识参数是与摩擦力相关的系数，则确定待辨识矩阵包括粘性摩擦系数μ_v、库伦摩擦系数μ_c、静摩擦力f_s和stribeck速度w_s，即Y＝(μ_c、μ_v、f_s、w_s)。

辨识结果将会影响到最终计算得到摩擦力矩的精度，本申请采用LM算法和遗传算法进行辨识计算。

步骤三：基于遗传算法得到待辨识矩阵的迭代结果，通过LM算法迭代求解得到待辨识矩阵的初始解，初始解作为遗传算法的初始化种群；

对stribeck模型进行预定采样次数N的模拟训练得到关于待辨识矩阵的模拟预测值y，预定采样次数N通常大于500次，并确定每次采样的模拟预测值与待辨识矩阵之间的关系得到偏差关系式D_n，其表达式为：

D_n＝y-Y；

求解

得到最小二乘结果作为LM算法的初始值：

其中，Y表示待辨识矩阵，D_n表示偏差关系式，N表示预定采样次数，n为参数，n表示第n次采样；

通过LM(Levenberg-Marquard)算法迭代求解得到待辨识矩阵的初始解。

LM算法能够保证函数不会陷入局部最小值，从而使得初始解较大程度的靠近最优解，便于后续的计算过程。

通过适应度函数得到每个个体的适应度，适应度函数利用现有的遗传算法中的即可，适应度是用来评价每个个体在优化计算过程中有可能达到或接近于最优解的优良程度，每个个体对应每个待辨识参数；

随后进入迭代过程，确定当前迭代次数对应的每个个体的适应度，若当前迭代次数未达到预定迭代次数T，预定迭代次数T也称为终止代数，表示遗传算法运行结束条件的一个参数。

随后对每个个体进行选择操作，产生新一代的种群，随后通过交叉概率Pc进行交叉操作，再通过变异概率Pm进行个体变异操作，并令当前迭代次数加一并重复通过适应度函数得到每个个体的适应度的步骤；

当前迭代次数达到预定迭代次数T时，则得到待辨识矩阵的迭代结果，最终的输出结果为：

Yf＝(μ_cf、μ_vf、f_sf、w_sf)。

则辨识后的stribeck模型表达式为：

步骤四：将迭代结果代入stribeck模型中得到待校准关节的总摩擦力矩。

步骤五：对取得的迭代结果进行验算，本申请引入拟合优度，其表示模拟预测值与实际数据均值回归平方和模拟预测值与实际数据的残差平方和相除所得的比值，拟合优度的取值范围为[0,1],当其值越接近1时，其拟合程度越好。

步骤六：基于动力学模型通过总重力矩和总摩擦力矩得到待校准关节的力矩合；

动力学模型的表达式为：

其中，q表示关节转角，

和

分别表示关节转角的一阶和二阶导数，

表示惯性项，

表示非线性耦合力矩项，与离心力和科里奥利力有关，g(q)表示总重力矩，τ_ext表示外界环境与待校准关节接触产生的力矩，τ_f表示总摩擦力矩，τ表示力矩合。

步骤七：通过力矩合得到待校准关节的对应电机的位置信号值，根据位置信号值驱动所述待校准关节到达指定位置。

通过力矩合与电机的位置信号值的关系式得到待校准关节的对应电机的位置信号值，其表达式为：

其中，K_m表示位置环增益矩阵，K_v表示速度环增益矩阵，K_p表示电机转矩灵敏度、q_d表示位置信号值q表示关节转角，

表示关节转角的一阶导数，τ表示力矩合。

K_m，K_v，K_p是与电机型号相关的参数，可以直接通过电机型号获取得到。

以上所述的仅是本申请的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

Claims (10)

1.一种基于动力学模型的骨科手术机器人控制方法，所述骨科手术机器人包括底座和多个关节，每个所述关节通过对应设置的电机驱动，其特征在于，所述方法包括：

确定待校准关节的待校准关节坐标系和其他关节的关节坐标系在底座的基准坐标系中的坐标变化关系，并确定待校准关节在所述待校准关节坐标系中的重力矢量；

通过所述坐标变化关系和所述重力矢量确定每个关节对所述待校准关节的重力矩并累加得到所述待校准关节受到的总重力矩；

将所述骨科手术机器人的关于摩擦力与摩擦力矩的stribeck模型中与摩擦力相关的系数作为待辨识参数、构建得到待辨识矩阵；

通过LM算法迭代求解得到所述待辨识矩阵的初始解并作为遗传算法的初始化种群；

基于遗传算法得到所述待辨识矩阵的迭代结果；

将所述迭代结果代入所述stribeck模型中得到所述待校准关节的总摩擦力矩；

基于动力学模型根据所述总重力矩和所述总摩擦力矩得到所述待校准关节的力矩合；

通过所述力矩合得到所述待校准关节的对应电机的位置信号值，根据所述位置信号值驱动所述待校准关节。

2.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述重力矢量g_i的计算公式为：

其中，g₀为待校准关节在基座坐标系的重力矢量，g₀＝[0,0,-9.81]^T，

表示所述基座坐标系变换至所述待校准关节坐标系中的旋转矩阵。

3.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述确定每个关节对所述待校准关节的重力矩，包括对于任意第j个关节：

根据所述待校准关节在所述待校准关节坐标系中的重力矢量确定所述第j个关节在所述待校准关节坐标系下的重力转化值

计算公式为：

其中，m_j表示所述第j个关节的质量，g_i表示待校准关节在所述待校准关节坐标系中的重力矢量；

根据所述待校准关节坐标系与所述第j个关节的关节坐标系之间坐标变化关系确定所述第j个关节在所述待校准关节坐标系下的重力坐标

计算公式为：

其中，

表示待校准关节坐标系和第j个关节坐标系的坐标变化关系，r_j表示第j个关节在第j个关节坐标系中的坐标；

根据所述第j个关节在所述待校准关节坐标系下的重力转化值和所述第j个关节在所述待校准关节坐标系下的重力坐标得到所述第j个关节对所述待校准关节的重力矩。

4.根据权利要求3所述的骨科手术机器人控制方法，其特征在于，所述重力矩

的计算公式为：

其中，

表示第j个关节在所述待校准关节坐标系下的重力转化值，

表示第j个关节在所述待校准关节坐标系下的重力坐标

的Z轴坐标。

5.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述总重力矩的计算公式为：

其中，X表示所述骨科手术机器人的自由度，

表示第j个关节对所述待校准关节的重力矩。

6.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述stribeck模型的表达式为：

其中，τ_f表示摩擦力矩；f_s表示静摩擦力；w表示角速度；w_s表示stribeck速度；f_v表示粘性摩擦力，其计算公式为：

f_c表示库伦摩擦力，其计算公式为：

其中，μ_v表示粘性摩擦系数，μ_c表示库伦摩擦系数，

表示关节转角的一阶导数，则所述待辨识矩阵包括粘性摩擦系数μ_v、库伦摩擦系数μ_c、静摩擦力f_s和stribeck速度w_s。

7.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述通过LM算法迭代求解得到所述待辨识矩阵的初始解并作为遗传算法的初始化种群，包括：

对所述stribeck模型进行预定采样次数的模拟训练得到关于所述待辨识矩阵的模拟预测值，并确定每次采样的所述模拟预测值与所述待辨识矩阵之间的关系得到偏差关系式；

求解

得到最小二乘结果作为LM算法的初始值；

其中，Y表示待辨识矩阵，D_n表示偏差关系式，N表示预定采样次数，n为参数，n表示第n次采样；

通过LM算法迭代求解所述初始值得到所述待辨识矩阵的初始解。

8.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述基于遗传算法得到所述待辨识矩阵的迭代结果，包括：

通过适应度函数得到每个个体的适应度，每个所述个体对应每个所述待辨识参数；

确定当前迭代次数对应的每个个体的适应度，若所述当前迭代次数未达到预定迭代次数，则对每个所述个体进行选择操作、交叉操作和变异操作，令迭代次数加一并重复所述通过适应度函数得到每个个体的适应度的步骤；

若当前迭代次数达到预定迭代次数时，则得到所述待辨识矩阵的迭代结果。

9.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述动力学模型的表达式为：

其中，q表示关节转角，

和

分别表示关节转角的一阶和二阶导数，

表示惯性项，

表示非线性耦合力矩项，g(q)表示总重力矩，τ_ext表示外界环境与待校准关节接触产生的外力矩，τ_f表示总摩擦力矩，τ表示力矩合。

10.根据权利要求1所述的骨科手术机器人控制方法，其特征在于，所述通过所述力矩合得到所述待校准关节的对应电机的位置信号值，包括：

通过力矩合与电机的位置信号值的关系式得到所述待校准关节的对应电机的位置信号值，其表达式为：

其中，K_m表示位置环增益矩阵，K_v表示速度环增益矩阵，K_p表示电机转矩灵敏度，q_d表示位置信号值，q表示关节转角，

表示关节转角的一阶导数，τ表示力矩合。

Images