CN101799660B - 用于操纵器的控制设备和方法 - Google Patents

Abstract

一种根据本发明的用于控制操纵器，尤其是机器人的方法，包括步骤：(S10)：存储参考路径(r_s[s(t)])；以及(S50，S70)：在沿所述路径行进时确定参考增量(Δq_s[s(t)])；其中，沿所述路径行进时，基于所述操纵器的运动学特性确定所述参考增量。

Description

用于操纵器的控制设备和方法

技术领域

根据权利要求1的前序，本发明涉及一种用于控制操纵器，尤其是机器人的方法，以及一种用于实施这种方法的用于操纵器的控制设备。

背景技术

在许多应用中，例如，通过即将被诸如工具中心点(TCP)的操纵器固有参考坐标系先后接近的位置的教导，操纵器的参考路径已被先验离线存储。当执行各自的工作程序时，插值设备基于规划为由所述已存位置开始的已存路径，在线确定操纵器的单关节的参考增量。该操纵器的控制目标是在由所述路径规划所确定的时间增量内实现所述参考增量。

因为，例如关节驱动无法施加必要力和必要转矩，这并不总是可行的。由此，在目前的实践中，往往手动降低路径速度，直到其满足转矩极限或其它限制。例如，在申请DE19959330A1中提到了这种所谓的覆盖(override)方法。

发明内容

本发明的一个目的在于改进操纵器在沿已存路径行进时的性能。

这一目的已经由一种具有权利要求1的特征的方法和具有权利要求14的特征的控制设备分别实现了。权利要求15和16分别涉及一种相应的计算机程序和计算机程序产品，尤其是一种存储介质或数据载体，而从属权利要求涉及具有优势的改进。

根据本发明，当所述操纵器沿所述路径行进时，在线将其动力学特性考虑在内。如果所述动力学特性不允许以预定的速度精确沿所述已存路径行进，可以以预定方式自动区别于所述路径，并且/或以预定方式自动降低所述路径速度。优选地，通过多标准优化在线确定所述反应。

为达到这一目的，首先将参考路径存储为，例如参考位置的序列

r_s＝{r_s，1，r_s，2，...}(1)

或参数化函数，例如，参考位置对时间t或路径参数s的高阶多项式样条

$r_s\left[s\left(t\right)\right]=\underset{i=0}{\mathrm{\Σ}}{a}_i\·t^i---\left(2\right)$

其中，优选地，所述操纵器的笛卡尔工作空间内限定一个参考位置，并且其可以限定操纵器固有参考坐标系，尤其是工具中心点TCP，相对于一个坐标系原点，例如笛卡尔坐标(x，y，z)的定位，和/或其相对于这样一个参考系的方位，例如欧拉或卡尔丹角(α，β，γ)

r_s[s(t)]＝[xyzαβγ]^T(3)

还可以在关节坐标q＝[q₁，...q_a]^T中直接限定一个参考位置，该关节坐标限定操纵器的一个关节的位置，例如，多关节型机器人的旋转关节的角坐标，其中，雅克比行列式J给出了操纵器固有参考坐标系的位置变化Δr和关节坐标变化Δq之间的转换关系

Δr＝J·Δq(4)

当沿所述路径行进时，在时间点t处，插值设备在线确定参考增量，凭其，操纵器的实际位置将在时间增量Δt内改变以沿所述存储的参考路径行进。可以在工作空间内，或优选地，在关节坐标中确定这些参考增量，使得它们可以在用于关节驱动的控制中作为参考值

Δq_s(t)(5)

总的来说，在当前的应用中，前馈控制和反馈控制都被称为控制，例如，PID单关节控制等等，其中，单个的Δq_i(t)作为参考关节角度更新。

所述插值设备可以首先为时间增量Δt确定一个参考位移Δr_s(t)，例如，通过在已存位置之间插值或通过求解已存函数的值

