操作包括工具的机器的方法与流程

本发明涉及一种操作包括工具的机器的方法，以及用于执行这种方法的机器。

背景技术：

机器，特别是建筑、采矿、土方工程、货物处理、林业、农业或其他此类工业中的机器，通常利用工具来执行工作、监测操作等。各种工具可以附接到多功能机器的臂装置。工具可以由操作者控制和/或由机器的控制系统自动控制。可能需要工具的准确位置，例如用于向操作者提供反馈，用于控制工具的监测和/或向控制系统提供反馈回路。

us-b2-9347205公开了一种具有工具加速度的工具，该工具加速度可操作地耦接到具有车身加速度的车身。通过接收来自安装在车身上的加速度计和安装在工具上的加速度计的加速度测量值来估计工具的姿态。至少部分地基于车身加速度测量和工具加速度测量来计算用于系统状态估计的状态向量估计。状态向量估计包括工具相对于车身的姿态的表示。除了加速度计测量之外，还可以从安装在车身上的陀螺仪和安装在工具上的陀螺仪接收角速度测量值。然后，至少部分地基于车身加速度测量、工具加速度测量、车身角速度测量和工具角速度测量来计算状态向量估计。然而，这种布置需要安装在车身和工具上的加速度计和陀螺仪，从而增加了成本和复杂性。

技术实现要素：

本发明涉及一种操作机器的方法，该机器包括被配置用于在至少三个自由度上运动的工具，该方法包括：接收输入；响应于输入，将工具从第一位置运动到第二位置，该运动被约束为第一平面中的第一运动或第二平面中的第二运动；接收来自附接到工具的imu并指示工具的运动的传感器数据；确定运动是第一运动还是第二运动；基于确定运动分别是第一或第二运动来选择第一或第二估计模式；并且实施第一或第二估计模式以基于传感器数据估计工具的轨迹和第二位置。

可以在第一和第二位置从imu接收传感器数据的顺序样本。该工具在第一和/或第二位置可以是静止的。工具可以在第一和/或第二位置运动。该方法可以包括以正常操作模式操作机器，其中工具在至少四个自由度上运动操作。第一和第二估计模式可以基于工具的估计轨迹来估计相对于第一位置的第二位置。可以定义参考系，可以确定参考系中的第一位置，并且可以基于工具从第一位置到第二位置的相对运动来确定参考系中的第二位置。该方法可以进一步包括基于从传感器数据导出的值达到或超过阈值来确定已经发生了运动。

该方法还可以包括在第一位置执行第一监测操作以产生第一监测数据；在第二位置执行第二监测操作以产生第二监测数据；并且基于第二位置的估计来关联第一和第二监测数据。传感器数据可包括指示工具的加速度的加速度数据和指示工具的角速度的角速度数据。第一和/或第二估计模式可以包括基于传感器数据和状态转移模型来估计第二位置。

该方法可以进一步包括将工具保持静止在第一位置，并且在传感器数据中存在重力干扰的情况下，第一和/或第二估计模式基于当工具在第一位置静止期间从imu接收的加速度数据来确定重力向量。该方法还可以包括：基于当工具在第一位置静止期间从imu接收的加速度数据确定重力向量；基于从imu接收的角速度数据确定表示在运动期间工具旋转的旋转数据；从imu接收指示运动和重力的加速度数据；并且基于重力向量、旋转数据和加速度数据估计工具的运动的加速度。

第一估计模式可以包括基于从imu接收的传感器数据确定轨迹和第二位置。第一估计模式可包括基于第一估计模型和传感器数据确定轨迹和第二位置。第一估计模式可包括接收指示运动的加速度数据并基于从imu接收的角速度数据通过解析运动期间的加速度方向来确定第二位置。第一估计模式可以包括实施卡尔曼滤波器。卡尔曼滤波器的状态向量可包括位置、速度、加速度、角速度和旋转四元数。在卡尔曼滤波器中，工具的运动可以被约束到第一平面。卡尔曼滤波器的测量向量可以包括基于从imu接收的加速度和角速度数据的加速度和角速度分量。第一平面可以是竖直平面。

