CN110209048A - 基于动力学模型的机器人时间最优轨迹规划方法、设备 - Google Patents

Abstract

本发明公开了一种基于动力学模型的机器人时间最优轨迹规划方法、设备，所述机器人时间最优轨迹规划方法包括步骤：通过动力学建模和参数化预定的机器人运动轨迹，将关节力矩约束和几何路径约束转换到参数空间；以时间最短为目标构造优化问题并通过数值积分法求解，得到机器人的最优运动参数；考虑加速度突变引起的关节振动问题，在相平面上对加速度进行三次样条曲线平滑。相比于传统的梯形加速度轨迹规划方法，本发明在关节力矩约束和几何路径约束下，优化机器人的运动参数，实现机器人的高速运动，充分利用关节电机的性能进一步提高机器人的运动速度，缩短执行任务的节拍时间，从而提高其作业效率，对提升机器人的整体性能具有重要意义。

Description

基于动力学模型的机器人时间最优轨迹规划方法、设备

技术领域

本发明涉及机器人轨迹规划，尤其涉及一种基于动力学模型的机器人时间最优轨迹规划方法、设备。

背景技术

随着机器人技术的发展，机器人已广泛应用于日常的生产、生活中，代替人类完成复杂、危险、繁重、乏味的重复性工作。在某些应用场景中，比如码垛、上下料等，不仅需要机器人准确地完成指定任务，而且希望机器人能够以尽可能快的速度完成任务，缩短节拍时间，从而提高其作业效率。

但是机器人的运动速度受到其关节电机、机械结构等条件的限制。通过增大关节电机功率和尺寸的方法，在一定程度上可以提高机器人的运动速度，但是一味地增大功率，会增加机器人的成本和能源消耗，并且电机尺寸增大的同时机器人本身的惯量也在增加，而惯量的增加不利于机器人的控制。

机器人的运动由轨迹规划实现，常规的机器人轨迹规划，采用梯形加速度轨迹规划，仅在运动学参数的约束下，求解得到表示机器人的运动学参数，这样的规划方法受到关节耦合的影响，并且根据经验或者特定的应用场景所设定的恒定加速度无法真实反映机器人的运行状态。实际上机器人所能提供的加速度随着其位姿和负载的变化而变化，与机器人的动力学特性有着密切的关系，因此常规的轨迹规划方法无法发掘出机器人潜在的性能。

一种更实用的方法是基于机器人的动力学模型，以时间最短作为目标函数并在满足关节电机力矩和笛卡尔空间中几何约束条件下优化机器人的运动参数，使机器人末端沿着指定轨迹运动的时间最短，即机器人时间最优轨迹规划问题。

发明内容

为了进一步提高机器人的运动速度，本发明提出了一种基于动力学模型的机器人时间最优轨迹规划方法、设备及介质，基于机器人动力学模型并通过参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间，以时间最短为目标构造优化问题，并采用数值积分法求解得到最优运动参数，最终在相平面对加速度进行三次样条曲线平滑，减小关节振动，实现机器人的高速运动。

本发明的目的至少通过如下技术方案之一实现：

一种基于动力学模型的机器人时间最优轨迹规划方法，包括步骤：

建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

进一步地，所述的建立机器人动力学模型具体包括步骤：

对于n自由度工业机器人，考虑机器人连杆质量、质心位置、惯性张量和摩擦系数等参数，建立n自由度工业机器人的动力学模型并表示为：

式中q，和分别表示关节转角、关节速度和关节加速度；M(q)表示惯性矩阵；表示科氏力和离心力耦合矩阵；表示摩擦力项，包含粘性摩擦和库伦摩擦；G(q)表示重力项，τ表示关节电机力矩矢量；

通过对式(1)的参数解耦得到式(2)所示的线性辨识模型，并通过加权最小二乘法辨识得到机器人的动力学参数，从而建立完整的机器人动力学模型：

