[发明专利]用于对共同的系统的多个能运动的模块的位置和朝向进行状态估计的方法

说明书

本发明涉及一种用于借助于惯性传感器（S1、S2）对共同的系统的多个能够相对于彼此通过关节（J1、J2）运动的模块的位置和朝向进行状态估计的方法，所述惯性传感器布置在所述模块处。在此，测定（100、101、110、111）至少一个向量对（或者），所述向量对代表所述关节（J1、J2）中的至少一个关节的和两个与所述关节（J1、J2）连接的模块的运动学的关系，并且将所述向量对纳入到状态估计中。

技术领域

本发明涉及一种用于对共同的系统的多个能够相对于彼此通过关节运动的模块的位置和朝向（Orientierung）进行状态估计的方法，在所述方法中将能运动的模块的和关节的运动学的关系纳入到状态估计中。此外，本发明涉及一种计算机程序以及存储所述计算机程序的机器可读的存储介质，当在计算设备上运行所述计算机程序时，所述计算机程序实施所述方法的每个步骤。最后，本发明涉及一种电子控制器，其被设定用于实施根据本发明的方法。

背景技术

目前在做功机器的领域中快速地推进自动化。为了使做功机器和其工具自动化，必要地通过状态估计来了解做功机器的和工具的位置和朝向。通过关节彼此连接的多个能够相对于彼此运动的模块的位置和朝向借助于布置在模块处的惯性传感器来测定。

在图1中示出了作为针对做功机器的实施例的、带有下部结构U和上部结构L1的挖掘机1，其中，所述上部结构L1能够通过第一关节J1相对于下部结构U沿水平线旋转。在上部结构L1处布置第一传感器单元S1，所述第一传感器具有测量上部结构相对于位置固定的基准坐标系的线性加速度和/或旋转速率的惯性传感器以及在该位置处测量地磁场的磁强计。所述挖掘机1具有带有其他的构件悬臂L2、调节悬臂L3和斗杆L4的挖掘机臂2以及挖斗3或者L5。所述悬臂L2通过第二关节J2与所述挖掘机1的上部结构L1连接，所述调节悬臂L3通过第三关节J3与所述悬臂L2连接，所述斗杆L4通过第四关节J4与所述调节悬臂L3连接，并且所述挖斗L5通过第五关节J5与所述斗杆L4连接。在每个构件L1、L2、L3、L4、L5处分别布置具有惯性传感器以及磁强计的传感器单元S1、S2、S3、S4、S5。所述惯性传感器是加速度传感器和旋转速率传感器，它们测量相应的传感器相对于位置固定的基准坐标系的线性加速度和旋转速率。此外，所述传感器单元S1、S2、S3、S4、S5能够具有分别测量每个关节J1、J2、J3、J4、J5的关节角度的关节角度传感器。以W来标记全局坐标系，所述挖掘机1处于在全局坐标系中。

在图2中示出了用于上部结构L1和悬臂L2的状态估计的本身已知的方法，并且在下文中根据该图来简要地解释。

针对下部结构U提供10变换矩阵和由此能推导的单位四元数，它们表明下部结构U在全局坐标系W中的位置和朝向。传感器单元S1、S2（也在图2中未示出的其他的传感器S3、S4、S5）、更准确地说惯性传感器和磁强计同时地或者相继地记录20、30测量信号。以ω标记的角速度、以a标记的加速度以及以m标记的磁场被记录作为测量信号。在左侧的索引在图2中表明相应的测量值在哪个参考坐标系中被记录。针对上部结构L1所测量的角速度、所测量的加速度和所测量的磁场穿过滤波器21，由此四元数尤其代表了上部结构的L1相对于全局坐标系W的旋转，并且测定上部结构L1的所估计的角速度。对于第一关节J1而言，现在借助于所述上部结构L1的四元数和所述下部结构U的单位四元数来测定40第一关节角度，并且而后由此随后测定41针对在下部结构U与上部结构L1之间的过渡的变换矩阵。最后，进行针对下部结构U的变换矩阵与针对在下部结构U与上部结构L1之间的过渡的变换矩阵的矩阵乘法42，以便得到针对在全局坐标系W与上部结构L1之间的过渡的变换矩阵。