第二运动可以是第二平面中的围绕枢轴的圆周运动并且可以是沿着离枢轴的恒定运动半径的圆周运动。枢轴可以在第一平面中。在第二估计模式中，可以接收运动半径，并且可以基于接收的运动半径、第二估计模型和从imu接收的传感器数据来估计第二位置。可以在校准模式期间估计运动半径。在校准模式中，工具可以在校准运动中运动，并且可以基于校准运动估计运动半径。校准模式可以包括：将工具从第一校准位置运动到第二校准位置；基于从imu接收的加速度和角速度数据确定第一和第二校准位置之间的距离；基于从imu接收的角速度数据确定第一和第二校准位置之间围绕枢轴的旋转角度；并基于距离和旋转角度估算运动半径。可以基于第二估计模型来估计运动半径。第二估计模型可以包括实施卡尔曼滤波器。卡尔曼滤波器的状态向量可包括位置、速度、旋转四元数和运动半径。第二平面可以是水平平面。

可以从至少一个输入设备接收输入。该方法可以包括操作控制器以执行上述方法步骤。本发明还提供了一种包括工具和控制系统的机器，该控制系统包括至少一个输入、附接到工具的imu和控制器。控制器可以被配置为执行上述方法。该工具可以附接到臂装置上，以使工具能够在至少三个自由度上运动。臂装置可以在枢轴处附接到机器的主体。该机器可包括至少一个致动器，用于操作臂装置以使工具在至少三个自由度、第一运动和/或第二运动中运动。本发明还可以提供存储用于执行上述方法的指令的计算机可读介质。

本发明还提供一种机器，包括：通过臂装置附接到主单元的工具；控制系统，用于控制臂装置以使工具在至少三个自由度上运动，控制系统包括至少一个输入、附接到工具的imu、用于控制臂装置和工具的运动的至少一个致动器；其中控制系统还包括控制器，被配置为：从至少一个输入接收输入；响应于输入，将工具从第一位置运动到第二位置，该运动被约束为第一平面中的第一运动或第二平面中的第二运动；接收来自imu并且指示工具的运动的传感器数据；确定运动是第一运动还是第二运动；基于确定运动分别是第一或第二运动，选择第一或第二估计模式；并且实施第一或第二估计模式以基于传感器数据估计工具的轨迹和第二位置。

附图说明

图1示出了根据本发明的机器的示意性侧视图；

图2示出了根据本发明的机器的示意性俯视图；

图3示出了说明根据本发明的操作机器和/或控制器的方法的流程图；以及

图4示出了根据本发明的机器的操作的监测模式的流程图。

具体实施方式

总体而言，本发明涉及一种操作包括工具的机器的方法，该工具包括用于检测和实施工具的运动的运动传感器。本发明一般性地涉及通过估计来确定机器的工具的位置并基于这种确定来操作机器。惯性测量单元(imu)提供工具运动的指示，其被约束到第一平面或第二平面。基于从imu输出的数据确定工具的位置和轨迹。确定的位置和/或轨迹可以用于后续操作，例如工作或监测操作。

机器

图1示出了机器10的实施例，在这种情况下是反铲装载机，包括工具11。机器10可以是任何合适类型的机器，包括多用途机器，例如挖掘机、挖土机、装载机、推土机、铲子、伐木机、收割机、物料搬运工和其他此类工作机器。机器10可包括主单元13，主单元13具有用于操作者的操作者舱14和其中的动力单元，例如内燃机，用于向地面接合装置15(例如轨道或车轮)提供动力。机器10可包括第一臂装置16，工具11(在这种情况下为铲斗)可通过耦接装置17附接到第一臂装置16。机器10还可包括通过第二臂装置18附接到主单元13的另外的工具12。工具11和/或另外的工具12可以是任何其他合适的类型，例如叉子、锤子、犁、操纵臂、多处理器、粉碎机、锯、剪刀、鼓风机、研磨机、耕耘机、压实机、开沟机、绞车、螺旋钻、刀片、扫帚、刀具、刨床、伐木器、砍伐头、抓斗、捣碎器、开膛手、耙子等。工具11可包括安装在其上的监测系统，例如探地雷达。

臂装置16可以包括第一臂19，第一臂19可以是悬臂，围绕工具枢轴20枢转地附接到主单元13。主单元13和第一臂19之间的中间连接件21可以提供第一臂19和中间连接件21之间围绕中间枢轴22的枢转附接。工具枢轴20和中间枢轴22可以基本上彼此正交。单元枢转致动器29可以连接在第一臂19和主单元13之间，以使第一臂19围绕工具枢轴20枢转。在如图所示的优选实施例中，一对单元枢转致动器29用于使中间连接件21围绕工具枢轴20倾斜。每个单元枢转致动器29的第一端可以连接到主单元13，并且每个单元枢转致动器29的第二端可以偏离工具枢轴20(并且偏离形成在中间连接件21和工具枢轴20之间的底部铰链部分)连接，使得中间连接件21可以围绕单元枢转致动器29旋转并且实施枢轴20。第一液压致动器23可以连接在第一臂19和中间连接件21之间，使第一臂19围绕中间枢轴22枢转。臂装置16可包括第二臂24，第二臂24可以是杆，围绕杆枢轴25可枢转地附接到第一臂19上。第二液压致动器26可以连接在第一臂19和第二臂24之间，以使第二臂24相对于第一臂19围绕杆枢轴25枢转。第三液压致动器27可以连接在第二臂24和耦接装置17之间，以将工具11相对于第二臂24围绕耦接枢轴28枢转。此外，尽管未示出，工具11可以连接到耦接装置17，使得它可以相对于其旋转。例如，耦接装置17可包括倾斜旋转器等。