式中表示回归矩阵，β表示机器人基础动力学参数集，在得到机器人动力学参数后，可根据机器人的运动参数计算得到对应的关节力矩，反之也可以将机器人的关节力矩约束转化为运动参数的约束。

进一步地，所述的参数化机器人预定轨迹具体包括步骤：

在任意时刻，在笛卡尔空间可用机器人位姿矢量P(x,y,z,Rx,Ry,Rz)表示机器人工具中心点的位姿，机器人正运动学模型可表示为：

P＝Γ(q) (3)

式(3)分别对时间求一阶、二阶导数：

式中，Γ(·)表示机器人的正运动学模型，J(q)表示机器人的雅克比矩阵，表示J(q)对时间的一阶导数；

为了减少优化变量的个数，将优化问题从关节空间转换到参数空间，参数化轨迹的过程中，用一个标量参数来表示机器人轨迹，则描述机器人位姿的矢量P及其导数可用参数表示为：

P＝f(s) (6)

式中，s表示机器人末端沿着轨迹方向距离起点的位移，表示为s(t)，和分别表示s对时间的一阶和二阶导数，f(s)表示机器人轨迹的参数方程，f′(s)和f″(s)分别表示f(s)对s的一阶和二阶导数；

联立式(3)～(8)，则机器人的关节位置、速度和加速度用参数s及其导数表示：

q＝Γ^-1(f(s)) (9)

式中，Γ^-1(·)表示机器人的逆运动学模型，J^-1(q)表示机器人雅可比矩阵的逆矩阵。声明：已经生成的机器人轨迹中不存在奇异点，则任意位置处的雅克比矩阵可逆。

通过上述推导得到了参数s和机器人位姿矢量P的一一对应关系，即轨迹参数方程f(s)，同时通过雅可比矩阵及其导数，得到了参数表达的机器人关节位置、速度和加速度。

进一步地，所述以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数具体包括步骤：

将式(9)～(11)代入式(2)，从而得到用参数s、和表示的机器人关节计算力矩，即

式中，A(s)、B(s)、D(s)和E(s)表示计算力矩的系数矩阵，其表达式与中的元素有关，无需显示定义，但与参数s有一一对应的关系；

将关节力矩的极限值代入式(12)，根据电机规格参数和机器人动力学特性得到参数s、和的约束条件，

τ_min≤τ_cal≤τ_max (13)

式中τ_min和τ_max分别表示机器人最小关节力矩矢量和最大关节力矩矢量；

通过上述变换，将关节力矩约束和几何路径约束转换为参数空间中对应参数的约束，因此，在参数空间，机器人的时间最优轨迹规划问题可描述为：

式中，T表示机器人运动的总时间，t₀、t_e分别表示起始时刻和终点时刻，s₀和s_e分别表示轨迹起点和终点对应的参数，表示机器人末端的移动速度，则有

由式(14)可得，为达到时间最优，必须在约束条件下，选择最大的移动速度在任意时刻机器人以最大的加速度加速或以最小的加速度减速，不存在匀速运动阶段，通过变换式(13)，可得到关节力矩约束下加速度的约束不等式：

式中，i＝1,2,…,n，n表示机器人关节数量。

将式(15)简写为，

为使极限加速度满足所有关节的力矩极限，将加速度取值范围取式(16)的交集，表示为，

式中，

以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数。

进一步地，所述以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数具体包括步骤：

求解机器人时间最优问题就是求解式(14)所示的优化问题，对于预定的机器人运动轨迹，以固定距离离散得到参数s的序列，计算对应s位置的计算力矩系数矩阵A(s)、B(s)、D(s)和E(s)，那么式(13)的未知参数只有和

由式(15)可知，在任意时刻机器人各个关节力矩能够组成关于加速度的2n个不等式约束，在关节力矩极限和s已知的情况下，是的二次函数，则该机器人位姿下的标量速度的可行域为2n条抛物线构成的交集；

