CN110125927A - 基于自适应遗传算法的机械臂轨迹规划方法及系统 - Google Patents

Abstract

本发明提供了一种自适应遗传算法的机械臂轨迹规划方法及系统，其方法包括：首先建立机械臂的运动学模型，并将机械臂的空间作业轨迹离散化为一系列空间插值点，进而利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；然后采用3‑5‑3样条插值算法所述关节插值点进行插值，得到机械臂的关节空间轨迹曲线，进而建立目标函数和约束条件；最后采用自适应遗传算法对机械臂的运动总能量进行优化，得到最优关节空间轨迹曲线。本发明的有益效果是：使机械臂在运动过程中能量消耗最小，保证求解出的轨迹为能量消耗最小的情况下的最优轨迹，且易于实施，易于应用于工业生产中。

Description

基于自适应遗传算法的机械臂轨迹规划方法及系统

技术领域

本发明涉及机械臂轨迹规划领域，尤其涉及一种自适应遗传算法的机械臂轨迹规划方法及系统。

背景技术

目前，工业机械臂作为机器人的一种，已经成为先进制造业的支撑技术，在焊接、切割、搬运、喷涂等工业领域发挥了很大的作用。随着机器人在各个领域应用的不断深入，人们对机器人各方面的性能要求也越来越高，随之而出现的各种工业机械臂的问题也急需攻关。其中，对机械臂展开轨迹规划及优化方法的研究尤为重要，通过轨迹规划可以改进机械臂的工作性能，很大程度上减小零件的损耗，进而提高工作效率和增长机械臂的工作时间。

但是传统的优化方法大部分基于一些最优化方法展开，没有涉及智能优化的领域。并且针对不同的场景功能需求也不尽相同，在室外高空或复杂局限环境中，由于所携带电池容量有限、电池的额定输出功率有限，在一段时间运行过程中可能会出现电池容量耗尽、超出电池额定功率等情况，影响正常作业，因此以能量消耗为优化性能目标，规划出一条光滑平稳的运动轨迹，能够使有限性能关节的机器人发挥其更大的作用，在能源紧张或者有限的情况下节省能量。

现有类似已公开的专利：

公告号为CN 106113034 A、公告日为2016年11月16日的中国专利文件公开了一种六自由度机械臂考虑力约束的轨迹规划方法。将力约束转化为接触形变和接触运动速度的约束，，再根据接触运动学，研究考虑动态接触冲击的机械臂运动速度优化方法和考虑接触形变的基于不同接触边缘的连续轨迹规划方法。

公告号为CN 107367938 A、公告日为2017年11月21日的中国专利文件公开了一种六自由度机械臂考虑力约束的轨迹规划方法。将机械臂的任务空间离散化成多个轨迹差值点，得到位姿矩阵序列和相应的时间节点序列，对位姿序列进行逆运算得到相应的关节位置，设定运动学速度约束条件，并根据约束条件通过三次多项式曲线插值依次连接每个对应的点，得到机械臂的关节轨迹曲线，建立时间目标函数，根据次函数采用多种群遗传算法对机械臂的运动总时间进行优化，得到机械臂的关节运动速度曲线、关节运动加速度曲线和关节运动脉动曲线。

公告号为CN 108621157 A、公告日为2018年10月9日的中国专利文件公开了一种基于模型约束的机械臂能量最优轨迹规划控制方法及装置。设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；建立以机械臂运动能耗为优化目标的非线性约束规划模型，并结合机械臂关节角度位置描述、力矩关于时间的连续函数以及摩擦力能耗求解目标函数的能量消耗；基于以机械臂运动能耗为优化目标的非线性约束规划模型，采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。

现有技术存在不足：

公告号为CN 108621157 A、公告日为2018年10月9日的中国专利文件公开了一种基于模型约束的机械臂能量最优轨迹规划控制方法及装置。通过三次样条插值算法得到关于时间的机械臂关节角度位置描述；建立以机械臂运动能耗为优化目标的非线性约束规划模型，并结合机械臂关节角度位置描述、力矩关于时间的连续函数的能量消耗采用序列二次规划算法迭代求解能量最优的轨迹规划的最小值，获得机械臂最优轨迹。

此发明专利以能量为优化目标，约束力矩与速度的情况下对能量进行优化，没有考虑在机械臂运动过程中，电机的功率是否超过电源的额定输出功率，导致影响正常作业；并且在通过序列二次规划算法迭代求解的过程中可能会陷入局部最优，并不能保证全局最优解。

发明内容