Δr_s(t)＝r_s(t+Δt)-r(t)(6)

然后，按照惯例由方程(4)和(6)可知，可以通过求解如下等式确定所述参考增量Δq_s(t)

Δr_s(t)＝J·Δq_s(t)(7)

然而，可能出现这种情况，由于，例如，可能需要很大的设备驱动力或驱动转矩，这类已确定的参考增量Δq_s(t)无法在时间增量Δt内实现。因此，根据本发明，基于并且将操纵器的动力学特性，尤其是优选为线性化的操纵器模型考虑在内，在沿所述路径行进时确定所述参考增量。

可以通过操纵器的运动方程式建模其动力学特性，

$M\·\frac{d^{2}q}{{\mathrm{dt}}^2}+h(q,\frac{\mathrm{dq}}{\mathrm{dt}})=\mathrm{\τ}---\left(8\right)$

所述运动方程式具有质量矩阵M，n阶时间导数dⁿ/dtⁿ，关节驱动力(在当前应用中，转矩也被称为力，例如驱动马达的转矩)τ＝[τ₁，...τ_A]^T，以及包括重力、摩擦力、回转力等等的向量h。一般来说，在本发明中，位置、尤其在关节坐标中，其时间导数、以及驱动力之间的每一个数学关系被称为“模型”，尤其是以表格形式存储或存储为参数化函数的关系。

通过所述运动方程式的线性化或离散化，以及/或对时间求微分，例如通过

$\frac{\mathrm{dq}}{\mathrm{dt}}\≈\frac{\mathrm{\Δ}{q}_s\left(t\right)}{\mathrm{\Δt}}---\left(9.1\right)$

$\frac{d^{2}q}{{\mathrm{dt}}^2}\≈\frac{\frac{\mathrm{\Δ}{q}_s\left(t\right)}{\mathrm{\Δt}}-\frac{\mathrm{dq}}{\mathrm{dt}}}{\mathrm{\Δt}}---\left(9.2\right)$

$\mathrm{\τ}=M\·\frac{d^{2}q\left(t\right)}{{\mathrm{dt}}^2}+h(q\left(t\right),\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}})\≈---\left(9.3\right)$

$M(\frac{\mathrm{\Δ}{q}_s\left(t\right)}{\mathrm{\Δ}{t}^2}-\frac{\mathrm{dq}\left(t\right)/\mathrm{dt}}{\mathrm{\Δt}})+h(q\left(t\right),\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}})$

由此，通过求解操纵器的该模型可以确定实现参考增量Δq_s(t)所需的驱动力τ是否超出了给定的驱动极限，例如，根据(7)确定参考增量Δq_s(t)以及根据(9.1)、(9.2)和(8)或(9.3)校验分别由模型(8)或(9.3)获得的驱动力是否在容许范围内：

τ_min≤τ≤τ_max(10)

如果并非如此，在一个优选实施方式中，所述参考增量被确定为以预先确定或预先限定的方式偏离所述参考运动或与所述参考运动不一致。为达到这一目的，所述参考增量可以被确定为，例如，尤其所述参考运动的线性、转换或映射，例如，通过乘上一个缩小或减少因子

由此，由对所述起初存储的参考路径进行插值或求解产生的参考运动被缩短或减少，直到将操纵器的动力学特性考虑在内而获得的所必需的驱动力τ在容许范围内。

在一个优选的实施方式中，预先确定与所述参考运动的其他偏差也是可行的，例如，通过确定一个包围所述参考运动的范围(tube)，其中，所述参考增量必须定位在所述范围内。然后，起初由对存储的参考路径进行插值或求解产生的参考运动在所述预定的范围内变化，直到，例如，将操纵器的动力学特性考虑在内而获得的必需的驱动力τ在容许范围内。这也是参考运动的参考增量的确定，如映射，尤其是线性映射，其中，所述映射描述容许偏差的预定空间，尤其是一个包围所述参考运动的范围

