一种耗能最少的2R欠驱动平面机械臂控制方法与流程

本发明涉及一种欠驱动运动系统控制技术领域，尤其涉及一种耗能最少的2r欠驱动平面机械臂控制方法。

背景技术：

acrobot是一种在垂直平面上运动的欠驱动双连杆机器人。这种机器人仅在肘部有一个驱动器，使得系统在重量、成本及能耗等方面具有很大的优势；同时，驱动装置的减少也使得机器人的动力学模型受到二阶的非完整条件约束，因此要对其进行控制设计具有很大的难度。

但是，由于现有的大多数欠驱动系统的控制方法仅能保证系统得到稳定控制，但是其控制过程中消耗的能量比较大，从而导致acrobot的控制成本较高。因此，为了使acrobot更具有实际应用价值，需要在保证系统稳定可控的条件下，尽可能降低控制过程中的能量消耗。

技术实现要素：

本发明的目的是提供一种耗能最少的2r欠驱动平面机械臂控制方法，该控制方法在保证2r欠驱动平面机械臂能够实现稳定在竖直向上平衡点的前提下，大大地降低控制过程中的能量消耗，在实际中更具有应用价值。

为了实现上述目的，本发明的技术方案包括以下步骤：

步骤1：根据二连杆平面臂动力学方程，针对2r欠驱动平面机械臂建立动力学模型；其中，所述2r欠驱动平面机械臂的第一关节为被动臂，其第二关节为主动臂，且所述动力学模型与第一关节和第二关节运动时的状态变量、第一关节和第二关节的结构参数相关。

步骤2：将2r欠驱动平面机械臂的运动区域分为起摆区域和可线性化区域。

步骤3：判别2r欠驱动平面机械臂所处区域，对2r欠驱动平面机械臂进行分区域控制。

步骤3.1：当判别2r欠驱动平面机械臂处于起摆区域时，驱动元件输出第一力矩τ1，使2r欠驱动平面机械臂进入可线性化区域。

其中，所述第一力矩τ1的计算方法为：首先按照耗能最低原理确定性能指标j1，并根据所述性能指标和所述动力学模型建立hamilton函数方程，将所述hamilton函数方程联立所述动力学模型，计算出所述第一力矩τ1。

步骤3.2：当判别2r欠驱动平面机械臂处于可线性化区域时，驱动元件输出第二力矩τ2，使2r欠驱动平面机械臂稳定在竖直向上的位置。

其中，所述第二力矩τ2的计算方法为：首先将所述动力学模型近似成线性系统模型，基于线性二次型调节器原理对所述线性系统模型进行计算，计算出所述第二力矩τ2。

作为一种具体实施例，当第一关节与竖直向上的方向的夹角小于30度、第二关节与第一关节之间的夹角小于30度、第一关节的角速度小于30度每秒，且第二关节的角速度小于30度每秒时，判别2r欠驱动平面机械臂处于可线性化区域，否则判别2r欠驱动平面机械臂处于起摆区域。

作为一种具体实施例，所述动力学模型为：

式中，f1(x)、f2(x)、b1(x)、b2(x)为与所述第一关节和第二关节的运动状态变量相关的动力学参数，τ表示驱动元件输出的力矩，x为所述第一关节和第二关节的运动状态变量的合集，即x＝[x1,x2,x3,x4]^t，x1表示第一关节的转动角度，x2表示第二关节的转动角度，x3表示第一关节的角速度，x4表示第二关节的角速度，表示第一关节的加速度，表示第二关节的加速度。

其中，

式中，c1,c2,c3,c4为与所述第一关节和第二关节的结构常量相关的动力学参数：c1＝m1lc1²+m2l1²+i1，c2＝m1lc2²+i2，c3＝m2l1lc2，c4＝m1lc1+m2l1，c5＝m2lc2。

式中，m1表示第一关节的质量，l1表示第一关节的长度，lc1表示第一关节的质心与远离驱动元件的端点的距离，i1表示第一关节的转动惯量，m2表示第二关节的质量，l2表示第二关节的长度，lc2表示第二关节的质心与远离驱动元件的端点的距离，i2表示第二关节的转动惯量。

进一步地，所述性能指标为其中，r表示第一力矩τ1的权值系数，w(x(tf),tf)表示与2r欠驱动平面机械臂终点时刻tf及其处于终点时刻的状态x(tf)相关的函数：

