一种基于粒子群算法的力反馈设备动力学参数估计算法的制作方法

本发明涉及力反馈设备参数的估计，具体为一种基于粒子群算法的力反馈设备动力学参数估计算法。

背景技术：

由于力触觉在人类感觉中具有显著的地位，研究者们尝试将力触觉特性引入遥操作机器人、虚拟现实等研究领域，为操作者在人机交互的过程中提供力触觉反馈信息。例如，核电站发生事故时，需要通过遥操作机器人完成检测、维修等任务，此时视频监控面临严重干扰，操作者借助力触觉反馈信息，可以有效提高操作的准确性、可靠性。在虚拟手术中，医生在虚拟人体上进行实时手术仿真训练，增加力触觉反馈信息，可以大幅提高培训效率，缩短培训周期，减轻一系列成本开销。另外，力触觉也常常应用在深海作业、医疗康复机器人、微创手术以及游戏和教育中，给操作者提供真实的力感和触感。而力触觉的实现依赖于力反馈设备，因此，力反馈设备的性能尤为重要。通常来说，力反馈设备的性能与两方面的因素有关。一方面与结构、减速器、传感器等硬件的选取有关，另一方面控制算法也是影响其性能的关键因素。通过控制算法可以消除力反馈设备中的自身惯量、摩擦力、重力等因素的影响，能够改进系统的透明性以及力反馈的逼真性。因此，对力反馈设备及其相关的控制算法进行研究是非常有意义的，是当前乃至今后一段时期内研究的热点。

研究力反馈设备控制算法的基础前提是分析力反馈设备的运动学和动力学。其中，动力学模型的精确建立，对于设备保持高速、高精度的运行以及改进控制器性能等方面都起着重要的作用，同时也能够降低设计的成本、减少研究时间。建立力反馈设备的关节动力学模型，需要得到该设备的动力学参数以及摩擦力参数，即确定各个关节的关节质量、惯性张量、质心、库伦摩擦力以及粘滞摩擦力等参数。然而，通常情况下，力反馈设备参数是未知或不准确的，并且设备在长期的使用过程中，会受到磨损、形变以及环境等因素的影响，导致参数产生偏差。因此，需要对动力学参数进行估计。如果参数估计的越准确，动力学模型的精度就会越高，这也就能使各关节力矩的估计值越精确，为操纵者提供一个更精准的力反馈。

获取力反馈设备参数的方法有：解体测量法，指通过将设备解体，然后测量设备的几何参数及其材料参数，然后得出其动力学参数的方法。该方法对设备关节质量、惯性张量、质心的获取是比较准确的。但操作复杂，可执行性低，并且参数值的结果容易受到设备的形状、材料等因素的影响，同时在解体过程中有可能会损伤或无法还原设备；cad建模法，建立在计算机图形技术基础之上的，根据设备的结构图，利用相应的动力学模型，求解出设备的参数。能够便捷的获得独立的参数值，并且在设备的设计阶段就可以获得力反馈设备的参数值，然后根据参数值来计算机器人的动力学特性。但是该方法容易受到连杆材质不均匀等设备制造工艺的影响，使动力学参数存在一定的误差；估算方法，无论是解体测量法还是cad建模法，除了存在上述问题外，都未考虑到设备在长期的使用过程中，会受到磨损、形变以及环境等因素的影响，导致模型参数产生偏差。honegger等人提出一种利用非线性自适应控制算法(详见：adaptivecontrolofthehexaglide,a6dofparallelmanipulator[c].proceedingsofinternationalconferenceonroboticsandautomation.ieee,1997,1:543-548.)对动力学参数进行在线辨识，但是该方法无法保证参数收敛。由于粒子群算法具有简单易行、快速收敛的优点，hosseinjahandideh等人利用粒子群算法(详见：useofpsoinparameterestimationofrobotdynamics；partone:noneedforparameterization[c].internationalconferenceonsystemtheory.ieee,2012.)对动力学参数进行估计，在该方法中，粒子各维度分别代表动力学模型的实际参数或者组合参数，根据假设的动力学模型，以力矩误差作为目标函数，对参数进行估计，相比于其它估计算法，pso算法估计参数具有收敛速度快的特点。但估计的力矩误差较大，参数估计的效果较差，更容易陷入局部最优。

技术实现要素：

基于上述背景，本发明提供了一种基于粒子群算法的力反馈设备动力学参数估计算法。具体为用一种改进的全面学习粒子群(improvedcomprehensivelearningparticleswarmoptimization,iclpso)算法来更好的估计力反馈设备的动力学参数。本发明是通过以下技术方案实现的。