为了解决上述问题，本发明提供了一种自适应遗传算法的机械臂轨迹规划方法及系统，一种自适应遗传算法的机械臂轨迹规划方法，主要包括以下步骤：

S101：根据机械臂的运动学参数，建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；所述运动学参数根据机械臂的实际物理结构获得；

S102：将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

S103：采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

S104：根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；所述目标函数如公式(1)所示，约束条件如公式(2)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax和分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

S105：根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

进一步地，步骤S101中，利用D-H建模方法建立机械臂的运动学模型。

进一步地，步骤S103中，用3-5-3样条插值算法所述关节插值点进行插值，包括以下步骤：

S201：在各所述关节插值点中选定两个点作为起始点和终点；然

S202：给定所述起始点和终点的速度与加速度的第一约束条件，以及三段样条插值轨迹的速度与加速度的第二约束条件；

S203：根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(3)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数。

进一步地，所述第一约束条件为：起始点和终点的减速度和速度均为0；所述第二约束条件为：第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹之间的速度与加速度均连续。

进一步地，步骤S104中，能量E_ji的计算公式如下公式(4)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

机械臂第j个关节的输出功率P_j计算如下公式(5)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

进一步地，第j个关节的扭矩τ_j的计算方法如公式(6)所示：

上式中，K为机械臂总动能，为各关节的动能之和：U为机械臂总势能：q_j为机械臂第j个关节的旋转角度；其中，第j个关节的动能K_j为：m_j和l_j分别为第j个关节的质量和长度，θ_j为第j个关节与其垂直方向或者水平方向的夹角，h_j为第j个关节质心的高度，其中，m_j、l_j、θ_j和h_j均为机械臂固有参数；ω_j和v_j分别为第j个关节的质心的角速度和线速度，通过下式求得：

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度。

进一步地，步骤S105中，根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化时，在每一步迭代过程中，均需要检验新产生种群是否有实数值为0的个体；若是，则重新随机产生该个体的值，以解决由于迭代步数过大，造成矩阵不满秩的问题；否则不做处理。

进一步地，一种A系统，其特征在于：包括以下模块：

运动学建立模块，用于建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；

坐标转换模块，用于将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

插值模块，用于采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

目标约束模块，用于根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；所述目标函数如公式(7)所示，约束条件如公式(8)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax和分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

轨迹优化模块，用于根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

进一步地，插值模块中，用3-5-3样条插值算法所述关节插值点进行插值，包括以下单元：

起始点设定单元，在各所述关节插值点中选定两个点作为起始点和终点；

条件设定单元，用于给定所述起始点和终点的速度与加速度的第一约束条件，以及三段样条插值轨迹的速度与加速度的第二约束条件；

插值单元，用于根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(9)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数。

进一步地，目标约束模块中，能量E_ji的计算公式如下公式(10)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

机械臂第j个关节的输出功率P_j计算如下公式(11)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

进一步地，第j个关节的扭矩τ_j的计算方法如公式(12)所示：

上式中，K为机械臂总动能，为各关节的动能之和：U为机械臂总势能：q_j为机械臂第j个关节的旋转角度；其中，第j个关节的动能K_j为：m_j和l_j分别为第j个关节的质量和长度，θ_j为第j个关节与其垂直方向或者水平方向的夹角，h_j为第j个关节质心的高度，其中，m_j、l_j、θ_j和h_j均为机械臂固有参数；ω_j和v_j分别为第j个关节的质心的角速度和线速度，通过下式求得：

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度。

本发明提供的技术方案带来的有益效果是：本发明所提出的技术方案，考虑了在机械臂运动过程中，约束其最大速度，最大加速度以及电机最大输入功率，保证电机功率不超过电源额定功率，并且通过采用自适应遗传算法对3-5-3多项式样条插值曲线进行优化，使机械臂在运动过程中能量消耗最小，保证求解出的轨迹为能量消耗最小的情况下的最优轨迹，且易于实施，易于应用于工业生产中。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例中一种自适应遗传算法的机械臂轨迹规划方法的流程图；

图2是本发明实施例中自适应遗传算法的自适应调整曲线示意图；

图3是本发明实施例中六自由度机械臂的空间连杆坐标系示意图；

图4是本发明实施例中优化后的各关节轨迹曲线示意图；

图5是本发明实施例中优化后的各关节旋转角加速度轨迹曲线示意图；

图6是本发明实施例中优化后的各关节功率示意图；

图7是本发明实施例中总功率变化曲线示意图；

图8是本发明实施例中优化后的机械臂各关节的能量消耗示意图；

图9是本发明实施例中自适应遗传算法具体实施流程图；

