CN106019942A - 一种基于动力学模型的机器人最大速度获取方法及系统 - Google Patents

Abstract

一种基于动力学模型的机器人最大速度获取方法及系统，方法包括：S1、建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：S2、根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；S3、基于速度增益系数λ_i(nT)和电机最大允许速度计算机器人最大速度。本发明考虑了各关节力矩的力矩约束，使得求解的最大速度必为机器人最大可承受速度；所求取的最大速度，仅与机器人具体的运动轨迹有关，与先前的工作区域无关，提高了机器人的利用效率；进一步的，还可求取每个位置点可承受的最大速度。

Description

一种基于动力学模型的机器人最大速度获取方法及系统

技术领域

本发明涉及机器人控制领域，尤其涉及一种基于动力学模型的机器人最大速度获取方法及系统。

背景技术

工业机器人在进行轨迹规划时，为使机器人工作效率最大化，需确定机器人的最大运行速度限制。目前机器人最大运行速度的限制主要有两个，即机器人关节最大速度限制和机器人末端最大速度限制。关节最大速度限制主要以最大关节速度为依据，或根据电机转速力矩曲线确定在满足输出转矩条件下的最大电机速度限制。机器人末端最大速度限制主要以机器人典型工作区间的最大速度为参考依据。

针对关节最大速度限制，由于机器人在不同位姿下，其实际可承受的最大关节速度不同，甚至会出现关节最大速度限制大于机器人实际可承受的最大关节速度，这势必引起机器人故障停机，造成机械本体的损坏。

针对末端最大速度，同样存在机器人末端最大速度与机器人位姿相关的问题。虽然可以通过雅克比矩阵建立机器人关节速度和机器人末端速度的映射关系，但如果单纯以最大关节速度计算最大末端速度，而不考虑机器人各关节实际输出力矩的限制，机器人可能因关节力矩超限而无法承受该最大速度。因此，在实际使用过程中，机器人最大末端速度主要以典型工作区域的最大速度为参考。为使机器人能在较大工作空间中正常运行，其典型工作区域需尽可能大。但工作区域越大，对应的可满足整个工作区域正常运行的最大速度限制也就越小，这将严重降低机器人的工作效率。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于动力学模型的机器人最大速度获取方法及系统。

本发明解决其技术问题所采用的技术方案是：构造一种基于动力学模型的机器人最大速度获取方法，包括以下步骤：