本发明所述的一种基于粒子群算法的力反馈设备动力学参数估计算法，按以下步骤：

通用力反馈设备的动力学方程具体如下：

其中，θ为关节角；为关节角速度；为关节角加速度；d为力反馈设备的惯性矩阵；c为离心力和科里奥利力矩阵；g为重力矩阵；fc为库伦摩擦系数矩阵；fv为黏滞摩擦系数矩阵；τ为关节力矩；δx为考虑建模不确定性而引入的补偿项。

需要估计的参数除了参数x外，还有补偿参数δx，如下所示：

x＝[l1,l2,l3,ma,iaxx,iayy,iazz,mbe,ibexx,ibeyy,ibezz,mc,icxx,icyy,l5,mdf,mdf

idfxx,idfyy,idfzz,l6,idf,fc1,fc2,fc3,fv1,fv2,fv3]

δx＝[a1,b1,a2,b2,a3,b3]

步骤1：设定力反馈设备关节角理想运动轨迹；

力反馈设备具有n个可旋转的关节j1、j2…jn，对应的n个关节角为θ1、θ2…θn。设定n个关节角的理想运动轨迹分别为：角度θ1(t)、θ2(t)…θn(t),角速度角加速度

步骤2：对关节角理想运动轨迹进行位置跟踪；

使用pid控制算法，控制力反馈设备的n个关节对理想轨迹进行位置跟踪，得到n个关节的输入力矩τ1、τ2…τn和角度θ1(t)、θ2(t)…θn(t)。利用非线性跟踪—微分器得到各关节的角速度和角加速度

步骤3：对关节角运动轨迹和输入力矩采样；

对运动轨迹：关节角度θ1(t)、θ2(t)…θn(t),关节角速度关节角加速度以及输入的关节力矩τ1、τ2…τn进行采样，其中t为采样时间，p为采样点数。得到采样轨迹：关节角度关节角速度和关节角加速度及力矩

步骤4：用改进的全面学习粒子群(iclpso)算法估计力反馈设备参数。

(a)：将估计的参数及关节角运动轨迹代入动力学方程中，计算得到估计的关节力矩，第i个关节、第p次采样的力矩计算公式如下：

上式中，为第i个关节、第p次采样得到的关节角度；为第i个关节、第p次采样得到的关节角速度；为第i个关节、第p次采样得到的关节角加速度

(b)：定义误差矩阵为计算测量的关节力矩τ与估计的关节力矩之间的误差得到误差矩阵e为：

定义目标函数：

f＝||e||2

(c)：设置iclpso算法的参数。粒子维度d；粒子的个数n；最大迭代次数max_iteration；最大评价次数max_efs；最大停滞次数max_pst；粒子搜索范围为xmin、xmax；

①初始化iclpso算法的参数，种群个数n、粒子维度d、搜索边界[xminxmax]、迭代次数iteration、评价次数efs、各粒子停滞次数pst，随机初始化粒子位置x、速度v；②评价粒子的位置，得到各粒子的适应度值，更新各粒子的个体历史最优位置pb和种群在整个搜索过程中找到的最优位置gb(全局最优位置)，更新iteration和efs；

③使用分层更新策略更新粒子速度v，更新粒子位置x，设置参数m＝n/2，k＝n/4。评价各粒子得到适应度值，更新各粒子的pb和gb，更新iteration和efs；

若粒子的f(pbi)＜f(s_pbm)，则粒子的速度使用下面的公式更新

否则，粒子的速度使用下面的公式更新

其中，pbi是个体的历史最优位置，s_pb为pb按适应度值从小到大排序后的位置，g_pb是pb产生的指导粒子，cn为s_pb中前k个位置的中心，c1和c2为加速度因子，r1和r2是0到1之间均匀分布的随机数。

④使用位置学习策略对粒子位置x进行更新。每个粒子都有学习其他粒子的能力，学习其他粒子的概率为plx＝0.25，如果0～1的随机数r3小于plx，则在此维度上，粒子学习另一个粒子；

⑤使用指导粒子学习策略对指导粒子进行更新。当粒子的停滞次数pst达到设置的最大停滞次数max_pst时，随机选择一个粒子的pb替代停滞粒子的个体指导粒子。另外，为了防止粒子过早的聚集在一起，损失种群的多样性。当粒子的pb更新时，指导粒子g_pb将会等于pb；

⑥使用全局最优学习策略优化gb。在每次迭代时，随机选择一个维度r4，让gb直接学习pb中的一个随机粒子

⑦判断算法是否满足结束条件，若满足找到使目标函数f最小的参数，此时的参数为估计的最优模型参数，算法结束；若不满足，则继续跳到步骤2继续运行。