标量速度的可行域对应着速度的取值范围，决定了该机器人位姿下所能达到的最大速度，在规划过程中，取值范围内的任意一个标量速度对应着机器人此时所能提供的极限加速度和

遍历整个s序列则可以求出每个s位置所对应的机器人最大标量速度，从而得到满足关节力矩约束条件的最大速度曲线(Maximum Velocity Curve，MVC)，整个相平面以最大速度曲线为界，其下方区域表示满足关节力矩约束条件的可行域，反之为不可行域，规划过程中只要速度曲线超越最大速度曲线进入不可行域则表示机器人的某个关节力矩超限；

设最大速度曲线用g(s)来表示s和之间的对应关系，则构造如下形式的表达式：

式中，且或 表示MVC的斜率；

时间最优问题的求解，关键在于确定和的转换点(switchpoint,sp)，因此确定和的转换点，通过转换点位置就能在规划时预先选取合适的加速度参数，所述转换点分为两类：一类是指在最大速度曲线上的转换点，通过式(18)辨识出，沿着最大速度曲线搜索，当κ(s)改变符号时，意味着该处是一个转换点；另一类是将可行域内加速曲线与减速曲线的交点作为转换点；

从轨迹的起点以向前数值积分得到加速曲线，从轨迹的终点以向后数值积分得到减速曲线，并确定加速曲线与减速曲线之间的转换点，从而迭代数值积分过程完成优化问题的求解，分别得到位置s、速度和加速度的序列，各序列在参数空间描述了机器人的运动轨迹。

进一步地，所述在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动具体包括步骤：

为了使轨迹变得平滑，在相平面上，对加速度进行三次样条曲线插补，使得加速度能平滑过渡，所述三次样条曲线插补函数为：

其边界条件为：s(t₀)＝s_t,s(t_f)＝s_e,

同时调整速度重新得到s，和的离散序列，完成时间最优轨迹规划，并将参数代入式(9)～(11)，得到机器人控制器的输入控制信号。

轨迹优化过程中都是选取机器人在不同位姿时所能提供的极限加速度，因此在加速度发生改变的位置，特别是转换点位置，加速度存在突变，通过进行三次样条曲线平滑使轨迹变得平滑。

本发明另一方面还提供了一种基于动力学模型的机器人时间最优轨迹规划装置，包括：

参数空间转换模块，用于建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

最优运动参数求解模块，用于以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

样条曲线插补模块，用于在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

本发明另一方面还提供了一种电子设备，包括存储器、处理器、存储在存储器上并可在处理器上运行的计算机程序，所述处理器运行所述程序时，实现如所述的基于动力学模型的机器人时间最优轨迹规划方法。

本发明另一方面还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如所述的基于动力学模型的机器人时间最优轨迹规划方法。

相比于传统的梯形加速度轨迹规划方法，本发明在关节力矩约束和几何路径约束下，优化机器人的运动参数，实现机器人的高速运动，进一步提高机器人的运动速度，缩短执行任务的节拍时间，从而提高其作业效率；能够充分利用关节电机的性能来提高机器人的运动速度，对提升机器人的整体性能具有重要意义。

附图说明

图1为机器人任意位姿处的可行域示意图。

图2为时间最优算法求解示意图。

图3为加速度三次样条曲线平滑示意图。

具体实施方式

为进一步理解本发明，下面结合附图和实施例对本发明做进一步说明，但是需要声明的是，本发明要求保护的范围并不局限于实施例表述的范围。

实施例

现以某六自由度工业机器人为研究对象，按照以下步骤进行机器人时间最优轨迹规划。

一种基于动力学模型的机器人时间最优轨迹规划方法，包括步骤：

建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

进一步地，所述的建立机器人动力学模型具体包括步骤：

对于n自由度工业机器人，考虑机器人连杆质量、质心位置、惯性张量和摩擦系数等参数，建立n自由度工业机器人的动力学模型并表示为：

