CN116117813A - 机械臂控制方法、机械臂、机器人及存储介质 - Google Patents

Abstract

本申请提供一种机械臂控制方法、机械臂、机器人及存储介质。方法包括：基于六自由度的机械臂对应的改进D‑H参数，建立机械臂的运动学模型；根据逆运动学对笛卡尔空间中的机械臂的关键路径点求逆解，得到关键路径点对应的关节角；采用五次多项式对关节角进行插值拟合，以确定机械臂在轨迹规划过程中的待优化参数；根据预设的自定义策略，确定机械臂的每个关节在运动中的时间参数；根据预设的遗传算法对待优化参数和时间参数进行寻优；根据优化后的时间参数和速度参数，控制机械臂相应的关节运动，以使机械臂运动至目标位姿。如此，有利于缩短机械臂运动的时长，提高机械臂的工作效率，以及有利于提高机械臂运动的稳定性，降低运动冲击。

Description

机械臂控制方法、机械臂、机器人及存储介质

技术领域

本发明涉及自动化控制技术领域，具体而言，涉及一种机械臂控制方法、机械臂、机器人及存储介质。

背景技术

随着工业生产的发展，机械臂被应用到的工作场景愈发多元化，相应地，行业对机械臂工作能力的要求也越来越高。轨迹规划与优化是机械臂控制的基础内容，是直接影响机械臂工作质量的关键要素。它是在综合考虑任务要求、本体性能和边界约束条件的情况下﹐规划出给定路径的机器人末端执行器位移、速度、加速度和冲击等因变量曲线光滑连续的时间序列。

当前的工业机械臂轨迹规划流程大多是在给定轨迹之后，人为地设置机械臂关节角度、速度等参数，然后通过示教器进行规划。这种基于人工经验的方式无法达到运行时间最短，且最终规划出来的轨迹很可能存在冲击，影响机械臂的工作效率与稳定性。

发明内容

有鉴于此，本申请实施例的目的在于提供一种机械臂控制方法、机械臂、机器人及存储介质，能够改善因人工经验的方式对机械臂的运动进行规划而导致机械臂的工作效率与稳定性低的问题。

为实现上述技术目的，本申请采用的技术方案如下：

第一方面，本申请实施例提供了一种机械臂控制方法，所述方法包括：

基于六自由度的机械臂对应的改进D-H参数，建立所述机械臂的运动学模型；

基于所述运动学模型，根据逆运动学对笛卡尔空间中的所述机械臂的关键路径点求逆解，得到关键路径点对应的关节角；

采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数；

根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数；

根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数；

根据所述目标参数中的优化后的时间参数和速度参数，对给定路径进行规划，并控制所述机械臂相应的关节运动，以使所述机械臂运动至目标位姿。

结合第一方面，在一些可选的实施方式中，基于六自由度的机械臂对应的改进D-H参数，建立所述机械臂的运动学模型，包括：

基于所述机械臂对应的所述D-H参数，建立D-H参数模型，将并将所述机械臂中相邻杠杆之间的位置关系转换为四个参数，分别为公共法线间的距离a_i、垂直于a_i所在平面内两轴的夹角α_i、两连杆的相对位置d_i和两连杆公垂线的夹角θ_i，并确定所述机械臂中的关节i-1到关节i的齐次变换矩阵：

将所述机械臂中相邻关节之间的齐次变换矩阵进行连乘，得到所述机械臂末端的执行器在基坐标系下的齐次变换矩阵，以作为六自由度的所述机械臂的所述运动学模型，表示为：

结合第一方面，在一些可选的实施方式中，基于所述运动学模型，根据逆运动学对笛卡尔空间中的所述机械臂的关键路径点求逆解，得到关键路径点对应的关节角，包括：

通过预设的Pieper解法，从所述运动学模型中提取所述机械臂的关节变量，并确定所述机械臂在目标位姿下的关节角，所述目标位姿下的关节角的计算公式为：

根据所述目标位姿下的关节角的计算公式，确定所述机械臂的关节角θ₁至θ₆的表达式，其中，所述机械臂的逆运动学解的数量为多组，选取与前一个关节角差最小的一组解作为所述关键路径点对应的关节角。