机器10可以包括机器控制系统，其可以包括通信地(经由有线或无线连接)连接到至少一个输入80、至少一个输出、至少一个传感器和至少一个机器通信模块的控制器81。控制器81可以是任何合适的已知类型，并且可以包括发动机控制单元(ecu)等。控制器81可以包括存储器和处理单元，存储器可以以数据的形式存储指令或算法，并且处理单元可以被配置为基于指令执行操作。存储器可以包括用于存储计算机程序指令的任何合适的计算机可访问或非暂时性存储介质，例如ram、sdram、ddrsdram、rdram、sram、rom、磁介质、光学介质等。处理单元可以包括能够执行存储器存储指令的任何合适的处理器，例如微处理器、单处理器、多处理器等。控制器81还可以包括图形处理单元，用于渲染对象以便在诸如显示器的输出上观看。

控制器81从至少一个输入80、至少一个传感器和/或机器通信模块接收数据，并基于指令执行操作，例如通过将数据发送到输出、操作至少一个传感器和/或机器通信模块、执行计算或实施基于逻辑的任务。控制器81可以被配置为操作第一臂装置16和/或第二臂装置18以运动工具和/或另外的工具11、12。

参考图2，至少一个传感器包括安装到工具11的惯性测量单元(imu)30。imu30被配置为感测并产生指示工具11的运动的传感器数据，特别是其线性加速度和角速度。imu30可以是至少六个自由度(即三个平移运动和三个旋转运动)imu，使得传感器数据包括指示工具11在三个维度上的加速度的加速度数据和指示工具11在三个维度上的角速度的角速度数据。imu30通常包括用于产生加速度数据的加速度计和用于产生角速度数据的陀螺仪。imu30可以连接到控制器81并将传感器数据提供给控制器81。加速度和角速度数据可以由imu30在固定的测量参考系中产生，该测量参考系的起点在imu30处。测量系在图1和图2中示出为主体系(xbody，ybody，zbody)。

机器10还可以包括监测系统(未示出)，该监测系统具有位于工具11上的至少一个监测设备。监测系统可以连接到机器控制系统和/或远程计算系统，例如通过网络以无线方式连接到远程计算系统。这样的监测系统可以使机器10能够实施监测模式以执行一个或多个监测操作以从至少一个监测设备产生监测数据。工具11可以在监测操作期间或在监测操作之间运动，并且监测系统可能需要在这种监测操作期间知道工具11的位置或运动。监测系统可以基于工具11的运动来处理至少监测的数据，例如用于进一步控制机器10。

监测系统可以包括雷达系统，优选地是地面穿透雷达系统，用于对地面绘制地图并检测地下特征。雷达系统可以包括至少一个位于工具11上的天线组件，用于在监测操作中周期性地扫描地面并产生监测数据。为了扫描大面积，工具11可以在地面上运动，同时在操作者的控制下或自动地执行多次扫描或监测操作。通过在扫描期间还知道工具11的位置和运动，监测系统可以产生指示地下特征的2d或3d地图。

运动约束

在正常操作模式中，机器10可操作以在三个自由度上平移工具11。例如，在图1和2中，工具11可以沿(xbody，ybody，zbody)中的任何一个平移。第一臂装置16可以由机器控制系统控制并且使得工具11能够相对于主单元13在三个自由度上运动(即，上、下、左右并且往返于主单元13))。在所示的示例中，工具11还能够通过相对于第二臂24和相对于工具枢轴20旋转而在两个自由度上旋转。然而，在其他实施例中，工具11可以能够以三个自由度旋转，例如通过包括倾斜旋转器的耦接装置17。或者，工具11可以仅在一个自由度上旋转，例如通过从第二臂24自由悬挂，使得它不能相对于主单元13旋转而不是通过臂装置16的旋转围绕工具枢转轴20旋转。因此，在正常操作模式中，工具11可以以至少四个自由度(即三个平移方向和至少一个旋转方向)运动。

