CN105565164A - 起重机臂的姿态和方向参考系统及方法 - Google Patents

Abstract

提供了用于确定起重机臂的姿态和方向角的方法和装置。起重机臂的角速度、起重机臂的滚动角、起重机臂的俯仰角、起重机臂的比力、和起重机臂的局部操作环境中的磁场都被感测并提供给处理器。在处理器中处理所有这些测量以估计起重机臂的姿态和方向角。

Description

起重机臂的姿态和方向参考系统及方法

本申请是申请日为2012年3月15日，申请号为201210149447.6，发明名称为“起重机臂的姿态和方向参考系统及方法”的申请的分案申请。

技术领域

本发明一般地涉及一种姿态和方向参考系统，以及更特别地涉及用于起重机臂的姿态和方向参考系统。

背景技术

塔式起重机被用在种种环境中。两个更常见的环境是建筑工地和造船厂，因为这种类型的起重机提供了高度和起重能力的结合。塔式起重机典型地包括基座、立柱(mast)和起重机臂。基座被固定到地面，并且也与立柱相连接。回转单元与立柱相连接并被用于使起重机旋转。此外，起重机臂包括承载部分、平衡臂(counterjib)部分和操作室。

起重机臂的承载部分通常携带负载。平衡臂部分与承载部分相连接，并且在承载部分携带负载的同时携带配重以平衡起重机臂。操作室通常位于立柱的顶部附近，并且可以附着到起重机臂。然而，其他塔式起重机可以将操作室安装在立柱下方的中途。不论操作室的具体位置，起重机操作者坐在操作室中并且控制起重机。在一些情况下，起重机操作者可以从地面远程控制一台或多台塔式起重机。

在一些环境中，多台塔式起重机可以相对靠近地操作。因此，虽然未必可能，但是已经假定的是，两台或者多台塔式起重机的起重机臂可能碰撞。因此，需要一种碰撞避免/警告系统，该系统能够确定在特殊场地处工作的起重机自身的起重机臂和其他起重机臂的三维(3D)角度方向(例如姿态和方向角)。本发明至少致力于这个需求。

发明内容

在一个实施例中，一种确定起重机臂的姿态和方向角的方法包括：感测起重机臂的角速度、感测起重机臂的滚动角、感测起重机臂的俯仰角、感测作用在起重机臂的一部分上的比力、以及感测至少紧接起重机臂的局部磁场。感测到的起重机臂的角速度、感测到的起重机臂的滚动角、感测到的起重机臂的俯仰角、感测到的比力、以及感测到的局部磁场都被提供给处理器。在处理器中，根据感测到的起重机臂的角速度和感测到的比力来计算起重机臂的平移速度，以及使用计算的起重机臂的平移速度和感测到的起重机臂的角速度来计算起重机臂的加速度。使用计算的起重机臂的加速度来从感测到的起重机臂的滚动角和感测到的起重机臂的俯仰角中去除起重机臂的加速度分量，以及由此提供修正的起重机臂的滚动角测量和修正的起重机臂的俯仰角测量。将校准参数应用到感测到的局部磁场，由此提供校准的磁场测量。根据校准的磁场测量结果来计算起重机臂的方向角。使用修正的起重机臂的滚动角测量、修正的起重机臂的俯仰角测量、和计算的起重机臂的方向角来估计起重机臂的姿态和方向角。

在另一个实施例中，一种起重机臂的姿态和方向参考系统，包括多个起重机臂角速度传感器、多个比力传感器、倾斜计、多个磁力计、以及处理器。每个起重机臂角速度传感器被配置成感测起重机臂的角速度并且提供表示起重机臂的角速度的角速度信号。每一个比力传感器被配置成感测作用在起重机臂上的比力并且提供表示该比力的比力传感器信号。倾斜计被配置成感测起重机臂的滚动角和起重机臂的俯仰角并且提供表示该起重机臂的滚动角和起重机臂的俯仰角的倾斜计信号。每一个磁力计被配置成感测至少紧接起重机臂的局部磁场并且提供表示该局部磁场的磁力计信号。处理器被耦合以接收角速度信号、比力传感器信号、倾斜计信号和磁力计信号，并响应于此而被配置为：根据感测到的起重机臂的角速度和感测到的比力来计算起重机臂的平移速度、使用计算的起重机臂的平移速度和感测到的起重机臂的角速度来计算起重机臂的加速度、使用计算的起重机臂的加速度来从感测到的起重机臂的滚动角和感测到的起重机臂的俯仰角中移除起重机臂的加速度分量，以及由此提供修正的起重机臂的滚动角测量和修正的起重机臂的俯仰角测量，将校准参数应用到感测到的局部磁场，以由此提供校准的磁场测量，根据校准的磁场测量来计算起重机臂的方向角，以及使用修正的起重机臂的滚动角测量、修正的起重机臂的俯仰角测量、和计算的起重机臂的方向角来估计起重机臂的姿态和方向角。