参数化所述映射，然后允许其最优化，即，确定相对于所述参考运动的偏差，使得将操纵器的动力学特性考虑在内而获得的必要驱动力τ在容许范围内

例如，

具有对所述参考运动的法向量和从法向量n_Δr(t)，b_Δr(t)或其他向量，其中，所述偏差可以通过，例如参数和/或通过选择向量被预先确定。

附加地或可替换地，如果用于实现所述参考运动的必要驱动力超出容许驱动力，用于时间增量的参考速度可以以预定的方式区别于所述参考运动速度，例如通过增大时间增量，通过减少参考运动和/或通过减少路径速度

优选地，可以通过优化确定所述参考增量。其中，例如，作为优化标准，所述参考运动和参考增量之间以预定方式的偏差

或

在容许(“zul”)驱动力和实现所述参考运动所必需的驱动力—尤其是根据所述(线性)模型的驱动力—之间的预留(reserve)

${\mathrm{\ψ}}_2=M\·\frac{d^{2}q\left(t\right)}{{\mathrm{dt}}^2}+h(q\left(t\right),\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}})-{\mathrm{\τ}}_{\mathrm{zul}}---\left(12.2\right)$

用于所述时间增量的参考速度和容许的笛卡尔速度之间的预留

${\mathrm{\ψ}}_3=|J\·\frac{\mathrm{\Δ}{q}_s\left(t\right)}{\mathrm{\Δt}}|-v_{\mathrm{zul}}---\left(12.3\right)$

用于所述时间增量的参考速度和参考关节速度之间的偏差：

${\mathrm{\ψ}}_4=\frac{\mathrm{\Δ}{q}_s\left(t\right)}{\mathrm{\Δt}}-\frac{\mathrm{dq}}{{\mathrm{dt}}_s}---\left(12.4\right)$

通过实现所述参考增量而达到的位形(poses)和奇异位形(singularposes)之间的预留

Ψ₅＝q_s-q_singul鋜(12.5)

实现所述参考运动所必需的驱动力：

${\mathrm{\Ψ}}_6=M\·\frac{d^{2}q\left(t\right)}{{\mathrm{dt}}^2}+h(q\left(t\right),\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}})=\mathrm{\τ}---\left(12.6\right)$

通过实现所述参考增量而达到的位形和优选(“bevorzugt”)位形之间的距离

Ψ₇＝J·q_s-r_bevorzugt(12.7)

和/或其他项目可以被优化。

以这种方式，例如，通过确定一个最佳缩放因子可以缩短由对所述起初存储的参考路径进行插值或求解而获得的参考运动，使得将操纵器的动力学特性考虑在内而获得的必要驱动力τ在容许范围内。所述单一优化标准可以被赋值为一个绝对标准

‖Ψ_i‖(13.1)，

一个最小、最大或平均标准