S1、建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，C(q(t),为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

S2、根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

S3、基于步骤S2中的速度增益系数λ_i(nT)和电机最大允许速度计算获得机器人最大速度。

在本发明所述的基于动力学模型的机器人最大速度获取方法中，所述步骤S2中，通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

在本发明所述的基于动力学模型的机器人最大速度获取方法中，步骤S2力矩τ_ia(nT)的计算包括：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于步骤S1中的所述动力学模型，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

在本发明所述的基于动力学模型的机器人最大速度获取方法中，所述步骤S3包括：

S31、从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

$\begin{array}{c}\mathrm{\λ}=\min \{{{\mathrm{\λ}}^2}_1\left(0\right),{{\mathrm{\λ}}^2}_1\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_1\left(nT\right),\\ {{\mathrm{\λ}}^2}_2\left(0\right),{{\mathrm{\λ}}^2}_2\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_2\left(nT\right),\\ \mathrm{......},\\ {{\mathrm{\λ}}^2}_N\left(0\right),{{\mathrm{\λ}}^2}_N\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_N\left(nT\right)\}\end{array}$

S32、根据步骤S31选出的速度增益系数λ以及步骤S2中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

在本发明所述的基于动力学模型的机器人最大速度获取方法中，所述步骤S3包括：

S31’、从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

S32’、根据步骤S31’选出的速度增益系数λ(nT)以及步骤S2中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度

本发明还公开了一种基于动力学模型的机器人最大速度获取系统，包括：

模型建立单元，用于建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，C(q(t),为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

速度增益系数求解单元，根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

最大速度求解单元，基于速度增益系数求解单元中得到的速度增益系数λ_i(nT)和电机最大允许速度计算机器人最大速度。

在本发明所述的基于动力学模型的机器人最大速度获取系统中，所述速度增益系数求解单元通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

在本发明所述的基于动力学模型的机器人最大速度获取系统中，力矩τ_ia(nT)的计算包括：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于所述动力学模型，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

在本发明所述的基于动力学模型的机器人最大速度获取系统中，最大速度求解单元包括：

速度增益系数筛选子单元，从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

$\begin{array}{c}\mathrm{\λ}=\min \{{{\mathrm{\λ}}^2}_1\left(0\right),{{\mathrm{\λ}}^2}_1\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_1\left(nT\right),\\ {{\mathrm{\λ}}^2}_2\left(0\right),{{\mathrm{\λ}}^2}_2\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_2\left(nT\right),\\ \mathrm{......},\\ {{\mathrm{\λ}}^2}_N\left(0\right),{{\mathrm{\λ}}^2}_N\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_N\left(nT\right)\}\end{array}$

全局最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ以及速度增益系数求解单元中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

在本发明所述的基于动力学模型的机器人最大速度获取系统中，最大速度求解单元包括：

速度增益系数筛选子单元，从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

局部最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ(nT)以及速度增益系数求解单元中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度

实施本发明的基于动力学模型的机器人最大速度获取方法及系统，具有以下有益效果：本发明基于动力学模型，考虑了各关节力矩的力矩约束，使得求解的最大速度必为机器人最大可承受速度；所求取的最大速度，仅与机器人具体的运动轨迹有关，与先前的工作区域无关，极大提高了机器人的利用效率；进一步的，本发明既可以求取机器人运动时适用于所有位置的最大速度，又可以求取机器人运动时每个位置点可承受的最大速度，其属于高级轨迹规划控制器变最大速度控制的关键参数，可最大化机器人工作效率。

附图说明

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

图1是本发明的基于动力学模型的机器人最大速度获取方法的流程图；

图2为初始速度下机器人各关节力矩图；

图3为初始速度下各个位置点最大的λ允许值示意图；

图4为最大速度下机器人各关节力矩图。

具体实施方式

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

参考图1，是本发明的基于动力学模型的机器人最大速度获取方法的流程图。

本发明的基于动力学模型的机器人最大速度获取方法包括以下步骤：

S1、建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

S2、根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

S3、基于步骤S2中的速度增益系数λ_i(nT)和电机最大允许速度计算机器人最大速度。

下面详细介绍各个步骤。

关于步骤S1：

众所周知，具有标准形式的机器人动力学模型的表达式如下：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)+G\left(q\right(t\left)\right)=\mathrm{\τ}---\left(1\right)$

其中，τ表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，C(q(t),为N×N科里奥利力及离心力矩阵，G(q(t))为N×1的重力向量，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

步骤S1的动力学模型是对公式(1)的机器人标准动力学模型进行简单的数学变换，使力矩τ_a(t)仅与惯性力、科里奥利力、速度、加速度有关，而与重力无关，改写过程为：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)=\mathrm{\τ}-G\left(q\right(t\left)\right)={\mathrm{\τ}}_a\left(t\right)$

关于步骤S2：

步骤S2中通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

其中，公式(B)的推导过程如下：

对公式(A)的动力学模型进行数学变换，假设s＝λt，则：

$\stackrel{\·}{q}\left(t\right)=dq/dt=dq/ds*ds/dt=\mathrm{\λ}*dq/ds={\mathrm{\λq}}^{\′}\left(s\right)---\left(B1\right)$

$\stackrel{\·\·}{q}\left(t\right)=d\left(\stackrel{\·}{q}\right(t\left)\right)/dt=\mathrm{\λ}*d^{2}q/{\mathrm{ds}}^2*ds/dt={\mathrm{\λ}}^2*d^{2}q/{\mathrm{ds}}^2={\mathrm{\λ}}^2{q}^{\′\′}\left(s\right)---\left(B2\right)$

将上面的公式(B1)和公式(B2)代入公式(A)，则可以推导得到公式(C)：

$\begin{array}{c}{\mathrm{\τ}}_a\left(t\right)=B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)\\ ={\mathrm{\λ}}^2\left(B\right(q\left(s\right)\left)q^{\′\′}\right(s)+q^{\′}(s\left)C\right(q\left(s\right),q^{\′}\left(s\right)\left)q^{\′}\right(s\left)\right)\\ ={\mathrm{\λ}}^2{\mathrm{\τ}}_a\left(s\right)\end{array}---\left(C\right)$