结合第一方面，在一些可选的实施方式中，采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数，包括：

利用五次多项式对所述机械臂的关节角进行描述，得到在所述五次多项式插值下的关节角函数s(t)、角速度函数v(t)以及角加速度函数a(t)，其中，所述五次多项式表示为：

θ(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵

所述关节角函数s(t)、所述角速度函数v(t)以及所述角加速度函数a(t)的表达式为：

式中，k₀、k₁、k₂、k₃、k₄、k₅为待求的系数值，t指时间；

将所述机械臂中的关节i的关节角记为θ_i，关节i+1的关节角记为θ_i+1，并确定所述关节i和所述关节i+1所对应的运动数据，所述运动数据包括关节初始位置、关节初始速度及关节初始角加速度，其中，所述关节初始位置为：

所述关节初始速度为：

所述关节初始角加速度为：

根据所述关节初始位置、所述关节初始速度及所述关节初始角加速度，联立方程得到：

基于联立得到的方程，根据预设的优化目标函数和第一约束条件，确定所述机械臂在轨迹规划过程中的所述待优化参数，其中，所述优化目标函数和第一约束条件为：

所述待优化参数为：

其中，n指所述机械臂中规划路径点的数量；t_n-1为第n-1段轨迹运动时间；

为关节j在第n-2个中间点的速度，j依次取1至6，所述中间点指所述规划路径点中除去首个路径点和最后一个路径点之外的路径点；q_imax指关节i的最大关节角；v_imax指关节i的最大角速度；a_imax指关节i的最大角加速度。

结合第一方面，在一些可选的实施方式中，根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数，包括：

当到达所述给定路径中的路径点时刻，所述机械臂的关节的角速度和角加速度均为零时，针对所述机械臂的六个关节中的每个关节，通过所述五次多项式插值拟合的角速度函数v(t)以及角加速度函数a(t)表达式，分别计算每个关节在最大速度约束和最大加速度约束下轨迹运动的最短时间t_{min_v}和t_{min_a}，并取t_{min_v}和t_{min_a}中的较大值作为相应关节轨迹运动的最短时间，表示为：

t_n-1,min＝max(t_{min_a},t_{min_v})

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数。

结合第一方面，在一些可选的实施方式中，根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数，包括：

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的最短时间T；

将所述最短时间T扩大至指定倍数，得到每段轨迹运动时间取值范围，以作为所述时间参数，表示为：

其中，λ为所述指定倍数，λ的取值大于1且小于等于2；J_n-1为n-1段的角度变化；J_sum为总的角度变化。

结合第一方面，在一些可选的实施方式中，根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数，包括：

获取算法参数和预设的机械臂参数,所述算法参数包括种群的大小Population、迭代次数Generation、选择算子Parent、初始交叉算子Cross₀及初始变异算子Variation₀，种群的成员为所述待优化参数和所述时间参数；

根据实数编码初始化种群个体，其中，不满足第二约束条件的个体被新的个体替换；

通过精英选择策略，对经过编码的种群中的个体进行适应度值排序，并根据所述选择算子Parent从种群中选择满足预设条件的个体组成用于遗传的父代种群，其中，每代适应度值最低的个体直接遗传；

对父代种群进行交叉变异生成新个体，以形成新的种群；记X和Y为进行交叉的个体，Z为进行变异的个体，C_p为交叉算子，V_p为变异因子，则遗传的新个体为：

其中，p指迭代次数，C_p、V_p为：

判断是否达到最大迭代次数或在指定代的精英适应度方差小于设定值，若达到所述最大迭代次数或在所述指定代的精英适应度方差小于所述设定值，则寻优终止，并得到所述目标参数。

第二方面，本申请实施例还提供一种机械臂，所述机械臂包括具有六自由度的关节，以及相互耦合的处理器及存储器，所述存储器内存储计算机程序，当所述计算机程序被所述处理器执行时，使得所述机械臂执行上述的方法。

第三方面，本申请实施例还提供一种机器人，包括机器人本体及上述的机械臂，所述机械臂设置在所述机器人本体上。

第四方面，本申请实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，当所述计算机程序在计算机上运行时，使得所述计算机执行上述的方法。

采用上述技术方案的发明，具有如下优点：

在本申请提供的技术方案中，在机械臂轨迹规划过程中，采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数；再根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数；以及根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数；最后根据所述目标参数中的优化后的时间参数和速度参数，对给定路径进行规划，并控制所述机械臂相应的关节运动，以使所述机械臂运动至目标位姿。如此，有利于缩短机械臂运动的时长，提高机械臂的工作效率，以及有利于提高机械臂运动的稳定性，降低运动冲击，改善因人工经验的方式规划出来的轨迹容易存在冲击的问题。

附图说明

本申请可以通过附图给出的非限定性实施例进一步说明。应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的机械臂控制方法的流程示意图。

具体实施方式

以下将结合附图和具体实施例对本申请进行详细说明，需要说明的是，在附图或说明书描述中，相似或相同的部分都使用相同的图号，附图中未绘示或描述的实现方式，为所属技术领域中普通技术人员所知的形式。在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

本申请实施例提供一种机械臂。机械臂包括具有六自由度的关节，以及处理模块及存储模块。存储模块内存储计算机程序，当计算机程序被所述处理模块执行时，使得机械臂能够执行下述机械臂控制方法中的相应步骤。

在本实施例中，机械臂为六轴机械臂，具有六个自由度的关节。机械臂的每个关节可以设置有电机，通过各电机的控制实现机械臂的六个自由度的运动控制。

本申请实施例还提供一种机器人，该机器人可以包括机器人本体及上述的机械臂。其中，一个机器人本体上可以设置有一个或多个机械臂，每个机械臂可以实现独立控制。

请参照图1，本申请还提供一种机械臂控制方法，可以应用于上述的机械臂或机器人中。

其中，机械臂控制方法可以包括如下步骤：

步骤110，基于六自由度的机械臂对应的改进D-H参数，建立所述机械臂的运动学模型；

步骤120，基于所述运动学模型，根据逆运动学对笛卡尔空间中的所述机械臂的关键路径点求逆解，得到关键路径点对应的关节角；

步骤130，采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数；

步骤140，根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数；

步骤150，根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数；

步骤160，根据所述目标参数中的优化后的时间参数和速度参数，对给定路径进行规划，并控制所述机械臂相应的关节运动，以使所述机械臂运动至目标位姿。

下面将对机械臂控制方法的各步骤进行详细阐述，如下：

在步骤110中，机械臂对应的改进D-H参数的获取方式为常规方式，这里不再赘述。示例性地，一种六轴机械臂的改进D-H参数可以如下表1。

表1：

在本实施例中，步骤110可以包括：

基于所述机械臂对应的所述D-H参数，建立D-H参数模型，将并将所述机械臂中相邻杠杆之间的位置关系转换为四个参数，分别为公共法线间的距离a_i、垂直于a_i所在平面内两轴的夹角α_i、两连杆的相对位置d_i和两连杆公垂线的夹角θ_i，并确定所述机械臂中的关节i-1到关节i的齐次变换矩阵，i依次取1至6的整数，齐次变换矩阵为：

将所述机械臂中相邻关节之间的齐次变换矩阵进行连乘，得到所述机械臂末端的执行器在基坐标系下的齐次变换矩阵，以作为六自由度的所述机械臂的所述运动学模型，表示为：

可理解地，机械臂的运动学模型即为运动学方程，可以通过上述的公式(2)来描述。机械臂末端的执行器可理解为机械臂末端用于抓取物体的部件。

步骤120可以包括：

通过预设的Pieper解法，从所述运动学模型中提取所述机械臂的关节变量，并确定所述机械臂在目标位姿下的关节角，所述目标位姿下的关节角的计算公式为：

根据所述目标位姿下的关节角的计算公式(3)，确定所述机械臂的关节角θ₁至θ₆的表达式，其中，所述机械臂的逆运动学解的数量为多组，选取与前一个关节角差最小的一组解作为所述关键路径点对应的关节角。

可理解地，利用Pieper解法，可以将关节变量从运动学方程中提取出来，从而求解出在目标位姿下的关节角，即，可以对笛卡尔空间中的机械臂的关键路径点求逆解。其中，目标位姿可理解为机械臂的末端在期望的指定空间位置时机械臂的姿态，目标位姿可以根据实际情况灵活确定，这里不再赘述。关键路径点可以理解为机械臂的节点或其他参照点(比如连杆的中心点)在运动过程中形成的轨迹上的点，可以根据实际情况灵活确定。

在本实施例中，利用公式(3)的三个等式中左右矩阵对应位置元素数值相等的原则，可以求解出六轴机械臂关节角θ₁至θ₆的表达式。由于六轴机械臂的逆运动学解数为8组，可以选取与前一个关节角差最小的一组解作为最终解，以作为关键路径点对应的关节角，如此，有利于降低误差。

在本实施例中，步骤130可以包括：

利用五次多项式对所述机械臂的关节角进行描述，得到在所述五次多项式插值下的关节角函数s(t)、角速度函数v(t)以及角加速度函数a(t)，其中，所述五次多项式表示为：

θ(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵ (4)

所述关节角函数s(t)、所述角速度函数v(t)以及所述角加速度函数a(t)的表达式为：

式中，k₀、k₁、k₂、k₃、k₄、k₅为待求的系数值，t指机械臂关节运动的时间；

假设将所述机械臂中的关节i的关节角记为θ_i，关节i+1的关节角记为θ_i+1，并确定所述关节i和所述关节i+1所对应的运动数据，所述运动数据包括关节初始位置、关节初始速度及关节初始角加速度，其中，所述关节初始位置为：

所述关节初始速度为：

所述关节初始角加速度为：

根据所述关节初始位置、所述关节初始速度及所述关节初始角加速度，联立方程得到：

基于联立得到的方程，根据预设的优化目标函数和第一约束条件，确定所述机械臂在轨迹规划过程中的所述待优化参数，其中，所述优化目标函数为：

所述第一约束条件为：

所述待优化参数为：

其中，n指所述机械臂中规划路径点的数量，规划路径点可以作为关键路径点；t_n-1为第n-1段轨迹运动时间；

为关节j在第n-2个中间点的速度，j依次取1至6，所述中间点指所述规划路径点中除去首个路径点和最后一个路径点之外的路径点；共有7n-13个待优化参数；q_imax指关节i的最大关节角；v_imax指关节i的最大角速度；a_imax指关节i的最大角加速度。

步骤140可以包括，包括：

假设到达所述给定路径中的路径点时刻，所述机械臂的关节的角速度和角加速度均为零，针对所述机械臂的六个关节中的每个关节，通过所述五次多项式插值拟合的角速度函数v(t)以及角加速度函数a(t)表达式，分别计算每个关节在最大速度约束和最大加速度约束下轨迹运动的最短时间t_{min_v}和t_{min_a}，并取t_{min_v}和t_{min_a}中的较大值作为相应关节轨迹运动的最短时间，表示为：

t_n-1,min＝max(t_{min_a},t_{min_v}) (12)

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数。

其中，根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数，包括：

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的最短时间T；

将所述最短时间T扩大至指定倍数，得到每段轨迹运动时间取值范围，以作为所述时间参数，表示为：

其中，λ为所述指定倍数，λ的取值大于1且小于等于2；J_n-1为n-1段的角度变化；J_sum为总的角度变化。

例如，为防止时间设置过短导致算法优化失败，计算的最短时间T可以扩大1.5倍，即λ的值为1.5，则每段轨迹运动时间取值范围为：

步骤150可以包括：

获取算法参数和预设的机械臂参数,所述算法参数包括种群的大小Population、迭代次数Generation、选择算子Parent、初始交叉算子Cross₀及初始变异算子Variation₀，种群的成员为所述待优化参数和所述时间参数，其中，机械臂参数指机械臂出厂时的相应参数，可以根据实际情况确定；

根据实数编码初始化种群个体，其中，不满足第二约束条件的个体被新的个体替换；

通过精英选择策略，对经过编码的种群中的个体进行适应度值排序，并根据所述选择算子Parent从种群中选择满足预设条件的个体组成用于遗传的父代种群，其中，每代适应度值最低的个体直接遗传；

对父代种群进行交叉变异生成新个体，以形成新的种群；记X和Y为进行交叉的个体，Z为进行变异的个体，C_p为交叉算子，V_p为变异因子，则遗传的新个体为：

其中，p指迭代次数，C_p、V_p为：

判断是否达到最大迭代次数或在指定代的精英适应度方差小于设定值，若达到所述最大迭代次数或在所述指定代的精英适应度方差小于所述设定值，则寻优终止，并得到所述目标参数。

在本实施例中，第二约束条件可理解为：机械臂上相应关节优化得到的角加速度不能超过准许的最大角加速度。若优化得到的角加速度超过最大角加速度，则视为不满足第二约束条件。

可理解地，若初始个体或变异个体不满足第二约束条件，则可以通过随机生成满足第二约束条件的新个体，来替换不满足第二约束条件的个体。

在本实施例中，选择满足预设条件的个体组成用于遗传的父代种群的方式可以为：从种群中，选择适应度值靠前的指定数量的个体，以及每代适应度值最低的个体，组成用于遗传的父代种群，其中，指定数量可以根据实际情况灵活确定。

迭代终止条件即为：判断是否达到最大迭代次数或在指定代的精英适应度方差小于设定值，若达到最大迭代次数或在指定代的精英适应度方差小于所述设定值，则寻优终止；若未达到最大迭代次数且在指定代的精英适应度方差小于设定值，则继续寻优。其中，指定代和设定值的具体值可以根据实际情况灵活确定。比如，指定代可以为5代。

示例性地，基于表2的D-H参数表，假设机械臂末端的路径点(即为给定路径或关键路径点)的坐标(单位：mm)为：

A(390，0，-299)

B(-108，188，-16)

C(-103，-123，-46)

通过运动学逆解得到各路径点对应的关节角度(单位：rad)为：

A’(-0.3556,-1.6919,-2.4463,-2.3252,1.5707,1.9263)

B’(-1.8816,0.3226,-1.7302,-0.4821,-1.5707,0.3108)

C’(0.6214,0.7147,-1.4891,0.7964,-1.5708,-2.1922)

采用五次多项式对关节角度进行插值拟合，抽象出目标优化问题的数学描述。规划路径点有3个，则共有8个待优化参数，分别为：

第一段六个关节运动轨迹之和为10.8566rad；第二段六个关节运动轨迹之和为6.9178rad，每段轨迹的时间参数(单位：s)范围为：

t₁∈(0,4.5257],t₂∈(0,2.9063]

获取算法参数和预设的机械臂参数，比如，设置种群的大小Population为50，迭代次数Generation为1000，选择算子Parent为30、初始交叉算子Cross0为0.5、初始变异算子Variation0为0.2。对种群中的个体进行适应度值排序，根据选择算子Parent从种群中选择满足要求的个体组成可遗传的父代种群，每代适应度值最低的个体直接遗传。然后对父代种群进行交叉变异生成新个体，形成新的种群。判断是否达到最大迭代次数或近5代的精英适应度方差小于设定值，若达到则寻优终止，并得到经过优化的目标参数。

最后，以目标参数中经过优化后的时间参数和速度参数对给定路径进行规划，完成六自由度机械臂运动。其中，给定路径包括多个预先规划的路径点，给定路径可以根据实际情况灵活设置。

基于上述设计，通过将遗传算法用于机械臂轨迹优化，在传统遗传算法的基础上，根据机器人关节约束条件，将选择、交叉变异因子进行改进，优化调整时间间隔及速度，可以得到最优运行时间，有利于提高机器人的工作效率。另外，基于五次多项式进行插值拟合，有利于减少机械臂在运动过程中的非必要冲击，保护机械臂。避免因人工方式规划出来的轨迹容易存在冲击，缩短机械臂的使用寿命，增加维护成本。

在本实施例中，处理模块可以是一种集成电路芯片，具有信号的处理能力。上述处理模块可以是通用处理器。例如，该处理器可以是中央处理器(Central Processing Unit，CPU)、数字信号处理器(Digital Signal Processing，DSP)、专用集成电路(ApplicationSpecific Integrated Circuit，ASIC)、现场可编程门阵列(Field－Programmable GateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。

存储模块可以是，但不限于，随机存取存储器，只读存储器，可编程只读存储器，可擦除可编程只读存储器，电可擦除可编程只读存储器等。在本实施例中，存储模块可以用于存储改进D-H参数、预设的自定义策略、预设的遗传算法、给定路径等。当然，存储模块还可以用于存储程序，处理模块在接收到执行指令后，执行该程序。

需要说明的是，所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的机械臂的具体工作过程，可以参考前述方法中的各步骤对应过程，在此不再过多赘述。

本申请实施例还提供一种计算机可读存储介质。计算机可读存储介质中存储有计算机程序，当计算机程序在计算机上运行时，使得计算机执行如上述实施例中所述的机械臂控制方法。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本申请可以通过硬件实现，也可以借助软件加必要的通用硬件平台的方式来实现，基于这样的理解，本申请的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，机械臂，或者网络设备等)执行本申请各个实施场景所述的方法。

综上所述，本申请实施例提供一种机械臂控制方法、机械臂、机器人及存储介质。在本方案中，采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数；再根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数；以及根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数；最后根据所述目标参数中的优化后的时间参数和速度参数，对给定路径进行规划，并控制所述机械臂相应的关节运动，以使所述机械臂运动至目标位姿。如此，有利于缩短机械臂运动的时长，提高机械臂的工作效率，以及有利于提高机械臂运动的稳定性，降低运动冲击，改善因人工经验的方式规划出来的轨迹容易存在冲击的问题。

在本申请所提供的实施例中，应该理解到，所揭露的装置、系统和方法，也可以通过其它的方式实现。以上所描述的装置、系统和方法实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种机械臂控制方法，其特征在于，所述方法包括：

基于六自由度的机械臂对应的改进D-H参数，建立所述机械臂的运动学模型；

基于所述运动学模型，根据逆运动学对笛卡尔空间中的所述机械臂的关键路径点求逆解，得到关键路径点对应的关节角；

采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数；

根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数；

根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数；

根据所述目标参数中的优化后的时间参数和速度参数，对给定路径进行规划，并控制所述机械臂相应的关节运动，以使所述机械臂运动至目标位姿。

2.根据权利要求1所述的方法，其特征在于，基于六自由度的机械臂对应的改进D-H参数，建立所述机械臂的运动学模型，包括：

基于所述机械臂对应的所述D-H参数，建立D-H参数模型，将并将所述机械臂中相邻杠杆之间的位置关系转换为四个参数，分别为公共法线间的距离a_i、垂直于a_i所在平面内两轴的夹角α_i、两连杆的相对位置d_i和两连杆公垂线的夹角θ_i，并确定所述机械臂中的关节i-1到关节i的齐次变换矩阵：

将所述机械臂中相邻关节之间的齐次变换矩阵进行连乘，得到所述机械臂末端的执行器在基坐标系下的齐次变换矩阵，以作为六自由度的所述机械臂的所述运动学模型，表示为：

3.根据权利要求2所述的方法，其特征在于，基于所述运动学模型，根据逆运动学对笛卡尔空间中的所述机械臂的关键路径点求逆解，得到关键路径点对应的关节角，包括：

通过预设的Pieper解法，从所述运动学模型中提取所述机械臂的关节变量，并确定所述机械臂在目标位姿下的关节角，所述目标位姿下的关节角的计算公式为：

根据所述目标位姿下的关节角的计算公式，确定所述机械臂的关节角θ₁至θ₆的表达式，其中，所述机械臂的逆运动学解的数量为多组，选取与前一个关节角差最小的一组解作为所述关键路径点对应的关节角。

4.根据权利要求1所述的方法，其特征在于，采用五次多项式对所述关节角进行插值拟合，以确定所述机械臂在轨迹规划过程中的待优化参数，包括：

利用五次多项式对所述机械臂的关节角进行描述，得到在所述五次多项式插值下的关节角函数s(t)、角速度函数v(t)以及角加速度函数a(t)，其中，所述五次多项式表示为：

θ(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵

所述关节角函数s(t)、所述角速度函数v(t)以及所述角加速度函数a(t)的表达式为：

式中，k₀、k₁、k₂、k₃、k₄、k₅为待求的系数值，t指时间；

将所述机械臂中的关节i的关节角记为θ_i，关节i+1的关节角记为θ_i+1，并确定所述关节i和所述关节i+1所对应的运动数据，所述运动数据包括关节初始位置、关节初始速度及关节初始角加速度，其中，所述关节初始位置为：

所述关节初始速度为：

所述关节初始角加速度为：

根据所述关节初始位置、所述关节初始速度及所述关节初始角加速度，联立方程得到：

基于联立得到的方程，根据预设的优化目标函数和第一约束条件，确定所述机械臂在轨迹规划过程中的所述待优化参数，其中，所述优化目标函数和第一约束条件为：

所述待优化参数为：

其中，n指所述机械臂中规划路径点的数量；t_n-1为第n-1段轨迹运动时间；

为关节j在第n-2个中间点的速度，j依次取1至6，所述中间点指所述规划路径点中除去首个路径点和最后一个路径点之外的路径点；q_imax指关节i的最大关节角；v_imax指关节i的最大角速度；a_imax指关节i的最大角加速度。

5.根据权利要求4所述的方法，其特征在于，根据预设的自定义策略，确定所述机械臂的每个关节在运动中的时间参数，包括：

当到达所述给定路径中的路径点时刻，所述机械臂的关节的角速度和角加速度均为零时，针对所述机械臂的六个关节中的每个关节，通过所述五次多项式插值拟合的角速度函数v(t)以及角加速度函数a(t)表达式，分别计算每个关节在最大速度约束和最大加速度约束下轨迹运动的最短时间t_{min_v}和t_{min_a}，并取t_{min_v}和t_{min_a}中的较大值作为相应关节轨迹运动的最短时间，表示为：

t_n-1,min＝max(t_{min_a},t_{min_v})

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数。

6.根据权利要求5所述的方法，其特征在于，根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的所述时间参数，包括：

根据所述六个关节的每段轨迹最短时间的最大值，确定所述机械臂在整段轨迹运动中的最短时间T；

将所述最短时间T扩大至指定倍数，得到每段轨迹运动时间取值范围，以作为所述时间参数，表示为：

其中，λ为所述指定倍数，λ的取值大于1且小于等于2；J_n-1为n-1段的角度变化；J_sum为总的角度变化。

7.根据权利要求1所述的方法，其特征在于，根据预设的遗传算法对所述待优化参数和所述时间参数进行寻优，直到满足预设的迭代终止条件，并得到优化后的待优化参数和时间参数以作为目标参数，包括：

获取算法参数和预设的机械臂参数,所述算法参数包括种群的大小Population、迭代次数Generation、选择算子Parent、初始交叉算子Cross₀及初始变异算子Variation₀，种群的成员为所述待优化参数和所述时间参数；

根据实数编码初始化种群个体，其中，不满足第二约束条件的个体被新的个体替换；

通过精英选择策略，对经过编码的种群中的个体进行适应度值排序，并根据所述选择算子Parent从种群中选择满足预设条件的个体组成用于遗传的父代种群，其中，每代适应度值最低的个体直接遗传；

对父代种群进行交叉变异生成新个体，以形成新的种群；记X和Y为进行交叉的个体，Z为进行变异的个体，C_p为交叉算子，V_p为变异因子，则遗传的新个体为：

其中，p指迭代次数，C_p、V_p为：

判断是否达到最大迭代次数或在指定代的精英适应度方差小于设定值，若达到所述最大迭代次数或在所述指定代的精英适应度方差小于所述设定值，则寻优终止，并得到所述目标参数。

8.一种机械臂，其特征在于，所述机械臂包括具有六自由度的关节，以及相互耦合的处理器及存储器，所述存储器内存储计算机程序，当所述计算机程序被所述处理器执行时，使得所述机械臂执行如权利要求1-7中任一项所述的方法。

9.一种机器人，其特征在于，包括机器人本体及如权利要求8所述的机械臂，所述机械臂设置在所述机器人本体上。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序，当所述计算机程序在计算机上运行时，使得所述计算机执行如权利要求1-7中任一项所述的方法。

Images