CN109115213B - 用于利用传感器融合来确定机器状态的系统和方法 - Google Patents

Abstract

公开了一种确定机器的实时状态的方法，包括：利用至少一个处理器从安装在机器的不同部件上的多个惯性测量单元(IMU)中的每一个接收信号的时间序列，该信号指示多个IMU中一个或多个安装在其上的机器的部件中每一个的运动测量值的加速度和角速率。针对机器的单独部件上IMU中的每一个，通过合并来自IMU的加速度测量值以及运动测量值的角速率，利用单独卡尔曼滤波模块融合从IMU接收的信号，以估计IMU安装在其上的机器的部件的输出联合角度。合并连续时间步中部件的输出联合角度的估计值和测量值，求解运动学方程以在连续时间步下确定机器部件的位置、速度和加速度中至少一个的实时值，以及将确定的实时值在机器部件的受控操作移动的实施中应用。

Description

用于利用传感器融合来确定机器状态的系统和方法

技术领域

本发明涉及用于确定机器状态的系统和方法，并且更具体地，涉及用于利用传感器融合来确定机器状态的系统和方法。

背景技术

机器，诸如举例而言，推土机、自动平地机、轮式装载机、轮式拖拉机铲运机，以及其他类型的重型设备被用来执行各种任务。对机器的有效控制需要精确和响应的传感器读数，以执行向机器控制或操作员近乎实时提供信息的计算。自主和半自主受控的机器通过依赖于从各种机器系统接收的信息，能够在很少或没有人类输入的情况下运行。例如，基于机器移动输入、地形输入和/或机器操作输入，机器可以被控制为远程地和/或自动地完成已编程的任务。通过在任务的执行期间从不同机器系统和传感器中的每一个接收适当的反馈，可以对机器操作做出连续的调节，其有助于确保完成任务中的精确性和安全性。然而，为此，由不同的机器系统和传感器所提供的信息应当是准确并可靠的。机器的位置、速度和所行进的距离以及机器的不同零件或部件的位置、移动和定向作为参数，其准确度对于机器及其操作的控制可能是重要的。

常规的机器通常利用导航或定位系统来确定各种工作参数，比如机器的位置、速度、俯仰率、偏航率以及滚转率。机器的位置和定向被称为机器的“姿态”。机器“状态”包括机器的姿态以及可以用于模拟机器运动特性和动态特性的各种附加的工作参数，比如表征机器的各种连接装置、接头、工具、液压装置以及电动力系统的参数。一些常规机器利用全球导航卫星系统(GNSS)数据、距离测量指示器(DMI)或里程计测量数据、惯性测量单元(IMU)数据等来确定这些参数。一些机器利用雷达(RADAR)传感器、声呐(SONAR)传感器、激光雷达(LIDAR)传感器、红外线(IR)或非红外线摄像机以及其他类似的传感器来帮助沿不同种类的地形安全并有效地引导机器。常规机器已试图融合这些不同类型的数据来确定陆基载具的位置。