式中q，和分别表示关节转角、关节速度和关节加速度；M(q)表示惯性矩阵；表示科氏力和离心力耦合矩阵；表示摩擦力项，包含粘性摩擦和库伦摩擦；G(q)表示重力项，τ表示关节电机力矩矢量；

通过对式(1)的参数解耦得到式(2)所示的线性辨识模型，并通过加权最小二乘法辨识得到机器人的动力学参数，从而建立完整的机器人动力学模型：

式中表示回归矩阵，β表示机器人基础动力学参数集，在得到机器人动力学参数后，可根据机器人的运动参数计算得到对应的关节力矩，反之也可以将机器人的关节力矩约束转化为运动参数的约束。

具体而言，所述的参数化机器人预定轨迹具体包括步骤：

在任意时刻，在笛卡尔空间可用机器人位姿矢量P(x,y,z,Rx,Ry,Rz)表示机器人工具中心点的位姿，机器人正运动学模型可表示为：

P＝Γ(q) (3)

式(3)分别对时间求一阶、二阶导数：

式中，Γ(·)表示机器人的正运动学模型，J(q)表示机器人的雅克比矩阵，表示J(q)对时间的一阶导数；

为了减少优化变量的个数，将优化问题从关节空间转换到参数空间，参数化轨迹的过程中，用一个标量参数来表示机器人轨迹，则描述机器人位姿的矢量P及其导数可用参数表示为：

P＝f(s) (6)

式中，s表示机器人末端沿着轨迹方向距离起点的位移，表示为s(t)，和分别表示s对时间的一阶和二阶导数，f(s)表示机器人轨迹的参数方程，f′(s)和f″(s)分别表示f(s)对s的一阶和二阶导数；

联立式(3)～(8)，则机器人的关节位置、速度和加速度用参数s及其导数表示：

q＝Γ^-1(f(s)) (9)

式中，Γ^-1(·)表示机器人的逆运动学模型，J^-1(q)表示机器人雅可比矩阵的逆矩阵。声明：已经生成的机器人轨迹中不存在奇异点，则任意位置处的雅克比矩阵可逆。

通过上述推导得到了参数s和机器人位姿矢量P的一一对应关系，即轨迹参数方程f(s)，同时通过雅可比矩阵及其导数，得到了参数表达的机器人关节位置、速度和加速度。

具体而言，所述以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数具体包括步骤：

将式(9)～(11)代入式(2)，从而得到用参数s、和表示的机器人关节计算力矩，即

式中，A(s)、B(s)、D(s)和E(s)表示计算力矩的系数矩阵，其表达式与中的元素有关，无需显示定义，但与参数s有一一对应的关系；

将关节力矩的极限值代入式(12)，根据电机规格参数和机器人动力学特性得到参数s、和的约束条件，

τ_min≤τ_cal≤τ_max (13)

式中τ_min和τ_max分别表示机器人最小关节力矩矢量和最大关节力矩矢量；

通过上述变换，将关节力矩约束和几何路径约束转换为参数空间中对应参数的约束，因此，在参数空间，机器人的时间最优轨迹规划问题可描述为：

式中，T表示机器人运动的总时间，t₀、t_e分别表示起始时刻和终点时刻，s₀和s_e分别表示轨迹起点和终点对应的参数，表示机器人末端的移动速度，则有

由式(14)可得，为达到时间最优，必须在约束条件下，选择最大的移动速度在任意时刻机器人以最大的加速度加速或以最小的加速度减速，不存在匀速运动阶段，通过变换式(13)，可得到关节力矩约束下加速度的约束不等式：

式中，i＝1,2,…,n，n表示机器人关节数量。

将式(15)简写为，

为使极限加速度满足所有关节的力矩极限，将加速度取值范围取式(16)的交集，表示为，

式中，

以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数。

具体而言，所述以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数具体包括步骤：