即，从公式(C)可以得到速度增益系数λ、力矩τ_a(t)、力矩τ_a(s)之间的关系如公式(D)：

${\mathrm{\λ}}^2=\frac{{\mathrm{\τ}}_a\left(t\right)}{{\mathrm{\τ}}_a\left(s\right)}---\left(D\right)$

基于公式(D)，我们把力矩τ_ia(nT)作为公式(D)中的τ_a(s)，各力矩约束范围作为公式(D)中的τ_a(t)，于是可以计算得到各个关节的每个采样周期的速度增益系数。由于公式(D)中的λ²肯定大于等于0，又τ_imin＜0且τ_imax＞0，所以，在τ_ia(nT)＞0τ_ia(nT)时，则选择τ_imax代入公式(D)，在τ_ia(nT)＜0时，则选择τ_imin代入公式(D)，于是可以得到上述的(B)。

从公式(A)可知，各关节力矩τ_a的计算依赖于速度、加速度以及位置，所以一个具体实施例中，步骤S2力矩τ_ia(nT)的计算具体为：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于步骤S1中的所述动力学模型的公式(A)，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

关于步骤S3：

步骤S3求取的最大速度有两种，一种是适用于所有位置的全局最大速度，一种是针对每个位置的局部最大速度。

当求取全局最大速度时，所述步骤S3包括：

S31、从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

$\begin{array}{c}\mathrm{\λ}=\min \{{{\mathrm{\λ}}^2}_1\left(0\right),{{\mathrm{\λ}}^2}_1\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_1\left(nT\right),\\ {{\mathrm{\λ}}^2}_2\left(0\right),{{\mathrm{\λ}}^2}_2\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_2\left(nT\right),\\ \mathrm{......},\\ {{\mathrm{\λ}}^2}_N\left(0\right),{{\mathrm{\λ}}^2}_N\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_N\left(nT\right)\}\end{array}$

S32、根据步骤S31选出的速度增益系数λ以及步骤S2中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

当求取局部最大速度时，所述步骤S3包括：

S31’、从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

S32’、根据步骤S31’选出的速度增益系数λ(nT)以及步骤S2中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度

下面结合具体的实施案例对本发明作进一步说明。

现以某SCARA机器人第一、二关节为最大速度求解实施对象，首先建立动力学模型如下：

$\begin{array}{c}(I_{l_1}+m_{l_1}{l}_1^2+I_{l_2}+m_{l_2}(a_1^2+l_2^2+2a_1{l}_2{c}_2\left)\right){\stackrel{\·\·}{q}}_1+(I_{l_2}+m_{l_2}(l_2^2+a_1{l}_2{c}_2\left)\right){\stackrel{\·\·}{q}}_2\\ -2m_{l_2}{a}_1{l}_2{s}_2{\stackrel{\·}{q}}_1{\stackrel{\·}{q}}_2-m_{l_2}{a}_1{l}_2{s}_2{{\stackrel{\·}{q}}_2}^2={\mathrm{\τ}}_1\\ (I_{l_2}+m_{l_2}(l_2^2+a_1{l}_2{c}_2\left)\right){\stackrel{\·\·}{q}}_1+(I_{l_2}+m_{l_2}{l}_2^2){\stackrel{\·\·}{q}}_2\\ +m_{l_2}{a}_1{l}_2{s}_2{{\stackrel{\·}{q}}_1}^2={\mathrm{\τ}}_2\end{array}$