在本发明中，机器10可以在约束操作模式下操作，其中工具11的运动被约束为小于正常操作模式中的自由度。受约束的操作模式可以例如在监测模式期间实施，或者在需要准确确定工具11的运动的任何工具操作模式期间实施。在受约束的操作模式中，工具11在接收到输入时，通过机器控制系统沿着轨迹从第一位置运动到第二位置。该输入可以来自操作者操作的操纵杆80，操作者已经输入第二位置的期望坐标的界面，基于由监测系统确定的用于提供合适的监测路径的自动控制策略等。

在本发明中，工具11可以在第一和/或第二位置处是静止的，因此其可以是工具11的轨迹的开始和/或结束位置。然而，相反，工具11可以沿着第一和第二位置处的轨迹运动。当工具11从起始位置运动到结束位置时，第一和第二位置可以是来自imu30的传感器数据的连续采样点处的工具11的位置。例如，控制器81可以基于传感器数据的采样和工具11在相对于工具11在先前采样点处的位置(即第一位置)的后续采样点处的位置(即第二位置)连续地估计工具11的轨迹。控制器81可以在工具11的起始位置和结束位置之间的每个采样点处多次重复该估计。结果，可以跟踪工具11的连续运动并估计工具11的结束位置。第一和第二位置可以是发生监测操作的位置，如下面进一步详细讨论的。

在约束操作模式中，工具11的运动被约束为第一平面中的第一运动或第二平面中的第二运动。工具11仅在第一运动或第二运动中运动。第一平面优选地是竖直平面，并且工具枢轴20在第一平面中(即，第一平面在图1中的(xbody，zbody)中)。第二平面优选地是水平平面并且垂直于工具枢轴20(即，第二平面是图2中的(xbody，ybody))。第二运动是围绕工具枢轴20的第二平面中的圆周运动40。运动半径l是从工具枢轴20到imu30的距离，并且在第二运动中是恒定的。应当理解，术语“水平平面”和“竖直平面”不必限于分别垂直于或沿着重力方向。机器10可以在斜坡上操作，使得水平平面可以被定义为平行于斜面的表面并且竖直平面可以被定义为垂直于水平平面。水平平面可以被定义为相对于重力方向处于与斜坡相对于重力方向倾斜度相同的倾斜度。

可以以多种方式应用工具11的运动对第一或第二运动的约束。机器控制系统可以被配置为在受约束的操作模式中自动地或者经由操作者经由输入80的请求来实施这样的约束。例如，操作者可以使用至少一个输入80(例如操纵杆)来控制仅在第一运动或第二运动中工具11的运动。机器控制系统还可以将来自操作者在至少一个输入80处的请求转换为仅仅作为第一运动或第二运动的工具11的运动。或者，工具11的运动可以由机器控制系统仅在第一或第二运动中自动控制。

可以相对于第一位置确定第二位置的位置。特别地，可以定义或固定参考系，并且在该参考系中确定轨迹和第一和第二位置。例如，如图1和图2所示，主体系可以被定义为测量系，并且参考系的原点可以被设置在主体系在第一位置的原点和方向。因此，有效地，参考系、主体系和测量系在第一位置处是相同的。或者，参考系的原点可以固定在机器10的一部分(例如主单元13)和通过测量或校准确定的参考系中的第一位置。因此，可以基于指示第一和第二位置之间的运动的传感器数据来确定相对于参考系中的第一位置的轨迹和第二位置。然而，当工具11在第一和第二位置之间旋转和/或平移时，测量系也相对于参考系与工具11一起旋转和/或平移。因此，传感器数据输出也可以被转换到参考系中以确定相对于参考系中的第一位置的轨迹和第二位置。

控制器操作

在图3中示出了根据本发明的操作机器10和/或机器控制系统的方法，其特别示出了确定或估计工具11的第二位置的方法。该方法可以作为指令存储在控制器内存。

在初始化步骤50中，控制器81可以加载包含来自其存储器的指令的程序并运行该程序。传感器数据51从imu30提供给控制器81。传感器数据51可以表示为传感器输出矩阵u(上标t表示根据标准惯例的矩阵的转置)：

u＝[a^tω^t]^t(1)

在时间点t测量系中的三轴加速度向量测量值a和三轴角速度向量测量值ω表示为：

a(t)＝[ax(t)ay(t)az(t)]^t(2)

ω(t)＝[ωx(t)ωy(t)ωz(t)]^t(3)

