具有工作臂的位置控制装置的移动工作机械和用于控制移动工作机械的工作臂的位置的方法

摘要

带有工作臂(6)位置控制装置的移动式工作机械(1)具有工作臂(6)，其以第一端在工作机械(1)的上部车身(3)上铰接地设置，以及工具(9)，其在工作臂(6)的第二端上可运动地设置。在上部车身(3)上设置至少一个倾斜传感器，并且在工作臂(6)上设置至少另一个倾斜传感器。此外在工作臂(6)上设置至少一个转动速率传感器。此外该工作机械(1)包括计算单元(17)，用于处理所述至少一个倾斜传感器、所述至少另一个倾斜传感器以及所述至少一个转动速率传感器的信号。

说明书

描述

本发明涉及一种移动式的工作机械，例如挖掘机、具有上部结构的载重汽 车或者农业-及林业设备，具有至少一个工作臂。这种工作机械的工作臂具有至 少多个彼此铰接的部段，其中在一个也称为斗杆(Stiel)的部段上安装工具，例 如铲、爪持器或者锤。

经常为这样的工作机械的操作者在显示器上显示出所述工作臂的、特别还 有工具的当前位置和姿势，由此操作者能够按照精确地预定的计划进行工作， 以便获得关于例如被移动的松散物料或者土壤或者要成形的或者已成形的基础 达到的高度、长度、深度或者倾斜度的直接的返馈消息。

这种操作显示例如从DE 201 16 666U1和从US 5,854,988A中获知。

在这里，所述工具的位置和姿势也由于良好的后装备性经常通过倾斜传感 器来确定，该倾斜传感器在工作臂的单个部段上安装。然后从单个臂部段的倾 斜通过由例如可调动臂、起重臂、斗杆和工具所组成的运动学的链的已知的几 何比例计算所述工具的位置和姿势。

但是因为作为倾斜传感器使用基于惯性测量原理的传感器、例如对重力敏 感的摆，所以它们对于由于震荡和振动的加速也很敏感，正如这在这种工作机 械中使用时不可避免地出现的那样。这种由运动决定的加速度能够极大地干扰 工具的位置和姿势的测量，或者使其短时不能进行。一种已知的用来抑制这种 干扰的措施在于：对于传感器信号进行低通滤波，使得对于在使用频带之外的 运动决定的加速进行抑制。

然而这同时产生许多缺点：一方面以这种方式不能抑制在使用频带之内的 加速，另一方面所述低通滤波注定产生直到数百毫秒的时间上的延迟，然而这 种延迟是可以容忍的，因为它总是仍然能够足够准确地显示位置，并且不妨碍 工作臂的手动操作。

从另外的技术领域已知，为了对于例如机器人、飞行体或者车辆进行位置 控制，使用基于加速度的倾斜传感器和转动速率传感器的组合。WO 01/57474A1 公开了一种这样的方法，其中为了计算一种位置而使用四元数显示 (Quaternionendarstellung)。

本发明的任务是，给出一种具有用于确定位置的装置的移动式的工作机械， 该装置具有足够小的延迟时间，以致于它不仅能够用于位置显示，而且也能够 用于对于机器的工作臂进行位置控制。但是工作臂的动力学特性不应在这种情 况下受到限制。本发明的另一任务是，给出一种用于这种位置控制的方法。

根据本发明，该任务通过独立权利要求的发明主题来解决。本发明的有利 的改进方案是从属权利要求的主题。

根据本发明的具有工作臂位置控制装置的移动式的工作机械具有工作臂， 该工作臂以第一端铰接地设置在工作机械的上部车身上。在该工作臂的另一端 上可运动地设置工具。

此外，工作机械具有多个传感器，确切地说，至少一个在上部车身上设置 的倾斜传感器、至少一个在工作臂上设置的另一个倾斜传感器和至少一个在工 作臂上设置的转动速率传感器。此外工作机械包括计算单元，用于处理所述至 少一个倾斜传感器、所述至少另一个倾斜传感器以及所述至少一个转动速率传 感器的信号。但是当然该计算单元也能够设置于在臂上固定的传感器模块之内。

按照本发明的基本思想，为了实现实时控制，应该实现最大约250ms的延 时。但是该数值也可以取决于所构造的工作臂硬件和使用目的地进行变化。因 此应该去掉测量值的低通滤波连同由此产生的信号延迟。另一方面，应该仍然 使用倾斜传感器，因为它们成本较低、较为耐用，此外容易在后装备，并且从 而特别适宜在移动式的工作机械上使用。为了使得所述倾斜测量较不容易地受 到上述通过运动感生的加速度的干扰，另外使用了转动速率传感器。该转动速 率传感器具有高的动力学的精确度，然而随之带来偏移和噪声这样的问题。虽 然在航空电子导航中应用的、并且基于萨格纳克(Sagnac)效应以及例如基于 使用纤维陀螺仪(Faserkreiseln)的转动速率传感器非常准确，然而与此相对， 微型机械式的转动速率传感器成本显著低，而且尤其也耐用，并且因此更好地 适合在移动式的工作机械上使用。由于这种转动速率传感器的上述的漂移问题， 根据本发明，其与倾斜传感器组合起来。