求解机器人时间最优问题就是求解式(14)所示的优化问题，对于预定的机器人运动轨迹，以固定距离离散得到参数s的序列，计算对应s位置的计算力矩系数矩阵A(s)、B(s)、D(s)和E(s)，那么式(13)的未知参数只有和

对于六自由度机器人，由式(15)可知，在任意时刻机器人各个关节力矩能够组成关于加速度的12个不等式约束，在关节力矩极限和s已知的情况下，是的二次函数，其函数关系如图1所示，12个不等式约束可表示为相平面上的12条抛物线(为了避免图像杂乱，图1中只表示出了3个关节对应的6条抛物线)，则该机器人位姿下的标量速度的可行域为12条抛物线构成的交集，如图1中阴影部分所示；

标量速度的可行域对应着速度的取值范围，决定了该机器人位姿下所能达到的最大速度，在规划过程中，取值范围内的任意一个标量速度对应着机器人此时所能提供的极限加速度和

遍历整个s序列则可以求出每个s位置所对应的机器人最大标量速度，从而得到满足关节力矩约束条件的最大速度曲线(Maximum Velocity Curve，MVC)，整个相平面以最大速度曲线为界，其下方区域表示满足关节力矩约束条件的可行域，反之为不可行域，规划过程中只要速度曲线超越最大速度曲线进入不可行域则表示机器人的某个关节力矩超限；

设最大速度曲线用g(s)来表示s和之间的对应关系，则构造如下形式的表达式：

式中，且或 表示MVC的斜率；

时间最优问题的求解，关键在于确定和的转换点(switchpoint,sp)，因此确定和的转换点，通过转换点位置就能在规划时预先选取合适的加速度参数，所述转换点分为两类：一类是指在最大速度曲线上的转换点，通过式(18)辨识出，沿着最大速度曲线搜索，当κ(s)改变符号时，意味着该处是一个转换点；另一类是将可行域内加速曲线与减速曲线的交点作为转换点，具体包括步骤：

1)根据式(15)中的不等式约束求出相平面上的MVC，作为轨迹规划过程中的约束曲线。根据关节电机的伺服周期确定数值积分时间常数为1ms；

2)从轨迹起点令采用数值积分法向前积分，如图2中的α-profile，若α-profile不与MVC相交，且与s＝s_e相交，转到3)，若与相交，不与s＝s_e相交，则轨迹无解；若α-profile与MVC相交，转到3；

3)从终点位置令采用数值积分法向后积分，如图2中的β-profile，若β-profile不与MVC相交，且与α-profile相交，转到6；若β-profile与MVC相交，转到4；

4)从α-profile与MVC的交点沿着MVC向前搜索，根据式(18)确定转换点；

5)从确定的转换点处以向后积分，构造新的β-profile，相交于α-profile，交点为新的转换点；从确定的转换点处以向前积分，构造新的α-profile，若交于MVC，则转到4)，若交于β-profile，交点为新的转换点，转到6；

6)以α-profile和β-profile串联各转换点，构成完整的最优轨迹，轨迹优化结束。

如图3所示，优化完成后，得到在参数空间表示机器人轨迹的s，和离散序列。同时为避免加速度突变引起的关节振动，对加速度进行三次样条曲线平滑，包括步骤：

为了使轨迹变得平滑，在相平面上，对加速度进行三次样条曲线插补，使得加速度能平滑过渡，所述三次样条曲线插补函数为：

其边界条件为：s(t₀)＝s_t,s(t_f)＝s_e,

平滑完成后，同时调整速度重新得到s，和的离散序列，完成时间最优轨迹规划，并将参数代入式(9)～(11)，得到机器人控制器的输入控制信号，完成机器人的时间最优轨迹规划。

轨迹优化过程中都是选取机器人在不同位姿时所能提供的极限加速度，因此在加速度发生改变的位置，特别是转换点位置，加速度存在突变，通过进行三次样条曲线平滑使轨迹变得平滑。

本发明另一方面还提供了一种基于动力学模型的机器人时间最优轨迹规划装置，包括：