图10是本发明实施例中一种自适应遗传算法的机械臂轨迹规划系统的模块组成示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

本发明的实施例提供了一种自适应遗传算法的机械臂轨迹规划方法及系统。

请参考图1，图1是本发明实施例中一种自适应遗传算法的机械臂轨迹规划方法的流程图，具体包括如下步骤：

S101：根据机械臂的运动学参数，建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；所述运动学参数根据机械臂的实际物理结构获得；

S102：将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

S103：采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

S104：根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；所述目标函数如公式(1)所示，约束条件如公式(2)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax、分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

S105：根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

步骤S101中，利用D-H建模方法建立机械臂的运动学模型。

步骤S103中，用3-5-3样条插值算法所述关节插值点进行插值，包括以下步骤：

S201：在各所述关节插值点中选定两个点作为起始点和终点；

S202：给定所述起始点和终点的速度与加速度的第一约束条件，以及三段样条插值轨迹的速度与加速度的第二约束条件；

S203：根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(3)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

所述第一约束条件为：起始点和终点的减速度和速度均为0；

所述第二约束条件为：第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹之间的速度与加速度均连续。

步骤S104中，能量E_ji的计算公式如下公式(4)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

τ_j的计算方法如公式(5)所示：

上式中，K为机械臂总动能，为各关节的动能之和：U为机械臂总势能：q_j为机械臂第j个关节的旋转角度；其中，第j个关节的动能K_j为：m_j和l_j分别为第j个关节的质量和长度，θ_j为第j个关节与其垂直方向或者水平方向的夹角，h_j为第j个关节质心的高度，其中，m_j、l_j、θ_j和h_j均为机械臂固有参数；ω_j和v_j分别为第j个关节的质心的角速度和线速度，通过下式求得：

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度；

机械臂第j个关节的输出功率P_j计算如下公式(6)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

在本发明实施例中，根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化的具体步骤，包括：

1)确定编码方式、初始化种群：

对第j个关节的三段插值函数时间的搜索空间中分别随机产生M组l位二进制串基因编码组成的种群，并将其分解为3-5-3次多项式三段轨迹对应的三个种群POPt1、POPt2、POPt3；M和l均为预设值；每个个体用l的二进制编码串转换为实数的解码函数Γ如公式(7)所示：

上式中，是m×l维的行向量，表示第t代的第i个个体，i∈{1,2,…,M}；v_k、u_k分别为第k个实数范围的上限和下限，随机产生基因码的值，并计算相应的实数；

2)根据产生的M组l位二进制串基因编码组成的种群，根据矩阵系数求出三段多项式；多项式对时间求导，得到三段多项式的速度函数，判断多项式最大速度是否符合公式(2)中的条件：并通过关节旋转的角速度和线速度，根据公式(6)求出各关节的功率，判断最大功率是否符合公式(2)中的条件：max{|P_j|}≤P_jmax；

3)构建适应度函数：适应度函数的构建有两个任务:首先使机械臂运动速度收敛到运动学约束内；然后使各段插值能量尽量减小。对2)的计算结果采用两个适应度函数切换的开关控制：如果三段中的任一段功率函数或者速度函数不符合公式(2)中的条件，则该段的目标函数为功率|P(j，i)|，否则目标函数切换为:以减小每段关节运行消耗能量为优化目标进行迭代；其中，j为M组种群的第j组，i为多项式插值的段数，取值范围是1、2、3；如果有的个体不满足其运动学约束，在下一步中，该个体被淘汰的几率会增加。

4)自适应遗传操作：

(a)选择：本发明实施例中采用的选择方式为轮盘赌选择机制，并采用优胜劣汰选择策略解决进化过程中出现过早收敛和停滞问题；

将适应度函数按升序排列，每次随机地从种群中挑选一定数目的个体，并将其中最好的选做父个体。

(b)交叉和变异：通过计算种群的平均适应度f_avg与种群最优适应度f_max之间的关系来判别遗传算法是否收敛于最优：当最优解的适应度与平均适应度接近，即|f_max-f_avg|变小时，种群收敛于最优解的可能性大于种群发散在解空间的可能性；反之亦然。P_c和P_m值依赖于|f_max-f_avg|的变化，当整个种群趋于收敛时，增大交叉率P_c和变异率P_m值，反之减小。为了得到好的效果，既要避免算法收敛于局部最优，也要防止优良基因受到破坏，因此适应度较优的个体应有较小的交叉率P_c和变异率P_m值，而适应度较劣的个体应有较大的交叉率P_c和变异率P_m值。这样提高了群体中表现优良的个体的交叉率和变异率，使它们不会处于一种近似停滞不前的状态。自适应交叉率P_c和变异率P_m分别如公式(8)和公式(9)所示：

