一种动力学补偿的机械臂零力平衡控制方法与流程

本发明属于机械臂柔性关节动力学建模领域，尤其涉及一种动力学补偿的机械臂零力平衡控制方法。

背景技术

实现关节精确位置控制所使用的前馈模型定义为主动模态解耦动力学模型，实现关节柔顺控制所使用的前馈模型定义为被动模态解耦动力学模型。关节进行关节精确位置控制和柔顺控制具有不同的前馈模型。主动模态和被动模态的动力学前馈模型的不同体现在被动模态中不包含期望运动前馈项，即被动模态前馈和规划轨迹的控制指令无关。被动模态动力学前馈模型实际上只提供克服额外内力、摩擦力和虚拟力的平衡力矩，因而被称作被动模态。该发明抛弃了传统主动模态控制中的期望运动前馈项，使得控制方式与规划轨迹的控制指令相互独立，很好的解决了机械臂柔性关节柔顺控制的动力学建模问题。

技术实现要素：

为了解决现有技术中柔性关节柔顺控制的动力学建模问题提出一种动力学补偿的机械臂零力平衡控制方法。该方法不考虑传统主动模态控制中的期望运动前馈项，使得控制方式与规划轨迹的控制指令相互独立，很好的解决了机械臂柔性关节柔顺控制的动力学建模问题。

为实现上述目的，本发明提供了如下方案：

一种动力学补偿的机械臂零力平衡控制方法，所述机械臂，包括关节，连杆，转子，减速器，旋转变压器，电机；所述关节还包括关节虚拟力传感器以及关节额外内力传感器；具体步骤为：

步骤1.获取关节虚拟力以及关节额外内力；

步骤2.根据上述获取的关节虚拟力以及关节额外内力，定义柔顺控制前馈模型被动模态解耦动力学模型，以获取关节运动需要满足的控制力矩；

步骤3.根据零力平衡准则计算被动模态下的实际控制力矩；

步骤4.根据实际控制力矩和角度驱动电机和转子。

进一步地，所述步骤1获取关节虚拟力以及关节额外内力使用传感器获取。

进一步地，所述定义柔顺控制前馈模型被动模态解耦动力学模型，所述被动模态是指关节的电机在摩擦力、虚拟力和额外内力补偿的作用下，保持关节平衡力矩运动的控制。

进一步地，所述被动模态解耦动力学模型定义为：

式中τi,m为第i关节运动需要满足的控制力矩，表示第i关节连杆端摩擦力，表示第i关节电机端摩擦力，τi,r表示第i关节虚拟力，τi,f表示第i关节额外内力，l表示减速比。

进一步地，所述关节连杆端摩擦力，具体建模方法为：

考虑库伦摩擦、静摩擦和stribeck摩擦的影响，第i关节连杆端的摩擦力表示为第i连杆位置和速度的函数：

式中ξci为连杆端的库伦摩擦系数；-ξmi为连杆端需要满足的摩擦力系数；ξsi为连杆端的静摩擦系数；ξτi为连杆端的stribeck摩擦系数；bi为连杆端的粘滞摩擦系数；为连杆端的摩擦误差补偿系数。

进一步地，所述关节电机端摩擦力，具体建模方法为：

采用lugre模型，第i关节电机端的摩擦力可以表示为第i电机位置和速度的函数：

式中κi,为电机的角度和角速度；κsi,为stribeck角度和速度；ξsi为静摩擦力；ξci为库伦摩擦力；为电机端的摩擦力；σi2为粘滞摩擦系数；为摩擦模型的内部状态变量和刚毛变形量；σi0为刚毛的刚度系数；σi1为刚毛的阻尼系数；为stribeck摩擦力。

进一步地，所述零力平衡准则为：τi，d-τi，m＝0(6)，其中τi，d为实际控制力矩。

进一步地，所述实际控制力矩计算模型为：