在又一个实施例中，一种起重机臂的姿态和方向参考系统，包括多个起重机臂角速度传感器、倾斜计、多个加速计、多个磁力计、显示设备和处理器。每个起重机臂角速度传感器被配置成感测起重机臂的角速度并且提供表示该起重机臂的角速度的角速度信号。倾斜计被配置成感测起重机臂的滚动角和起重机臂的俯仰角并且提供表示该起重机臂的滚动角和起重机臂的俯仰角的倾斜计信号。每个加速计被配置成感测作用在检验质量(proofmass)上的力并且提供表示该力的比力传感器信号。每个磁力计被配置成感测至少紧接起重机臂的局部磁场并且提供表示该局部磁场的磁力计信号。处理器被耦合到显示设备并且进一步被耦合以接收角速度信号、倾斜计信号、比力信号和磁力计信号。在接收角速度信号、倾斜计信号、比力信号和磁力计信号时处理器被配置：使用感测到的作用在验证质量上的力、重力和加速计偏差的预测来计算修正的加速度计测量、使用感测到的角速度和角速度传感器偏差的预测来计算修正的角速度测量、使用修正的角速度测量来计算起重机臂平移速度的估计值、根据倾斜计信号和起重机臂平移速度的估计值来计算修正的起重机的滚动角和俯仰角、根据磁力计信号来计算起重机臂的方向角，实现第一滤波器，该第一滤波器接收计算的起重机臂的速度的估计值、计算起重机臂的速度的预测、以及计算加速计偏差的预测，实现第二滤波器，该第二滤波器接收修正的起重机臂的滚动角和俯仰角的计算值和起重机臂的方向角的计算值、计算角速度传感器偏差的预测、以及确定起重机臂的姿态和方向角，以及对显示设备提供图像渲染显示命令，该图像渲染显示命令使得显示设备能够在其上渲染起重机臂姿态和方向角。

此外，起重机臂的姿态和方向系统及方法的其他期望的特征和特性将根据随后的详细描述和所附的权利要求书，连同附图和先前的背景而变得清楚。

附图说明

在下文中将结合以下附图对本发明进行描述，其中相同的附图标记表示相同的元件，其中：

图1描绘了塔式起重机的一个实施例的侧视图；

图2描绘了可以用于图1的塔式起重机中的起重机臂的姿态和方向参考系统(AHRS)的功能框图；

图3以流程图的方式描绘了由图2的起重机臂AHRS所实现的过程；

图4描绘了在执行图3的过程时更详细地描绘的具有在处理器内实现的各种功能的图2的起重机臂AHRS。

具体实施方式

接下来的详细描述本质上仅仅是示例性的，并不旨在限制本发明或者本发明的应用和使用。如在这里所使用的，词语“示例性”意味着“用作例子、实例或者说明”。因此，在这里被描述为“示例性”的任何实施例不必被解释为比其他实施例优选或者有利。在这里所描述的所有实施例都是提供来使得本领域技术人员能够制造或使用本发明的示例性实施例，并且不限制由权利要求所限定的本发明的范围。此外，不意在由先前的技术领域、背景、简要概要或者下面的详细说明中所呈现的任何表达的或暗示的理论所限制。在这点上，尽管姿态和方向参考系统在这里被描述为与塔式起重机一起实现，但将理解的是，其能够与诸如俯仰式起重机之类的其他类型的起重机一起实现。

