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

权利要求书

1.一种确定起重机臂的姿态和方向角的方法，包括如下步骤：

感测起重机臂的角速度；

感测起重机臂的滚动角；

感测起重机臂的俯仰角；

感测作用在起重机臂的一部分上的比力；

感测至少紧接起重机臂的局部磁场；

将感测到的起重机臂的角速度、感测到的起重机臂的滚动角、感测到的起重机臂的俯仰角、感测到的比力、以及感测到的局部磁场提供给处理器；以及

在处理器中：

根据感测到的起重机臂的角速度和感测到的比力来计算起重机臂的平移速度；

使用所计算的起重机臂的平移速度和感测到的起重机臂的角速度来计算起重机臂的加速度；

使用所计算的起重机臂的加速度来从感测到的起重机臂的滚动角和感测到的起重机臂的俯仰角中去除起重机臂的加速度分量，以及由此提供修正的起重机臂的滚动角测量和修正的起重机臂的俯仰角测量；

将校准参数应用于感测到的局部磁场，以由此提供校准的磁场测量；

根据校准的磁场测量来计算起重机臂的方向角；

使用修正的起重机臂的滚动角测量、修正的起重机臂的俯仰角测量、和所计算的起重机臂的方向角来估计起重机臂的姿态和方向角。

2.权利要求1的方法，进一步包括：

计算起重机臂的角速度传感器偏差；

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

3.权利要求2的方法，其中使用修正的起重机臂的角速度测量来计算起重机臂的速度。

4.权利要求2的方法，其中：

使用三个正交布置的速率陀螺仪来感测起重机臂的角速度；

所述速度陀螺仪均位于起重机臂上的位置处；以及

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

5.权利要求1的方法，其中：

通过使用三个正交布置的加速计感测作用在检验质量上的力的和来感测作用在起重机臂上的比力；以及

所述方法进一步包括：

在处理器中计算加速计偏差修正；以及

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

6.权利要求1的方法，进一步包括：

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

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

7.权利要求1的方法，其中滚动角和俯仰角均使用倾斜计和加速计中的一个或多个来被感测。

8.权利要求1的方法，其中使用三个正交布置的磁力计来感测所述至少紧接起重机臂的局部磁场。

9.权利要求1的方法，进一步包括在显示设备上渲染起重机臂的姿态和方向角。

10.一种起重机臂的姿态和方向参考系统，包括：

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

多个比力传感器，其被配置为感测作用在起重机臂上的比力并且提供表示该比力的比力传感器信号；

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

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

处理器，被耦合以接收角速度信号、比力传感器信号、倾斜计信号和磁力计信号，并响应于此而被配置为：

根据感测到的起重机臂的角速度和感测到的比力来计算起重机臂的平移速度；

使用所计算的起重机臂的平移速度和感测到的起重机臂的角速度来计算起重机臂的加速度；

使用所计算的起重机臂的加速度来从感测到的起重机臂的滚动角和感测到的起重机臂的俯仰角中去除起重机臂的加速度分量，以及由此提供修正的起重机臂的滚动角测量和修正的起重机臂的俯仰角测量；

将校准参数应用于感测到的局部磁场，以由此提供校准的磁场测量；

根据校准的磁场测量来计算起重机臂的方向角；以及

使用修正的起重机臂的滚动角测量、修正的起重机臂的俯仰角测量、和所计算的起重机臂的方向角来估计起重机臂的姿态和方向角。