因此，a(t)是来自加速度计的加速度数据，ω(t)是来自陀螺仪的角速度数据，其通过校准在测量系中。

在初始数据处理步骤53中，控制器81接收、评估和校准传感器数据51。在校准期间，工具11静止在第一位置，并且控制器81基于存储在控制器81上的校准指令52校准传感器数据。校准指令52可以包括低通滤波器，并且可以首先使用低通滤波器对传感器数据的原始样本进行滤波，以从传感器数据中去除噪声等。校准指令52还可以包括，其可以在应用低通滤波器之后，在一段时间内估计传感器数据的平均值。

此外，在校准期间，可以确定加速度计数据中的重力加速度的分量，优选地在传感器数据输出51的滤波和调整到平均值之后。特别地，控制器81在工具11在第一位置是静止的同时确定测量系中的重力向量，其包括加速度计的输出。主体系中的加速度可以定义如下：

g是重力加速度。是在第一位置(即在t＝0)从参考系到测量系(这里也是主体系)的旋转矩阵。控制器81在初始数据处理步骤53期间计算使得在工具11运动时，由测量系中的重力加速度分量的大小定义的旋转矩阵在第一位置处是已知的。

在运动检测步骤54中，控制器81基于传感器数据确定工具11是否在运动。具体地，控制器81可以基于从传感器数据导出的值达到或超过阈值来确定正在发生运动。例如，传感器输出矩阵u的一个或多个分量可能超过阈值。阈值可以通过实验确定，并且可以足够高，使得不会超过由传感器噪声和/或机器10和/或工具11的振动产生的值。例如，该值可以包括加速度向量测量值a的估计能量，并且阈值是阈值能量，高于该阈值能量确定正在发生运动。在“用于移动电话用户的运动模式识别和步骤检测算法(motionmoderecognitionandstepdetectionalgorithmsformobilephoneusers)”，susi，renaudin和lachapelle，sensors，2013，13，1539-1562；doi：10.3390/s130201539中公开了一种合适的方法。阈值可以是传感器数据的值的平方和，其可以通过指定因子进行平方。

如果在运动检测步骤54中没有检测到运动，则控制器81可以返回到初始数据处理步骤53，可以继续基于校准指令52和传感器数据输出51校准传感器数据，并且可以实施运动检测步骤54直到检测到运动。

如果在运动检测步骤54中检测到运动，则控制器81可以实施模式确定步骤55，在模式确定步骤55中可以确定工具11是否正在经历第一或第二运动。该确定可以基于传感器数据。例如，如果a和/或ω落在第一值范围内，则控制器81可以确定正在发生第一运动，并且如果a和/或ω落在第二值范围内，则控制器81可以确定正在发生第二运动。第一和第二范围可以通过实验确定并存储在控制器81上。或者，控制器81可以基于从至少一个输入80接收的输入数据确定正在发生第一或第二运动。至少一个输入80可以是由操作者操作的开关，以指示它们在第一或第二运动中操作工具11。或者，至少一个输入80可以是操作者用来控制工具11的运动的输入，例如操纵杆，并且控制器81可以基于操作者对至少一个输入80的操作确定是否正在发生第一或第二运动(例如，通过确定操作者已指示机器10以第一或第二运动运动工具11)。

如果在模式确定步骤55中控制器81确定工具11正在经历第一或第二运动，则其分别实施第一估计模式56或第二估计模式57以基于传感器数据优选地在参考系中估计轨迹和第二位置。特别地，控制器81可以接收传感器数据58、59(其可以根据校准指令52校准)，处理传感器数据58、59并产生轨迹和位置数据60、61。位置数据60、61可以是对参考系中的工具11的第二位置的估计。轨迹数据可以通过参考系中的多个位置表示工具11的轨迹。

控制器81在第一估计模式56或第二估计模式57中应用第一和第二估计模型，以基于工具11是否正在经历第一或第二运动来确定轨迹和第二位置。在处理步骤期间，控制器81可以基于存储在控制器81上的估计指令来实施至少一个估计模型，优选地是状态估计算法。该至少一个状态估计算法可以是卡尔曼滤波器，优选地是扩展卡尔曼滤波器(ekf)。可以针对第一估计模式56或第二估计模式57中的每一个实施不同的估计模型或卡尔曼滤波器。通常，在卡尔曼滤波器中，可以基于基于运动模型的预测方程来更新包括工具11的轨迹和位置的状态向量，并且基于测量更新方程。

第一估计模式

在第一估计模式56中，可以基于传感器数据确定工具11的轨迹和第二位置。具体地，控制器81可以从imu30接收传感器数据，并基于第一估计模型和传感器数据确定轨迹和第二位置。