首先参照图1，描绘了塔式起重机100的一个实施例的侧视图。描绘的起重机100是塔式起重机，尽管还可以使用许多其他类型的起重机中的任一类型。描绘的起重机100包括基座102、立柱104和回转单元106。基座被固定到表面108，比如地面，并且被用于支撑构成塔式起重机100的部件的剩余部分。可被实现为可调高度的立柱的立柱104在一端耦合到基座102。回转单元106旋转地耦合到立柱104的相反端，并且附加地耦合到起重机臂结构110，该起重机臂结构110包括承载部分112、平衡臂部分114和操作室116。在进一步进行之前，要注意的是，当术语“起重机臂”在这里使用时，其包括：整个起重机臂结构110、承载部分112和平衡臂部分114，仅仅承载部分112或者仅仅平衡臂部分114。

在所描绘的实施例中包括多个格状结构元件的承载部分112在一端耦合到回转单元106并且从那里延伸到第二端。缆绳滑车118可以被安装在承载部分112上并且可以可控制地移动到承载部分112的端部之间的多个位置。平衡臂部分114在与承载部分112相反的一侧上耦合到回转单元106，并且具有与其耦合的配重122。操作室116被耦合到回转单元106，并且至少在所描绘的实施例中，位于承载部分112的下方。

置于操作室116内的操作者，控制塔式起重机110。特别地，操作者通过多个未说明的电机和齿轮组，可以使回转单元106旋转，并且因此使起重机臂相对于立柱104绕着第一正交轴124旋转。在操作期间，起重机臂的动力学特性，以及环境状况，可能附加地引起起重机臂绕着第二正交轴126(描绘为一个点来表示进出纸面的轴)和第三正交轴128旋转。如在这里所使用的，绕着第一正交轴124的旋转改变了起重机臂的方向角，绕着第二正交轴126的旋转改变了起重机臂的俯仰角，以及绕着第三正交轴128的旋转改变了起重机臂的滚动角。

在一些情况下，塔式起重机100可以与一台或者多台其他的未说明的塔式起重机相对靠近地操作。因此，为了更进一步降低起重机臂110将与另一个塔式起重机的臂相碰撞的可能性，描绘的塔式起重机100附加地配备有起重机臂的姿态和方向参考系统(AHRS)。在图2中描绘了起重机臂AHRS200的示例性实施例，并且现在将参照图2来对其描述。

描绘的起重机臂AHRS200包括多个起重机臂角速度传感器202(202-1、202-2、202-3)、多个磁力计204(204-1、204-2、204-3)、多个加速计206(206-1、206-2、206-3)、倾斜计208、处理器210和显示设备212。起重机臂角速度传感器202均被配置成感测起重机臂的角速度并且提供表示该起重机臂的角速度的角速度信号。磁力计204均被配置成感测至少紧接起重机臂的局部磁场，并且提供表示该局部磁场的磁力计信号。更具体地，磁力计204提供了对沿着测量轴方向解析的局部磁场向量的测量。由于磁力计204被附着到起重机臂110，磁力计204和起重机臂110具有固定的相对方向。因此，磁力计204的方向与起重机臂110的方向直接相关联。加速计206均被配置成感测作用在检验质量(未示出)上的力，并且提供表示该力的比力信号。倾斜计208被配置成感测起重机臂的滚动角和俯仰角，并且提供表示该起重机臂的滚动角和俯仰角的倾斜计信号。

将理解的是，起重机臂角速度传感器202的数量和类型、磁力计204的数量和类型、加速计206的数量和类型可以变化。然而，在所描绘的实施例中，使用三个正交布置的速率陀螺仪(“gyros”)来实现起重机臂角速度传感器202，使用三个正交布置的磁力计来实现磁力计204，以及使用三个正交布置的加速计来实现加速计206。尽管所使用的特定类型的速率陀螺仪202、磁力计204、加速计206、倾斜计208也可以变化，但是在一个特定实施例中，使用了由霍尼韦尔国际有限公司所制造的HG1171惯性测量单元(IMU)，其将所有这些装置包括在单个外壳中。将理解的是，在其他实施例中，可以使用分开容纳的传感器。