参数空间转换模块，用于建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

最优运动参数求解模块，用于以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

样条曲线插补模块，用于在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

为了实现上述实施例，本发明另一实施例还提供了一种电子设备，包括存储器、处理器、存储在存储器上并可在处理器上运行的计算机程序，所述处理器运行所述程序时，实现如所述的基于动力学模型的机器人时间最优轨迹规划方法。

为了实现上述实施例，本发明另一实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如所述的基于动力学模型的机器人时间最优轨迹规划方法。

本发明能够在关节力矩约束和几何路径约束下，优化得到机器人的时间最优运动参数。在关节驱动器规格确定的情况下，充分发挥关节驱动器的性能来尽可能的提高机器人的运动速度，有助于提升机器人的整体性能。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (9)

1.一种基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，包括步骤：

建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

2.根据权利要求1所述的基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，所述的建立机器人动力学模型具体包括步骤：

建立n自由度工业机器人的动力学模型并表示为：

式中q，和分别表示关节转角、关节速度和关节加速度；M(q)表示惯性矩阵；表示科氏力和离心力耦合矩阵；表示摩擦力项，包含粘性摩擦和库伦摩擦；G(q)表示重力项，τ表示关节电机力矩矢量；

通过对式(1)的参数解耦得到式(2)所示的线性辨识模型，并通过加权最小二乘法辨识得到机器人的动力学参数，从而建立完整的机器人动力学模型：

式中表示回归矩阵，β表示机器人基础动力学参数集。

3.根据权利要求2所述的基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，所述的参数化机器人预定轨迹具体包括步骤：

在任意时刻，在笛卡尔空间可用机器人位姿矢量P(x,y,z,Rx,Ry,Rz)表示机器人工具中心点的位姿，机器人正运动学模型可表示为：

P＝Γ(q) (3)

式(3)分别对时间求一阶、二阶导数：

式中，Γ(·)表示机器人的正运动学模型，J(q)表示机器人的雅克比矩阵，表示J(q)对时间的一阶导数；

参数化轨迹的过程中，用一个标量参数来表示机器人轨迹，则描述机器人位姿的矢量P及其导数可用参数表示为：

P＝f(s) (6)

式中，s表示机器人末端沿着轨迹方向距离起点的位移，表示为s(t)，和分别表示s对时间的一阶和二阶导数，f(s)表示机器人轨迹的参数方程，f′(s)和f″(s)分别表示f(s)对s的一阶和二阶导数；

联立式(3)～(8)，则机器人的关节位置、速度和加速度用参数s及其导数表示：

q＝Γ^-1(f(s)) (9)

式中，Γ^-1(·)表示机器人的逆运动学模型，J^-1(q)表示机器人雅可比矩阵的逆矩阵。

4.根据权利要求3所述的基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，所述以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数具体包括步骤：

将式(9)～(11)代入式(2)，从而得到用参数s、和表示的机器人关节计算力矩，即

式中，A(s)、B(s)、D(s)和E(s)表示计算力矩的系数矩阵，其表达式与中的元素有关，无需显示定义，但与参数s有一一对应的关系；

将关节力矩的极限值代入式(12)，根据电机规格参数和机器人动力学特性得到参数s、和的约束条件，

τ_min≤τ_cal≤τ_max (13)

式中τ_min和τ_max分别表示机器人最小关节力矩矢量和最大关节力矩矢量；

通过上述变换，将关节力矩约束和几何路径约束转换为参数空间中对应参数的约束，因此，在参数空间，机器人的时间最优轨迹规划问题可描述为：

式中，T表示机器人运动的总时间，t₀、t_e分别表示起始时刻和终点时刻，s₀和s_e分别表示轨迹起点和终点对应的参数，表示机器人末端的移动速度，则有

由式(14)可得，为达到时间最优，必须在约束条件下，选择最大的移动速度在任意时刻机器人以最大的加速度加速或以最小的加速度减速，不存在匀速运动阶段，通过变换式(13)，可得到关节力矩约束下加速度的约束不等式：