${\mathrm{\Ψ}}_i=[{\mathrm{\Ψ}}_{1.i},{\mathrm{\Ψ}}_{2,i},...{\mathrm{\Ψ}}_{n,i}]\⇒\left\{\begin{array}{c}\mathrm{Min}\{{\mathrm{\Ψ}}_{1,i},{\mathrm{\Ψ}}_{2,i},...{\mathrm{\Ψ}}_{n,i}\}\\ \mathrm{Max}\{{\mathrm{\Ψ}}_{1,i},{\mathrm{\Ψ}}_{2,i},...{\mathrm{\Ψ}}_{n,j}\}\\ \frac{{\mathrm{\Ψ}}_{1,i}+{\mathrm{\Ψ}}_{2,i}+...{\mathrm{\Ψ}}_{n,i}}{n}\end{array}\right.---\left(13.2\right),$

等等，其中，可以通过补偿函数或等式或不等式约束将不同的优化标准，尤其是竞争标准考虑在内，并且优选地，在可以同时优化不同的优化标准的多标准优化中求解，例如，帕累托优化、所述优化标准的加权总和等等。

基于所述选择的优化标准，尤其是基于与所述预定路径的偏差的加权、用于实现的必要驱动力和仍可容许的最大驱动力之间的预留、以及随之产生的速度，然后可以确定所述参考增量Δq_s(t)，使得操纵器可以完全利用其最大容许驱动力和速度，并且，如果这还不足以实现所述参考增量Δr_s(t)，由此开始以受控方式沿限定方向偏离和/或降低其速度。

如果所述优化没有寻找到容许的解决方案，例如，由于为了不超出所述容许驱动力，必须大幅度缩短所述参考运动，可以发出相关信息和/或停止沿所述路径行进。

如果操纵器是冗余的，尤其是如果其达到了奇异位置，使用优化也是具有优势的。这是因为通过求解所述优化标准(12.1)，为每一奇异位置寻找到至少一组关节坐标也是可能的。就此而言，如果操纵器在已存参考路径的一部分内具有比对于在笛卡尔工作空间内实现参考位置所必须的自由度更多的自由度，操纵器相对于所述部分是冗余的。例如，由于预定的用于定位的三个坐标和用于导向的三个坐标已经很好的限定了笛卡尔工作空间内的一个参考位置，具有七个或更多自由度的操纵器通常是冗余的。另一方面，如果，例如由于没有预先确定一个相对于轴的方位，用于描述的必需的定位坐标少于三个，用于描述的必需的导向坐标少于三个，相对于这一参考位置，具有较少自由度的操纵器也是冗余的。

附图说明

由从属权利要求及实施方式得出另外的优势和特征。其中，部分通过附图示出：

图1示出了具有一个关节和根据本发明一种实施方式的控制设备的机器人；

图2示出了根据本发明一种实施方式的控制方法。

具体实施方式

作为一个非常简单的例子，图1示出了具有一个单旋转关节的机器人1，该机器人将在时间周期T内沿图1中描述的作为参考路径的参考系的X-Z平面中的半圆行进。

为实现这一目的，在步骤S10(参照图2)中，首先将所述参考路径存储为关于时间t的参数化函数：

$r_s\left(t\right)=\left[\begin{array}{c}-\cos \left(\frac{\mathrm{\π}\·t}{T}\right)\\ 0\\ \sin \left(\frac{\mathrm{\π}\·t}{T}\right)\\ 0\\ \mathrm{\π}\·(1-\frac{t}{T})\\ 0\end{array}\right]---(2\text{'}),$

其中，参考位置r_s(t)表示TCP相对于工作空间内参考系的原点的以笛卡尔坐标(x，y，z)表示的定位以及其相对于所述参考系的以卡尔丹角(α，β，γ)(参照(3))表示的方位。

当使用垂直导向臂在时间点t＝0,5T沿图1所描述的路径行进时，插值设备在线确定关节角度q相对于y轴的参考增量Δq，凭此，为了沿所述参考路径行进，操纵器的实际位置将在时间增量Δt内改变。为了达到这一目的，在步骤S20(参照图2)中，所述插值设备首先通过求解所述已存函数的值确定所述时间增量：

$\mathrm{\Δt}=\frac{T}{4}$

参考运动Δr_s(t)：

$\mathrm{\Δ}{r}_s\left(\frac{T}{2}\right)=\left[\begin{array}{c}\frac{1}{\sqrt{2}}\\ 0\\ \frac{1}{\sqrt{2}}\\ 0\\ -\frac{\mathrm{\π}}{4}\\ 0\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 1\\ 0\\ -\frac{\mathrm{\π}}{2}\\ 0\end{array}\right]=\left[\begin{array}{c}\frac{1}{\sqrt{2}}\\ 0\\ \frac{1-\sqrt{2}}{\sqrt{2}}\\ 0\\ \frac{\mathrm{\π}}{4}\\ 0\end{array}\right]---(6\text{'})$

在步骤S25(未示出)中，根据等式(7)，通过，例如仅将方位考虑在内，确定临时的轴线角更新 $\mathrm{\Δ}{\tilde{q}}_s\left(\frac{T}{2}\right)=\frac{\mathrm{\π}}{4}.$

根据本发明，基于操纵器在沿所述路径行进时的动力学特性确定所述真实或实际的参考增量可以通过集中于TCP的质量m＝1kg的臂的运动方程来构建所述动力学特性模型，臂长l＝1m，重力常数g以及施加在关节上的驱动转矩τ：

通过线性化所述运动方程以及根据(9.1)-(9.3)的时间导数，然后在步骤S30中，可以通过求解操纵器的模型(8’)的值来确定实现所述临时的轴线角更新的必要驱动转矩τ是否超出了容许驱动极限：

${\mathrm{\τ}}_{\min }\≤\mathrm{\τ}=m\·[\frac{\mathrm{\Δ}{\tilde{q}}_s\left(\frac{T}{2}\right)}{\mathrm{\Δ}{t}^2}-\frac{{\frac{\mathrm{dq}}{\mathrm{dt}}|}_{\frac{T}{2}}}{\mathrm{\Δt}}]\≤{\mathrm{\τ}}_{\max }---(10\text{'})$

其中，时间点0,5T处的关节速度可以被测量。如果实现所述临时的轴线角更新的必要驱动转矩在容许驱动限制(步骤S30中的“J”)的容许范围[τ_min，τ_max]内，可以直接将其确定为真实或实际的参考增量

$\mathrm{\Δ}{q}_s\left(\frac{T}{2}\right)\←\mathrm{\Δ}{\tilde{q}}_s\left(\frac{T}{2}\right)---\left(S50\right)$

尽管如此，如果实现所述轴线角更新的必要驱动转矩超出了容许驱动极限(步骤S30中的“N”)，这时，确定所述参考增量，使得其以由公式(11)或(11a，b)所确定的预定的方式区别于所述参考运动，这样，由求解所述已存的参考路径的值得到的参考运动被缩短，直到所述必要驱动转矩τ在所述容许范围内。

为达到这一目的，在步骤S40中确定参数和参考增量使得参考运动和参考增量之间的偏差

或

的绝对值的加权w和容许驱动转矩与必要驱动转矩之间的预留

${\mathrm{\Ψ}}_2=m\·[\frac{\mathrm{\Δ}{q}_s\left(\frac{T}{2}\right)}{\mathrm{\Δ}{t}^2}-\frac{{\frac{\mathrm{dq}}{\mathrm{dt}}|}_{\frac{T}{2}}}{\mathrm{\Δt}}]-{\mathrm{\τ}}_{\mathrm{zul}}---(12.2\text{'})$

的负值的加权w的总和，即模具优化标准：

w₁·|Ψ₁|-w₂·Ψ₂，w₁，w₂≥0

成为最小值。当然，还可以通过补偿函数或等式或不等式约束将等式(12.1’)和(12.2’)考虑在内。就此而言，具有法向量和从法向量n_Δr(t)，b_Δr(t)的映射限定所述参考增量相对于所述参考运动的偏差，所述偏差在包围着由的上限限定的参考运动Δr_s的范围内。

由此，根据加权w，所述优化使用缩放因子缩短参考增量，直到使用相对于所述参考运动的最小必要偏差获得驱动转矩的足够预留，所述预留可以为零，这样可以完全利用可用的驱动转矩(S70)。这一参考增量被转移给PID关节控制，用于相应地移动机器人1。

在一个未示出的改进中，可供选择地所述优化降低速度直至达到所述驱动转矩的足够预留。

如果所述优化无法寻找到任何可接纳的解决方案(S60：“J”)，发出相关信息并且停止沿所述路径行进(S80)。

在另一个未示出的改进中，可以省略步骤S30。这是因为，如果保持所述驱动转矩的必要预留，并且保持完整的参考运动，通过步骤S40中各自的权重以及使所述优化将所述临时的轴线角更新确定为参考增量步骤S30中的准校验仍然具有优势，因为一方面其能更有效地确定所述临时轴线角更新，并且另一方面可以以简单的方式确保所述控制没有相对于所述驱动转矩的一个不必要大的预留值而不必要地缩短所述参考运动。

Claims (14)

1.一种用于控制操纵器的方法，包括以下步骤：

存储参考路径；以及

在沿所述路径行进时确定参考增量；

其特征在于，

基于所述操纵器在沿所述路径行进时的动力学特性确定所述参考增量；其中，

求解所述操纵器的模型，以确定沿所述路径行进时的所述参考增量；其中，当沿所述路径行进时，为时间增量确定参考运动，并且基于所述操纵器的动力学特性和所述参考运动确定所述参考增量；

其中，通过在存储位置或关节坐标之间插值或通过求解存储的函数来确定参考运动；

基于所述操纵器的动力学特性，确定实现所述参考运动所必需的驱动力是否超出容许驱动力；

所述参考增量以预定的方式区别于所述参考运动，和/或，如果实现所述参考运动所必需的驱动力超出容许驱动力和/或如果实现所述参考运动所必需的速度超出容许速度，用于时间增量的参考速度是预先确定的并区别于参考运动速度；

通过映射确定以预定方式区别于所述参考运动的参考增量；

在一个包围所述参考运动的预定范围内确定所述参考增量。

2.根据权利要求1所述的方法，其特征在于,所述操纵器的模型是线性化模型。

3.根据权利要求1所述的方法，其特征在于，所存储的参考路径包括操纵器固有参考坐标系的位置。

4.根据权利要求3所述的方法，其特征在于，所述固有参考坐标系的位置包括：定位和/或方位,关节坐标和/或参数化函数。

5.根据权利要求1所述的方法，其特征在于，所述映射是所述参考运动的线性映射。

6.根据权利要求1所述的方法，其特征在于，基于缩放的参考运动确定所述参考增量。

7.根据权利要求1所述的方法，其特征在于，通过优化确定参考增量。

8.根据权利要求7所述的方法，其特征在于，所述优化包括作为优化标准的，所述参考运动和参考增量之间的偏差，实现所述参考运动所必需的驱动力和容许驱动力之间的预留，用于所述时间增量的参考速度和容许速度之间的预留，用于所述时间增量的参考速度和参考关节速度之间的偏差，通过实现所述参考增量而达到的位形和一个奇异位形之间的预留，实现所述参考运动所必需的驱动力，和/或通过实现所述参考增量而达到的位形和一个优选位形之间的距离。

9.根据权利要求7所述的方法，其特征在于，所述优化包括一个或更多个优化标准。

10.根据权利要求9所述的方法，其特征在于，所述一个或更多个优化标准为补偿函数和/或等式或不等式约束。

11.根据权利要求7-10中任意一项所述的方法，其特征在于，所述操纵器关于至少一部分所存储的参考路径是冗余的。

12.根据权利要求1所述的方法，其特征在于，所述操纵器是机器人。

13.一种用于操纵器的控制设备，包括：

用于存储参考路径的存储装置；以及

用于在沿所述路径行进时确定参考增量的插值装置；

所述控制设备适于执行根据前述任意一项权利要求的方法；

其中，求解所述操纵器的模型，以确定沿所述路径行进时的所述参考增量；其中，当沿所述路径行进时，为时间增量确定参考运动，并且基于所述操纵器的动力学特性和所述参考运动确定所述参考增量；

其中，在一个包围所述参考运动的预定范围内确定所述参考增量。

14.根据权利要求13所述的设备，其特征在于，所述操纵器是机器人。