w(x(tf),tf)＝tf+e(x)-ed+ε，式中，e(x)表示系统能量函数，ed表示当2r欠驱动平面机械臂处于竖直向上平衡点时的系统势能，ε为合适的正数。

其中，

ed＝(c4+c5)g，g表示重力加速度。

所述hamilton函数方程为式中，λ(tk)表示引入的随时间变化的拉格朗日乘子变量，tk表示驱动元件运行的第k个时间点，b(x(tk))表示非线性系统的转矩增益系数，f(x(tk))表示欠驱动运动系统双关节耦合函数，x(tk)表示第k个时间点对应的所述第一关节和第二关节的运动状态变量；则计算出所述第一力矩τ1为：

m表示驱动元件的额定转矩。

其中，x2(tk)表示第k个时间点对应的第二关节的角速度。

其中，且λ(tk)的初始值，即当tk＝0时，λ(0)＝[-2.967,6.617,1.705,0.994]^t。

进一步地，步骤3所述线性系统模型为：

其中，a1,a2,b1,b2为根据泰勒公式计算出的f1(x),f2(x),b1(x),b2(x)在x＝[0,0,0,0]^t时的近似值；则通过步骤3.2计算出所述第二力矩τ2为：

τ2＝-r^-1b^tpx。

其中，r表示权值系数，b表示步骤1所述的动力学模型经过近似化计算后转矩的增益系数，p表示里黎卡提方程pa+a^tp-pbr^-1b^tp+q＝0的解，a表示状态增益系数。

其中，所述状态增益系数a和转矩增益系数b的计算方式为：

基于所述欠驱动系统双关节耦合函数f(x(tk))与转矩增益系数b(x(tk))，根据泰勒定理，在平衡点处，在误差允许范围内通过近似化计算得到f(x(tk))＝ax(tk)，b(x(tk))＝b，其中，a,b均为常值矩阵。

具体地，所述驱动元件为伺服电机。

具体地，所述第一关节和第二关节的运动状态变量x＝[x1,x2,x3,x4]^t通过传感器检测得出。

与现有技术相比，本发明具有以下技术效果：

针对欠驱动平面机械臂这类二阶非完整系统，本发明通过动力学分析主动臂与欠驱动臂之间的耦合关系，建立针对2r欠驱动平面机械臂的动力学方程，然后将控制目标分两个阶段完成：当欠驱动平面机械臂处于起摆区域时，按照耗能最低的原理设计性能指标函数，然后根据此性能指标函数设计hamilton函数方程，最后将此hamilton函数方程联立动力学方程，通过计算机求解得到在起摆区域内驱动元件输出力矩的变化规律；而在可线性化区域内，将非线性系统近似为线性化系统，按照线性二次型镇定调节器原理设计可线性化区域内驱动元件输出力矩的变化规律，使欠驱动平面机械臂能够稳定在竖直向上的平衡点。本控制方法使用的是基于最优控制中的一种耗能最低的控制方法。在此方法的设计过程中，以控制力矩作为主要的性能指标，使得此方法在保证2r欠驱动平面机械臂能够实现稳定控制的前提下，极大的降低了欠驱动系统的能量消耗，在实际中更具有应用价值。

另外，本控制方法中使用的是具有一般意义下的机械臂结构参数的表示方法，当需要更换不同的欠驱动机械臂进行控制时，只需要更新机械臂的结构参数即可，使得欠驱动系统稳定控制的普适性有了很大的提高。

为了更好地理解和实施，下面结合附图详细说明本发明。

附图说明

图1是2r欠驱动平面机械臂的结构示意图。

具体实施方式

实施例1

请参阅图1，本发明的2r欠驱动平面机械臂的控制方法包括以下步骤：

步骤1：根据二连杆平面臂动力学方程，针对2r欠驱动平面机械臂建立动力学模型；其中，所述2r欠驱动平面机械臂的第一关节为被动臂，其第二关节为主动臂，且所述动力学模型与第一关节和第二关节运动时的状态变量、第一关节和第二关节的结构参数相关：

式中，f1(x)、f2(x)、b1(x)、b2(x)为与所述第一关节和第二关节的运动状态变量相关的动力学参数，τ表示驱动元件输出的力矩，x为所述第一关节和第二关节的运动状态变量的合集，即x＝[x1,x2,x3,x4]^t，x通过安装在2r欠驱动平面机械臂上的传感器检测得出；其中，x1表示第一关节的转动角度，x2表示第二关节的转动角度，x3表示第一关节的角速度，x4表示第二关节的角速度，表示第一关节的加速度，表示第二关节的加速度。