与现有技术相比，本发明的有益效果是：

本发明通过使用四种策略：分层更新策略、位置学习策略、指导粒子学习策略、全局最优学习策略，有效地解决在传统pso算法中存在的“两步向前，一步向后”问题以及早熟问题。与传统pso算法对比，本算法不仅收敛速度更快，而且获得的模型参数也更为精确，能够为各关节提供精确的力矩估计值，提升控制算法的性能。

附图说明

图1为一种改进的全面学习粒子群(iclpso)算法流程图。

具体实施方式

本发明将通过以下实例作进一步说明。

步骤1：设定力反馈设备关节角理想运动轨迹；

力反馈设备具有3个可旋转的关节j1、j2、j3，对应的3个关节角为θ1、θ2、θ3。设定3个关节角的理想运动轨迹分别为：角度θ1(t)、θ2(t)、θ3(t),角速度角加速度具体如下：

角度：θ1＝0.5sintθ2＝0.5sintθ3＝0.2sint

角速度：

角加速度：

步骤2：对关节角理想运动轨迹进行位置跟踪；

使用pid控制算法，控制力反馈设备的3个关节对理想轨迹进行位置跟踪，得到3个关节的输入力矩τ1、τ2、τ3和角度θ1(t)、θ2(t)、θ3(t)。利用非线性跟踪—微分器得到各关节的角速度和角加速度

步骤3：对关节角运动轨迹和输入力矩采样；

对运动轨迹：关节角度θ1(t)、θ2(t)、θ3(t),关节角速度关节角加速度以及输入的关节力矩τ1、τ2、τ3进行采样，其中t＝30s，p＝200。得到采样轨迹：关节角度关节角速度和关节角加速度及力矩

步骤4：用改进的全面学习粒子群(iclpso)算法估计力反馈设备参数。

(a)：将估计的参数及关节角运动轨迹代入动力学方程中，计算得到估计的关节力矩，第i个关节、第p次采样的力矩计算公式如下：

上式中，为第i个关节、第p次采样得到的关节角度；为第i个关节、第p次采样得到的关节角速度；为第i个关节、第p次采样得到的关节角加速度

(b)：定义误差矩阵为计算测量的关节力矩τ与估计的关节力矩之间的误差得到误差矩阵e为：

定义目标函数：

f＝||e||2

(c)：设置iclpso算法的参数。粒子维度d＝34；粒子的个数n＝34；最大迭代次数max_iteration＝10000；最大评价次数max_efs＝340000；最大停滞次数max_pst＝7；粒子搜索范围为xmin＝0.5x、xmax＝2x(x为负时，上下边界互换)；①初始化iclpso算法的参数，种群个数n、粒子维度d、搜索边界[xminxmax]、迭代次数iteration、评价次数efs、各粒子停滞次数pst，随机初始化粒子位置x、速度v，②评价粒子的位置，得到各粒子的适应度值，更新各粒子的个体历史最优位置pb和种群在整个搜索过程中找到的最优位置gb(全局最优位置)，更新iteration和efs；

③使用分层学习模式更新粒子速度v，设置参数m＝n/2，k＝n/4。更新粒子位置x，评价各粒子得到适应度值，更新各粒子的pb和gb，更新iteration和efs；

若粒子的f(pbi)＜f(s_pbm)，则粒子的速度使用下面的公式更新

否则，粒子的速度使用下面的公式更新

其中，pbi是个体的历史最优位置，s_pb为pb按适应度值从小到大排序后的位置，g_pb是pb产生的指导粒子，cn为s_pb中前k个位置的中心，c1和c2为加速度因子，r1和r2是0到1之间均匀分布的随机数。

④使用位置学习策略对粒子位置x进行更新。每个粒子都有学习其他粒子的能力，学习其他粒子的概率为plx＝0.25，如果0～1的随机数r3小于plx，则在此维度上，粒子学习另一个粒子；

⑤使用指导粒子学习策略对指导粒子进行更新。当粒子的停滞次数pst达到设置的最大停滞次数max_pst时，随机选择一个粒子的pb替代停滞粒子的个体指导粒子。另外，为了防止粒子过早的聚集在一起，损失种群的多样性。当粒子的pb更新时，指导粒子g_pb将会等于pb；

⑥使用全局最优学习策略优化gb。在每次迭代时，随机选择一个维度r4，让gb直接学习pb中的一个随机粒子

⑦判断算法是否满足结束条件，若满足找到使目标函数f最小的参数，此时的参数为估计的最优模型参数，算法结束；若不满足，则继续跳到步骤2继续运行。

以上所述仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形、改进及替代，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。