这种解决方案具有的优点是：其确切地按照在移动式的工作机械上的使用 来定制，因为它为了实时地控制工作臂的或者在其上安装的工具的位置而具有 足够的准确度，但是同时能够以低的成本实现，并且非常结实并且不容易受到 干扰。因此通过将具有非常好的静态准确度的倾斜传感器和具有非常好的动态 准确度的转动速率传感器组合起来，借助于数据合并算法或者估计算法就能够 进行所述实时控制。

工作臂可以具有一些彼此铰接地连接的部段，其中所述工具例如安装在最 后的部段的末端上。在每一种情况下，工作臂包括至少一个部段。

在一种实施方式中，在每一部段上设置另一倾斜传感器以及一转动速率传 感器。

在一种可选的实施方式中，在每一部段上设置另外两个倾斜传感器以及一 转动速率传感器。在这种实施方式中，不仅可以测量所述加速度的一空间分量 (例如在x方向上)，而且能够同时测量两个分量(例如在x和在z方向上)，这 在规定的倾斜角的范围内改善了测量的灵敏度。

倾斜传感器可以基于不同的测量原理，并且例如具有摆动体(Pendel- )和/或起到折射作用的液体镜或者说液面(refraktierende Flüssigkeits- spiegel)。它们也可以作为电容式的或者测量电导率(konduktometrische)的 倾斜传感器构造，然而优选地构造为微型机械式的加速度传感器。

所述转动速率传感器尤其构造为微型机械式的传感器。

该位置控制装置适合在移动式的工作机械中使用，例如挖掘机、伸缩臂式 装卸车、挖掘装卸车、轮式装卸车、装载起重机或者林业机械。

根据本发明的另一方面，给出一种用于控制移动式的工作机械的工作臂的 位置的方法，其中所述位置确定具有下面的步骤：确定在工作臂上作用的加速 度这例如可以在所有三个空间方向上进行，其中借助于三个加速度传感器来 测量三个分量a_x、a_y和a_z。但是也可以仅在x和z方向上测量所述加速度就已足 够。从在工作臂上作用的加速度在假定工作臂静止的情况下，对于倾斜角θ 计算第一值θ_s，这里该倾斜角θ被定义为所述工作臂围绕着y轴倾斜的角度。

在这里，对于在工作臂上安装的加速度传感器来说适用下面的关系，因为 为了确定倾斜角θ而要进行从工作臂的部段的或者传感器的静止系向上部车身 的静止系的坐标变换：

$a_{x,s}={\stackrel{·}{v}}_x-{\omega }_z{v}_y+{\omega }_y{v}_z-g\sin \theta$

$a_{y,s}={\stackrel{·}{v}}_y+{\omega }_z{v}_x-{\omega }_x{v}_z+g\sin \phi \cos \theta ---\left(1\right)$

$a_{z,s}={\stackrel{·}{v}}_z-{\omega }_y{v}_x+{\omega }_x{v}_y+g\cos \phi \cos \theta$

这里用a表示在物体上固定的轴线x、y和z的方向上传感器的测量值，下 标S表示各传感器。v表示工作臂的速度，ω表示它的角速度，而φ和θ表示所 谓的欧拉角(Eulerwinkel)滚动角(Rollwinkel)和倾斜角。g表示重力加速度。

(1)中的公式的各第一项说明从整个工作机械或者工作臂或者部段的平动 产生的加速度，而各中间的项说明从整个工作机械或者工作臂或者部段的转动 产生的加速度。也就是说这些项分别与各最后的项不同地说明“干扰”，因为它 们涉及这样的加速度，这些加速度不从地球的重力场的影响产生，从而不描述 重力场中的相应的传感器的倾斜。

因为在y方向上测量的加速度传感器，正如从(1)的第二行中所见的那样， 在不同的滚动角φ的情况下仅测量上述干扰，所以当假定对于静态的情况(工 作臂静止)下的倾斜角θ计算第一值θ_s时也许能够省去它。通过(1)的第一和 第三行说明的加速度信号基本上通过在x和z方向上的线性运动以及通过围绕y 轴的转动(向心和科里奥利项(Coriolisterme))来干扰。

