[发明专利]起重机臂的姿态和方向参考系统及方法

权利要求书

1.一种系统，包括：

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

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

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

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

接收倾斜计信号；

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

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

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

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

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

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

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

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

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

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

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

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

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

计算偏差修正；以及

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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