其中，为第1，2连杆绕质心坐标系Z轴的转动惯量，为第1，2连杆质量(含电机等转配部件)，a₁，a₂为第1，2连杆长度，l₁，l₂为第1，2连杆质心距离连杆起始处的长度，q₁，q₂，为第1，2连杆旋转的角度，速度及加速度，sⁱ，ci为第i个连杆位置的正余弦运算，τ₁，τ₂为第1，2连杆的力矩。

按照上述步骤S2，设定初始第一二关节起止位置为[-120,120]°，设定第一二关节速度均为600°/s，设置加速度为0，可得在该组轨迹参数下，各关节运动的力矩τ_ia(nT)及速度曲线分别如图2所示。根据上述步骤S2设置第一二关节力矩限制均为[-63.5Nm,63.5Nm]，可求得针对每一个位置点的可取的λ值并绘制成曲线如图3所示，该图中最小的被允许的λ值为1.7874，为说明本发明的有效性，此处暂不考虑电机最大转速，则最大速度为1.7874*600°/s＝1072.4°/s，计算在上述最大速度运动情况下各关节力矩曲线如图4所示。

通过图4可以看出，当机器人以最大速度1072.4°/s运行时，第一关节电机实际输出转矩在某些位置可达最大关节电机输出扭矩，这说明此时机器人运行的速度确实是机器人最大可承受的速度；但在任何一个位置点，各关节实际输出扭矩均小于或等于最大关节扭矩限制值，这说明了机器人此速度下运行是切实可行的。以上两个方面证明了基于本发明所求解的最大速度在保证机器人正常运行的前提下，切实提高了机器人的利用效率。

相应的，本发明还公开了一种基于动力学模型的机器人最大速度获取系统，包括：

模型建立单元，用于建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

速度增益系数求解单元，根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

最大速度求解单元，基于速度增益系数求解单元中得到的速度增益系数λ_i(nT)和电机最大允许速度计算机器人最大速度。

其中，所述速度增益系数求解单元通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

其中，力矩τ_ia(nT)的计算包括：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于所述动力学模型，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

其中，最大速度求解单元包括：

速度增益系数筛选子单元，从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

$\begin{array}{c}\mathrm{\λ}=\min \{{{\mathrm{\λ}}^2}_1\left(0\right),{{\mathrm{\λ}}^2}_1\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_1\left(nT\right),\\ {{\mathrm{\λ}}^2}_2\left(0\right),{{\mathrm{\λ}}^2}_2\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_2\left(nT\right),\\ \mathrm{......},\\ {{\mathrm{\λ}}^2}_N\left(0\right),{{\mathrm{\λ}}^2}_N\left(T\right),\mathrm{......},{{\mathrm{\λ}}^2}_N\left(nT\right)\}\end{array}$

全局最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ以及速度增益系数求解单元中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

或者，最大速度求解单元包括：

速度增益系数筛选子单元，从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

局部最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ(nT)以及速度增益系数求解单元中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度

综上所述，实施本发明的基于动力学模型的机器人最大速度获取方法及系统，具有以下有益效果：本发明基于动力学模型，考虑了各关节力矩的力矩约束，使得求解的最大速度必为机器人最大可承受速度；所求取的最大速度，仅与机器人具体的运动轨迹有关，与先前的工作区域无关，极大提高了机器人的利用效率；进一步的，本发明既可以求取机器人运动时适用于所有位置的最大速度，又可以求取机器人运动时每个位置点可承受的最大速度，其属于高级轨迹规划控制器变最大速度控制的关键参数，可最大化机器人工作效率。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (10)

1.一种基于动力学模型的机器人最大速度获取方法，其特征在于，包括以下步骤：

S1、建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

S2、根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

S3、基于步骤S2中的速度增益系数λ_i(nT)和电机最大允许速度计算获得机器人最大速度。

2.根据权利要求1所述的基于动力学模型的机器人最大速度获取方法，其特征在于，所述步骤S2中，通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

3.根据权利要求1所述的基于动力学模型的机器人最大速度获取方法，其特征在于，步骤S2力矩τ_ia(nT)的计算包括：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于步骤S1中的所述动力学模型，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