2008年2月7日公布的Levinson等人的美国专利申请公布文本第2008/0033645号('645公布文本)中公开了一种可以用来确定机器的位置的示范性系统。'645公布文本的系统利用来自诸如全球定位系统(GPS)的传感器的定位数据以及来自LIDAR(光检测和测距)装置的现场数据来确定机器的定位或位置。具体地，数据被用来创建地形的高分辨率地图并且关于该地图定位机器的位置。

尽管'645公布文本的系统在确定机器的位置方面可能是有用的，但是该系统可能无法在航位推算的同时(即，当GPS信号不可用的时间段期间)针对机器的位置提供准确的估计值。此外，当GPS信号由于多路径误差、位置跳动等而不可靠或错误时，'645公布文本的系统可能无法提供准确的位置信息，这是因为'645公布文本的系统并未校验GPS信号的准确度。

根据本发明的用于利用传感器融合来确定机器状态的系统和方法旨在解决以上所阐述问题和/或现有技术的其他问题中的一个或多个。

发明内容

在一个方面，本发明涉及一种确定机器的实时状态的方法。该方法包括：利用至少一个处理器从安装在机器的不同部件上的多个惯性测量单元(IMU)中的每一个接收信号的时间序列，该信号指示多个IMU中一个或多个安装在其上的机器的部件中每一个的运动测量值的加速度和角速率。该方法还包括利用至少一个处理器的单独卡尔曼滤波器模块融合从机器的单独部件上的IMU中每一个接收的信号。对来自多个IMU中每一个的信号的融合包括：合并来自IMU的加速度测量值以及运动测量值的角速率，以在一系列时间步的某时间步期间估计IMU安装在其上的机器的部件的输出联合角度；基于来自先前时间步的输出联合角度的估计，在后续时间步中生成对该部件的输出联合角度的先验估计；以及将输出联合角度的先验估计与先验估计的准确度的估计和从IMU接收的当前测量值合并以产生输出联合角度的精确后验估计。该方法进一步包括：将机器的多个部件的输出联合角度的精确后验估计彼此相互并参考机器参考框架进行融合，以参考机器参考框架确定机器的多个部件中每一个的当前输出联合角度的最佳估计；利用机器的部件的当前输出联合角度的最佳估计以及表征机器的结构设计信息来求解运动学方程；以及在一系列时间步的连续时间步下，通过该运动学方程的解确定感兴趣的每个机器部件的位置、速度和加速度中至少一个的实时值。该方法又进一步包括在每个感兴趣机器部件的受控操作移动的实施中应用该确定的实时值。

在另一个方面，本发明涉及一种用于确定机器的实时状态的系统。该系统包括：多个惯性测量单元(IMU)，其安装在机器的单独部件上，IMU各自配置为生成信号的时间序列，该信号指示IMU安装在其上的机器的部件的运动测量值的加速度和角速率；以及与IMU中每一个进行通信的至少一个处理器。至少一个处理器中的每一个被配置为利用至少一个处理器的单独卡尔曼滤波器模块融合从IMU接收的信号。对来自多个IMU中每一个的信号的融合包括：合并来自IMU的加速度测量值以及运动测量值的角速率，以在一系列时间步的某时间步期间估计IMU安装在其上的机器的部件的输出联合角度；基于来自先前时间步的输出联合角度的估计，在后续时间步中生成对该部件的输出联合角度的先验估计；以及将输出联合角度的先验估计与先验估计的准确度的估计和从IMU接收的当前测量值合并以产生输出联合角度的精确后验估计。系统的至少一个处理器还被配置为：将机器的多个部件的输出联合角度的精确后验估计彼此相互并参考机器参考框架进行融合，以参考机器参考框架确定机器的多个部件中每一个的当前输出联合角度的最佳估计；利用机器的部件的当前输出联合角度的最佳估计以及表征机器的结构设计信息来求解运动学方程；以及在一系列时间步的连续时间步下，通过该运动学方程的解确定感兴趣的每个机器部件的位置、速度和加速度中至少一个的实时值。系统的至少一个处理器还被配置为在每个感兴趣机器部件的受控操作移动的实施中应用该确定的实时值。

在又另一个方面，本发明涉及一种非瞬态性计算机可读存储装置，其存储有用于使处理器能够执行确定机器实时状态的方法的指令。该方法可以包括：从安装在机器的不同部件上的多个惯性测量单元(IMU)中的每一个接收信号的时间序列，该信号指示多个IMU单元中一个或多个安装在其上的机器的部件中每一个的运动测量值的加速度和角速率。该方法还包括利用处理器的单独卡尔曼滤波模块融合从机器的单独部件上的IMU中每一个所接收的信号。对来自多个IMU中每一个的信号的融合包括：合并来自IMU的加速度测量值以及运动测量值的角速率，以在一系列时间步的某时间步期间估计IMU安装在其上的机器的部件的输出联合角度；基于来自先前时间步的输出联合角度的估计，在后续时间步中生成对该部件的输出联合角度的先验估计；以及将输出联合角度的先验估计与先验估计的准确度的估计和从IMU接收的当前测量值合并以产生输出联合角度的精确后验估计。该方法进一步包括：将机器的多个部件的输出联合角度的精确后验估计彼此相互并参考机器参考框架进行融合，以参考机器参考框架确定机器的多个部件中每一个的当前输出联合角度的最佳估计；利用机器的部件的当前输出联合角度的最佳估计以及表征机器的结构设计信息来求解运动学方程；以及在一系列时间步的连续时间步下，通过该运动学方程的解确定感兴趣的每个机器部件的位置、速度和加速度中至少一个的实时值。该方法还进一步包括在每个感兴趣机器部件的受控操作移动的实施中应用该确定的实时值。

附图说明

图1是示范性公开的机器的直观说明，其可以利用根据本发明的示范性实施例的用于确定机器的实时状态的系统和方法来操作；

图2是图1的用于确定机器状态的示范性公开的传感器融合系统的图解说明；

图3是对来自图2的传感器融合系统的输出的示范性应用的图解说明，用于提供在控制图1的示范性公开的机器的操作和姿态中所使用的实时信息；

图4是对来自图2的传感器融合系统的输出的示范性应用的图解说明，用于在图1的示范性公开的机器的选定操作期间提供升高的液压压力输出；以及

图5是图2的传感器融合系统的示范性实施方式的图解说明。

具体实施方式

图1示出了包括多个惯性测量单元(IMU)24、25、26和27的机器10，它们应用在机器10的部件或部分上的不同位置处，比如在机器机身14、吊杆17、斗杆18以及铲斗(或其他工具)19上。在根据本发明的各种示范性实施例的机器状态控制系统中，安装在机器10的不同部件和/或部分上的IMU可以替代或补充常规传感器，比如俯仰和滚转传感器32、缸位置传感器34以及旋转位置传感器36。每个IMU包括一个或多个加速度计、一个或多个陀螺仪、以及在一些情况下包括磁力计，用于提供指示相对于地球磁极的方向的信号。每个IMU的加速度计和陀螺仪提供信号，该信号可以用于识别IMU相对于参考车身框架并由此相对于IMU所附接的机器部件的位置和定向。IMU可以为诸如位置感测缸和旋转位置传感器的传统传感器提供更低成本且更可靠的替代方案，并且通过仅将IMU焊接在机器的不同外部部分上而无需拆卸机器或机器的部件，还可以更易于对现有机器进行改装。已配置为向机器的各个系统和子系统发送控制信号的机电控制模块(ECM)可以被重新编程并配置为从改装的IMU接收信号，同时该信号被处理并转换成实时输入，由ECM用来基于该输入修改控制信号。

根据特定的机器应用和配置，可以添加或拆除各种非IMU传感器230(参见图2)。非IMU传感器可以包括被包含作为视觉系统一部分的各种感知传感器、诸如上部结构位置/速度传感器22的位置和/或速度传感器、配置为提供指示通过激光测量的位置的信号的激光捕捉传感器28、缸位置传感器34、液压系统传感器、电气系统传感器、制动系统传感器、燃料系统传感器以及向ECM提供实时输入以用于在监测机器的系统和子系统的状态并控制其操作中使用的其他传感器。

机器10可以配置为执行与诸如采矿、建筑、农业、交通运输、发电的行业或者本领域已知的任何其他行业相关联的一些类型的操作。例如，机器10可以是运土机，比如拖运卡车、推土机、装载机、铲斗机、挖掘机、自动平地机、轮式拖拉机铲运机或者任何其他运土机。机器10通常可以包括履带组件或者安装在车身上(在履带组件之间)的其他牵引装置12(即，地面接合装置)，其支承旋转框架，形成上部结构的机器机身14安装在该旋转框架上。旋转框架和机器机身14可以支承操作员站或舱、安装在舱内的集成显示器15、操作员控制装置(比如安装在舱内的集成操纵杆)以及驱动牵引装置12以推进机器10的一个或多个发动机和传动系。吊杆17可以枢转地安装在机器机身14的近端处并且通过一个或多个流体致动缸(例如，液压缸或气压缸)、电动马达或其他机电部件相对于机器机身14铰接。斗杆18可以枢转地安装在吊杆17的远端处并且通过一个或多个流体致动缸、电动马达或其他机电部件相对于吊杆铰接。工具19(比如铲斗)可以安装在斗杆18的远端处，其任选地通过一个或多个流体致动缸、电动马达或其他机电部件相对于斗杆18铰接，并且提供有地面接合工具或用于执行各种任务的其他附件。

图2是根据本发明的传感器融合系统的示范性实施例的框图。传感器融合系统可以配置为向机器状态控制系统50提供准确、实时的输入，该机器状态控制系统配置为用于控制机器10的各种操作方面。机器状态控制系统50可以与机器ECM相关联或者可以是其整体部分。传感器融合系统可以配置接收来自多个IMU 210和附加非IMU传感器230的信号、以及指示各种操作员命令的信号，比如通过操作员移动操纵杆或者其他输入装置或操作员控制装置16产生的信号。“传感器融合”指的是将传感数据或源自不同来源的数据合并，从而使得所得到的信息具有比单独使用该来源可能具有的更低的不确定度。传感器融合系统还可以配置为接收关于特定机器的尺寸设计的信息，传感器系统通过尺寸设计信息数据库250与其相关联。针对特定机器从设计信息数据库250接收的特定尺寸设计信息可以被与传感器融合系统相关联的处理器241使用，并配置为通过结合运动特性库模块260和/或通过利用基于物理特性的方程或算法对运动特性和动态特性的经验推导来推导机器10的运动特性和动态特性。各种传感器和处理器可以通过任何合适的架构来彼此相连接，包括有线和/或无线网络的任何组合。此外，此类网络可以集成到任何本地局域网(LAN)、广域网(WAN)和/或因特网。

IMU 210可以按照多种不同的位置和定向应用于机器，包括在机器机身14、吊杆17、斗杆18以及作业工具(例如，铲斗)19的不同部分上。IMU可以沿机器的各个部分以多个位置和定向改装，并且可以根据特定的机器应用和配置添加和拆除。从每个IMU接收的原始数据可以通过卡尔曼滤波器进行处理，如以下将详细进行描述的。在一些实施方式中，用于每个IMU传感器的卡尔曼滤波器可以包含为IMU的一部分，而在其他实施方式中，卡尔曼滤波器可以是提供作为单独传感器融合系统一部分的单独传感器融合模块的一部分。

每个IMU的陀螺仪通过角速度变化感测定向，而每个IMU的加速度计关于重力感测方向的变化。陀螺仪测量值有随时间推移而漂移的趋势，这是因为它们仅感测变化并且没有固定的参考框架。增加加速度计数据能够尽可能减小并更佳估计陀螺仪数据中的偏差以减小传播误差并改善定向读数。当系统更接近于固定参考点时，加速度计可以提供在静态计算中更准确的数据，而当系统已经处于运动时陀螺仪更擅于检测定向。从与机器的不同部分和/或部件中每一个相关联的IMU的加速度计和陀螺仪所接收的指示运动的线性加速度和角速率的信号，可以由卡尔曼滤波器合并以更准确地预测机器的单独部件的输出角度、速度和加速度。

与安装在单独机器部件上的每个IMU相关联的卡尔曼滤波器采用测量值，并通过改变平均因子以相比于分配给实际测量值的权重优化分配给估计值或预测值的权重来找到将来值的估计，由此在真实值的最佳估计上收敛以用于输出机器的每个部件的联合角度、速度和加速度。平均因子通过所预测的不确定性的量度(有时被称为协方差)加权，以挑选在预测值和测量值之间某处的值。卡尔曼滤波器通过利用递归和迭代过程中反馈控制的形式估计机器状态，每次迭代包括时间更新或“预测”阶段以及测量或“校正阶段”。在由卡尔曼滤波器执行每次迭代期间，通过比较测量值的估计中的误差和该值的实际测量中的误差来确定“增益”或加权。卡尔曼增益等于估计中的误差与估计中的误差和实际测量中的误差的和之间的比值。随后通过先前的估计和新测量值来计算该值的当前估计。随后确定并反馈该值的估计中的新误差，以用于确定将在下次迭代中应用的增益。通过卡尔曼滤波器提供的合并或融合信息可以提供关于俯仰率、偏航率、滚转率、吊杆角度、斗杆角度以及取决于连接配置和安装在机器的不同部分或部件上的IMU数量的其他角度的准确和实时的信息。

如图2的示范性实施例中所示，根据本发明的传感器融合系统的卡尔曼滤波器可以配置为估计由IMU所提供的陀螺仪信息(比如每个部件的俯仰率、偏航率和滚转率)的偏差。由于每个部件上点的线性位置和角位置是通过对来自IMU的运动线性加速度和角速率进行两次积分计算得到的，因为测量值中的较小误差被积分放大，所计算的信息可能随时间推移而漂移，越来越偏离实际位置。因此，卡尔曼滤波器的陀螺仪偏差方面增大了通过由IMU提供的信息计算的联合角度的准确度。机器的单独部分或部件中每一个的输出联合角度可以在机器级别上彼此相互融合，以便在两个以上部件相对于彼此按照基本上固定的定向保持时对该两个以上部件相对于机器的移动进行解释。例如，图1中所示的机器10的吊杆17上的IMU传感器25和斗杆18上的IMU传感器26两者可以在吊杆17向上移动时指示输出联合角度相对于全局参考框架的变化，尽管吊杆17和斗杆18之间的实际角度可能并未改变。在机器级别上将吊杆17和斗杆18的每一个的输出联合角度进行融合可以提供这种信息，从而使得可以实时确定单独机器部件上的不同点相对于机器参考框架和全局参考框架的实际位置。

如在图2的示范性实施例中进一步示出的，已经在机器级别上由卡尔曼滤波器240融合的输出联合角度可以由运动特性库模块260接收。运动特性库模块260可以配置接收来自卡尔曼滤波器240的输出联合角度以及来自尺寸设计信息数据库250的特定于机器10的尺寸设计信息，并且求解机器上每个部件或感兴趣点处的框架旋转和位置。框架可以具有偏移，偏移被应用于源自IMU的信息以便求解机器上的任何特定点，并且所有已更新的位置信息可以提供给机器状态控制系统50，其可以与机器ECM相关联或者编程为它的一部分。

在挖掘机或者其他机器的情况下，其中IMU可以安装在操作期间通过弧线旋转或摆动的机器的部分上，与安装在机器的那些部分上的IMU中每一个相关联的三维位置信息可以反馈给摆动补偿模块220。摆动补偿模块220可以配置为通过补偿向心加速度，校正由安装在机器的旋转或摆动部分上的IMU所提供的加速度信息。在将该信息提供给卡尔曼滤波器240之前可以对从IMU 210接收的加速度信息执行这种校正。

附加的非IMU传感器230可以包括能够生成指示与机器10的性能相关的参数值或机器参数的信号的任何装置。例如，非IMU传感器230可以包括被配置为产生指示吊杆和/或斗杆摆动速度、吊杆和/或斗杆在全局和机器参考框架中的位置以及作业工具角度的信号的传感器。还可以包括有效载荷传感器并将其配置为提供指示机器10的有效载荷的信号。还可以包括滑移传感器并将其配置为提供指示机器100的滑移的信号。附加的非IMU传感器可以包括能够提供指示以下的信号的装置：机器10正在其上工作的地面的坡度、外面温度、轮胎压力(如果牵引装置12是轮子的话)、在各个流体致动控制装置中的液压或气压压力、电压、电流，和/或正供应给电子控制装置的功率等。

非IMU传感器230可以包括一个或多个定位装置，其能够提供指示机器的定位和/或机器的各个部件相对于全局或局部参考框架的位置的信号。例如，定位装置可以采用全球卫星系统装置(例如，GPS或GNSS装置)，其接收或确定与机器10相关的位置信息，并且可以提供机器位置的独立测量值。定位装置和任何其他非IMU传感器230可以配置为向一个或多个接口装置传送指示该接收或确定的位置信息或者和各种机器操作参数有关的其他信息的信号，比如操作员舱中的集成显示器15以用于显示实时的机器运行特征。来自IMU 210和非IMU传感器230的信号可以引导到控制器，该控制器配置为包括卡尔曼滤波器240，并且卡尔曼滤波器240可以配置为通过与存储装置243和存储器245相关联的一个或多个处理器241实现。控制器的一个或多个处理器241可以配置为实现卡尔曼滤波处理，其包括在传感器融合模块242中执行的传感器融合。卡尔曼滤波器240还可以配置为在陀螺仪偏差估计模块244中执行陀螺仪偏差估计，以便补偿由与IMU相关联的一个或多个陀螺仪提供的读数中随时间推移出现的任何漂移。在一些示范性实施例中，定位装置可以接收GPS信号作为指示机器10的定位的定位信号并将接收到的定位信号提供给处理器241以用于进一步处理。另外，定位装置还可以提供与定位信号相关联的不确定度测量。然而，本领域普通技术人员可以理解的是，如有需要，可以修改所公开的示范性实施例以利用对机器10的定位的其他指示器。

非IMU传感器230还可以包括一个或多个感知传感器，其可以包括能够描述机器10附近环境的现场数据的任何装置。感知传感器可以体现为对位于机器10周围360度的目标进行检测和测距。例如，感知传感器可以通过LIDAR装置、RADAR(无线电检测和测距)装置、SONAR(声音导航和测距)装置、摄像机装置或者本领域已知的另一装置来体现。在一个实例中，感知传感器可以包括发射检测波束的发射器以及接收该检测波束的反射的相关接收机。基于反射波束的特征，可以确定从机器10上感测传感器的实际感测定位到所感测物体的一部分的距离和方向。通过在多个方向上利用波束，感知传感器可以生成机器10的周围环境的图片。例如，如果感知传感器通过LIDAR装置或利用多激光波束的另一装置来体现，该感知传感器(比如安装在机器的斗杆18上的激光捕捉器传感器28)可以生成一大片点作为描述机器10附近环境的现场数据。应当注意的是，在一些实施例中，现场数据可以被限制于机器10的前侧(180度或更小)。在其他实施例中，感知传感器可以针对位于机器10周围360度的目标生成现场数据。

IMU 210可以包括提供机器10或者更具体地提供IMU所安装在其上的机器的部件或部分(比如机器机身14、吊杆17、斗杆18，以及铲斗或其他工具19)的角速率和加速度。例如，IMU 210可以包括6自由度(6DOF)IMU。6DOF IMU由3轴加速度计、3轴角速率陀螺仪以及有时2轴倾斜仪组成。IMU 210的每一个可以通过将IMU焊接到机器的一部分或部件上来改装到现有机器上，其中需要机器的该特定部分或部件的实时位置、定向和运动的精确信息。机器的电子控制模块(ECM)或者其他机器控制器可以编程为从IMU接收信号并至少部分基于从IMU接收的输入实现各种机器控制。在本发明的一些示范性实施方式中，响应于从IMU接收的信号而又ECM实现的控制可以包括致动一个或多个电子或电动液压螺线管，其配置为控制一个或多个阀的打开和闭合从而调节对一个或多个流体致动缸的加压液压或气压流体的供应。与IMU相关联的3轴角速率陀螺仪可以配置为提供指示机器100或者IMU传感器安装在其上的机器的特定部分的俯仰率、偏航率和滚转率。3轴加速度计可以配置为在x、y和z方向上提供指示机器100或者IMU传感器安装在其上的机器的部分的线性加速度。

卡尔曼滤波器模块240可以与处理器241、存储装置243以及存储器245相关联，它们一起包含在单个装置中和/或单独地提供。处理器241可以包括一个或多个已知的处理装置，比如来自Intel^TM所制造的Pentium^TM或Xeon^TM系列、AMD^TM所制造的Turion^TM系列的微处理器、太阳微系统公司(Sun Microsystems)所制造的各种处理器中任一种或者任何其他类型的处理器。存储器245可以包括一个或多个存储装置，其配置为存储由卡尔曼滤波器240用来执行和所公开实施例有关的特定功能的信息。存储装置243可以包括易失性或非易失性、磁、半导体、磁带、光、可移除、不可移除、或者其他类型的存储装置或计算机可读介质或装置。存储装置243可以存储程序和/或其他信息，比如和处理从一个或多个传感器接收的数据有关的信息，如以下更详细进行讨论的。

在一个实施例中，存储器245可以包括从存储装置243或其他地方加载的一个或多个位置估计程序或子程序，当由处理器241执行时，执行符合所公开实施例的各种过程、操作或处理。例如，存储器245可以包括一个或多个程序，该一个或多个程序使卡尔曼滤波器240能够除了其他方面之外，采集来自里程计、定位装置、感知传感器、IMU210中的任何一个或多个以及非IMU传感器230中的任何一个或多个的数据，并且根据所公开的实施例(比如关于图5所讨论的那些实施例)处理该数据，以及基于处理的数据估计机器10和以及机器的各个部分和部件的位置。

在某些示范性实施例中，位置估计程序可以使处理器241的卡尔曼滤波器240能够处理所接收的信号，以估计机器10的不同部分或部件的实时位置和定向。卡尔曼滤波器实现了可以用于确定随时间观测到的测量值(比如以时间序列获取的测量值)的准确值。卡尔曼滤波器的一般操作涉及两个阶段，传播或“预测”阶段以及测量或“更新”阶段。在预测阶段，来自时间序列中的先前时间步的值估计被用来生产先验值估计。在更新阶段，在预测阶段计算的先验估计与先验估计的准确度的估计(例如，不确定性的方差)和当前测量值合并以产生精确后验估计。卡尔曼滤波器是一种多输入、多输出数字滤波器，其可以基于其噪声输出最优地实时估计系统的状态。这些状态是将系统行为完整描述为时间的函数(比如，位置、速度、电压电平等等)所需的全部变量。多噪声输出可以被认为是多维信号正噪声，系统状态是指示每个变量的真实值的期望未知信号。卡尔曼滤波器240可以配置为过滤噪声测量值，比如作为信号从安装在机器10的不同部分和部件上的多个IMU接收的测量值，以估计期望信号。由卡尔曼滤波器通过IMU和非IMU传感器所提供的信号推算的估计值就它们最大程度减小信号的均方差估计的意义而言是统计学最优的。针对噪声测量值的装填不确定度估计可以被确定作为协方差矩阵，其中协方差矩阵的每个对角项是标量随机变量的方差或不确定度。增益调度模块222可以配置为计算当将每个连续预测装置估计与连续实际测量值进行合并以获得更新的“最佳”估计时要使用的权重(或增益)。随着卡尔曼滤波器240随时间推移从IMU 210和非IMU传感器230接收多个测量值，卡尔曼滤波器的递归算法在时间上顺序地处理多个测量值中的每一个，对于每个新的测量值迭代地重复其自身，并且仅使用从先前循环存储的值(由此节省了存储器并减少了计算时间)。

在一个示范性实施例中，存储器245可以包括从存储装置243或其他地方加载的一个或多个姿态估计程序或子程序，当由处理器241执行时，执行符合所公开实施例的各种过程、操作或处理。例如，存储器245可以包括一个或多个程序，该一个或多个程序使卡尔曼滤波器240能够除了其他方面之外，采集来自以上所提到的单元的数据并且根据所公开的实施例(比如关于图5所讨论的那些实施例)处理该数据，以及基于处理的数据估计机器10的状态。

在某些实施例中，存储器245可以存储实现指令的程序，其将卡尔曼滤波器240(更具体地，处理器241)配置为实施利用卡尔曼滤波器来估计机器10的状态的方法。在某些示范性实施例中，卡尔曼滤波器可以配置为在其计算中利用以下方程。对于传播或“预测”阶段，卡尔曼滤波器可以配置为利用以下一般方程：

对于测量或“更新”阶段，卡尔曼滤波器可以配置为利用以下一般方程：

在以上方程中，可以是某些状态变量(例如，俯仰率、偏航率、滚转率、位置、速度等)的先验状态估计，其可以基于来自前一个时间步的状态变量的值/>来计算。F、G和H可以是适当的状态转移矩阵。在测量或“更新”阶段，卡尔曼滤波器240可以利用方程(3)计算卡尔曼增益K_k，其中P是误差协方差矩阵，R是阐明与不同状态变量相关的方差的矩阵。例如，R矩阵中的值可以指定与给定状态变量的测量值相关联的不确定度。在测量阶段，卡尔曼滤波器240还可以获得对状态变量的独立测量并将该独立测量设定为y_k。利用来自“预测”极端的先验估计/>测量值y_k及卡尔曼增益K_k(通过增益调度模块222施加的)，卡尔曼滤波器240可以利用方程(4)计算后验状态估计/>

图5示出了卡尔曼滤波器500的示范性配置。在卡尔曼滤波器500的预测阶段501中，卡尔曼滤波器500可以利用来自一个或多个IMU(比如来自IMU传感器中加速度计的线性加速度和来自IMU传感器中陀螺仪520的运动值的角速率)以及牵引装置速度传感器530的一个或多个输入来计算特定状态变量(例如，俯仰率、偏航率、滚转率、位置速度等)的先验状态估计。在预测阶段501中，卡尔曼滤波器500可以执行方程(1)和(2)。例如，在预测阶段，卡尔曼滤波器500可以利用来自上一时间步的状态变量的值以及来自一个或多个IMU 210和/或迁移装置速度传感器530的输入来计算一个或多个状态变量的/>(后验状态估计)。在一些实施方式中，/>可以从更新阶段502获得作为之前时间步的输出值。

在预测阶段501之后，卡尔曼滤波器500可以实施更新阶段502以利用例如方程(4)计算后验状态估计例如，卡尔曼滤波器500可以利用来自预测阶段501的先验估计/>测量值y_k以及卡尔曼增益K_k来计算后验状态估计/>如图5中所示，在更新阶段502，滤波器500还可以接收来自IMU的倾斜仪522的加速度值和来自一个或多个定位装置(比如用于机器的卫星可视的GPS装置540)的定位信号作为输入。

卡尔曼滤波器可以将从定位装置和IMU的倾斜仪接收的输入设定为方程(4)中的测量值y_k。另外，在其中机器10是挖掘机或者包括在操作期间通过弧线旋转或摆动的部分或部件的其他机器的示范性实施例中，卡尔曼滤波器可以从位于机器的旋转或摆动部分(例如，吊杆17和斗杆18)上的IMU接收加速度值，该加速度值已经在摆动消除模块220中进行了预处理以补偿在摆动运动期间出现的向心加速度。如图2的示范性实施例中所示，针对来自IMU 210的值的向心加速度补偿可以在将加速度数据在方程(4)中使用之前对来自IMU210的原始加速度数据执行。

而且，如上所述，图5的示范性实施例中的卡尔曼滤波器500可以配置为在更新阶段执行方程(3)和(5)。利用以上方程，卡尔曼滤波器可以配置为生成后验状态估计作为输出503。非限制地，后验状态估计/>可以包括机器的状态，并且可以包括诸如速度、位置、加速度、定向等的参数。

卡尔曼滤波对于合并来自若干不同的间接和噪声测量值以尝试估计不可直接测量的变量而言示非常有用的。例如，IMU的陀螺仪通过对角速率进行积分测量定向，因此来自陀螺仪的输出信号可能随时间推移而漂移。IMU中的倾斜仪和方向朝向特征(罗盘)可以为定向提供不同的噪声但是无漂移的测量值。在图2的示范性实施例中，卡尔曼滤波器可以配置为利用从增益调度模块22检索得到的权重适当地对两种来源的信息进行加权以最佳利用来自每个信息来源的所有数据。

在利用卡尔曼滤波器240确定机器10的状态中，滤波器还可以配置为考虑机器10的其他操作参数。例如，如果机器10是挖掘机，则卡尔曼滤波器可以配置为考虑机器10是否正在挖掘、倾倒、在挖掘和倾倒位置之间摆动、行驶到新的位置等。当机器10处于以上操作状态中的一种或多种时，可以改变卡尔曼滤波器的某些参数来在某些输入参数中反映准确度或置信度。例如，当机器10正在从一个位置行驶到另一个时，卡尔曼滤波器可以配置为向来自IMU倾斜仪的输入应用来自增益调度模块222的较低的权重(增益)。为了降低应用于来自IMU的倾斜仪输入的权重，卡尔曼滤波器可以配置为增大方程(3)中和倾斜仪相关联的方差“R”的值。类似地，当机器10正在挖掘时，卡尔曼滤波器可以配置为增大应用于倾斜仪输入的权重以在倾斜仪输入的准确度方面反映更高的置信度。例如，为了在准确度方面指示更高的置信度，卡尔曼滤波器可以配置为向来自IMU倾斜仪的输入应用来自增益调度模块22的更高的权重(增益)并减小与倾斜仪输入相关联的“R”的值。

图3和图4示出了根据本发明的利用来自具有卡尔曼滤波器的传感器融合系统的输出的机器状态控制系统的示范性实施方式。图3和图4的详细描述在接下来的章节中提供。

工业实用性

本发明所公开的机器状态控制系统50可以应用于可以受益于对将系统行为完全描述为时间的函数(比如每个机器部件运动的位置、速度、线性加速度和角速率)所需要的变量的准确实时监测的任何机器或机器系统。本发明所公开的与改装地附接至机器的不同部分或部件的多个IMU和非IMU传感器相结合的传感器融合系统，通过利用与安装在多个机器部件中每一个上的每个IMU相关联的卡尔曼滤波器，可以提供对所有不同机器部件的位置和定向的改进的估计。

在所公开传感器融合系统的一些示范性实施方式中，卡尔曼滤波器240可以利用机器参数、里程计参数、以及从安装在机器的各个部分和部件上的IMU接收的IMU输入来传播或“预测”机器状态。例如，卡尔曼滤波器240可以预测以下状态：每个机器部件相对于全局和机器参考框架以及机器自身相对于全局参考框架的位置、前向速度、角速度、联合角度和角度定向(姿势)。除了从每个IMU接收的指示运动的加速度和角速率的信号之外，与安装在不同机器部件上的每个IMU相关联的每个卡尔曼滤波器可以从各种不同的非IMU传感器(比如，里程计、接近度传感器，以及其他感知传感器)接收信号，该信号指示操作期间机器的部件移动的距离或者机器的部件与潜在障碍物之间的距离。通过将接收到的比例因子乘以牵引装置的旋转数目，卡尔曼滤波器还可以计算机器10本身所行进的距离。通过将来自安装在机器的特定部分或部件上的IMU传感器的指示线性加速度的信号进行积分，卡尔曼滤波器还可以计算机器10的速度或者机器的部分或部件的速度。卡尔曼滤波器可以利用来自IMU的信号来计算一个或多个IMU安装在其上的机器或者的机器的部分或部件的速度，并对得到的速度加权以生成预测速度。在一些实施方式中，可以调节机器本身所行进的距离以解释机器的滑移。每个卡尔曼滤波器还可以从一个或多个IMU接收指示机器的或者机器的部分的运动角速率(滚转率、偏航率和俯仰率)的信号。通过对运动的角速率进行积分，卡尔曼滤波器240可以确定每个机器部件或者机器本身的姿势或角度定向(滚转、朝向和俯仰)。

卡尔曼滤波器可以利用一个或多个传播状态来相对于机器参考框架和相对于全局参考框架传播或“预测”机器10的位置或机器的部分或部件的位置。例如，通过利用运动的角速率和预测速度，卡尔曼滤波器可以预测机器的或者机器部件的位置。如以上所讨论的，卡尔曼滤波器还可以计算预测位置的不确定度，其可以被设置为等于卡尔曼滤波器的误差协方差矩阵所指出的不确定度。机器上的各个位置还可以独立于IMU进行确定。若已确定了独立的位置测量值，则卡尔曼滤波器可以配置为将预测的位置信息和独立的位置测量值进行融合以针对每个定位确定更新的位置估计。可以利用卡尔曼滤波器测量值更新方程来确定更新的位置估计。若已确定了机器10的更新的位置估计，则卡尔曼滤波器240还可以确定每个IMU的偏差。如以上所讨论的，可以由卡尔曼滤波器执行的偏差参数估计的实例是在对所测量角速率积分之后角位置确定的陀螺仪偏差的估计。

在根据本发明的实施方式的机器状态控制系统的一个示范性应用中，关于机器和机器的部分或部件的位置和定向(姿态)的准确、已更新、实时信息可以向信息交换接口350提供返回，以便实现机器控制，从而获得对机器和机器的部件的最优定位和操作以用于改善生产率和可靠性。在一些实施方式中，反馈可以通过指导操作员如何实现能得到改善的机器行走和稳定性并由此提高生产率的控制来辅助操作员。在其他实施方式中，在信息交换接口350处接收的信息可以引起自主和半自主控制命令信号的产生，该自主或半自主控制命令信号被提供给各种机器系统和子系统以用于实现机器姿态的改变和机器部件的相对位置和定向的改变。在一个示范性实施方式中，如图3中所示，来自机器传感器的关于机器连接位置和速度、机器俯仰率和滚转率以及挖掘机的吊杆和斗杆的摆动角度的传感器反馈可以与视觉系统和感知传感器320所提供的指示施工现场的障碍物或其他特征的定位的信号以及从各种操作员控制装置324接收到的信号进行融合。融合的数据可以提供给信息交换接口350以便实现控制命令信号的生成，该控制命令信号改变各种螺线管致动器、节流阀控制装置、流体缸致动装置、电子控制装置、以及运动控制装置的操作以在挖掘操作期间得到机器的最优定位。在一些实施方式中信息交换接口350可以向人类操作员提供准确且实时的更新信息，以及作为与自主或半自主控制系统的信息接口，该自主或半自主控制系统使用该信息来处理控制命令信号以用于操作自动或半自动地运行各种机器系统和机器子系统。

在利用挖掘机挖掘时，机器状态控制系统50可以通过关于挖掘机的运动特性或动态特性历史和/或经验数据确定，在挖掘期间为了最佳的稳定性，机器的车身应当平行于挖掘连接装置，同时挖掘机的履带的惰轮指向前部连接装置。关于机器俯仰和滚转的反馈还可以提供给信息交换接口350，使得如果当挖掘机的履带向前指向时惰轮下方的底脚较差，信息交换接口可以引起控制命令信号的产生，该控制命令信号引起机器的姿态的变化以改善底脚并防止机器俯仰和滚转，以及操纵机器来与地面完全接触以抵消挖掘力。铲斗或其他工具的角度以及通过挖掘操作期间在任何特定时间点斗杆和吊杆的特定定向所实现的杠杆作用还可能影响挖掘机的挖掘效率。根据本发明的机器状态控制系统可以在挖掘期间向信息交换接口提供关于连接位置的实时效率的连续更新的反馈信息。反馈信息可以在挖掘期间引起铲斗的角度或斗杆的位置的变化，以便通过实现铲斗的更佳装载、铲斗的更快装载改善机器的效率，和/或改善连接装置的运动特性，其可以实现对机器动力的更好使用并盖申机器的使用寿命。从机器状态控制系统提供给信息交换接口的信息还可以引起控制命令信号的变化，其引起机器姿态的变化以避免在挖掘时车身相对于连接装置以90度定向，这对于稳定性或挖掘而言并非是最优的。当机器控制装置对信息交换接口350处提供的信息做出响应以实现控制从而使得机器的车身平行于挖掘连接装置同时挖掘机履带的惰轮指向前部连接装置时，结果是能量不会浪费在将机器提离地面，利用带来的循环时间减少而提高了生产率，以及减小了在最终传动部件上的负荷而改善了机器的使用寿命并减少了故障停机时间。

在根据本发明的机器状态控制系统的另一示范性应用中，如图4中所示，传感器融合系统可以利用以下输入来接收、合并和处理从操作员控制装置324接收的操作员命令输入：即来自IMU和非IMU机器传感器的输入，其测量连接位置、流体压力、发动机速度、机器姿态、机器部件位置和定向(包括俯仰率、偏航率和滚转率)；来自视觉系统320的输入，该视觉系统包括提供指示目标的存在和定位的信号的感知传感器；以及来自液压系统传感器430的输入。在一些实施方式中，来自液压系统传感器的信号输入可以指示一些状况，在这些状况下需要流体致动缸有更高的压力以避免停顿。为了避免在机器部件和结构上的不必要应力，机器状态控制系统50可以配置为，在可能且不会对机器造成损坏或形成不稳定的情况下，自动地调节液压泵450的升压压力并命令一个或多个释放阀425的释放压力设定点的受控斜升。来自根据本发明的各种实施例的传感器融合系统的融合传感器输出允许机器状态控制系统来确定在提升或挖掘操作期间机器何时可能正在停顿或者将要停顿。机器状态控制系统随后可以基于融合传感器反馈信息并接合操作员命令确定何时提升释放阀425的释放压力设定以及提升多少。

在一个示范性实施方式中，挖掘机可能正在提升重负载或者正在执行挖掘操作，且吊杆致动缸可能处于其最大压力，泵输出压力等于吊杆致动缸中的压力且在铲斗和斗杆仍然移动的同时吊杆停顿。机器状态控制系统可以通过从传感器融合系统接收的包括指示机器的俯仰率和滚转率的数据的准确、实时融合传感器数据来确定机器是否处于不稳定和/或过度应力状态。机器状态控制系统可以确定，吊杆致动缸的释放压力可以按照受控斜升方式增大以使吊杆再次移动而不会超过可接受的应力水平，并且与此同时保持机器的稳定性。

除了根据本发明的机器状态控制系统的示范性实施方式之外，机器状态控制系统可以输出命令来调节向机器上各个流体致动缸提供加压流体的泵的最大输出压力。如图4中所示，机器状态控制系统50从传感器融合系统接收融合的传感器数据，包括操作员输入、测量的连接装置位置、流体压力、发动机速度、机器俯仰率和滚转率以及诸如目标的存在和定位的现场数据。机器状态控制系统可以确定正在进行何种操作，并且通过针对不同操作所确立的高压截止，电子地调节系统中允许的最大压力。系统由此可以防止了在机器的各个部件和结构上出现过度应力，并且通过减慢泵流速、改变摆动马达位移或者超驰控制从操作员输入接收的阀命令，还防止了对部件的过度扭转或在高速率下将部件猛烈撞入目标。对于机器的部件而言，比如挖掘机的吊杆和斗杆，其可以包括用于在挖掘和倾倒位置之间移动吊杆和斗杆的相关专用液压摆动回路，摆动回路中的液压压力或者摆动马达位移可以根据从机器状态控制系统接收的实时输出命令电子地进行限制。在一些实施方式中，设置在摆动回路或机器上的其他液压回路中的一个或多个泵可以适用于零位移或接近零位移操作配置。机器状态控制系统50可以确定正在进行何种操作，并且在某些情况下将一个或多个泵的唯一调节到零位移或接近零位移。一个或多个泵的位移可以被调节到足够低的值，从而仅补偿系统的泄露，并且通过极低位移模式下的泵或多个泵所供应的流体致动缸引起的连接装置的移动将不会引起连接装置或其他机器部件的过度应力。作为对半自主模式下超驰控制从操作员输入接收的阀命令或其他控制命令的补充或替代，机器状态控制系统50可以通过与信息交换接口350相关联的一个或多个显示器，或者通过操纵杆、操作员座椅、投射到操作员舱的挡风玻璃上平视显示器(HUD)中的触觉反馈，或者通过声音和被实施用来指导操作员并改进未来操作控制命令的其他刺激，直接向操作员提供反馈。

对于本领域技术人员显而易见的是，可以对所公开的用于确定机器的实时状态的方法和系统做出各种修改和改变。通过考虑所公开机器状态控制系统的说明书和实践，其他实施例和实施方式对于本领域技术人员而言将是显而易见的。旨在将说明书和实例认为是仅作为示范性的，真实范围由以下权利要求书及其等价物指出。

Claims (10)

1.一种确定机器的实时状态方法，包括：

利用至少一个处理器从安装在所述机器的不同部件上的多个惯性测量单元中的每一个接收信号的时间序列，所述信号指示所述多个惯性测量单元中一个或多个安装在其上的所述机器的所述部件中每一个的运动测量值的加速度和角速率；

针对所述机器的单独部件上所述惯性测量单元中的每一个，利用所述至少一个处理器的单独卡尔曼滤波器模块融合从所述惯性测量单元接收的所述信号，对来自所述多个惯性测量单元中每一个的所述信号的融合包括：

合并来自所述惯性测量单元的加速度测量值以及运动测量值的角速率，以在一系列时间步的某时间步期间估计所述惯性测量单元安装在其上的所述机器的所述部件的输出联合角度；

基于来自先前时间步的所述输出联合角度的估计，在后续时间步中生成对所述部件的所述输出联合角度的先验估计；

将所述输出联合角度的所述先验估计与所述先验估计的准确度的估计和从所述惯性测量单元接收的当前测量值合并以产生所述输出联合角度的精确后验估计；

将所述机器的多个部件的所述输出联合角度的所述精确后验估计彼此相互并参考机器参考框架进行融合，以参考所述机器参考框架确定所述机器的所述多个部件中每一个的当前输出联合角度的最佳估计；

利用所述机器的所述部件的当前输出联合角度的所述最佳估计以及表征所述机器的结构设计信息来求解运动学方程；

在所述一系列时间步的连续时间步下，通过所述运动学方程的解确定感兴趣的每个机器部件的位置、速度和加速度中至少一个的实时值；以及

在每个感兴趣机器部件的受控操作移动的实施中应用所述确定的实时值。

2.根据权利要求1所述的方法，其中针对所述机器的所述单独部件上所述多个惯性测量单元中每一个的所述单独卡尔曼滤波器模块执行对从与每个惯性测量单元相关联的陀螺仪接收的测量值的偏差的估计，并且补偿所述偏差。

3.根据权利要求1所述的方法，进一步包括：在所述机器的摆动运动期间，通过感兴趣的每个机器部件的连续位置确定机器部件是否正在沿弧线移动，并且在将加速度测量值输入到卡尔曼滤波器模块之前，在从安装在所述机器部件上的惯性测量单元接收的所述加速度测量值中补偿正在沿弧线移动的所述机器部件的向心加速度。

4.根据权利要求1所述的方法，进一步包括基于从惯性测量单元接收的实际测量值的准确度的估计，确定输出联合角度的后验估计的准确度。

5.根据权利要求4所述的方法，进一步包括：基于从每个惯性测量单元接收的连续实际测量值，相对于与每个连续后验估计相关联的权重，确定与机器部件的输出联合角度的每个连续先验估计相关联的权重，并分配代表相对权重的卡尔曼增益。

6.根据权利要求5所述的方法，进一步包括通过从预订增益调度检索状态协方差矩阵和估计测量值协方差矩阵来分配所述卡尔曼增益，所述状态协方差矩阵表示机器部件的每个输出联合角度的所述先验估计中的预测可变性，所述估计测量值协方差矩阵表示从每个惯性测量单元接收的所述实际测量值中的预测可变性。

7.根据权利要求1所述的方法，进一步包括通过针对每个机器部件的所述运动学方程的所述解，确定位于所述机器部件上的每个惯性测量单元相对于全局参考框架的定位和角度定向。

8.根据权利要求7所述的方法，进一步包括对位于机器部件上的惯性测量单元确定的定位和角度定向来应用偏移，以求解所述机器部件上感兴趣的另一点的定位或角度旋转中的至少一个。

9.根据权利要求1所述的方法，进一步包括：

利用所述至少一个处理器从位于所述机器上的非惯性测量单元传感器接收信号，来自所述非惯性测量单元传感器的所述信号指示机器部件的位置、速度和加速度中的至少一个；以及

利用所述单独卡尔曼滤波器模块将从所述非惯性测量单元传感器接收的所述信号与从安装在机器部件上的惯性测量单元接收的信号进行融合，所述单独卡尔曼滤波器模块还被用来融合从所述惯性测量单元接收的所述信号。

10.一种用于确定机器的实时状态系统，包括：

多个惯性测量单元，所述多个惯性测量单元安装在所述机器的单独部件上，所述惯性测量单元各自配置为生成信号的时间序列，所述信号指示所述惯性测量单元安装在其上的所述机器的所述部件的运动测量值的加速度和角速率；以及

至少一个处理器，所述至少一个处理器与所述惯性测量单元中每一个进行通信，所述至少一个处理器中的每一个配置为：

利用所述至少一个处理器的单独卡尔曼滤波器模块融合从所述惯性测量单元接收的所述信号，对来自所述多个惯性测量单元中每一个的所述信号的融合包括：

合并来自所述惯性测量单元的加速度测量值以及运动测量值的角速率，以在一系列时间步的某时间步期间估计所述惯性测量单元安装在其上的所述机器的所述部件的输出联合角度；

基于来自先前时间步的所述输出联合角度的估计，在后续时间步中生成对所述部件的所述输出联合角度的先验估计；以及

将所述输出联合角度的所述先验估计与所述先验估计的准确度的估计和从所述惯性测量单元接收的当前测量值合并以产生所述输出联合角度的精确后验估计；以及

将所述机器的多个部件的所述输出联合角度的所述精确后验估计彼此相互并参考机器参考框架进行融合，以参考所述机器参考框架确定所述机器的所述多个部件中每一个的当前输出联合角度的最佳估计；

利用所述机器的所述部件的当前输出联合角度的所述最佳估计以及表征所述机器的结构设计信息来求解运动学方程；

在所述一系列时间步的连续时间步下，通过所述运动学方程的解确定感兴趣的每个机器部件的位置、速度和加速度中至少一个的实时值；以及

在每个感兴趣机器部件的受控操作移动的实施中应用所述确定的实时值。