不论起重机臂角速度传感器202、磁力计204、加速计206、和倾斜计208的具体实现方式，处理器210被耦合以分别从它们接收角速度信号、磁力计信号、比力信号和倾斜计信号。响应于这些信号，处理器210被配置成确定起重机臂的姿态和方向角。处理器210附加地对显示设备212提供图像渲染显示命令。图像渲染显示命令引起显示设备212在其上渲染所确定的起重机臂的姿态和方向角。

在进一步进行之前，应当注意的是，显示设备212可以通过使用适于以起重机操作者可观看的格式渲染图像和/或文本数据的许多已知的显示设备中的任何一个显示设备来实现。这种显示设备的非限制性的例子包括各种阴极射线管(CRT)显示器，以及各种平板显示器，比如各种类型的LCD(液晶显示器)和TFT(薄膜晶体管)显示器，这里仅仅列举了一些。

处理器210被配置成实现各种功能，以便于根据角速度信号、磁力计信号、比力信号、以及倾斜计信号来确定起重机臂的姿态和方向角。特别地，并且如图2进一步所描绘的，处理器210实现了两个Kalman滤波器—第一Kalman滤波器214和第二Kalman滤波器216。如下文将更详细地描述的，在这里被称为速度Kalman滤波器的第一Kalman滤波器214，计算起重机臂的速度的预测和加速计偏差的预测。用于速度Kalman滤波器214的测量向量是根据修正的角速度测量所计算的起重机臂的速度。计算的起重机臂110的速度也用于计算起重机臂110的加速度，该起重机臂110的加速度用于修正由倾斜计所感测的滚动角和俯仰角。在这里被称为四元数Kalman滤波器的第二Kalman滤波器216，计算角速度传感器偏差的预测，并且确定起重机臂的姿态和方向角。用于四元数Kalman滤波器216的测量向量包括上面提到的修正的起重机臂的滚动角和俯仰角，以及根据磁力计信号所确定的起重机臂的方向角。应该注意的是，起重机臂的方向角是相对于起重机臂的初始角方向的方向角。

如通常所知晓的，Kalman滤波器实现了迭代的两步预测-修正过程来估计状态向量。这个过程的预测部分有时被称为“时间更新”，因为支配了状态向量的微分方程(例如动态模型)按时向前传播。来自这个过程的预测部分的计算结果可以被称为状态向量的先验估计值。这个过程的修正部分有时被称为“测量更新”，因为使用测量向量来修正预测步骤中所计算的状态向量的先验估计值。来自该过程的修正部分的计算结果可以被称为状态向量的后验估计值。

起重机臂AHRS200，如刚刚提到的，包括两个Kalman滤波器214、216。两个Kalman滤波器214、216一起操作。因此，在处理器210中实现的整个过程包括两个预测步骤和两个修正步骤。此外，如此配置所描绘的Kalman滤波器214、216使得测量向量驱动预测步骤和修正步骤。在处理器210中实现的整个过程300在图3中以流程图的形式描绘，并且包括接下来的迭代步骤：由速度Kalman滤波器214的预测(302)、由四元数Kalman滤波器216的预测(304)、由速度Kalman滤波器214的修正(306)、各种中间计算(308)、以及由四元数Kalman滤波器304的修正(310)。由于过程是迭代的，因此这些过程步骤顺序地反复不断地被执行。

现在参照图4，其更详细地描绘了处理器210内所实现的各种功能，图3中描绘的并且上文一般地描述的过程现在将被更详细地描述。首先执行速度Kalman滤波器214的预测步骤(302)。在这个步骤期间，速度Kalman滤波器214计算起重机臂的速度402的预测和加速计偏差404的预测。通过使用在下文中更详细地描述的动态模型406来计算这些预测。驱动速度Kalman滤波器214的预测步骤(302)所使用的测量是补偿的角速度测量424和补偿的加速计测量408。补偿的角速度测量是从已经由角速度传感器偏差420(例如，来自在先时间步骤的后验估计值)所补偿的角速度传感器202提供的角速度信号。补偿的加速计测量408是从已经补偿了加速计偏差404(例如来自在前时间步骤中的后验估计值)和重力412的加速计206提供的加速计信号。