4.根据权利要求3所述的基于动力学模型的机器人最大速度获取方法，其特征在于，所述步骤S3包括：

S31、从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

λ＝min{λ² ₁(0),λ² ₁(T),......,λ² ₁(nT),

λ² ₂(0),λ² ₂(T),......,λ² ₂(nT),

......,

λ² _N(0),λ² _N(T),......,λ² _N(nT)}

S32、根据步骤S31选出的速度增益系数λ以及步骤S2中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

5.根据权利要求3所述的基于动力学模型的机器人最大速度获取方法，其特征在于，所述步骤S3包括：

S31’、从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

S32’、根据步骤S31’选出的速度增益系数λ(nT)以及步骤S2中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度

6.一种基于动力学模型的机器人最大速度获取系统，其特征在于，包括：

模型建立单元，用于建立机器人的关节力矩与速度以及加速度之间的不显含重力、仅显含与位置相关的惯性力、离心力和科里奥利力的动力学模型：

$B\left(q\right(t\left)\right)\stackrel{\·\·}{q}\left(t\right)+\stackrel{\·}{q}\left(t\right)C\left(q\right(t),\stackrel{\·}{q}(t\left)\right)\stackrel{\·}{q}\left(t\right)={\mathrm{\τ}}_a\left(t\right)---\left(A\right)$

其中，τ_a(t)表示N×1的关节力矩向量，B(q(t))为N×N惯量矩阵，为N×N科里奥利力及离心力矩阵，N为机器人关节数，为加速度信息，为速度信息，q(t)为位置信息；

速度增益系数求解单元，根据设定的位置、速度以及加速度，计算每个采样周期的各关节的力矩τ_ia(nT)，且根据各关节力矩的力矩约束范围[τ_imin,τ_imax]，计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)；

最大速度求解单元，基于速度增益系数求解单元中得到的速度增益系数λ_i(nT)和电机最大允许速度计算机器人最大速度。

7.根据权利要求6所述的基于动力学模型的机器人最大速度获取系统，其特征在于，所述速度增益系数求解单元通过以下公式计算得到各个关节的每个采样周期的速度增益系数λ_i(nT)：

其中，τ_imin＜0且τ_imax＞0；n表示采样序号，n为大于等于0的正整数，i表示关节序号，i＝1,2,...,N。

8.根据权利要求6所述的基于动力学模型的机器人最大速度获取系统，其特征在于，力矩τ_ia(nT)的计算包括：设定运动轨迹的起止位置、各关节运行速度的恒定值各关节加速度值为0、机器人采样周期为T,基于所述动力学模型，计算得到对应于每个采样周期的各关节力矩为τ_ia(nT)。

9.根据权利要求8所述的基于动力学模型的机器人最大速度获取系统，其特征在于，最大速度求解单元包括：

速度增益系数筛选子单元，从所有的关节的所有的周期下的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的速度增益系数：

λ＝min{λ² ₁(0),λ² ₁(T),......,λ² ₁(nT),

λ² ₂(0),λ² ₂(T),......,λ² ₂(nT),

......,

λ² _N(0),λ² _N(T),......,λ² _N(nT)}

全局最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ以及速度增益系数求解单元中设定的运行速度计算得到适用于所有位置点的全局最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的最大速度

10.根据权利要求8所述的基于动力学模型的机器人最大速度获取系统，其特征在于，最大速度求解单元包括：

速度增益系数筛选子单元，从每个采样周期下的所有的关节的速度增益系数中筛选出最小的速度增益系数作为本次运动轨迹的每个位置点的速度增益系数：

λ(nT)＝min{λ² ₁(nT),λ² ₂(nT),......,λ² _N(nT)}

局部最大速度求解子单元，根据速度增益系数筛选子单元选出的速度增益系数λ(nT)以及速度增益系数求解单元中设定的运行速度计算得到某个位置点的局部最大速度为并从和电机最大允许速度中选取最小值作为本次运动轨迹的某个位置点的最大速度