式中和是θbi和θmi的估计值，θbi为连杆旋转角度，θmi为连杆需要满足的控制力矩下的连杆旋转角度，φ为经验角度值；表示第i关节静摩擦系数常量；表示虚拟力前馈模型；表示额外内力；sat(ri,εi,r)表示误差有关的饱和函数，ri表示第i关节系统虚拟力，表示系统虚拟力误差精度，并且满足：

其中表示第i关节额外内力系数常量，表示第i关节连杆端摩擦力系数常量，表示第i关节动摩擦力系数，ρki表示第i关节电机端摩擦力系数常量，表示第i关节运动需要满足的控制力矩的系数常量。

本发明还提供一种执行上述方法的动力学补偿的机械臂零力平衡控制系统，包括：所述机械臂，包括关节，连杆，转子，减速器，旋转变压器，电机；所述关节还包括关节虚拟力传感器以及关节额外内力传感器；

传感器单元，获取关节虚拟力以及关节额外内力；

定义单元，根据上述获取的关节虚拟力以及关节额外内力，定义柔顺控制前馈模型被动模态解耦动力学模型，以获取关节运动需要满足的控制力矩；

计算单元，根据零力平衡准则计算被动模态下的实际控制力矩；

控制单元，根据实际控制力矩驱动电机和转子。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种一种动力学补偿的机械臂零力平衡控制方法的流程图；

图2为本发明提供的具有被动模态的柔性关节的原理图。

具体实施方式

本发明的目的是提供一种动力学补偿的机械臂零力平衡控制方法，以解决现有技术中柔性关节柔顺控制的动力学建模问题提出一种动力学补偿的机械臂零力平衡控制方法。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明进一步详细的说明。

如图1和2所示，一种动力学补偿的机械臂零力平衡控制方法，所述机械臂，包括关节，连杆，转子，减速器，旋转变压器，电机；所述关节还包括关节虚拟力传感器以及关节额外内力传感器；具体步骤为：

步骤1.获取关节虚拟力以及关节额外内力；

步骤2.根据上述获取的关节虚拟力以及关节额外内力，定义柔顺控制前馈模型被动模态解耦动力学模型，以获取关节运动需要满足的控制力矩；

步骤3.根据零力平衡准则计算被动模态下的实际控制力矩；

步骤4.根据实际控制力矩驱动电机和转子。

具体地，所述步骤1获取关节虚拟力以及关节额外内力使用传感器获取。

具体地，所述定义柔顺控制前馈模型被动模态解耦动力学模型，所述被动模态是指关节的电机在摩擦力、虚拟力和额外内力补偿的作用下，保持关节平衡力矩运动的控制。

具体地，所述被动模态解耦动力学模型定义为：

式中τi,m为第i关节运动需要满足的控制力矩，表示第i关节连杆端摩擦力，表示第i关节电机端摩擦力，τi,r表示第i关节虚拟力，τi,f表示第i关节额外内力，l表示减速比。

具体地，所述关节连杆端摩擦力，具体建模方法为：

考虑库伦摩擦、静摩擦和stribeck摩擦的影响，第i关节连杆端的摩擦力表示为第i连杆位置和速度的函数：

式中ξci为连杆端的库伦摩擦系数；-ξmi为连杆端需要满足的摩擦力系数；ξsi为连杆端的静摩擦系数；ξτi为连杆端的stribeck摩擦系数；bi为连杆端的粘滞摩擦系数；为连杆端的摩擦误差补偿系数。

具体地，所述关节电机端摩擦力，具体建模方法为：

采用lugre模型，第i关节电机端的摩擦力可以表示为第i电机位置和速度的函数：

式中κi,为电机的角度和角速度；κsi,为stribeck角度和速度；ξsi为静摩擦力；ξci为库伦摩擦力；为电机端的摩擦力；σi2为粘滞摩擦系数；为摩擦模型的内部状态变量和刚毛变形量；σi0为刚毛的刚度系数；σi1为刚毛的阻尼系数；为stribeck摩擦力。

具体地，所述零力平衡准则为：τi，d-τi，m＝0(6)，其中τi，d为实际控制力矩。

具体地，所述实际控制力矩计算模型为：