在四元数Kalman滤波器的预测步骤(304)期间，四元数Kalman滤波器216计算起重机臂的3D角方向414(俯仰角、滚动角和方向角)的预测，以及角速度传感器偏差420的预测。这些预测也使用动态模型422来计算，这在下文更详细地描述。驱动四元数Kalman滤波器的预测步骤(304)所使用的测量是补偿的角速度测量424。

速度Kalman滤波器的修正步骤(306)通过使用提供给测量模型407的计算的起重机臂的速度426来被驱动。测量模型407，类似动态模型406，将在下文被进一步描述。提供给测量模型407的起重机臂的速度426根据修正的角速度测量424和角速度传感器202的(在起重机臂110上的)已知位置428来被计算。如刚刚提到的，使用从角速度传感器202提供的角速度信号和角速度传感器偏差420的后验估计值来计算修正的角速度测量424。另外注意的是，在速度Kalman滤波器的修正步骤(302)之后计算的起重机臂的速度402和加速计偏差404的预测是起重机臂的速度402和加速计偏差404的后验估计值。

在中间计算步骤(308)期间，执行了若干中间计算。这些计算包括使用起重机臂的速度402的后验估计值和(使用角速度传感器偏差420的后验估计值所计算的)修正的起重机臂的角速度测量424来计算起重机臂的加速度432。使用计算的起重机臂的加速度432来对从倾斜计208提供的倾斜计信号应用加速度补偿434。这个补偿从感测到的起重机臂的滚动角和感测到的起重机臂的俯仰角中去除了起重机臂的加速度分量，由此提供修正的起重机臂的滚动角测量和修正的起重机臂的俯仰角测量436。将磁力计校准参数438应用到从磁力计204提供的磁力计信号，由此产生校准的磁力计测量416。校准参数438可以在初始的对准过程期间被确定。然后使用校准的磁力计测量416来计算起重机臂110的(相对于其初始角方向的)方向角422。然后将修正的起重机臂的滚动角和俯仰角测量436和方向角442转换成四元数444并且提供给四元数Kalman滤波器216。应该注意的是，转换到四元数和接下来使用四元数只是可以用来将3D角方向参数化的一种技术，并且也可以使用许多其他的姿态参数化方法。一些非限制性的例子包括欧拉角、Rodriques参数、以及方向余弦，这里仅仅列举了一些。

在中间计算步骤(308)之后，执行四元数Kalman滤波器修正步骤(310)。使用根据磁力计信号计算的修正的滚动角和俯仰角434和根据校准的磁力计测量438计算的(并且被转换成四元数444的)方向角442来驱动该步骤。这些值被提供给测量模型423，这也在下文中被进一步描述。计算起重机臂的3D角方向414(例如滚动角、俯仰角和方向角)的后验估计值，以及角速度传感器偏差420。起重机臂的3D角方向414的后验估计值被用于生成提供给显示设备212的图像渲染显示命令。显示设备212，如先前所提到的，渲染了起重机臂姿态和方向角的图像。

在上文讨论中提到，将描述分别在速度Kalman滤波器214和四元数Kalman滤波器216中的动态模型406和422，与将描述分别在速度Kalman滤波器214和四元数Kalman滤波器216中的测量模型407和423一样。为了完整，现在将提供这些描述。首先从速度Kalman滤波器214开始，其实现的动态模型在数学上表示如下：

$Q_{v,w}=diag\left[\begin{array}{cccccc}{\mathrm{\σ}}_{fx}^2& {\mathrm{\σ}}_{fy}^2& {\mathrm{\σ}}_{fz}^2& \frac{2{\mathrm{\σ}}_{f1x}^2}{{\mathrm{\τ}}_a}& \frac{2{\mathrm{\σ}}_{f1y}^2}{{\mathrm{\τ}}_a}& \frac{2{\mathrm{\σ}}_{f1z}^2}{{\mathrm{\τ}}_a}\end{array}\right],$

