本发明涉及一种机械臂控制技术，尤其是涉及一种基于模型约束的机械臂能量最优轨迹规划控制方法及装置。
背景技术：
随着工业机器人的发展，其生产技术逐步提高，成本也相对降低，这让像“人”一样的仿生机器人(例如机械臂)进入人们生活中的门槛越来越低。人们对未来智能化机器人充满期待，不仅期待机器人可以与人类互动，越来越像人类手臂的外观，而且希望体积越来越小，性能越来越好功能。但通常的电机额定扭矩有限，尽管有三倍的瞬间扭矩过载能力，仍不能充分发挥电机的性能，而且对电机也有很大的损耗。随着伺服电机技术的发展，促使使用越来越小体积的电机，从而达到机器人关节小的目的，电机扭矩随着转速的增加而降低，充分发挥电机性能提高机械臂运动速度、降低能量消耗等方面成为学者们研究的热点。
在实际应用中，能量最优是一个非常重要的性能指标，因为，以能量为优化目标不仅能够规划出一条光滑的轨迹，而且还能提高轨迹跟踪的精度，同时还能减少机器人执行器和操作臂上的应力，能够使有限性能关节的机器人发挥其更大的作用，在能源紧张或者有限的情况下节省能量(例如太空中机器人以及潜海勘探机器人等)。
技术实现要素：
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于模型约束的机械臂能量最优轨迹规划控制方法及装置。
本发明的目的可以通过以下技术方案来实现：
一种基于模型约束的机械臂能量最优轨迹规划控制方法，包括：
设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；
基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；
建立以机械臂运动能耗为优化目标的非线性约束规划模型，并结合机械臂关节角度位置描述、力矩关于时间的连续函数以及摩擦力能耗求解目标函数的能量消耗；
基于以机械臂运动能耗为优化目标的非线性约束规划模型，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。
所述设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述，包括：
获取机械臂运动轨迹的关键参数变量；
设计离散网格并将离散的点通过三次样条插值算法获取关节连续的运动轨迹；
对运动轨迹进行微分分别得到速度、加速度、加速度变化率的描述。
所述动力学模型为：
其中：τ为关节力矩，m(θ)为n*n的正定对称矩阵，
所述非线性约束规划模型的目标函数为：
其中：e为约束模型的总能耗，pdynamics,j为关节j的关节力矩所消耗的功率，pfriction,j为关节速度和力矩的乘积，n为机械臂关节数。
所述基于以机械臂运动能耗为优化目标的非线性约束规划模型，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹，具体为：
基于以机械臂运动能耗为优化目标的非线性约束规划模型，通过输入运动轨迹的起点、途径点和终点位置，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。
一种基于模型约束的机械臂能量最优轨迹规划控制装置，包括：
运动轨迹求解模块，用于设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；
力矩-时间函数求解模块，与运动轨迹求解模块连接，用于基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；
目标函数求解模块，分别与运动轨迹求解模块和力矩-时间函数求解模块连接，用于建立以机械臂运动能耗为优化目标的目标函数，并结合机械臂关节角度位置描述、力矩关于时间的连续函数以及摩擦力能耗求解目标函数的能量消耗；
非线性规划算法的规划求解模块，与目标函数求解模块连接，用于基于以机械臂运动能耗为优化目标的非线性约束规划模型，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。
所述运动轨迹求解模块包括
用于获取机械臂运动轨迹的关键参数变量的第一单元；
用于设计离散网格并将离散的点通过三次样条插值算法获取关节连续的运动轨迹的第二单元，与第一单元连接；
用于对运动轨迹进行微分分别得到速度、加速度、加速度变化率的描述的第三单元，与第二单元连接。
所述动力学模型为：
其中：τ为关节力矩，m(θ)为n*n的正定对称矩阵，
所述非线性约束规划模型的目标函数为：
其中：e为约束模型的总能耗，pdynamics,j为关节j的关节力矩所消耗的功率，pfriction,j为关节速度和力矩的乘积，n为机械臂关节数。
所述非线性规划算法的规划求解模块具体用于基于以机械臂运动能耗为优化目标的非线性约束规划模型，通过输入运动轨迹的起点、途径点和终点位置，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。
与现有技术相比，本发明具有以下有益效果：
1)所涉及的能量最优轨迹规划算法简单，无需大量复杂的计算，具有较好的实时性、消耗能量最少。
2)通过设计连散网格可以将离散的运动轨迹拟合成比较精确的连续函数，能够更加有效的研究机械臂的运动轨迹；
3)本发明充分发挥伺服关节电机的性能，为机器人关节体积设计越来越小，性能越来越好功能提供方法。
4)本发明能够充分水平多关节机械臂电机性能，具有较强的鲁棒性，提高机器人的工作效率和使用寿命；。
附图说明
图1为方法的流程示意图；
图2为本发明的控制流程示意图；
图3为三次样条曲线获取运动轨迹示意图；
图4(a)为无摩擦力时机械臂的运动轨迹示意图；
图4(b)为无摩擦力时机械臂的运动状态示意图；
图5(a)为有摩擦力时机械臂的运动轨迹示意图；
图5(b)为有摩擦力时机械臂的运动状态示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
本发明提出基于模型约束的机械臂能量最优轨迹规划控制方法及装置，以机械臂关节力矩所消耗的能量与摩擦力的能耗总和为能量优化目标的非线性约束规划问题，以达到充分发挥有限性能伺服关节的机械臂作用。并设计离散网格对各个关节角度位移进行描述，进而再利用微分和动力学模型分别获取相关参数。
如图1和图2所示，本发明提供一种基于模型约束的能量最优轨迹规划控制方法，包括以下步骤：
1)为充分发挥有限性能伺服电机关节机械臂作用，将机械臂关节摩擦力的能耗和力矩所消耗的能量总和作为能量优化目标，定义能量消耗最优控制的目标函数为：
2)
3)其中：e为约束模型的总能耗，pdynamics,j为关节j的关节力矩所消耗的功率，pfriction,j为关节速度和力矩的乘积，n为机械臂关节数。
4)获取机械臂运动轨迹的关键参数变量，设计离散网格将离散的点通过三次样条插值算法获取关节连续的运动轨迹。如图3所示，定义轨迹运行时间为tn，假设该时间被平均分为n段t1,t2,...,tn，其中di＝ti+1-ti＝1/(n-1)，t1＝0，则取θ1,θ2,...,θn为关节空间中的关键点，选取网格点上的n个变量θj(t＝t1),θj(t＝t2),...,θj(t＝tn)为第j关节变量，利用三次样条插值算法构造机械臂连续的运动轨迹，通过微分分别得到速度、加速度、加加速度的描述；
3)根据步骤2)中关节连续的运动轨迹结合动力学方程获取关节力矩关于时间的函数，所述动力学方程为：
其中：τ为关节力矩，m(θ)为n*n的正定对称矩阵，
4)将步骤2)中运动轨迹和步骤3)中力矩-时间函数结合摩擦力能耗带入目标函数中获取运动轨迹的能量消耗；
5)基于以机械臂运动能耗为优化目标的非线性约束规划模型，通过输入运动轨迹的起点、途径点和终点位置，采用序列二次规划算法(sqp)迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。
利用本发明方法可以在实际应用中，以能量为优化目标能够规划出一条光滑的轨迹，提高轨迹跟踪的精度，减少机器人执行器和操作臂上的应力，使有限性能关节的机器人发挥其更大的作用，在能源紧张或者有限的情况下节省能量，具有较强的鲁棒性，提高机器人的工作效率和使用寿命；
实施例一
本实施例以水平多关节scara工业机器人为实施例进行介绍无摩擦力情况下能量最优的轨迹规划求解，具体步骤如下：
第一步，设置初始条件时间节拍t＝1s，大臂摆动角度45度，小臂摆动40度带入到运动轨迹求解器中进行求解水平多关节机器人连续的运动轨迹；
第二步，根据第一步中关节连续的运动轨迹带入到力矩-时间函数求解器中计算出力矩关于时间的函数；
第三步，将第一步和第二步计算出的运动轨迹和力矩-时间函数带入到无摩擦力能耗的目标函数求解器中求解目标函数；
第四步，利用规划求解器，将关节状态约束以及各个求解器获取的参数进行迭代优化获取能量最优轨迹以及最小能耗为1.07j。无摩擦轨迹规划运动状态示意图如图4(a)和(b)所示。
实施例二
本实施例以水平多关节scara工业机器人为实施例进行介绍增加摩擦力情况下能量最优的轨迹规划求解，具体步骤如下：
第一步，设置初始条件时间节拍t＝1s，大臂摆动角度45度，小臂摆动40度带入到运动轨迹求解器中进行求解水平多关节机器人连续的运动轨迹；
第二步，根据第一步中关节连续的运动轨迹带入到力矩-时间函数求解器中计算出力矩关于时间的函数；
第三步，将第一步和第二步计算出的运动轨迹和力矩-时间函数带入到有摩擦力能耗的目标函数求解器中求解目标函数；
第四步，利用规划求解器，将关节状态约束以及各个求解器获取的参数进行迭代优化获取能量最优轨迹以及最小能耗为1.383j。有摩擦轨迹规划运动状态示意图如图5(a)和(b)所示。
实施例一和实施例二，讨论了针对有无摩擦力进行基于模型约束的能量最优轨迹规划控制求解。
无摩擦轨迹规划运动状态示意图如图3所示大臂小臂同时逆时针摆动时。机械臂的大臂和小臂在启动时直接加速到最大速度，在将要到达目标点位置时从最大速度直接减速到停止，从而达到能量最优。当考虑摩擦力的作用，在同种情况下(图3所示)可以看出小臂的速度和加速度变化比较复杂且总体消耗的能量高于无摩擦力情况下的运动。其主要原因是由于实施例二中选用经典的库伦+粘性摩擦力模型，摩擦力的大小与速度成正比，使得在考虑摩擦力的情况下，其能耗变高且达到能量最优的情况变得更加复杂。本方法实验了理论上无摩擦力情况下的能量最优的轨迹规划，同时满足实际应用当中存在摩擦力情况下复杂的能量最优的轨迹规划，达到能量最小的求解，实现节约能量提高机器人的工作效率和工作寿命。