式中和是θbi和θmi的估计值，θbi为连杆旋转角度，θmi为连杆需要满足的控制力矩下的连杆旋转角度，φ为经验角度值；表示第i关节静摩擦系数常量；表示虚拟力前馈模型；表示额外内力；sat(ri,εi,r)表示误差有关的饱和函数，ri表示第i关节系统虚拟力，表示系统虚拟力误差精度，并且满足：

其中表示第i关节额外内力系数常量，表示第i关节连杆端摩擦力系数常量，表示第i关节动摩擦力系数，ρki表示第i关节电机端摩擦力系数常量，表示第i关节运动需要满足的控制力矩的系数常量。

本发明所提出的平衡控制方法的具体计算方法为：

步骤一：定义柔顺控制被动模态解耦动力学模型，如图1和2所示。

定义实现关节柔顺控制所使用的前馈模型为被动模态解耦动力学模型。被动模态控制是指关节的电机在摩擦力、虚拟力和额外内力补偿的作用下，保持关节平衡力矩运动的控制。关节柔顺控制使用的被动模态解耦动力学模型可以定义为：

式中τi,m为第i关节运动需要满足的控制力矩，表示第i关节连杆端摩擦力，表示第i关节电机端摩擦力，τi,r表示第i关节虚拟力，τi,f表示第i关节额外内力，l表示减速比。其中最为主要的是对关节连杆端和电机端的摩擦力建模。

(1)关节连杆端摩擦力模型

考虑库伦摩擦、静摩擦和stribeck摩擦的影响，第i关节连杆端的摩擦力表示为第i连杆位置和速度的函数：

式中ξci为连杆端的库伦摩擦系数；-ξmi为连杆端需要满足的摩擦力系数；ξsi为连杆端的静摩擦系数；ξτi为连杆端的stribeck摩擦系数；bi为连杆端的粘滞摩擦系数；为连杆端的摩擦误差补偿系数。

(2)关节电机端摩擦力模型

采用lugre模型，第i关节电机端的摩擦力可以表示为第i电机位置和速度的函数：

式中κi,为电机的角度和角速度；κsi,为stribeck角度和速度；ξsi为静摩擦力；ξci为库伦摩擦力；为电机端的摩擦力；σi2为粘滞摩擦系数；为摩擦模型的内部状态变量和刚毛变形量；σi0为刚毛的刚度系数；σi1为刚毛的阻尼系数；为stribeck摩擦力。

步骤二，设计被动模态下的鲁棒自适应算法

为了实现关节柔顺性，设计被动模态下的鲁棒自适应算法如下：

式中和是θbi和θmi的估计值，θbi为连杆旋转角度，θmi为连杆需要满足的控制力矩下的连杆旋转角度，φ为经验角度值；表示某个常量；表示虚拟力前馈模型；表示额外内力；sat(ri,εi,r)表示误差有关的饱和函数。并且满足：

将公式(6)带入(1)，忽略不确定部分的影响，则有：

τid-τi,m＝0(8)

公式(8)说明，被动模态控制律计算的力矩用来平衡了关节中摩擦力、虚拟力和额外内力。

使用上述方法运行本发明的动力学补偿的机械臂零力平衡控制方法能够解决现有技术中柔性关节柔顺控制的动力学建模问题。该方法不考虑传统主动模态控制中的期望运动前馈项，使得控制方式与规划轨迹的控制指令相互独立，很好的解决了机械臂柔性关节柔顺控制的动力学建模问题。

本发明还提供一种执行上述方法的动力学补偿的机械臂零力平衡控制系统，包括：所述机械臂，包括关节，连杆，转子，减速器，旋转变压器，电机；所述关节还包括关节虚拟力传感器以及关节额外内力传感器；

传感器单元，获取关节虚拟力以及关节额外内力；

定义单元，根据上述获取的关节虚拟力以及关节额外内力，定义柔顺控制前馈模型被动模态解耦动力学模型，以获取关节运动需要满足的控制力矩；

计算单元，根据零力平衡准则计算被动模态下的实际控制力矩；

控制单元，根据实际控制力矩以及角度驱动电机和转子。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。