上式中，f′是要交叉或变异个体适应度值，P_cmin及P_cmax分别表示交叉率取值的下限和上限；P_mmin及P_mmax分别表示变异率取值的下限和上限。为保证每一代的优良个体不被破坏，采用精英选择策略，选择适应度最优的个体，直接复制到下一代中；P_cmax＝0.9，P_cmin＝0.4，P_mmax＝0.1，P_mmin＝0.01；

5)构成新的M×3种群POPt1、POPt2、POPt3；

6)满足终止条件则算法结束，否则转到步骤2)；

7)完成所有关节的能量优化。

遗传算法需要循环迭代，本发明实施例中使用的自适应遗传算法是在优化目标的搜索空间内搜索，在计算多项式时，如果迭代步数很大，易造成矩阵不满秩，这影响了该方法在复杂目标优化的应用，所以在每一步迭代过程中，都要检验新产生种群是否有实数值为0的个体。如果有，则要重新随机产生该个体的值。公式(8)和公式(9)的交叉率、变异率自适应调整曲线如图2所示，(a)为交叉率调整曲线，(b)为变异率调整曲线。

在本发明实施例中，以某六自由度机械臂为例，采用D-H建模算法对该六自由度机械臂进行运动学建模，建立的空间连杆坐标系如图3所示；

设定设置机械臂运动轨迹起始点与终点，起始点A＝[0°,16°,-10°,0°,-6°,0°]，终点B＝[40°,61°,-45°,0°,16°,15°]，并针对该机械臂本体性能及有限电池输出功率设定约束条件，包括最大功率小于450W，角速度控制范围：[-35°/s，35°/s]，以及角加速度控制范围：[-1°/s²，1°/s²]；得到优化后的各关节轨迹曲线如图4所示；优化后的各关节旋转角加速度轨迹曲线如图5所示；优化后的各关节功率以及总功率变化曲线如图6和图7所示；优化后的机械臂各关节的能量消耗如图8所示；具体流程图如图9所示。

可以看出，在运动过程中，通过自适应遗传算法优化机械臂运动轨迹后，机械臂各关节的功率均小于350W，总功率接近400W，总输出功率不超过电池的输出功率，并且总能量消耗为539.7J，大大减少了机器人消耗的能量，并且各关节运动曲线平滑，震动更小，使得机械臂能够在一定时间内以最小能量消耗到达预定目标点，防止负载过大，保证其正常运行。

请参阅图10，图10是本发明实施例中一种自适应遗传算法的机械臂轨迹规划系统的模块组成示意图，包括依次连接的：运动学建立模块11、坐标转换模块12、插值模块13、目标约束模块14和轨迹优化模块15；

运动学建立模块11，用于建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；

坐标转换模块12，用于将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

插值模块13，采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

目标约束模块14，用于根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；所述目标函数如公式(10)所示，约束条件如公式(11)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax和分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

轨迹优化模块15，用于根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

插值模块13中，用3-5-3样条插值算法所述关节插值点进行插值，包括以下单元：

起始点设定单元，用于在各所述关节插值点中选定两个点作为起始点和终点；

条件设定单元，用于给定所述起始点和终点的速度与加速度的第一约束条件，以及三段样条插值轨迹的速度与加速度的第二约束条件；

插值单元，用于根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(12)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数。

目标约束模块14中，能量E_ji的计算公式如下公式(13)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

机械臂第j个关节的输出功率P_j计算如下公式(14)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度。

本发明的有益效果是：本发明所提出的技术方案，考虑了在机械臂运动过程中，约束其最大速度，最大加速度以及电机最大输入功率，保证电机功率不超过电源额定功率，并且通过采用自适应遗传算法对3-5-3多项式样条插值曲线进行优化，使机械臂在运动过程中能量消耗最小，保证求解出的轨迹为能量消耗最小的情况下的最优轨迹，且易于实施，易于应用于工业生产中。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：包括以下步骤：

S101：根据机械臂的运动学参数，建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；所述运动学参数根据机械臂的实际物理结构获得；

S102：将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

S103：采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

S104：根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；其中，目标函数如公式(1)所示，约束条件如公式(2)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax和分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

S105：根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

2.如权利要求1所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：步骤S101中，利用D-H建模方法建立机械臂的运动学模型。

3.如权利要求1所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：步骤S103中，用3-5-3样条插值算法所述关节插值点进行插值，具体步骤包括：

S201：在各所述关节插值点中选定两个点作为起始点和终点；