优选地，在第一估计模型中，参考系被设置为第一位置处的主体系。主体系是测量系。参考系相对于主单元13的位置可以通过直接测量或校准来确定。

在第一估计模型中，控制器81可以通过确定重力向量并基于重力向量从传感器数据中去除重力加速度来考虑从imu30接收的加速度数据中的重力。这可以基于在初始数据处理步骤53和等式(4)中确定的主体系和旋转矩阵中的重力加速度。因此，工具11在第一位置处的主体系中从第一位置到第二位置的运动的估计加速度可以由控制器81确定为：

ab(t)是当工具从第一位置运动到第二位置时由imu30产生的加速度数据。是旋转矩阵，它定义了工具11的主体系在第一和第二位置之间的旋转(即，第一位置处的参考系和第二位置的主体系之间的旋转)。当工具11从第一位置运动到第二位置时，控制器81可以基于角速度数据和该数据与从imu30接收的相关联加速度数据之间的相关性来计算该旋转矩阵。因此，估计的加速度可以转换到参考系中。

在第一估计模型中，通过对估计的运动加速度进行积分(两次)来估计工具11的第二位置。运动也仅限于第一平面，因此，第一估计模型的运动方程导致更准确得确定第二位置。

在第一估计模式56中，优选地通过实施扩展卡尔曼滤波器(ekf)作为第一估计模型的一部分来估计第二位置。以下是用于第一估计模式56的特别合适的ekf的描述。

第一估计模式：示例估计模型

在ekf中，在参考系中确定工具11的运动。控制器81存储状态向量，该状态向量表示在包括imu30的位置及其导数的参考系中由ekf在第一估计模式56中估计的状态。因此，状态向量分别包括工具11的位置pref，速度vref，加速度aref和角速度向量ωrsf。具体地，状态向量是：

因此，状态向量还包括旋转四元数q。旋转四元数是表示取向的已知计算方法。合适的描述可以在“使用向量观察的姿态确定(attitudedeterminationusingvectorobservations)”，markley，f.landis，flightmechanics(estimationtheorysymposium，1992；第537-551页)中找到。

控制器81还存储过程模型，该过程模型表示基于在第一估计模式56中引起这种转变的输入来控制形成状态随时间的转变的运动的规律的近似。在过程模型中，工具的运动仅被约束于第一平面。过程模型是以下一阶向量微分方程，用状态空间表示：

传感器输出矩阵u在等式(1)中定义。非线性向量函数f是：

f(x)＝[f1(x)f2(x)f3(x)f4(x)f5(x)]^t∈r¹⁶(8)

第一项是位置的导数，因此是参考系中的速度。然而，由于运动仅在第一平面中，所以沿yref轴线没有速度。因此，第一项被约束于：

f1(x)＝[vx0vz](9)

第二项是速度的导数，因此是参考系中的加速度。同样，由于运动仅在第一平面中，因此沿yref轴线没有加速度。因此，第二项被约束于：

f2(x)＝[ax0az](10)

第三项是参考系中加速度的导数。假设加速度在第一和第二位置之间是恒定的，则将其设置为零。特别是在第一和第二位置是连续样本的情况下，这通常以短的中间时间间隔发生：

f3(x)＝0(11)

第四项是参考系中角速度的导数。假设角加速度在第一和第二位置之间是恒定的，则将其设置为零。特别是在第一和第二位置是连续样本的情况下，这通常以短的间隔时间间隔发生：

f4(x)＝0(12)

第五项是四元数的导数，其中ωb是主体系中的角速度：

控制器81还存储过程模型雅可比矩阵f，其是状态向量的向量函数的泰勒级数展开中的第一系数。它是用于将ekf本地线性化为当前状态估计的机制：

为方便起见，使用以下已知项：

过程模型雅可比矩阵f中的每项如下：

可以对过程模型雅可比矩阵f进行离散化以获得离散时间转移矩阵φ，其是如下的一组一阶微分方程的解的标准形式，其中i是单位矩阵并且t是时间：

φ＝exp(f·dt)≈i+f·dt(21)

控制器81还在参考系中存储表示来自imu30的输出的测量向量，如下：

参考系aref中的加速度以与等式(5)类似的方式计算如下：