式中，i＝1,2,…,n，n表示机器人关节数量。

将式(15)简写为，

为使极限加速度满足所有关节的力矩极限，将加速度取值范围取式(16)的交集，表示为，

式中，

以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数。

5.根据权利要求4所述的基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，所述以关节电机的伺服周期作为数值积分时间常数，从轨迹起点向前和轨迹终点向后进行数值积分，并确定转换点位置，从而通过迭代数值积分过程求解时间最优运动参数具体包括步骤：

求解机器人时间最优问题就是求解式(14)所示的优化问题。对于预定的机器人运动轨迹，以固定距离离散得到参数s的序列，计算对应s位置的计算力矩系数矩阵A(s)、B(s)、D(s)和E(s)，那么式(13)的未知参数只有和

由式(15)可知，在任意时刻机器人各个关节力矩能够组成关于加速度的2n个不等式约束，在关节力矩极限和s已知的情况下，是的二次函数，则该机器人位姿下的标量速度的可行域为2n条抛物线构成的交集；

标量速度的可行域对应着速度的取值范围，决定了该机器人位姿下所能达到的最大速度，在规划过程中，取值范围内的任意一个标量速度对应着机器人此时所能提供的极限加速度和

遍历整个s序列则可以求出每个s位置所对应的机器人最大标量速度，从而得到满足关节力矩约束条件的最大速度曲线，整个相平面以最大速度曲线为界，其下方区域表示满足关节力矩约束条件的可行域，反之为不可行域，规划过程中只要速度曲线超越最大速度曲线进入不可行域则表示机器人的某个关节力矩超限；

设最大速度曲线用g(s)来表示s和之间的对应关系，则构造如下形式的表达式：

式中，且或 表示MVC的斜率；

确定和的转换点(switch point,sp)，通过转换点位置选取合适的加速度参数，所述转换点分为两类：一类是指在最大速度曲线上的转换点，通过式(18)辨识出，沿着最大速度曲线搜索，当κ(s)改变符号时，意味着该处是一个转换点；另一类是将可行域内加速曲线与减速曲线的交点作为转换点；

从轨迹的起点以向前数值积分得到加速曲线，从轨迹的终点以向后数值积分得到减速曲线，并确定加速曲线与减速曲线之间的转换点，从而迭代数值积分过程完成优化问题的求解，分别得到位置s、速度和加速度的序列，各序列在参数空间描述了机器人的运动轨迹。

6.根据权利要求5所述的基于动力学模型的机器人时间最优轨迹规划方法，其特征在于，所述在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动具体包括步骤：

为了使轨迹变得平滑，在相平面上，对加速度进行三次样条曲线插补，使得加速度能平滑过渡，所述三次样条曲线插补函数为：

其边界条件为：s(t₀)＝s_t,s(t_f)＝s_e,

同时调整速度重新得到s，和的离散序列，完成时间最优轨迹规划，并将参数代入式(9)～(11)，得到机器人控制器的输入控制信号。

7.一种基于动力学模型的机器人时间最优轨迹规划装置，其特征在于，包括：

参数空间转换模块，用于建立机器人动力学模型和参数化机器人预定轨迹，将关节力矩约束和几何路径约束转换到参数空间；

最优运动参数求解模块，用于以时间最短为目标构造优化问题并通过数值积分法求解得到机器人的最优运动参数；

样条曲线插补模块，用于在相平面对优化得到的加速度进行三次样条曲线平滑，避免加速度突变造成运动时机器人关节的振动。

8.一种电子设备，包括存储器、处理器、存储在存储器上并可在处理器上运行的计算机程序，所述处理器运行所述程序时，实现如权利要求1至6中任一项所述的基于动力学模型的机器人时间最优轨迹规划方法。

9.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至6中任一项所述的基于动力学模型的机器人时间最优轨迹规划方法。