为了从加速度传感器的测量值对于倾斜角θ计算第一值θ_s，例如如下方式 进行：

当假定工作臂静止，即静态的情况下，从(1)产生：

a_x，s＝-g sinθ_s

a_z，s＝g cosθ_s

由此对于倾斜角θ所查找的第一值θ_s按照下式给出：

$\tan {\theta }_s=-\frac{a_{x,s}}{a_{z,s}}---\left(3\right)$

如从(2)中得出，也可以仅用一个传感器按照下式为静止的臂确定θ_s，

$\sin {\theta }_s=-\frac{a_{x,s}}{g}---\left(4\right).$

然而这附带地产生缺点：θ_s的值域被限制在区间-90°...90°内。至少对于工作机 械的斗杆，亦即对于其上固定有工具的那一臂部段和对于工具自身来说，该值 域太小，以致不能说明实际的情况。此外，此时由于正弦函数在-90°和90°附近 的梯度很小，所以测量的灵敏度小。代替它的可以是，在(2)中使用具有两个 参量(atan2)值域在-180°和180°之间的反正切函数。

为了对于倾斜角θ确定第一值θ_s，这里假定工作臂静止。在工作臂运动并 且工作机械的上部车身可能转动的情况下，所述干扰通过(1)中的第一项和中 间的项迅速地增大，以致根据(3)的倾斜角的确定不再提供足够精确的结果。 因此这种方式对于工作臂的位置的实时控制来说是不足够的。

因此另外测量工作臂的一角速度ω，更确切地说测量围绕y轴的转动的至 少一个分量ω_y。亦即此外从刚体动力学知道下面的方程组，其建立转动速率或 者角速度ω与欧拉角θ、φ和ψ以及它们的时间导数之间的关系：

$\stackrel{·}{\theta }=\cos \phi ·{\omega }_y-\sin \phi ·{\omega }_z$

$\stackrel{·}{\psi }=\frac{\sin \phi }{\cos \theta }{\omega }_y+\frac{\cos \phi }{\cos \theta }{\omega }_z$

从中对于不同的滚动角φ产生：

$\stackrel{·}{\theta }={\omega }_y$

$\stackrel{·}{\psi }=\frac{{\omega }_z}{\cos \theta }$

因此通过在时间区间t上对于角速度ω_y进行积分能够得到在动态情况下用 于倾斜角θ的第二值θ_d：

θ_d＝θ₀+∫ω_ydt。

然而这里出现的问题是：例如偏移误差和传感器噪声被连续地积分，由此 即使在较短的时间之后也产生从实际的倾斜角θ离开的如此大的偏差，使得测 量准确度不能满足对它提出的要求。这至少在使用微型机械式的转动速率传感 器时是这种情况。其他的传感器、例如光纤陀螺传感器能够提供较大的并且在 许多情况下足够的准确度。但是它们具有成本高而且较不结实的缺点，以致它 们不太适合在移动式的工作机械上使用。

现在为了一方面利用能够良好地描述静态情况的倾斜传感器、另一方面利 用虽然能够良好地描述动态情况但是具有经过较长的时间段会产生漂移问题的 转动速率传感器这两种可用的设备对于倾斜角θ得到足够精确的值，使用一种 估计算法，以便从所述两个值θ_s和θ_d得到用于实际的倾斜角θ的估计值从 在x和z方向上测量的加速度a_x，s和a_z，s通过atan2函数按照(3)为静态情况计 算出角度θ_s，该角度然后作为估计量用于对于转动速率ω_y进行积分。为此把积 分结果和估计值的差向估计算法反馈。以这种方式还消除错误的起始值θ_o。

为此可以使用公知的估计算法，例如卡尔曼滤波器或者从控制技术获知的 观察方法，其也许还能够附带估计转动速率传感器的偏移误差。

该结果是用于实际的倾斜角θ的估计值

本发明的方法具有的优点是：仅用三个传感器—两个加速度传感器和一个 转动速率传感器—就能够以一定的准确度确定工作臂或者工作臂的规定点、例 如工具的悬挂点的位置，这种准确度使得能够进行一种实时控制。

在一种实施方式中，工作臂具有多个彼此铰接的部段，并且为每一部段单 独地对于倾斜角确定估计值式中下标i指相应的部段。

在一种实施方式中，从计算在移动式的工作机械的工作臂上安装的工具 的位置和姿势，并且可能还控制该位置和姿势。

然而也可以从计算出至少一个给所述移动式的工作机械的工作臂分配的 液压缸的偏转，并且可能还控制该偏转。

下面根据附图详细地说明本发明的实施例。

图1示意性地示出根据本发明的实施方式的作为挖掘机构造的移动式工作 机械；

图2示出按照图1的工作机械的工作臂上的几何比例的原理图；