式中，c1,c2,c3,c4为与所述第一关节和第二关节的结构常量相关的动力学参数：

c1＝m1lc1²+m2l1²+i1，c2＝m1lc2²+i2，c3＝m2l1lc2，c4＝m1lc1+m2l1，

c5＝m2lc2。

式中，m1表示第一关节的质量，l1表示第一关节的长度，lc1表示第一关节的质心与远离驱动元件的端点的距离，i1表示第一关节的转动惯量，m2表示第二关节的质量，l2表示第二关节的长度，lc2表示第二关节的质心与远离驱动元件的端点的距离，i2表示第二关节的转动惯量。

步骤2：将2r欠驱动平面机械臂的运动区域分为起摆区域和可线性化区域。

步骤3：判别2r欠驱动平面机械臂所处区域，对2r欠驱动平面机械臂进行分区域控制。

步骤3.1：当第一关节与竖直向上的方向的夹角不小于30度、第二关节与第一关节之间的夹角不小于30度，且第一关节的角速度不小于30度每秒、第二关节的角速度不小于30度每秒时，即当所述第一关节和第二关节的运动状态变量的绝对值时，其中，i＝1,2,3,4，判别2r欠驱动平面机械臂处于起摆区域。

进而按照能量最低原理确定性能指标其中，r表示第一力矩τ1的权值系数，w(x(tf),tf)表示与欠驱动系统终点时刻tf及状态x(tf)相关的函数：

w(x(tf),tf)＝tf+e(x)-ed+ε；其中，e(x)表示系统能量函数，ed表示当2r欠驱动平面机械臂处于竖直向上的位置时的系统势能，ε为合适的正数。

其中，

ed＝(c4+c5)g，g表示重力加速度。

进而根据性能指标和步骤1的动力学模型建立hamilton函数方程：

其中，λ(tk)表示引入的随时间变化的拉格朗日乘子变量，tk表示驱动元件运行的第k个时间点，x(tk)表示第k个时间点对应的所述第一关节和第二关节的运动状态变量，f(x(tk))表示欠驱动运动系统双关节耦合函数，b(x(tk))表示非线性系统的转矩增益系数。

其中，且λ(tk)的初始值，即当tk＝0时，λ(0)＝[-2.967,6.617,1.705,0.994]^t。

其中，x2(tk)表示第k个时间点对应的第二关节的角速度。

将hamilton函数方程联立步骤1所述的动力学模型，计算出使2r欠驱动平面机械臂从起摆区域上摆到可线性化区域，伺服电机所需要输出的第一力矩τ1为：

式中，m表示驱动元件的额定转矩。

步骤3.2：当第一关节与竖直向上的方向的夹角小于30度、第二关节与第一关节之间的夹角小于30度，且第一关节的角速度小于30度每秒、第二关节的角速度小于30度每秒时，即当所述第一关节和第二关节的运动状态变量的绝对值时，其中，i＝1,2,3,4，判别2r欠驱动平面机械臂进入可线性化区域。

首先将步骤1建立的动力学方程近似成线性系统模型：

其中，a1,a2,b1,b2为根据泰勒公式计算出的f1(x),f2(x),b1(x),b2(x)在x＝[0,0,0,0]^t时的近似值。

则通过步骤3计算出伺服电机所需要输出的第二力矩τ2为：

τ2＝-r^-1b^tpx，式中，r表示权值系数，b表示步骤1所述的动力学模型经过近似化计算后转矩的增益系数，p表示里黎卡提方程pa+a^tp-pbr^-1b^tp+q＝0的解，a表示状态增益系数。

所述状态增益系数a和转矩增益系数b通过以下方式计算出：

基于欠驱动系统双关节耦合函数f(x(tk))与转矩增益系数b(x(tk))，根据泰勒定理，在平衡点处，在误差允许范围内通过近似化计算得到f(x(tk))＝ax(tk)，b(x(tk))＝b，其中，a,b均为常值矩阵。

本发明并不局限于上述实施方式，如果对本发明的各种改动或变形不脱离本发明的精神和范围，倘若这些改动和变形属于本发明的权利要求和等同技术范围之内，则本发明也意图包含这些改动和变形。