S202：给定所述起始点和终点的速度与加速度的第一约束条件，以及三段样条插值轨迹的速度与加速度的第二约束条件；

S203：根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(3)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数。

4.如权利要求3所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：步骤S202中，所述第一约束条件为：起始点和终点的减速度和速度均为0；所述第二约束条件为：第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹之间的速度与加速度均连续。

5.如权利要求1所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：步骤S104中，能量E_ji的计算公式如下公式(4)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

机械臂第j个关节的输出功率P_j计算如下公式(5)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

6.如权利要求5所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：第j个关节的扭矩τ_j的计算方法如公式(6)所示：

上式中，K为机械臂总动能，为各关节的动能之和：U为机械臂总势能：q_j为机械臂第j个关节的旋转角度；其中，第j个关节的动能K_j为：m_j和l_j分别为第j个关节的质量和长度，θ_j为第j个关节与其垂直方向或者水平方向的夹角，h_j为第j个关节质心的高度，其中，m_j、l_j、θ_j和h_j均为机械臂固有参数；ω_j和v_j分别为第j个关节的质心的角速度和线速度，通过下式求得：

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度。

7.如权利要求1所述的一种自适应遗传算法的机械臂轨迹规划方法，其特征在于：步骤S105中，根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化时，在每一步迭代过程中，均需要检验新产生种群是否有实数值为0的个体；若是，则重新随机产生该个体的值，以解决由于迭代步数过大，造成矩阵不满秩的问题；否则不做处理。

8.一种自适应遗传算法的机械臂轨迹规划系统，其特征在于：包括以下模块：

运动学建立模块，用于建立机械臂的运动学模型；所述运动学模型包括：正运动学模型和逆运动学模型；

坐标转换模块，用于将机械臂的空间作业轨迹离散化为一系列空间插值点，并利用所述逆运动学模型，将所述空间插值点转换至关节空间下对应的关节插值点；

插值模块，采用3-5-3样条插值算法对所述关节插值点之间的轨迹进行插值，以依次连接各空间插值点，得到机械臂的关节空间轨迹曲线；

目标约束模块，用于根据机械臂的关节空间轨迹曲线，建立目标函数和约束条件；所述目标函数如公式(7)所示，约束条件如公式(8)所示：

上式中，f₂(t)为机械臂的运动总能量；E_j1、E_j2和E_j3分别为机械臂第j个关节的第一段、第二段和第三段轨迹所消耗的能量；P_j为机械臂第j个关节的输出功率，为机械臂第j个关节的角速度；P_jmax和分别为第j个关节输出功率和角速度的最大值，为预设值；其中，j＝1,2,3,…,n，n为机械臂的关节总数；

轨迹优化模块，用于根据目标函数和约束条件，采用自适应遗传算法对机械臂的运动总能量进行优化，从而得到机械臂的最优关节空间轨迹曲线。

9.如权利要求8所述的一种自适应遗传算法的机械臂轨迹规划系统，其特征在于：插值模块中，用3-5-3样条插值算法所述关节插值点进行插值，包括以下单元：

起始点设定单元，用于在各所述关节插值点中选定两个点作为起始点和终点；

条件设定单元，用于给定所述起始点和终点的速度与加速度的第一约束条件和三段样条插值轨迹的速度与加速度的第二约束条件；

插值单元，用于根据所述起始点、终点、起始点和终点之间已知的各关节插值点、第一约束条件和第二约束条件，采用公式(9)对起始点和终点之间的各关节插值点进行插值，得到第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的表达式：

上式中，h_j1(t)，h_j2(t)和h_j3(t)分别为第j个关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹；a_j10、a_j11、a_j12、a_j13、a_j20、a_j21、a_j22、a_j23、a_j24、a_j25、a_j30、a_j31、a_j32和a_j33均为待求解的系数；t1、t2和t3分别为第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹的运行时间；其中，j＝1,2,3,…,n，n为机械臂的关节总数。

10.如权利要求8所述的一种自适应遗传算法的机械臂轨迹规划系统，其特征在于：目标约束模块中，能量E_ji的计算公式如下公式(10)所示：

上式中，E_ji为第j个关节的第i段轨迹所需要的能量；为第j个关节的矢量角速度，由3-5-3样条曲线求导获得；ti为第i段多项式轨迹的运动时间，i＝1,2,3；τ_j为第j个关节的扭矩；

机械臂第j个关节的输出功率P_j计算如下公式(11)所示：

上式中，j＝1,2,…,n，n为机械臂关节总数。

上式中，J(q)为机械臂雅克比固定公式，为各个关节矢量角速度。