初始主体系中的重力加速度分量在第一位置的等式(4)中确定。ab是加速度计的输出，因此位于新的主体系中。是表示第二位置处的主体系与参考系之间的旋转的旋转矩阵。当工具11从第一位置运动到第二位置时，控制器81可以基于角速度数据和该数据与从imu30接收的相关联加速度数据之间的相关性来计算该旋转矩阵。因此，估计的加速度可以转换到参考系中。参考系中的角速度向量ωref从来自主体系中的陀螺仪的角速度输出ωb旋转，如下：

控制器81还存储如下测量模型，其中h是测量模型雅可比矩阵：

z＝h·x(25)

控制器81还存储可以表示状态向量x中的估计误差的误差协方差矩阵p，可以表示过程模型中的误差，在过程模型中被视为噪声的过程模型协方差矩阵q，和可以表示传感器数据中的噪声的观测协方差矩阵r。

为了实施ekf，控制器81开始初始化步骤。在初始化步骤期间，控制器81从其存储器初始值(即，在时间k＝0处)确定或检索状态向量误差协方差矩阵过程协方差矩阵和观测协方差矩阵例如，被设置为零，并且从控制器81存储器检索和的预设值。预设值可以通过机器10的实验来确定，并且被设置为增加ekf的收敛速度和准确度。

控制器81随后通过使用等式(7)和(21)实施以下预测等式来执行预测步骤以在时间k处确定状态向量估计和协方差估计

控制器81随后执行更新步骤以使用传感器数据输出58(即测量值)在时间k处更新状态向量估计和协方差估计因此，通过实施以下更新方程来更新时间k处的状态向量估计和协方差估计

其中，为方便起见：

控制器81可以多次实施预测和更新步骤，以便改进状态向量的估计。一旦预测和更新步骤完成，工具11在参考系中的估计的第二位置由第一项表示。

第二估计模式

在第二估计模式57中，可以基于传感器数据和假设在第一和第二位置之间的运动期间为恒定的运动半径l来估计工具11的轨迹和第二位置。优选地，在校准模式中估计运动半径l。在另一步骤中，基于估计的运动半径l、第二估计模型和传感器数据来估计第二位置。

在校准模式中，控制器81可以接收运动半径l的指示，该指示可以通过物理测量来确定并且由操作者通过输入提供给机器控制系统。然而，可以通过操作工具11来估计运动半径l，以将第二运动中的短距离从第一校准位置运动到第二校准位置。基于从校准运动导出的传感器数据估计运动半径l。例如，基于从imu30接收的加速度和角速度数据估计第一和第二校准位置和轨迹。然后确定第一和第二校准位置之间的距离。使用角速度数据确定第一和第二校准位置之间围绕枢轴的旋转角度。然后可以基于距离和旋转角度确定运动半径l。

在第二估计模式57中，控制器81基于第二估计模型仅在第二平面中的运动的约束，并且优选地仅在第二平面中的圆周运动40，更精确地确定工具11的运动。例如，在第二估计模型中，控制器81可以通过对从imu30接收的角速度速率进行积分来导出imu30在第二位置的取向。在第一和第二位置之间接收的加速度数据被积分两次以提供位置数据，使用根据角速度数据计算的取向进行校正。因此，在知道运动半径l恒定的情况下确定相对于第一位置和轨迹的第二位置。

具体地，可以通过将主体系定义为使其x轴平行于运动半径l对准来将imu30的位置向量约束在主体系中来对第二运动建模。因此，主体系中imu30的位置向量是：

pbody＝[l00](34)

工具11在参考系中的位置可以描述为：

是定义参考系和主体系之间的旋转的旋转矩阵。可以将参考系设置为第一位置处的主体系。结果，可以通过确定来估计参考系中的第二位置，这通过估计过程实施。当工具11从第一位置运动到第二位置时，控制器81可以基于角速度数据和该数据与从imu30接收的相关联加速度数据之间的相关性来计算。

主体系中铲斗的惯性速度向量可以如下确定：

是主体系中imu30的视在速度，由于运动半径l在第二运动中是恒定的，所以它是零。ωbody是主体系中铲斗的角速度向量，其可以根据角速度数据确定。

在第二估计模式57中，第二估计模型包括用于估计运动半径l和第二位置的ekf。具体地，控制器81可以最初使用ekf估计运动半径l，然后随后将估计的运动半径l用作另一个ekf中的固定值，以便确定工具11的第二位置。以下是描述用于第二估计模式56的特别合适的ekf。

第二估计模式：示例估计模型

控制器81将状态向量存储在参考系中，该参考系包括运动半径l，位置pinertial，速度vinertial和旋转四元数q，如下：

控制器81还存储过程模型作为以下一阶微分方程和非线性向量函数f：

f(x，u)＝[f1(x，u)f2(x，u)f3(x，u)f4(x，u)]^t(39)