图3示出在根据本发明的位置控制的实施方式中置于上面的信号流的图 表；

图4示出受运动干扰的倾斜信号的曲线图；

图5示出利用低通滤波来确定倾斜角的曲线图；

图6示出借助于转动速率传感器来确定带有漂移的倾斜角的曲线图；以及

图7示意性地示出对于倾斜角的根据本发明地估计用的方块图。

在所有的附图中给相同的部分提供相同的附图标记。

图1示意性地示出一种作为挖掘机构造的移动式的工作机械1。工作机械1 在该实施方式中具有基底部分2，它在地基5上站立或者行驶，还有上部车身3， 其可围绕竖直的轴线4转动。

工作臂6与上部车身3铰接地连接，该工作臂包括第一部段7和第二部段 8，它们同样彼此铰接地连接。在第二部段8上铰接一工具9，这里该工具9的 固定点也称作工具中心点(TCP-Tool Center Point)10。在上部车身和第一部 段7之间、在部段7和8之间以及在第二部段8和工具9之间设置液压缸11， 它们分别定义一种角度θ_i。

在上部车身3上设置一种传感器单元13，在第一部段7上设置一种传感器 单元14，在第二部段8上设置一种传感器单元15以及在工具9上设置一种传感 器单元16。每个传感器单元13、14、15和16在本实施方式中包括两个倾斜传 感器和一个转动速率传感器，其中所述两个倾斜传感器测量在x和z方向上的 加速度，转动速率传感器测量围绕y轴转动的角速度。

在一个未图示的实施方式中，也可以去掉每一传感器单元的所述倾斜传感 器之一，并且如果估计到由于上部车身运动的干扰较小，则给上部车身3分配 的传感器单元也可以仅具有倾斜传感器而没有转动速率传感器。

传感器单元13、14、15和16的测量数据被输送给移动的工作机械1的计 算单元17，该计算单元由此进行特别是TCP 10的位置确定和位置控制、以及工 具9的姿势的确定和控制。但是用于确定位置的计算单元也可以位于在臂上安 装的传感器模块之内。

图2表示按照图1的移动式的工作机械1的工作臂6上几何比例的原理草 图。

上部车身3以角度θ₁、第一部段以角度θ₂、第二部段以角度θ₃、并且工具 9以角度θ₄对于重力加速度g方向的垂线倾斜。已知在第一转动点18和第二转 动点19之间或者在第二转动点19和TCP 10之间的距离l₂和l₃，它们表示第一 部段7和第二部段8的长度，使得通过倾斜角θ_i的确定就能够计算出TCP 10的 位置和工具9的姿势。也就是说，预先知道臂部段的几何学的(结构的)数据。

图3表示在根据本发明的位置控制的实施方式中上级信号流的图表。

这里在第一步骤中，在运动学链的所有构件—亦即在上部车身3、第一部段 7、第二部段8和工具9上进行加速度的测量和转动速率的测量。从这些测量值 动态地确定所述倾斜角θ_i。

最后，正如关联图2所示出的那样，从倾斜角θ_i可以求得工具9的位置和姿 势。然而代之或者补充地，也可以从中计算液压缸11的偏转。

图4表示当单独从公式(3)、即仅仅借助于倾斜传感器进行倾斜角计算时， 通过运动所干扰的倾斜信号的曲线图。这里由于运动决定的加速度在计算的倾 斜角与实际倾斜角之间局部地产生了极大的偏差。

图5示出了借助于低通滤波从公式(3)确定倾斜角的曲线图，这里通过低 通滤波所产生的时间延迟十分明显。

图6表示借助于转动速率传感器按照公式(7)、亦即不借助倾斜传感器来获 取带有漂移的倾斜角的曲线图。

图7示意性地示出用于单个传感器单元用的倾斜角θ进行根据本发明地估计 的方块图，其中为了计算静态的倾斜角θ_s使用传感器单元的倾斜传感器的测量 值a_x，s和a_z，s，并且为了计算动态的倾斜角θ_d对于同一传感器单元的转动速率传 感器的测量值ω_y进行积分。接着在估计算法中反馈(zurückkoppeln)静态和动 态的倾斜角的差，以便得到倾斜角的估计值

作为估计算法，这里使用公知的方法、例如卡尔曼滤波器或者观察方法或者 从中导出的或者修改的方法。

附图标记列表

1    工作机械

2    基底部分

3    上部车身

4    轴线

5    地基

6    工作臂

7    第一部段

8    第二部段

9    工具

10   工具中心点

11   液压缸

13   传感器单元

14   传感器单元

15   传感器单元

16   传感器单元

17   计算单元

18   第一转动点

19   第二转动点