其中是传感器的速度向量，是起重机臂的测量的角速度向量，是起重机臂的测量的比力向量，是局部重力向量，是速率陀螺仪偏差向量，是速率陀螺仪测量噪声向量，是加速计偏差向量，是加速计偏差零漂向量，是加速计偏差漂移速率向量，是加速计测量噪声向量，是加速计偏差高斯-马尔可夫驱动过程噪声向量，σ_f是加速计测量噪声的标准偏差，σ_f1是加速计偏差高斯-马尔可夫驱动过程噪声的标准偏差，τ_a是加速计偏差高斯-马尔可夫过程的相关时间，Q_v,w是由加速计测量噪声向量和加速计偏差高斯-马尔可夫驱动过程噪声向量所定义的速度Kalman滤波器过程噪声向量的功率谱密度，以及C_bN是从起重机臂导航框架到传感器体框架的方向余弦矩阵。

以及由速度Kalman滤波器214所实现的测量模型在数学上表示如下：

其中，除了那些先前定义过的变量，是传感器相对于起重机臂的旋转中心的位置向量，σ_g是速率陀螺仪测量噪声的标准偏差，以及R_v是根据补偿的角速度测量和传感器位置向量所计算的起重机臂角速度的协方差矩阵。

在四元数Kalman滤波器216中实现的动态模型在数学上表示如下：

$Q_{q,w}=diag\left[\begin{array}{cccccc}{\mathrm{\σ}}_{gx}^2& {\mathrm{\σ}}_{gy}^2& {\mathrm{\σ}}_{gz}^2& \frac{2{\mathrm{\σ}}_{g1x}^2}{{\mathrm{\τ}}_g}& \frac{2{\mathrm{\σ}}_{g1y}^2}{{\mathrm{\τ}}_g}& \frac{2{\mathrm{\σ}}_{g1z}^2}{{\mathrm{\τ}}_g}\end{array}\right],$

其中，除了那些先前定义过的变量，是估计的四元数的向量误差分量，是速率陀螺仪偏差零漂向量，是速率陀螺仪偏差漂移速率向量，是速率陀螺仪偏差高斯-马尔可夫驱动过程噪声向量，σ_g1速率陀螺仪偏差高斯-马尔可夫驱动过程噪声的标准偏差，τ_g是速率陀螺仪偏差高斯-马尔可夫过程的相关时间，以及Q_q,w是由速率陀螺仪测量噪声向量和速率陀螺仪偏差高斯-马尔可夫驱动过程噪声向量所定义的四元数Kalman滤波器过程噪声向量的功率谱密度。

以及由四元数Kalman滤波器216所实现的测量模型在数学上表示如下：

$-q_4\widehat{\stackrel{\‾}{q}}+{\stackrel{\‾}{q}}^x\widehat{\stackrel{\‾}{q}}+{\hat{q}}_4\stackrel{\‾}{q}=\left[\begin{array}{cc}I& 0\end{array}\right]\[\frac{\mathrm{\δ}\stackrel{\‾}{q}\left(t\right)}{{\stackrel{\→}{b}}_{g1}\left(t\right)}\]$

$R_q=diag\[\begin{array}{ccc}{\[\frac{{\mathrm{\σ}}_{inc,x}}{2}\]}^2& {\[\frac{{\mathrm{\σ}}_{inc,y}}{2}\]}^2& {\[\frac{{\mathrm{\σ}}_{mag}}{2}\]}^2\end{array}\],$

其中，除了那些先前定义过的变量，是估计的四元数的向量分量，是估计的四元数的标量分量，是根据从倾斜计信号计算的修正的滚动角和俯仰角434，和从校准的磁力计测量438计算的相关方向角442所计算的四元数的向量分量，q₄是根据从倾斜计信号计算的修正的滚动角和俯仰角434，和从校准的磁力计测量438计算的相关方向角442所计算的四元数的标量分量，σ_inc,x是滚动角倾斜计测量的标准偏差，σ_inc,y是俯仰角倾斜计测量的标准偏差，σ_mag是根据校准的磁力计测量438所计算的方向角测量的标准偏差，以及R_q是根据从倾斜计信号计算的修正的滚动角和俯仰角434，和从校准的磁力计测量438计算的相关方向角442所计算的起重机臂姿态和方向角测量的协方差矩阵。

这里所公开的起重机臂的姿态和方向参考系统和方法可以被用于确定起重机臂的姿态和方向角。如果所公开的系统被安装在其他起重机中，相同的信息可以从在特定场地工作的其他起重机臂提供。起重机臂的姿态和方向参考系统与其他姿态和方向参考系统区别在于它实现了两阶段Kalman滤波器(例如，速度Kalman滤波器214和四元数Kalman滤波器216)来估计起重机臂的姿态和方向角，估计起重机臂的速度以从加速计测量中移除加速度分量，由此修正倾斜计测量，以及起重机臂的速度的测量基于的是起重机臂的动力学特性。