第一项是参考系中位置的导数，因此是参考系中的速度：

第二项是参考系vinertial中速度的导数，因此是参考系中工具11的加速度。加速度可以从主体系中的加速度计测量值a导出，并使用旋转矩阵将其旋转到参考系中。此外，加速度计测量值a包括必须补偿的重力加速度g。因此，第二项设定如下：

第三项是四元数的导数：

第四项是运动半径l的导数。由于工具11仅在圆周运动40中运动，因此运动半径l不会改变，因此导数为零：

f4(x，u)＝0(43)

控制器81还如下存储过程模型雅可比矩阵f：

过程模型雅可比矩阵f中的每项如下：

控制器81还存储包括伪测量的测量向量z。伪测量在“在gps中断期间辅助ins的伪测量(pseudo-measurementsasaidingtoinsduringgpsoutages)，itzikklein，sagifilin，tomertoledo，navigation，第57卷，第1期，第25-34页”中进一步详细讨论。测量向量z设置为：

zp和zv基于等式(35)和(36)确定：

zp是位置上的平等约束。在等式(35)中，仅通过纯旋转将参考系中的位置转换到主体系中的位置。因此，在理想模型zp＝0中，但这不是估计的情况。因此，在估计第二位置时不会为零的zp的值可以被描述为“伪测量”。

在等式(36)中，仅具有垂直于运动半径l的分量，因为沿着运动半径l的速度分量由于恒定的运动半径l而为零。等于角速度和主体系中的位置的叉积。因此，在理想模型中，zv＝0，但在估计中不是这种情况。因此，在估计第二位置时不为零的zv的值可以被描述为“伪测量”。

控制器81还存储如下测量模型，其中h是测量模型雅可比矩阵：

随后如关于第一估计模式56和等式27至33所讨论的那样实施ekf。

工业适用性

机器10可以在工具操作模式下操作，其中例如由操作者或自动地控制机器10以在第一位置执行第一工具操作而在第二位置执行第二工具操作。第一和第二工具操作可以是工作，例如挖掘，并且可以实施约束操作模式以向操作者和/或控制器81提供反馈以确认工具11的位置。

工具操作模式可以是如图4所示的监测模式，在此期间可以实施约束操作模式以在监测模式期间估计工具11的轨迹和位置。如图4所示，在监测模式70中，监测系统可以在第一位置执行第一监测操作71以产生第一监测数据72，例如映射第一位置的工具11下方的地面的雷达数据。随后，控制器81和/或操作者可以仅在第一或第二运动中沿着轨迹将工具11运动73到第二位置，在此期间imu30产生传感器数据并且在控制器81处接收74传感器数据。在第二位置，监测系统可以执行第二监测操作75以产生第二监测数据76，例如映射第二位置的工具11下方的地面的雷达数据。当工具11沿其轨迹运动时，可以多次执行这种监测操作。

随后，控制器81可以处理77第一和第二监测数据和传感器数据，包括确定关于受约束操作模式所描述的轨迹和第二位置。结果，估计相对于第一位置(或任何预定参考系)的轨迹和第二位置。随后，控制器81基于轨迹和/或第二位置的估计将第一和第二监测数据72,76相关联以产生处理数据。可以将得到的处理数据显示78给操作者。例如，控制器81可以产生第一和第二位置下方的地面的基于雷达的地图。可以将结果地图显示给操作者。

如果工具11的运动不受约束，则仅使用来自imu30的传感器数据估计第二位置可能是不可行的，因为重力加速度会淹没传感器数据。特别地，在监测模式70中，工具11的运动加速度相对较小，因此难以与重力加速度分离。典型的现有技术方法是将来自imu30的传感器数据与来自另一传感器(例如gps等)的数据组合。或者，臂装置16的几何形状可以用角度编码器等测量。然而，在机器10中，添加另一传感器可能是不切实际的，例如太复杂或昂贵。此外，除了在工具11上之外向机器10添加另一传感器意味着如果机器10安装了正确的传感器，则工具11可用于估计轨迹和第二位置。

然而，通过将工具11的运动约束为仅第一和第二运动，可以基于仅来自imu30的传感器数据更精确地估计轨迹和第二位置。结果，不需要额外的传感器来估计第二位置，从而降低成本和复杂性。此外，工具11可以在机器10之间交换，而不管它们包括哪些附加传感器，并且仍然可以估计工具11的位置。如果工具11包括监测系统，则这是特别有益的，因为工具11具有监测系统和imu30可以应用于各种不同的机器10。