本领域技术人员将理解，与在这里公开的实施例相结合描述的各种说明的逻辑块、模块、电路和算法步骤可以被实现为电子硬件、计算机软件或者两者的组合。在上文按照功能和/或逻辑块部件(或模块)以及各种处理步骤来描述了一些实施例和实现方式。然而，应当理解的是，这些块部件(或模块)可以由配置成执行特定功能的任何数量的硬件、软件和/或固件部件来实现。为了清楚地说明硬件和软件的这种可交换性，各种说明性的部件、块、模块、电路和步骤在上文按照它们的功能性被一般地描述。这种功能性是实现为硬件还是软件取决于对整个系统所施加的特定应用和设计约束。本领域技术人员可以对每一个特定的应用以变化的方式来实现所描述的功能性，但是这种实现决定不应当被解释为引起了从本发明的范围的偏离。例如，系统或部件的实施例可以采用各种集成电路部件，例如，存储器元件、数字信号处理元件、逻辑元件、查找表等，这可以在一个或者多个微处理或其他控制设备的控制下执行各种功能。此外，本领域技术人员将理解，在这里所描述的实施例仅仅是示例性的实现方式。

与在这里公开的实施例相结合描述的各种说明性的逻辑块、模块和电路可以用以下来实现或执行：通用处理器、数字信号处理器(DSP)、特定用途集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑设备、分立的门或者晶体管逻辑、分立的硬件部件、或者设计成执行在这里描述的功能的它们的任意组合。通用处理器可以是微处理器，但是在替代方式中，处理器可以是任何传统的处理器、控制器、微控制器或者状态机。处理器还可以实现为计算设备的组合，例如DSP和微处理器的组合、多个微处理器、与DSP核相结合的一个或者多个微处理器、或者任何其他这种配置。词语“示例性的”在这里被专门用来表示“作为例子、实例或者说明”的意思。在这里被描述为“示例性的”的任何实施例不一定被解释为比其他实施例优选或有利。

与这里所公开的实施例相结合描述的方法或算法的步骤可以直接体现在硬件中、体现在由处理器所执行的软件模块中，或者体现在二者的组合中。软件模块可以驻留在RAM存储器、闪存、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可移动磁盘、CD-ROM，或者本领域已知的任何其他形式的存储媒介中。示例性的存储媒介耦合到处理器使得处理器可以从存储介质读取信息并且对存储媒介写入信息。在替代方式中，存储介质可以被集成到处理器。处理器和存储媒介可以在ASIC中驻留。该ASIC可以在用户终端中驻留。在替代方式中，处理器和存储媒介可以在用户终端中作为分立部件驻留。

在本文档中，诸如第一和第二等等的相关的术语，可以单独用于将一个实体或者动作与另一个实体或者动作相区分，而不一定需要或者暗示这些实体或动作之间的任何实际的这种关系或者顺序。诸如“第一”、“第二”、“第三”等等的数字顺序，仅仅表示多个中的不同的单个，而不暗示任何顺序或次序，除非由权利要求的语言所明确限定。在任何权利要求中的文本次序不暗示过程步骤必须按照这种次序以时间或者逻辑顺序来执行，除非由权利要求语言所明确限定。过程步骤可以在不偏离本发明的范围的情况下以任何顺序交换，只要这种交换不使权利要求的语言相矛盾并且不在逻辑上无意义。

此外，根据上下文，用于描述两个不同元件之间的关系的诸如“连接”或“耦合到”的词语，不暗示必须在这些元件之间进行直接的物理连接。例如，两个元件可以通过一个或多个附加元件物理地、电地、逻辑地或者以其他方式相互连接。

虽然在本发明的前述详细描述中已经呈现了至少一个示例性的实施例，但应当理解的是，存在大量的变形。还应当理解的是，一个或多个示例性的实施例仅仅是例子，并且不意在以任何方式限制本发明的范围、应用或者配置。相反，之前的详细描述将为本领域技术人员提供用于实现本发明的示例性的实施例的便利的路线图。要理解的是，可以在不偏离如在所附的权利要求中所阐述的本发明的范围的情况下，在示例性的实施例中所描述的元件的功能和布置中作出各种改变。

Claims (12)

1.一种系统，包括：

处理器，耦合到一个或多个传感器以接收角速度信号、比力信号和磁力计信号，所述处理器被配置成在接收角速度信号、比力信号和磁力计信号时：

实现第一滤波器，所述第一滤波器接收计算的起重机臂的速度的估计值，并且计算起重机臂的速度的预测和加速计偏差的预测；以及

实现第二滤波器，所述第二滤波器接收修正的起重机臂的滚动角和修正的起重机臂的俯仰角的计算值和起重机臂的方向角的计算值，并且计算角速度传感器偏差的预测以及确定起重机臂的姿态和方向角。

2.根据权利要求1所述的系统，其中所述处理器还被配置成：

接收倾斜计信号；

使用感测到的作用在检验质量上的力、重力和加速计偏差的预测来计算修正的加速计测量；

使用感测到的角速度和角速度传感器偏差的预测来计算修正的角速度测量；

使用修正的角速度测量和补偿的加速计测量来计算起重机臂速度的估计值；

根据倾斜计信号和起重机臂速度的估计值来计算修正的起重机的滚动角和俯仰角；以及

根据磁力计信号来计算起重机臂的方向角。

3.根据权利要求2所述的系统，其中所述处理器还被配置成：

计算起重机臂角速度传感器偏差修正；以及

根据所感测的起重机臂的角速度和起重机臂角速度传感器偏差修正来计算修正的起重机臂的角速度测量。

4.根据权利要求3所述的系统，其中所述处理器使用修正的起重机臂的角速度测量来计算起重机臂的速度。

5.根据权利要求4所述的系统，其中：

所述一个或多个传感器包括多个起重机臂角速度传感器，所述多个起重机臂角速度传感器包括三个正交布置的速率陀螺仪，每个速率陀螺仪均位于起重机臂上的位置处；以及

所述处理器使用修正的起重机臂的角速度测量和每个速率陀螺仪的位置来计算起重机臂的速度。

6.根据权利要求1所述的系统，其中所述处理器还被配置成响应于比力信号：

计算偏差修正；以及

使用偏差修正和重力来补偿感测到的比力，以由此提供补偿的比力测量。

7.根据权利要求6所述的系统，其中：

其中所述一个或多个传感器包括比力传感器，所述比力传感器包括三个正交布置的加速计，所述加速计被配置成感测作用在检验质量上的力的和；以及

偏差修正包括加速计偏差修正。

8.根据权利要求1所述的系统，其中所述处理器还被配置成：

将修正的起重机臂的滚动角测量、修正的起重机臂的俯仰角测量和所计算的起重机臂的方向角中的每个转换成四元数；以及

使用所述四元数来估计起重机臂的姿态和方向角。

9.根据权利要求1所述的系统，其中所述多个磁力计包括三个正交布置的磁力计。

10.根据权利要求1所述的系统，还包括：耦合到所述处理器的显示设备，所述处理器还被配置成：

对所述显示设备提供图像渲染显示命令，所述图像渲染显示命令使得所述显示设备能够在其上渲染起重机臂姿态和方向角。

11.根据权利要求1所述的系统，其中耦合到所述处理器的所述一个或多个传感器包括：

一个或多个加速计，每个加速计被配置成感测作用在检验质量上的力并且提供表示该力的比力传感器信号；以及

一个或多个磁力计，每个磁力计被配置成感测至少紧接起重机臂的局部磁场并且提供表示该局部磁场的磁力计信号。

12.根据权利要求11所述的系统，其中耦合到所述处理器的所述一个或多个传感器包括：

一个或多个起重机臂角速度传感器，每个起重机臂角速度传感器被配置成感测起重机臂的角速度并且提供表示该起重机臂的角速度的角速度信号；以及

倾斜计，被配置成感测起重机臂的滚动角和起重机臂的俯仰角并且提供表示该起重机臂的滚动角和起重机臂的俯仰角的倾斜计信号。