专利名称：移动体姿态角检测设备的制作方法
技术领域：
本发明涉及移动体姿态角检测设备，更具体地，涉及精确地检测例如机器人的移动体的姿态角的技术。
背景技术：
加速度传感器和角速度传感器用在例如机器人的移动体的姿态控制中。当三个正交轴表示为x轴、y轴和z轴，在这三个轴的方向上的加速度由三个加速度传感器检测，在这三个轴的方向上的角速度由三个角速度传感器检测。围绕这些轴的角，即姿态角，通过对来自角速度传感器的输出进行时间积分而获得，从而可以计算俯仰角(pitch angle)、侧倾角(roll angle)和横摆角(yaw angle)。
公开号为JP-A-2004-268730的日本专利申请描述了利用从陀螺仪传感器输出的姿态数据和加速度数据来执行姿态控制的技术。
然而，当角速度传感器(横摆率传感器)的偏移和漂移变大时，因为通过角速度的积分获得姿态角，偏移和漂移值逐渐累积并且最终成为非常大的值。这些值随着时间增加并且最终发散(diverge)。该问题可以通过利用具有小偏移和漂移的角速度传感器来避免，但是这样类型的传感器大、重并且昂贵。
另一方面，可以通过用加速度传感器来检测重力方向和利用此重力方向在合适的时刻修正由角速度传感器获得的姿态角，来修正来自积分的误差的累积。然而，不幸地，当移动体的运动变化快时以及除了重力之外的加速度频繁产生时不能使用该方法。
发明内容
因而本发明提供了一种能够检测移动体的姿态角的设备，其通过简单的结构而无需昂贵的、高精确性的角速度传感器来防止积分误差的累积。
本发明的一个方案涉及移动体姿态角检测设备，其包括检测移动体的角速度的角速度传感器；检测所述移动体的加速度的加速度传感器；角速度姿态角计算装置，用于由所述角速度计算所述移动体的姿态角，作为角速度姿态角；加速度姿态角计算装置，用于由所述加速度计算所述移动体的姿态角，作为加速度姿态角；角速度姿态角低范围分量提取装置，用于提取所述角速度姿态角的低范围分量；加速度姿态角低范围分量提取装置，用于提取所述加速度姿态角的低范围分量；差值计算装置，用于计算所述角速度姿态角的低范围分量和所述加速度姿态角的低范围分量之间的差，作为误差；误差去除装置，用于从所述角速度姿态角去除所述误差；输出装置，用于输出由所述误差去除装置去除了所述误差的角速度姿态角，作为所述移动体的姿态角；及反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度姿态角反馈到所述角速度姿态角计算装置。
在本发明的此方案中，所述角速度姿态角低范围分量提取装置和所述加速度姿态角低范围分量提取装置包括用于sin转换和cos转换所述角速度姿态角的装置；用于提取通过所述sin转换和所述cos转换获得的值的低范围分量的装置；和用于对所述提取的低分量的值执行atan2计算或atan计算的装置。
本发明的另一方案涉及移动体姿态角检测设备，其包括检测移动体的角速度的角速度传感器；检测所述移动体的加速度的加速度传感器；角速度姿态角计算装置，用于从所述角速度计算所述移动体的姿态角，作为角速度姿态角；加速度姿态角计算装置，用于从所述加速度计算所述移动体的姿态角，作为加速度姿态角；差值计算装置，用于计算所述角速度姿态角和所述加速度姿态角之间的差；低范围分量提取装置，用于提取所述差的低范围分量作为误差；误差去除装置，用于从所述角速度姿态角去除所述误差；输出装置，用于输出由所述误差去除装置去除了所述误差的角速度姿态角，作为所述移动体的姿态角；和反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度姿态角反馈到所述角速度姿态角计算装置。
本发明的另一方案涉及移动体姿态角检测设备，其包括检测移动体的角速度的角速度传感器；检测所述移动体的加速度的加速度传感器；用于从所述角速度计算在经过一小段时间ts之后定义小旋转角的小角度矩阵的装置；用于从所述小角度矩阵和被反馈回来的姿态矩阵计算新的角速度系统姿态矩阵的装置；用于从所述加速度计算倾斜角的装置；用于从所述倾斜角计算加速度系统姿态矩阵的装置；角速度系统低范围分量提取装置，用于提取所述角速度系统姿态矩阵的低范围分量；加速度系统低范围分量提取装置，用于提取所述加速度系统姿态矩阵的低范围分量；差值计算装置，用于计算所述角速度系统姿态矩阵的所述低范围分量和所述加速度系统姿态矩阵的所述低范围分量之间的差，作为误差；误差去除装置，用于从所述角速度系统姿态矩阵去除所述误差；输出装置，用于从由所述误差去除装置去除了所述误差的角速度系统姿态矩阵计算姿态角，并且输出所述结果作为所述移动体的姿态角；和反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度系统姿态矩阵反馈到所述用于计算所述新的角速度系统姿态矩阵的装置。
本发明的又一方案涉及移动体姿态角检测设备，其包括检测移动体的角速度的角速度传感器；检测所述移动体的加速度的加速度传感器；用于从所述角速度计算在经过一小段时间ts之后定义小旋转角的小角度四元数的装置；用于从所述小角度四元数和被反馈回来的四元数计算新的角速度系统四元数的装置；用于从所述加速度计算倾斜角的装置；用于从所述倾斜角计算加速度系统四元数的装置；角速度系统低范围分量提取装置，用于提取所述角速度系统四元数的低范围分量；加速度系统低范围分量提取装置，用于提取所述加速度系统四元数的低范围分量；差值计算装置，用于计算所述角速度系统四元数的所述低范围分量与所述加速度系统四元数的所述低范围分量之间的差，作为误差；误差去除装置，用于从所述角速度系统四元数去除所述误差；输出装置，用于从由所述误差去除装置去除了所述误差的角速度系统四元数计算姿态角，并且输出所述结果作为所述移动体的姿态角；和反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度系统四元数反馈到所述用于计算所述新的角速度系统四元数的装置。
在本发明中，角速度姿态角，其为基于由角速度传感器检测到的角速度计算出的姿态角，被提供给低范围分量提取装置。角速度姿态角包括来自积分误差的累积的偏移量。因此，当姿态角的真值(truevalue)被分为高范围分量和低范围分量时，角速度姿态角包括(高范围姿态角真值+低范围姿态角真值+低范围偏移量)。低范围分量提取装置去除高范围分量并且提取(低范围姿态角真值+低范围偏移量)。同时，加速度姿态角，其为基于由加速度传感器检测到的加速度计算出的姿态角，也被提供给低范围分量提取装置。加速度姿态角包括高范围噪声分量和低范围姿态角真值，但是不包括来自积分误差的累积的偏移量。低范围分量提取装置去除高范围分量并且提取低范围姿态角真值。因此，通过将这些低范围分量提取装置的输出提供给差值计算装置，并且计算(低范围姿态角真值+低范围偏移量)-(低范围姿态角真值)，可以仅提取出偏移量(即，误差)。然后通过从角速度姿态角去除所提取的偏移量可以检测到高精确性的姿态角。同样，从角速度姿态角去除所提取的偏移量并且反馈该结果防止积分误差的累积，提高输出响应度，并且使得输出稳定。高范围噪声量包括振动加速度噪声和电噪声分量，以及除伴随测量体移动的重力之外的加速度分量。特别地，来自除伴随测量体移动的重力之外的加速度分量的噪声在具有大运动量的小测量体例如机器人中变成大的噪声分量，这在获得姿态角中是主要障碍。
同样在本发明中，基于由角速度传感器检测到的角速度计算出的角速度系统姿态矩阵被提供给低范围分量提取装置。角速度系统姿态矩阵包括来自积分误差的累积的偏移量。因此，当姿态的真值被分为高范围分量和低范围分量时，角速度系统姿态矩阵包括(高范围姿态真值+低范围姿态真值+低范围偏移量)。低范围分量提取装置去除高范围分量并且提取(低范围姿态真值+低范围偏移量)。同时，基于由加速度传感器检测到的加速度计算出的加速度系统姿态矩阵也被提供给低范围分量提取装置。加速度系统姿态矩阵包括高范围噪声分量、高范围姿态真值分量，以及低范围姿态真值，但是不包括来自积分误差的累积的偏移量。低范围分量提取装置去除高范围分量并且提取低范围姿态真值。因此，通过将这些低范围分量提取装置的输出提供给差值计算装置，并且计算(低范围姿态真值+低范围偏移量)-(低范围姿态真值)，可以仅提取出偏移量(即，误差)。然后通过从角速度系统姿态矩阵去除所提取的偏移量可以检测到高精确性的姿态角，以获得没有任何误差的角速度系统姿态矩阵，然后从此角速度系统姿态矩阵计算姿态角。同样，从角速度系统姿态矩阵去除所提取的偏移量并且反馈该结果防止积分误差的累积，提高输出响应度，并且使得输出稳定。此外，可以通过计算来自角速度系统姿态矩阵的四元数和计算来自加速度系统姿态矩阵的四元数，在所述四元数阶段(stage)提取低范围分量并且计算这两者之间的差值，来相似地提取出偏移量。姿态矩阵和四元数本质是等价的，但是利用四元数减少了计算次数，因而缩短了处理时间。
通过防止积分误差的累积而无需使用大、重并且昂贵的传感器，本发明可以精确地检测移动体的姿态角。
结合附图，通过阅读本发明的优选实施例的如下详细描述将更好地理解本发明的特点、优点以及技术和工业意义，其中 图1为本发明的第一示例性实施例的框图； 图2为本发明的第二示例性实施例的框图； 图3为本发明的第三示例性实施例的框图； 图4为本发明的第四示例性实施例的框图； 图5为本发明的第五示例性实施例的框图； 图6为本发明的第六示例性实施例的框图； 图7为图示了参考坐标系(XYZ)和传感器坐标系(xyz)之间的关系的图； 图8为图示了参考坐标系中的姿态角(侧倾角、俯仰角、横摆角)的图； 图9为图示了在传感器坐标系中随着时间的变化的图； 图10为图示了在传感器坐标系中的小的旋转角的图； 图11为图示了倾斜角的图； 图12为本发明第七示例性实施例的框图； 图13为本发明第八示例性实施例的框图； 图14为本发明第九示例性实施例的框图； 图15为本发明第十示例性实施例的框图； 图16为本发明第十一示例性实施例的框图； 图17为本发明第十二示例性实施例的框图； 图18为本发明第十三示例性实施例的框图； 图19为本发明第十四示例性实施例的框图； 图20为本发明第十五示例性实施例的框图； 图21为图20中的低通滤波器的框图； 图22为本发明第十六示例性实施例的框图； 图23为本发明第十七示例性实施例的框图； 图24为图23中的微分器的框图； 图25为根据本发明的第十七示例性实施例的流程图； 图26为示出了Z轴旋转和姿态角φ之间的关系的图； 图27为示出了姿态角φ在Z轴旋转180±20度的图； 图28为示出了通过了LPF(fc＝中)的用于SCA处理、姿态角φ和旋转角的信号的图； 图29为示出了通过了LPF(fc＝小)的用于SCA处理、姿态角φ和旋转角的信号的图； 图30为示出了姿态角θ和Y轴旋转之间的关系的图； 图31为图示了姿态角θ在Y轴旋转90±20度的图； 图32为示出了通过了LPF(fc＝中)的用于SCA处理、姿态角θ和旋转角的信号的图； 图33为示出了通过了LPF(fc＝小)的用于sin-cos转换、姿态角θ和旋转角的信号的图。
具体实施例方式
本发明的示例性实施例将参照附图，使用诸如机器人作为移动体和传感器单元作为姿态角检测设备，来进行描述。
下面描述本发明的第一示例性实施例。图1为示出了根据本发明的第一示例性实施例的移动体姿态角检测设备的结构的框图。包括角速度传感器10和加速度传感器20的传感器单元1设置在机器人上的预定位置。
角速度传感器10检测在传感器坐标系(即，传感器单元的坐标系)上的角速度。围绕轴的角速度被表示为ωx、ωy和ωz。角速度传感器10将检测到的角速度输出到小角度矩阵计算器12。
此小角度矩阵计算器12计算坐标转换矩阵，用于计算当在参考坐标系XYZ中观察时的传感器坐标系xyz。小角度矩阵在下面描述。小角度矩阵计算器12将计算出的小角度矩阵输出到矩阵加法计算器14。
矩阵加法计算器14通过将从小角度矩阵计算器12输入的由角速度传感器10检测到的当前小角度矩阵，与从输出侧反馈回来的姿态角矩阵相加来执行积分计算。然后矩阵加法计算器14将积分后的矩阵输出到矩阵姿态角计算器16。
矩阵姿态角计算器16从输入的积分后的矩阵来计算姿态角，并且将结果作为角速度姿态角SYA输出到低通滤波器LPF18和减法器32。
低通滤波器(即，角速度系统低通滤波器)18从角速度姿态角SYA提取低范围分量并且将其输出到微分器30。角速度姿态角SYA包括真值的姿态角(姿态角真值)和来自积分误差的偏移量。低通滤波器18从角速度姿态角SYA去除高范围姿态角真值，并且提取低范围姿态角真值和偏移量。
同时，加速度传感器20检测在传感器坐标系xyz中的各轴方向上的加速度。检测到的加速度被表示为Gx、Gy和Gz。然后加速度传感器20将检测到的加速度输出到倾斜角计算器22。
倾斜角计算器22计算参考坐标系XYZ的Z轴和传感器坐标系xyz的各轴之间的角度。所计算出的倾斜角被表示为λx、λy和λz。然后倾斜角计算器22将所计算出的倾斜角输出到加速度矩阵计算器24。
加速度矩阵计算器24利用输入的倾斜角来计算当在参考坐标系观察时关于传感器坐标系的姿态矩阵，并且将所计算的姿态矩阵输出到矩阵姿态角计算器26。
然后，矩阵姿态角计算器26从输入的矩阵来计算姿态角，并且将计算出的姿态角作为加速度姿态角SGA输出到低通滤波器28，与矩阵姿态角计算器16相似。
低通滤波器(即，加速度系统低通滤波器)28为一种滤波器，其中的截止频率和衰减因子与它们在低通滤波器18中设置得相同。低通滤波器28从加速度姿态角SGA提取低范围分量并且将其输出到微分器30。加速度姿态角SGA包括高范围分量中的高范围姿态角伪值和低范围分量中的低范围姿态角真值。在加速度姿态角中没有发生来自积分误差的偏移。因为低通滤波器28的截止频率和衰减因子设置得与它们在低通滤波器18中相同，所以角速度系统的低范围姿态角真值与加速度系统的低范围姿态角真值相同。角速度系统的频率特性也与加速度系统的频率特性相同。因而，通过两个低范围姿态角真值的减法计算只有偏移分量被准确地输出。然而，当与角速度系统和加速度系统产生的姿态角相对应的分量的响应度预先已知时，低通滤波器的截止频率和衰减因子可以调整为与它们的特性相匹配。该方法在适应范围窄而移动特性预先已知时是有效的。
如上所述，来自低通滤波器18的(低范围姿态角真值加上偏移量)，以及来自低通滤波器28的(低范围姿态角真值)被输入到微分器30。然后微分器30计算这两者之间的差值并且提取偏移量(即，偏移量＝(低范围姿态角真值+偏移量)-(低范围姿态角真值))。然后微分器30将通过微分计算获得的偏移量输出到减法器32。
减法器32计算从矩阵姿态角计算器16输入的角速度姿态角SYA和从微分器30输入的偏移量之间的差值，然后去除包括在来自矩阵姿态角计算器16的角速度姿态角SYA中的偏移量。如上所述，来自矩阵姿态角计算器16的角速度姿态角SYA包括姿态角真值(高范围分量+低范围分量)和偏移量。然后减法器32通过减法(即，低范围姿态角真值＝(低范围姿态角真值+偏移量)-(偏移量))提取低范围姿态角真值。然后减法器32将通过减法获得的姿态角真值输出到输出装置34。减法器32还将姿态角真值输出到姿态角矩阵计算器36。
姿态角矩阵计算器36从输入的姿态角来计算姿态角矩阵，并且将其反馈到矩阵加法计算器14。然后姿态角矩阵计算器36执行矩阵姿态角计算器16的反运算(inverse operation)，并且从姿态角生成姿态角矩阵。然后矩阵加法计算器14通过将反馈回来的姿态角矩阵加到基于角速度传感器10新检测到的角速度计算出的小角度矩阵来执行积分计算，然后从角速度计算姿态角。
输出装置34将已经由减法器32去除了偏移量的姿态角输出到外部部件。在机器人的例子中，主处理器(或主处理机)输入由传感器单元检测到的姿态角等，并且对机器人的姿态执行反馈控制。输出装置34响应来自主处理器的命令在预定的时刻将姿态角输出到主处理器。由角速度传感器10检测到的角速度或由加速度传感器检测到的加速度可以提供到输出装置34，并且除了姿态角之外，输出装置34还可以输出角速度或加速度到主处理器。这些要被输出的是由来自主处理器的命令或设置的参数来确定。输出装置34在给定的时刻将所有的角速度、加速度和姿态角输出到主处理器，并且在另一个时刻选择性地输出其中的一个。尽管图中未示出，通过姿态角矩阵计算器36获得的姿态矩阵也可以经由输出装置34输出到机器人。这是当在机器人内执行矩阵计算时的有效方法。
如上所述，因而去除了偏移量的准确的姿态角被检测和输出。因为偏移量是基于由加速度传感器20检测到的在各轴向上的加速度提取出来的，然后从角速度姿态角去除偏移量并且结果被反馈，角速度传感器10不需要是昂贵的、重的并且极其精确的传感器。
此后，将描述在小角度矩阵计算器12、矩阵姿态角计算器16、倾斜角计算器22和矩阵姿态角计算器26中执行的计算。
首先，将描述姿态矩阵。在离散时间n中的姿态矩阵T(n)用于表示参考坐标系XYZ中的传感器坐标系。姿态矩阵T(n)由4×4元素组成，如下面的表达式1所示。
[表达式1]
按照矩阵T(n)的含义，第一列(a，b，c)、第二列(d，e，f)和第三列(g，h，i)分别指的是当从参考坐标系中看时传感器坐标系n的x轴、y轴和z轴的方向矢量。第四列表示参考坐标系中的传感器坐标系n的原点(即，典型地，当有平移时，平移量在第四列中表示)。当原点固定时，表示位置转换的第四列中的第一到第三行的元素设置为0。如图7中所示，传感器坐标系的原点O位于参考坐标系的(0，0，0)，x轴矢量在参考坐标系上具有分量(a，b，c)，y轴矢量在参考坐标系上具有分量(d，e，f)，z轴矢量在参考坐标系上具有分量(g，h，i)。
下面描述用于从姿态角的侧倾角、俯仰角和横摆角获得姿态矩阵T(n)的方法。该方法基于来自减法器32的姿态角通过姿态角矩阵计算器36来计算姿态矩阵。为了表示姿态矩阵T(n)，来自矩阵的旋转变换必须要考虑到旋转轴的顺序。如图8中所示，当使用典型地用于机器人的侧倾角、俯仰角和横摆角时，它们被定义为发生的三种旋转，即，首先是围绕z轴的旋转φ，接下来是在所述旋转之后围绕y轴的旋转θ，最后是在所述旋转之后围绕x轴的旋转ψ(应注意轴的旋转顺序是特定的)。
倾角、俯仰角和横摆角的转换矩阵表示为RPY(φ，θ，ψ)。RPY(φ，θ，ψ)是从左到右相乘的旋转变换矩阵的乘积，如下面的表达式2所示。
[表达式2] RPY(φ，θ，ψ)＝Rot(z，φ)Rot(y，θ)Rot(x，ψ) (2)
表达式3更详细地示出了表达式2。
[表达式3]
当全部写出表达式3时，可以表示为表达式4。
[表达式4]
欧拉角可以用于代替侧倾角、俯仰角和横摆角作为姿态角。用欧拉角，当首先围绕z轴发生旋转φ，接下来是在所述旋转之后围绕y轴发生旋转θ，最后是在所述旋转之后围绕x轴发生旋转ψ时，转换矩阵表示为Euler(Eφ，Eθ，Eψ)，并如表达式5所示。
[表达式5] Euler(Eφ，Eθ，Eψ)＝Rot(z，Eφ)Rot(y，Eθ)Rot(x，Eψ) (5)
表达式5在下面的表达式6中详细示出。
[表达式6]
当全部写出表达式6时，可以表示为表达式7。
[表达式7]
参考坐标系被表示为O-XYZ，并且初始传感器坐标系被表示为O0-x0y0z0。参考坐标系和坐标系O0-x0y0z0在时刻t＝0通过坐标转换A(0)相关联。坐标系在时刻t＝tn被表示为On-xnynzn。每一个坐标系的原点O、O0、On是相同的并且位置没有移动。如图9中所示，当从坐标系O(n-1)-x(n-1)y(n-1)z(n-1)变到On-xnynzn时，由于移动体的姿态的变化，O(n-1)-x(n-1)y(n-1)z(n-1)和On-xnynzn通过从传感器输出获得的矩阵A(n)相关联。当从参考坐标系中看时，通过在表达式8中将转换A(n)从右相乘获得传感器坐标系T(n)。当传感器坐标系的原点随着时间移动时，将已经随着时间移动的坐标连续地输入，作为矩阵A的第四列的第一到第三个元素。矩阵A中的第四列不受传感器坐标系的旋转的影响，所以在此不作特别的描述。
[表达式8] A(n)＝A(0)A(1)............A(n-1)A(n) (8)
接下来，描述用于引入(introduce)来自传感器输出的小旋转矩阵A(n)的矩阵的方法。此方法利用小角度矩阵计算器12，基于来自角速度传感器10的角速度来计算小角度矩阵。在传感器坐标系的每个轴上设置三个角速度传感器中的一个。如图10中所示，三个角速度传感器测量围绕传感器的轴x、y和z的角速度。当在表达式4中旋转角Δφ、Δθ、Δψ足够小时，可得到如下表达式。
[表达式9] sinΔφ≈Δφ，sinΔθ≈Δθ，sinΔψ≈Δψ(9) [表达式10] cosΔφ≈1，cosΔθ≈1，cosΔψ≈1 (10) 因此，这可以利用围绕传感器x轴的小旋转角Δψ、围绕传感器y轴的小旋转角Δθ以及围绕传感器z轴的小旋转角Δφ通过表达式11来表示。表达式11的结果类似于不取决于旋转顺序的情况，因为矩阵中的每一个元素由小旋转角独立的表示。
[表达式11]
如表达式12和13所示，小角度和传感器输出通过小旋转角Δψ、Δθ、Δφ，角速度传感器输出ωx、ωy、ωz，以及采样周期ts相关联。采样周期ts相对于旋转运动足够早，所以在采样周期ts的时间内的旋转足够小，因此被认为是小旋转角。
[表达式12] Δψ＝ωxts(12) [表达式13] Δθ＝ωyts(13) [表达式14] Δφ＝ωzts(14)
因此，A(n)矩阵可以通过表达式15来表示。
[表达式15]
接下来，描述用于从姿态矩阵获得姿态角的方法。该方法利用矩阵姿态角计算器16，基于来自矩阵加法计算器14的姿态矩阵(即，积分后的姿态矩阵)来计算姿态角。该方法还利用矩阵姿态角计算器26，基于来自加速度矩阵计算器24的姿态矩阵来计算姿态角。
姿态矩阵T(n)如表达式16所示。
[表达式16]
横摆角φ为 [表达式17] φ＝atan2(b，a) (17) 并且姿态角φ的变化范围为-πφ≤π。
俯仰角θ为 [表达式18] θ＝atan2(-c，cosφ·a+sinφb) (18) 并且姿态角θ的变化范围为-π/2＜θ＜π/2。
侧倾角ψ为 [表达式19] ψ＝atan2(sinφ·g-cosφ·h，-sinφ·d+cosφ·e)(19) 并且姿态角ψ的变化范围为-π＜ψ≤π。
如果使用欧拉角，则使用表达式21到23。
[表达式20] Eφ为 [表达式21] Eφ＝atan2(b，a) (21) Eθ为 [表达式22] Eθ＝atan2(cosφ·g+sinφ·h，i) (22) 并且Eψ为 [表达式23] Eψ＝atan2(-sinφ·a+cosφ·b，-sinφ·d+cosφ·e)(23)
接下来，描述矩阵的正规化(normalization)。
[表达式24]
用姿态矩阵T(n)，在计算后，姿态矩阵中的每一列可能不是单位矢量，所以通过表达式25执行正规化以使得表达式24中的每一列矢量的大小成为1。这是通过图5中的正规器(normalizer)/正交器(orthogonalizer)54和56执行的计算，在后面进行描述。
[表达式25]
这里，p1和p2由表达式26和27提供。
[表达式26] [表达式27]
然后，正规化后的每一个元素被再次重置为 [表达式28]
此外，下面描述矩阵的正交化(orthogonalization)。
[表达式29]
对于姿态矩阵T(n)，在计算后，姿态矩阵中的列可能不是正交轴，所以执行使得表达式29中的列矢量正交化的正交化过程(在该例子中，z轴用作参考)。为了获得与z轴和y轴正交的新的x’轴，要获得a’、b’和c’。
[表达式30] a’＝ei-fh(30) [表达式31] b’＝fg-di(31) [表达式32] c’＝dh-eg(32)
接下来，为了获得与z轴和x’轴正交的新的y’轴，要获得d’、e’和f’。
[表达式33] d’＝hc’-ib’(33) [表达式34] e’＝ia’-gc’(34) [表达式35] f’＝gb’-ha’ (35)
然后从获得的a’到f’，来获得已经正交化的姿态矩阵T(n)。
[表达式36]
这里，描述atan2。atan2(y，x)是具有两个变量x和y的计算器函数。atan2的应用范围比通常使用的atan函数广。
[表达式37] ξ＝atan2(y，x)(37) 当x＞0并且y＞0时，-π＜ξ≤π [表达式38] ξ＝tan-1(y/x) (38) 以及当x＞0并且y＜0时， [表达式39] ξ＝tan-1(y/x) (39) 以相同方式， 当x＜0并且y＞0时，则ξ＝π+tan-1(y/x)， 当x＜0并且y＜0时，则ξ＝-π+tan-1(y/x)， 当x＝0并且y＞0时，则ξ＝π/2， 当x＝0并且y＜0时，则ξ＝-π/2， 以及当x＝0并且y＝0时，则ξ＝0。
接下来，描述对倾斜角的计算。该方法利用倾斜角计算器22，基于通过加速度传感器20获得的加速度来计算倾斜角。倾斜角指的是在传感器x、y、z轴和参考Z轴之间的角λx、λy、λz。即，λx是x轴和Z轴之间的角，λy是y轴和Z轴之间的角，λz是z轴和Z轴之间的角。λx、λy和λz的范围为0≤(λx，λy，λz)≤π。图11示出了倾斜角和重力矢量。
倾斜角如下面所示从设置在传感器坐标上的加速度传感器获得。利用表达式40到42来正规化加速度Gx、Gy和Gz，并且获得正规化后的加速度Gx’、Gy’和Gz’。
[表达式40] [表达式41] [表达式42]
利用表达式43到45从加速度Gx’、Gy’和Gz’获得倾斜角λx、λy和λz。
[表达式43] λx＝acos(-Gx’)(43) [表达式44] λy＝acos(-Gy’)(44) [表达式45] λz＝acos(-Gz’)(45)
接下来，描述从倾斜角λx、λy和λz获得姿态矩阵T(n)的过程。这是利用加速度矩阵计算器24，基于来自倾斜角计算器22的倾斜角来获得姿态矩阵的计算。
[表达式46] c＝cos(λx) (46) [表达式47] [表达式48] b＝0(48) [表达式49] f＝cos(λy) (49) [表达式50] d＝-cf/a (50) [表达式51]当0≤λz＜π/2时(51) [表达式52]当π/2＜λz≤π时 (52) [表达式53] e＝0当λz＝π/2时(53) [表达式54] g＝-ce (54) [表达式55] h＝cd-af (55) [表达式56] i＝ae(56) 从上述结果获得姿态矩阵T(n)。
接下来，描述用于从姿态矩阵T(n)获得倾斜角λx、λy和λz的过程。
[表达式57] [表达式58] λx＝acos(c)(58) [表达式59] λy＝acos(f)(59) [表达式60] λz＝acos(i)(60)
下面继续描述第二示例性实施例。图2为本发明的第二示例性实施例的框图。此示例性实施例与第一示例性实施例的不同在于，微分器30计算低通滤波器18的输出和低通滤波器28的输出之间的差值，并且将其输出到乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42执行PID(即，比例积分微分)控制。即，乘法器38执行将来自微分器30的偏移量与预先设置的系数相乘的计算，并且将计算值(比例项)输出到减法器32。积分器40执行将来自微分器30的偏移量随着时间累积的值与预先设置的系数相乘的计算，并且将计算值(积分项)输出到减法器32。微分器42执行来自微分器30的偏移量随着时间微分的值与预先设置的系数相乘的计算，并且将计算值(微分项)输出到减法器32。所述系数可以是固定的或者它们也可以如下所述是可变的。减法器32从由矩阵姿态角计算器16输入的角速度姿态角减去这些项(即，比例项+积分项+微分项)的和，并且将结果作为减去偏移量的移动体的姿态角输出到输出装置34和姿态角矩阵计算器36。减法器32的输出由姿态角矩阵计算器36转换为姿态矩阵然后被反馈。乘法器38主要确定反馈程度，积分器40累积长期偏离的量并且确定反馈量，微分器42确定有关短期变化的反馈量。利用此类型的PID控制短期和长期特性均得以改善。
接着，描述第三示例性实施例。图3为本发明第三示例性实施例的框图。此示例性实施例与第二示例性实施例的不同在于，来自外部姿态角44的绕Z轴的姿态角，即横摆角，被提供到矩阵姿态角计算器26。加速度传感器20检测在轴x、y和z方向上的加速度。然而，当使用了G传感器时，横摆角φ，即绕Z轴的姿态角不确定，所以需要将横摆角φ固定为恒定值。在此示例性实施例中，横摆角被单独检测并且被提供给矩阵姿态角计算器26，因此使得也可以准确地计算横摆角φ。更具体地，根据如上所述的计算，矩阵姿态角计算器26计算横摆角φ、侧倾角θ和俯仰角ψ，作为来自姿态矩阵T(n)的姿态角。然而，这些中的横摆角φ用从外部部件输入的横摆角φ代替，这防止了横摆角φ的发散。
接着，下面描述第四示例性实施例。图4为本发明第四示例性实施例的框图。在前述的各示例性实施例中，来自矩阵姿态角计算器16的角速度姿态角输出到低通滤波器18，来自矩阵姿态角计算器26的加速度姿态角输出到低通滤波器28。然而，在第四示例性实施例中，来自矩阵加法计算器14的姿态矩阵输出到低通滤波器(ALPF)46，来自加速度矩阵计算器24的姿态矩阵输出到低通滤波器(ALPF)48，并且低范围分量在矩阵阶段而不是在姿态角阶段提取。低通滤波器46和48适合4×4姿态矩阵中的每个矩阵分量并且提取相应的低范围分量。低通滤波器46和48的截止频率设置为相同，衰减因子也相同。在从姿态矩阵提取低范围分量之后，低通滤波器(角速度系统)46将姿态矩阵输出到矩阵姿态角计算器50。与矩阵姿态角计算器16相似，矩阵姿态角计算器50从姿态矩阵计算角速度姿态角并且将其输出到微分器30。低通滤波器46和48应用到表示旋转的分量，即4×4姿态矩阵中的3×3矩阵分量的第一列(a，b，c)、第二列(e，d，f)和第三列(g，h，i)，因此使得容易提取相应的低范围分量。还有，低通滤波器可以应用到全部六个分量，即从3×3矩阵分量中，在一侧上的三个相对的角分量，以及在另一侧的三个非对角线分量，并且可以产生等价的矩阵滤波器。
同样，在从姿态矩阵提取低范围分量之后，低通滤波器(加速度系统)48将姿态矩阵输出到矩阵姿态角计算器26。与第三示例性实施例相似，来自外部姿态角44的绕Z轴的姿态角，即横摆角，可被提供到加速度系统。然而，在第三示例性实施例中，由外部传感器检测到的横摆角φ被提供到矩阵姿态角计算器26，但是在第四示例性实施例中，横摆角φ被提供到加速度矩阵计算器24。当然这样做是为了使用低通滤波器48来提取姿态矩阵的低范围分量，也是因为需要包括由外部传感器检测到的横摆角φ作为姿态矩阵的分量。
接着，下面描述第五示例性实施例。图5为本发明第五示例性实施例的框图。第五示例性实施例与第四示例性实施例的不同在于，在低通滤波器46和48之后增加正规器/正交器54和56。当通过低通滤波器46和48从姿态矩阵提取低范围分量时，姿态矩阵变形使得正规化和正交化不再能被保证。因此利用正规器/正交器54和56通过正规化和正交化来修正该变形。正规化和正交化的方法如前所述。
接着，下面描述第六示例性实施例。图6为本发明第六示例性实施例的框图。该示例性实施例与第四示例性实施例的不同在于，设置了运动频率传感器52，其输入来自角速度传感器10的角速度和来自加速度传感器20的加速度，并且检测在机器人，即移动物体的运动中的特定频率。运动频率传感器52从角速度和加速度中提取经常出现的频率分量，并且将其输出到低通滤波器46和48。一个例子为当机器人，即移动物体用两条腿行走时的两足移动中的特定频率。低通滤波器46和48根据从运动频率传感器52输入的频率来适应性地调整截止频率。运动频率越大，低通滤波器46和48越要增大截止频率。此外，运动频率传感器52将运动频率输出到执行PID控制的乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42根据输入的运动频率调整乘法器系数。根据运动中的特定频率来适应性地调整截止频率和PID控制的系数，以此方式提高响应度，以及跟随和适应的能力。
接着，描述第七示例性实施例。图12为根据本发明第七示例性实施例的移动体姿态角检测设备的框图。
角速度传感器10检测传感器坐标系(即，传感器单元的坐标系)中的角速度。围绕轴的角速度被表示为ωx、ωy和ωz。角速度传感器10将检测到的角速度输出到小角度矩阵计算器12。
小角度矩阵计算器12计算用于计算当从参考坐标系XYZ中看时的传感器坐标系xyz的坐标转换矩阵。小角度矩阵在下面描述。小角度矩阵计算器12将计算出的小角度矩阵输出到矩阵加法计算器14。
矩阵加法计算器14通过将从小角度矩阵计算器12输入的由角速度传感器10检测到的当前小角度矩阵加到从输出侧反馈回来的姿态矩阵来执行积分计算。然后矩阵加法计算器14将积分后的矩阵(即，角速度系统姿态矩阵)输出到低通滤波器(ALPF)46。矩阵加法计算器14还将积分后的矩阵输出到减法器32。
低通滤波器(即，角速度系统低通滤波器)46从角速度系统姿态矩阵的分量中提取低范围分量并且将其输出到微分器30。角速度系统姿态矩阵包括真姿态(即，姿态真值)和来自积分误差的偏移量。低通滤波器46从角速度系统姿态矩阵中去除高范围姿态真值，并且提取低范围姿态真值和偏移量。
同时，加速度传感器20检测在传感器坐标系xyz中的每个轴的方向上的加速度。检测到的加速度被表示为Gx、Gy和Gz。加速度传感器20将检测到的加速度输出到倾斜角计算器22。
倾斜角计算器22计算在传感器坐标系xyz的每个轴和参考坐标系XYZ中的Z轴之间的角度。计算出的倾斜角度被表示为λx、λy和λz。然后倾斜角计算器22将计算出的倾斜角输出到加速度矩阵计算器24。
加速度矩阵计算器24利用输入的倾斜角，来计算当在参考坐标系中看时有关传感器坐标系的姿态矩阵，并且将此结果作为加速度系统姿态矩阵输出到低通滤波器(ALPF)48。
低通滤波器(即，加速度系统低通滤波器)48为截止频率和衰减因子设置得与其在低通滤波器46中相同的滤波器。低通滤波器48从加速度系统姿态矩阵中提取低范围分量并且将其输出到微分器30。加速度系统姿态矩阵包括产生在高范围中的噪声、高范围姿态真值和低范围姿态真值。在加速度系统姿态矩阵中没有发生来自积分误差的漂移。因为低通滤波器48的截止频率和衰减因子设置得与其在低通滤波器46中相同，角速度系统的低范围姿态角真值与加速度系统的低范围姿态角真值相同。角速度系统的频率特性也与加速度系统的频率特性相同。因此，通过两个低范围姿态角度真值的减法运算，只有漂移分量被准确地输出。然而，当与由角速度系统和加速度系统产生的姿态相对应的分量的响应预先已知时，可以调整低通滤波器的截止频率和衰减因子来匹配它们的特性。当适应范围窄但是移动特性预先已知时此方法有效。
如上所述，来自低通滤波器46的(低范围姿态真值加上漂移分量)，以及来自低通滤波器48的(低范围姿态真值)输入进微分器30。然后微分器30计算这两者之间的差值，并且提取偏移量(即，偏移量＝(低范围姿态真值+偏移量)-(低范围姿态真值))。然后微分器30将通过微分计算获得的偏移量输出到减法器32。
减法器32计算从矩阵加法计算器14输入的角速度系统姿态和从微分器30输入的偏移量之间的差值(即，矩阵差值)，然后去除包括在来自矩阵加法计算器14的角速度系统姿态矩阵中的偏移量。如上所述，来自矩阵加法计算器14的角速度系统姿态矩阵包括姿态真值和偏移量。然后减法器32通过减法来提取姿态真值(即，姿态真值＝(姿态真值+偏移量)-(偏移量))。然后减法器32将用减法获得的姿态真值的角速度系统姿态矩阵输出到矩阵姿态角计算器37。矩阵姿态角计算器37计算来自姿态矩阵的姿态角并且将其作为移动体的姿态角输出到输出装置34。减法器32还将减去偏移量的姿态矩阵反馈到矩阵加法计算器14。然后矩阵加法计算器14通过将反馈的姿态矩阵加到基于由角速度传感器10新检测到的角速度而计算出的小角度矩阵，来执行积分计算，并且从角速度计算新的角速度系统姿态矩阵。
输出装置34将来自矩阵姿态角计算器37的减去偏移量的姿态角输出到外部部件。在机器人的例子中，主处理器(主机处理器)输入由传感器单元检测到的姿态角等，并且对机器人的姿态执行反馈控制。输出装置34响应来自主处理器的命令在预定时间将姿态角输出到主处理器。由角速度传感器10检测到的角速度或由加速度传感器检测到的加速度可以被提供到输出装置34，并且除了姿态角之外，输出装置34还可以将角速度或加速度输出到主处理器。这些中哪个将被输出是由来自主处理器的命令或设置的参数确定的。输出装置34在给定的时间将所有角速度、加速度和姿态角输出到主处理器，并且在其他时刻选择性地输出这些中的一个。
如上所述，因此检测并且输出了去除了偏移量的准确的姿态角。因为基于由加速度传感器20检测到的每个轴的方向上的加速度来提取偏移量，然后从角速度姿态中去除偏移量并且反馈结果，角速度传感器10不需要是很贵、很重和特别精确的传感器。
接着，下面描述第八示例性实施例。图13是本发明的第八示例性实施例的框图。此示例性实施例与第七示例性实施例的不同在于，微分器30计算低通滤波器46的输出和低通滤波器48的输出之间的差值并且将结果输出到乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42执行PID控制。即，乘法器38执行将来自微分器30的偏移量与预先设置的系数相乘的计算，并将计算的值(比例项)输出到减法器32。积分器40执行将来自微分器30的偏移量随着时间累积的值与预先设置的系数相乘的计算，并将计算的值(积分项)输出到减法器32。微分器42执行将来自微分器30的偏移量随着时间的微分的值与预先设定的系数相乘的计算，并且将计算的值(微分项)输出到减法器32。对偏移量的矩阵的每个分量执行乘法计算、积分计算和微分计算。系数可以是固定的或者它们可以如下面所述是可变的。减法器32从由矩阵加法计算器14输入的角速度系统姿态矩阵中减去这些项的和(即，比例项+积分项+微分项)，来计算减去偏移量的角速度系统姿态矩阵，并且将其输出到矩阵姿态角计算器37。来自减法器32的输出被反馈到矩阵加法计算器14。乘法器38主要确定反馈度，积分器40累积长期偏移的量并且确定反馈量，微分器42确定有关短期变化的反馈量。利用此类型的PID控制短期和长期特性均得以改善。
此外，在图13中，绕Z轴的姿态角，即横摆角φ，从外部姿态角44提供到加速度矩阵计算器24。加速度传感器20检测在每个轴x、y和z方向上的加速度。然而，当使用了G传感器时，横摆角φ，即绕Z轴的姿态角不确定，所以需要将横摆角φ固定为恒定值(例如0)。在此示例性实施例中，横摆角被单独检测并且被提供到加速度矩阵计算器24，这还防止了横摆角φ的发散。
接着，描述第九示例性实施例。图14为本发明第九示例性实施例的框图。第九示例性实施例与第八示例性实施例的不同在于，在低通滤波器46和48之后增加正规器/正交器54和56。当通过低通滤波器46和48从姿态矩阵提取低范围分量时，姿态矩阵变形使得正规化和正交化不再能被保证。因此利用正规器/正交器54和56通过正规化和正交化来修正该变形。正规化和正交化的方法如前所述。低通滤波器46和48被应用到表示旋转的分量，即4×4姿态矩阵中的3×3矩阵分量的第一列(a，b，c)、第二列(d，e，f)和第三列(g，h，i)，因此使得容易提取相应的低范围分量。还有，低通滤波器可以应用到全部六个分量，即从3×3矩阵分量中，在一侧上的三个相对的角分量，以及在另一侧的三个非对角线分量，并且可以产生等价的矩阵滤波器。
接着，描述第十示例性实施例。图15为本发明第十示例性实施例的框图。第十实施例与第八实施例的不同在于，设置了运动频率传感器52，其输入来自角速度传感器10的角速度和来自加速度传感器20的加速度，并且检测在机器人，即移动物体的运动中的特定频率。运动频率传感器52从角速度和加速度中提取经常出现的频率分量，并且将其输出到低通滤波器46和48。一个例子为当机器人，即移动物体用两条腿行走时的两足移动中的特定频率。低通滤波器46和48根据从运动频率传感器52输入的频率来适应性地调整截止频率。运动频率越大，低通滤波器46和48越要增大截止频率。此外，运动频率传感器52将运动频率输出到执行PID控制的乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42根据输入的运动频率调整乘法器系数。根据运动中的特定频率来适应性地调整截止频率和PID控制的系数，以此方式提高响应度，以及跟随和适应的能力。
接着，描述第十一示例性实施例。图16是本发明的第十一示例性实施例的框图。前述示例性实施例的每个都利用了姿态矩阵。然而，此示例性实施例利用四元数代替姿态矩阵。小角度四元数计算器13利用从角速度传感器10获得的小角度来产生小角度四元数。利用此小角度四元数，小旋转被应用到反馈回来的四元数，而且通过四元数积分计算器15获得此旋转后的四元数。然后此积分的四元数被输出到减法器32。输出到减法器32的相同的输出还被输入到四元数低通滤波器63。此四元数低通滤波器63提取姿态的低范围分量。在倾斜角计算器22获得此倾斜角后，加速度四元数计算器25利用来自加速度传感器20的输出来获得加速度四元数。然后此加速度四元数被输入到四元数低通滤波器67。四元数低通滤波器67提取姿态的低范围分量。微分器30计算角速度系统四元数的低范围分量和加速度系统四元数的低范围分量之间的差值，并且提取包括在角速度四元数中的偏移量。然后微分器30将该偏移量输出到减法器32。
减法器32计算从四元数积分计算器15输入的角速度系统四元数和来自从微分器30的偏移量之间的差值，并且从角速度系统四元数中去除偏移量。然后减法器32将去除了偏移量的角速度系统四元数输出到四元数积分计算器15和四元数姿态角计算器70。
因为反馈的四元数为去除了偏移量的四元数，所以可以防止积分误差的累积。四元数姿态角计算器70从输入的四元数来计算姿态角，并且将结果输出到输出装置34。
以下，将描述计算四元数的方法和从所述四元数计算姿态角的方法。
首先，描述四元数。四元数是具有二维自由度的多个素数被扩展为具有四维自由度。四元数典型地表示为 q＝a+bi+cj+dk 这里，a、b、c和d为实数，i、j和k为基(大小＝1)。基的乘积具有如下关系。
i×l＝l×i＝i j×l＝l×j＝j k×l＝l×k＝k i×j＝-j×i＝k j×k＝-k×j＝i k×i＝-i×k＝j
此外，关于二个四元数q1＝a1+b1i+c1j+d1k和q2＝a2+b2i+c2j+d2k，和与差为 q1+q2＝a1+a2+(b1+b2)i+(c1+c2)j+(d1+d2)k q1-q2＝a1-a2+(b1-b2)i+(c1-c2)j+(d1-d2)k 乘积为 q1q2＝(a1+b1i+c1j+d1k)×(a2+b2i+c2j+d2k)＝(a1a2-b1b2-c1c2-d1d2)+(a1b2+b1a2+c1d2-d1c2)i+(a1c2-b1d2+c1a2+d1b2)j+(a1d2+b1c2-c1b2+d1a2)k
利用虚数单元(unit)i、j和k来如下表示四元数。
P＝(a；xi，yj，zk)
利用四元数Q，当在参考坐标XYZ中看时传感器坐标系xyz的关系，即，姿态为 Q＝(cosη/2；αsinη/2，βsinη/2，γsinη/2) 这示出了一坐标系(传感器坐标系)，其中参考坐标系围绕由参考坐标系表示的旋转轴(α，β，γ)旋转角度η，并且表示了传感器坐标系的姿态。
下面，描述角速度传感器输出的四元数的表示。根据小旋转角Δψ、Δθ和Δφ，角速度传感器输出ωx、ωy和ωz，以及采样周期ts，旋转角和传感器输出之间的关系如下。
Δψ＝ωx×ts Δθ＝ωy×ts Δφ＝ωz×ts 当用四元数表示此时的旋转变换时，我们得到 qx＝(cosΔψ/2；sinΔψ/2i，0j，0k) qy＝(cosΔθ/2；0i，sinΔθ/2j，0k) qz＝(cosΔφ/2；0i，0j，sinΔφ/2k) 绕x轴、y轴和z轴的旋转变换一起被表示。当旋转角α足够小时，我们得到 qxqyqz＝(1+ΔψΔθΔφ/8；(Δψ/2-ΔθΔφ/4)i，(Δθ/2+ΔψΔφ/4)j，(Δφ/2-ΔψΔθ/4)k)
下面，描述来自四元数的旋转变换的积分。当旋转变换q1为 q1＝(cosη1/2；αsinη1/2，βsinη1/2，γsinη1/2)时，则旋转变换q2为 q2＝(cosη2/2；αsinη2/2，βsinη2/2，γsinη2/2)。
首先，旋转变换q1执行于参考坐标系，然后旋转变换q2执行于该旋转之后的坐标系。来自两个旋转变换的变换q3为 q3＝q1×q2 ＝(cosη1/2；αsinη1/2，βsinη1/2，γsinη1/2)×(cosη2/2；αsinη2/2，βsinη2/2，γsinη2/2)
下面，描述四元数的正规化。用表示旋转的四元数，在计算后，执行正规化来使得变换q的大小(范数)等于1。当表示旋转的四元数用q来表示时，获得q的大小然后通过将q除以其大小来执行正规化，因而获得q’。
q＝(ε0；ε1，ε2，ε3) |q|＝(ε02+ε12+ε22+ε32)0.5 q’＝q/|q| 四元数是保持正交条件的表示方法，所以不需要进一步的正交化。
下面，描述从四元数到姿态角的变换。当 a＝ε02+ε12-ε22-ε32 b＝2(ε1ε2+ε0ε3) c＝2(ε1ε3-ε0ε2) d＝2(ε1ε2-ε0ε3) e＝ε02-ε12+ε22-ε32 f＝2(ε2ε3+ε0ε1) g＝2(ε1ε3+ε0ε2) h＝2(ε2ε3-ε0ε1) i＝ε02-ε12-ε22+ε32 在用四元数表示的变换q＝(ε0；ε1，ε2，ε3)中，通过如下表达式获得姿态角。
横摆角φ＝atan2(b，a) 俯仰角θ＝atan2(-c，cosφ·a+sinφ·b) 侧倾角ψ＝atan2(sinφ·g-cosφ·h，-sinφ·d+cosφ·e)
下面，描述从姿态角(侧倾角、俯仰角、横摆角)到四元数的变换。从姿态角(ψ，θ，φ)的旋转变换用四元数表示。
绕x轴的旋转为 R(ψ；i)＝cosψ/2+isinψ/2 绕y轴的旋转为 R(θ；j)＝cosθ/2+jsinθ/2 绕z轴的旋转为 R(φ；k)＝cosφ/2+ksinφ/2 当把它们乘到一起，我们得到 S＝R(φ；k)×R(θ；j)×R(ψ；i) ＝(cosφ/2+ksinφ/2)×cosθ/2+jsinθ/2)×(cosψ/2+isinψ/2)
如果用四元数表示的变换q在此时表示为q＝(ε0；ε1，ε2，ε3)，则旋转角成为S的计算结果的实部，而旋转轴成为S的虚部。
ε0＝cosφ/2·cosθ/2·cosψ/2+sinφ/2×sinθ/2·sinψ/2 ε1＝-sinφ/2·sinθ/2·cosψ/2+cosφ/2·cosθ/2·sinψ/2 ε2＝sinφ/2·cosθ/2·sinψ/2+cosφ/2·sinθ/2·cosψ/2 ε3＝sinφ/2·cosθ/2·cosψ/2-cosφ/2·sinθ/2·sinψ/2
此时，从cosα/2＝ε0获得旋转角α。此外，旋转角为(ε1，ε2，ε3)。
接着，下面描述本发明的第十二示例性实施例。图17为本发明的第十二示例性实施例的框图。此示例性实施例与第十一示例性实施例的不同在于，设置了乘法器38、积分器40和微分器42用于执行PID控制，并且由外部姿态角44检测到的横摆角被提供到加速度四元数计算器25。所述结构与图13中所示的相对应。乘法器38执行将来自微分器30的偏移量(四元数)与预先设置的系数相乘的计算，并将计算的值(比例项)输出到减法器32。积分器40执行将来自微分器30的偏移量(四元数)随着时间积累的值与预先设置的系数相乘的计算，并将计算的值(积分项)输出到减法器32。微分器42执行将来自微分器30的偏移量(四元数)随着时间微分的值与预先设定的系数相乘的计算，并且将计算的值(微分项)输出到减法器32。系数可以是固定的或者它们可以是可变的。减法器32计算输入的角速度系统四元数与这些项的和(即，比例项+积分项+微分项)之间的差值，计算去除了偏移量的角速度系统四元数，并且将结果输出到四元数姿态角计算器70。
接着，描述第十三示例性实施例。图18为本发明的第十三示例性实施例的框图。此示例性实施例与第十二示例性实施例的不同在于，在低通滤波器63和67之后设置正规器72和74。此结构与图14中所示的相对应。角速度系统四元数和加速度系统四元数分别用低通滤波器63和67处理。然而，此处理引起变形以致它们可能变得反正规化(denormalized)。因此姿态角的检测精确性可以通过利用正规器72和74修正该变形来维持。
接着，描述第十四示例性实施例。图19为本发明的第十四示例性实施例的框图。此示例性实施例与第十二示例性实施例的不同在于，设置了运动频率传感器52，其输入来自角速度传感器10的角速度和来自加速度传感器20的加速度，并且检测在机器人，即移动物体的运动中的特定频率。此结构与图15中所示的相对应。运动频率传感器52从角速度和加速度中提取经常出现的频率分量，并且将它们输出到低通滤波器63和67。低通滤波器63和67根据从运动频率传感器52输入的频率来适应性地调整截止频率。运动频率越大，低通滤波器63和67越要增大截止频率。此外，运动频率传感器52将运动频率输出到执行PID控制的乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42根据输入的运动频率调整乘法器系数。根据运动中的特定频率来适应性地调整截止频率和PID控制的系数，以此方式提高响应度，以及跟随和适应的能力
接着，描述第十五示例性实施例。图20为根据本发明的第十五示例性实施例的移动体姿态角检测设备的框图。此示例性实施例与第一示例性实施例的不同在于，低通滤波器18和28执行sin转换，cos转换，以及atan2转换(即，它们均执行SCA转换)。
角速度传感器10检测传感器坐标系(即，传感器单元的坐标系)中的角速度。绕轴的角速度被表示为ωx、ωy和ωz。角速度传感器10将检测到的角速度输出到小角度矩阵计算器12。
小角度矩阵计算器12计算用于计算当在参考坐标系XYZ中看时的传感器坐标系xyz的坐标转换矩阵。小角度矩阵在下面描述。小角度矩阵计算器12将计算的小角度矩阵输出到矩阵加法计算器14。
矩阵加法计算器14通过将从小角度矩阵计算器12输入的由角速度传感器10检测到的当前小角度矩阵加到从输出侧反馈回来的姿态角矩阵来执行积分计算。然后矩阵加法计算器14将积分后的矩阵输出到矩阵姿态角计算器16。
矩阵姿态角计算器16从输入的积分后的矩阵计算姿态角，并且将此结果作为角速度姿态角SYA输出到低通滤波器LPF18和减法器32。
低通滤波器(即，角速度系统低通滤波器)18从角速度姿态角SYA提取低范围分量并且将其输出到微分器30。角速度姿态角SYA包括真值的姿态角(姿态角真值)和来自积分误差的偏移量。低通滤波器18将高范围姿态角真值从角速度姿态角SYA中去除，并且提取低范围姿态角真值和偏移量。
同时，加速度传感器20检测在传感器坐标系xyz中的每个轴的方向上的加速度。检测到的加速度被表示为Gx、Gy和Gz。然后加速度传感器20将检测到的加速度输出到倾斜角计算器22。
倾斜角计算器22计算参考坐标系XYZ的Z轴和传感器坐标系xyz的每个轴之间的角度。计算出的倾斜角被表示为λx、λy和λz。然后倾斜角计算器22将计算出的倾斜角输出到加速度矩阵计算器24。
加速度矩阵计算器24利用输入的倾斜角，来计算当在参考坐标系中看时的传感器坐标系，并且将结果输出到矩阵姿态角计算器26。
与矩阵姿态角计算器16相似，矩阵姿态角计算器26然后从输入的矩阵来计算姿态角，并且将计算出的姿态角作为加速度姿态角SGA输出到低通滤波器28。
低通滤波器(即，加速度系统低通滤波器)28为截止频率和衰减因子设置得与它们在低通滤波器18中相同的滤波器。低通滤波器28从加速度姿态角SGA中提取低范围分量并且将其输出到微分器30。加速度姿态角SGA包括在高范围分量中的高范围姿态角伪值和在低范围分量中的低范围姿态角真值。在加速度姿态角中没有发生来自积分误差的偏移。因为低通滤波器28中的截止频率和衰减因子设置得与它们在低通滤波器18中相同，角速度系统的低范围姿态角真值与加速度系统的低范围姿态角真值相同。角速度系统的频率特性也与加速度系统的频率特性相同。因此，通过两个低范围姿态角真值的相减计算，只有偏移分量被准确地输出。然而，当与由角速度系统和加速度系统产生的姿态角相对应的分量的响应预先已知时，可以调整低通滤波器的截止频率和衰减因子来匹配它们的特性。当适应范围窄但是移动特性预先已知时此方法有效。
如上所述，来自低通滤波器18的(低范围姿态角真值加上偏移分量)，以及来自低通滤波器28的(低范围姿态角真值)被输入到微分器30。然后微分器30计算这两者之间的差值并且提取偏移量(即，偏移量＝(低范围姿态角真值+偏移量)-(低范围姿态角真值))。然后微分器30将通过微分计算获得的偏移量输出到减法器32。
减法器32计算从矩阵姿态角计算器16输入的角速度姿态角SYA和从微分器30输入的偏移量之间的差值，然后去除包括在来自矩阵姿态角计算器16的角速度姿态角SYA中的偏移量。如上所述，来自矩阵姿态角计算器16的角速度姿态角SYA包括姿态角真值(高范围分量+低范围分量)和偏移量。然后减法器32通过减法来提取低范围姿态角真值(即，低范围姿态角真值＝(低范围姿态角真值+偏移量)-(偏移量))。然后减法器32将通过减法获得的姿态角真值输出到输出装置34。减法器32还将姿态角真值输出到姿态角矩阵计算器36。
姿态角矩阵计算器36从输入的姿态角来计算姿态角矩阵，并且将其反馈到矩阵加法计算器14。然后姿态角矩阵计算器36执行矩阵姿态角计算器16的反运算，并且从姿态角产生姿态角矩阵。然后矩阵加法计算器14通过将反馈的姿态角矩阵加到基于由角速度传感器10新检测到的角速度而计算出的小角度矩阵，来执行积分计算，然后从角速度来计算姿态角。
输出装置34将通过减法器32去除了偏移量的姿态角输出到外部部件。在机器人的例子中，主处理器(主机处理器)输入由传感器单元检测到的姿态角等，并且对机器人的姿态执行反馈控制。输出装置34响应来自主处理器的命令在预定时间将姿态角输出到主处理器。由角速度传感器10检测到的角速度或由加速度传感器检测到的加速度可以被提供到输出装置34，并且除了姿态角，输出装置34还可以将角速度或加速度输出到主处理器。这些中的哪个将被输出是由来自主处理器的命令或设置的参数来确定的。输出装置34在给定的时刻将所有角速度、加速度和姿态角输出到主处理器，并且在其他时刻选择性地输出它们其中一个。
如上所述，因此检测并且输出了去除了偏移量的准确的姿态角。因为基于由加速度传感器20检测到的每个轴的方向上的加速度来提取偏移量，然后从角速度姿态角中去除偏移量并且反馈结果，角速度传感器10不需要是很贵、很重和特别精确的传感器。
同时，矩阵姿态角计算器16和26从姿态矩阵计算姿态角，它们利用函数atan2执行计算。结果，尽管实事上初始的姿态角连续地变化，但是姿态角在计算中可能不连续地变化。这是姿态角表达中的不连续性。因此，来自矩阵姿态角计算器16和26的姿态角使用为，如同它们不具有由低通滤波器提取的低范围分量。而是，低范围分量是在转换到连续姿态角之后提取的。
图21为低通滤波器18的框图。低通滤波器18具有对姿态角进行sin转换和cos转换的转换器，以及atan2计算器或atan计算器。低通滤波器28具有与低通滤波器18相同的结构，所以省略其描述。
sin-cos转换器18a、18b和18c对三个轴的每个的姿态角执行sin和cos转换并且输出结果。例如，如果角速度姿态角为S1ψ、S1θ和S1φ，sin-cos转换器18a对S1ψ计算sin(S1ψ)和cos(S1ψ)并且输出结果。然后sin和cos转换后的值被分别输出到低通滤波器18d和18e。然后低通滤波器18d和18e从sin和cos转换后的值提取低范围分量并且将结果输出到atan2计算器18j。atan2计算器18j利用sin和cos转换后的值计算atan2并且将结果作为姿态角S2ψ输出。此外，sin-cos转换器18b对S1θ计算sin(S1θ)和cos(S1θ)并且输出结果。然后sin和cos转换后的值被分别输出到低通滤波器18f和18g。然后低通滤波器18f和18g从sin和cos转换后的值提取低范围分量并且将结果输出到atan计算器18k。然后atan计算器18k利用sin和cos转换后的值计算atan，并且将结果作为姿态角S2θ输出。此外，sin-cos转换器18c对S1φ计算sin(S1φ)和cos(S1φ)并且输出结果。sin和cos转换后的值被分别输出到低通滤波器18h和18i。然后低通滤波器18h和18i从sin和cos转换后的值提取低范围分量并且将结果输出到atan2计算器18m。atan2计算器18m利用sin和cos转换后的值计算atan2，并且将结果作为姿态角S2φ输出。
以此方式结合的sin-cos转换以及atan2计算或atan计算，可以消除由于姿态角的不连续造成的低通滤波器的不良操作，从而稳定地提取低范围分量。在sin和cos转换之后的值在±1的范围内连续并且平滑地变化，所以在滤波器处理中没有产生离散(dispersion)或非正常值。通过利用在滤波器处理后使用sin和cos转换后的值来执行atan2计算过程，值被再转换到姿态角分量，使得姿态角分量的滤波器处理可以被实现而值并未超过姿态角表达范围。
接着，描述第十六示例性实施例。图22为本发明的第十六示例性实施例的框图。此示例性实施例与第十五示例性实施例的不同在于，微分器30计算低通滤波器18的输出和低通滤波器28的输出之间的差值，并且将结果输出到乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42执行PID控制。即，乘法器38执行将来自微分器30的偏移量与预先设置的系数相乘，并且将计算的值(比例项)输出到减法器32。积分器40执行将来自微分器30的偏移量随着时间累积的值与预先设置的系数相乘的计算，并将计算的值(积分项)输出到减法器32。微分器42执行将来自微分器30的偏移量随着时间微分的值与预先设定的系数相乘的计算，并且将计算的值(微分项)输出到减法器32。系数可以是固定的或者它们可以如下所述是可变的。减法器32从由矩阵姿态角计算器16输入的角速度姿态角中减去这些项的和(即，比例项+积分项+微分项)，并且将结果作为减去偏移量的移动体的姿态角输出到输出装置34和姿态角矩阵计算器36。来自减法器32的输出被姿态角矩阵计算器36转换为姿态矩阵，并且反馈结果。乘法器38主要确定反馈度，积分器40累积长期的偏移量并且确定反馈量，微分器42确定关于短期变化的反馈量。利用此类PID控制短期和长期特性均得以改善。
接着，描述第十七示例性实施例。图23为本发明的第十七示例性实施例的框图。此示例性实施例与第十六示例性实施例的不同在于，来自外部姿态角44的绕Z轴的姿态角，即横摆角，被提供到矩阵姿态角计算器26。加速度传感器20检测在每个轴x、y和z方向上的加速度。然而，当使用了G传感器时，横摆角φ，即绕Z轴的姿态角不确定，所以需要将横摆角φ固定为恒定值。在此示例性实施例中，横摆角被单独检测并且被提供到矩阵姿态角计算器26，因此还使横摆角φ能被准确计算。更具体地，矩阵姿态角计算器26根据如上所述的计算，来计算横摆角φ、侧倾角θ和俯仰角ψ，作为来自姿态矩阵T(n)的姿态角。然而，这些中的横摆角φ用从外部部件输入的横摆角φ代替，这防止了横摆角φ的发散。
此外，在此示例性实施例中，设置了微分器31代替微分器30。微分器31包括sin-cos转换器和atan2计算器或atan计算器，就像低通滤波器18和28。
图24为微分器31的框图。来自低通滤波器18的角速度姿态角和来自低通滤波器28的加速度姿态角均被提供到微分器31。三个轴的角速度姿态角被分别表示为ξn0ψ、ξn0θ和ξn0φ，并且三个轴的加速度姿态角被分别表示为ξm0ψ、ξm0θ和ξm0φ。
sin-cos转换器31a对ξn0ψ执行sin和cos转换，并且将相应的结果输出到atan2计算器31g。atan2计算器31g利用sin和cos转换后的值执行atan2计算，并且将结果输出到微分器31n。同时，sin-cos转换器31b对ξm0ψ执行sin和cos转换，并且将相应的值输出到atan2计算器31h。atan2计算器31h利用sin和cos转换后的值执行atan2计算，并且将结果输出到微分器31n。微分器31n计算这两者之间的差值并且提取偏移量，然后将偏移量输出到sin-cos转换器31r。sin-cos转换器31r和atan2计算器31u执行相同的计算并且输出ξd1ψ作为偏移量。sin-cos转换器31c对ξn0θ执行sin和cos转换，并且将相应的值输出到atan计算器31i。atan计算器31i利用sin和cos转换后的值执行atan计算，并且将结果输出到微分器31p。同时，sin-cos转换器31d对ξm0θ执行sin和cos转换，并且将相应的值输出到atan计算器31j。atan计算器31j利用sin和cos转换后的值执行atan计算，并且将结果输出到微分器31p。微分器31p计算这两者之间的差值并且提取偏移量，然后将偏移量输出到sin-cos转换器31s。sin-cos转换器31s和atan计算器31v执行相同的运算并且输出ξd1θ作为偏移量。sin-cos转换器31e对ξn0φ执行sin和cos转换，并且将相应的值输出到atan2计算器31k。atan2计算器31k利用sin和cos转换后的值执行atan2计算，并且将结果输出到微分器31q。同时，sin-cos转换器31f对ξm0φ执行sin和cos转换，并且将相应的值输出到atan2计算器31m。atan2计算器31m利用sin和cos转换后的值执行atan2计算，并且将结果输出到微分器31q。微分器31q计算这两者之间的差值并且提取偏移量。然后微分器31q将偏移量输出到sin-cos转换器31t。sin-cos转换器31t和atan2计算器31w执行相同的运算并且输出ξd1φ作为偏移量。
以此方式，可以防止发生由于角速度姿态角和加速度姿态角中的不连续造成的非正常计算。还防止发生由于当计算姿态角差值时姿态角的不连续造成的非正常计算。此外，还消除了偏移量的姿态角中的不连续，所以可以防止PID控制的不良操作。
图25为根据此示例性实施例的流程图。首先，对角速度姿态角ξn1和加速度姿态角ξm1执行sin和cos转换，并且执行atan2或atan计算(S101)。然后，计算在所述计算之后的值的差值(S102)。通过该计算获得的值，即ξd0＝ξm1-ξn1，是偏移量。然后执行sin-cos转换和atan2或atan计算以获得ξd1(S103)。在计算出偏移量ξd1之后，对该偏移量执行PID控制。即，计算比例项、积分项和微分项 ξd1p＝P×ξd1 ξd1i＝I×ξd1 ξd1d＝D×ξd1 然后将它们相加到一起来获得ξs0(即，ξs0＝ξd1p+ξd1i+ξd1d)(S104和S105)。
然后，可以计算角速度姿态角ξn1和ξs0之间的差值。然而，用在ξm＝-π和ξp＝+π之间的值ψ和φ，以及在ξm＝-π/2和ξp＝+π/2之间的值θ来限定姿态角，所以如果ξs0超过-π和+π之间的值，用ψ和φ控制变得不稳定。因此，如果ξs0超过此范围，需要用该范围内的最近的值来代替。因此，首先确定ξs0是否小于ξm＝-π(S106)。如果ξs0小于ξm＝-π，那么用ξm＝-π来代替ξs0并且形成ξs1(S108)。同样，如果ξs0大于ξp＝π(S107)，那么用ξp＝π来代替ξs0并且形成ξs1(S109)。当ξs0在ξm＝-π和ξp＝π之间时没有问题出现，所以该值形成ξs1并且计算其与ξn1之间的差值(S111)。另一方面，如果ξs0的值超过ξm＝-π/2和ξp＝π/2之间的值，则用θ控制变得不稳定。因此，如果ξs0超过此范围，需要用在该范围内的最接近的值来代替。因此，首先确定ξs0是否小于ξm＝-π/2(S106)。如果ξs0小于ξm＝-π/2，那么用ξm＝-π/2+δ来代替ξs0并且形成ξs1(S108)。同样，如果ξs0大于ξp＝π/2(S107)，那么用ξp＝π/2-δ来代替ξs0并且形成ξs1(S109)。这里，δ为实数并且表示计算用的最小有效数。当ξs0在ξm＝-π/2和ξp＝π/2之间时没有问题出现，所以该值形成ξs1并且计算其与ξn1之间的差值(S111)。然而，当ξs0变成-π/2时，使其成为-π/2+δ，并且当ξs0变成π/2时，使其成为π/2-δ。上述流程使得能够输出稳定的姿态角。
以此方式，根据此示例性实施例，通过执行sin、cos和atan2转换(即，SCA转换)，参数中的不连续得到消除。姿态角的不连续的补充说明以及微分器31的操作如下。
下面，说明姿态角的不连续性。姿态角用绕X轴的ψ、绕Y轴的θ和绕Z轴的φ通过(ψ，θ，φ)来表示。各自的范围为 -π＜ψ≤π(弧度)或-180＜ψ≤180(度) -π/2＜θ＜π/2(弧度)或-90＜θ＜90(度) -π＜φ≤π(弧度)或-180＜φ≤180(度)
图26示出了姿态角φ和当有绕Z轴的旋转时的旋转角之间的关系。当Z轴从-720旋转变化到720度时，φ在±180度的范围中振荡。当φ达到180度时，其突然翻转并且开始接近-180度。这是产生不连续的机理。尽管移动体随着时间连续地旋转(如图中通过Z轴旋转所示)，但是姿态角φ是不连续的。
图27更具体地示出了当±20度的sin波类型连续旋转振荡在φ＝180度附近应用。在图中，Z轴旋转以在180度的±20度的sin波类型振荡连续地变化，但是φ在±180度之间非连续地变化。
图28和29同时示出了来自作为SCA处理的本发明的结果。根据所示结果，来自SCA处理的波形与图27中所示的φ相似。从图中明显看出，解决了波形变形或与在现有技术中的波形Mφ完全不相似的问题。这对于ψ也一样。结果是，可以准确地检测到在大操作范围上的姿态角。
图30示出了θ和当有绕Y轴的旋转时的旋转角之间的关系。当Y轴从-720旋转变化到720度时，θ在±90度的范围中振荡。当θ达到90度时，其突然翻转并且开始接近-90度。这是产生不连续的机理。尽管移动体随着时间连续地旋转，但是姿态角θ是不连续的。
图31具体地示出了当±20度的sin波类型连续旋转振荡在θ＝90度附近应用。Y轴旋转连续变化，但是θ在±90度之间非连续地变化。
图32和33同时示出了使用SCA处理的本发明的结果。根据所示结果，来自SCA处理的波形与图31中所示的θ相似。从图中明显看出，解决了波形变形或与在现有技术中的波形Mθ完全不相似的问题。结果是，可以准确地检测到在大操作范围上的姿态角。
下面，说明SCA微分器。微分器31计算关于姿态角(ψ，θ，φ)的每个分量的差值。例如，关于φ，我们说在图26中角速度姿态角ξnoφ为175度，加速度姿态角ξmo为-25度。当执行差值计算时，ξno-ξmo＝200度。因为这超过了φ的±180度的限定范围，出现不合理性。以此方式，计算操作导致φ趋向于超过限定的范围。因此，微分器31执行操作来将φ带入限定范围内。此操作为sin、cos和atan2转换(即，SCA转换)。更具体地，获得ξnoφ-ξmoφ的sin值和cos值。然后利用sin值和cos值通过atan2计算获得ξd1φ。ξd1φ在此时落入限定的范围内。根据条件，还可以利用使得200-360＝-160度的方法。然而，很多其大大超过±180度，例如±360度、±540度、±720度等的情况是可能的。根据条件，有必要描述极大的，本质上为无穷数的组合。同样，根据条件，数字值可能在计算中剧烈地变化，因此导致不连续，这将使得控制极其困难。实际上，对象为叠加了噪声的信号。因此，当噪声在±180度附近叠加时，产生了极大的、不连续的振荡。结果是，由于其与计算准确性的关系，造成其在计算中容易变得不稳定，使得不能再获得高准确性和稳定的输出。SCA转换过程是用于数学地平滑地执行此的方法。其是使超过限定范围的数据通过计算处理适合于限定的范围的过程。并且因此类似正规化。
ξnoφ其本身也可能在处理的计算中的一个超过限定范围。因此，通过用sin和cos表示ξnoφ以及计算atan2，首先使得ξnoφ适合在±180度范围内。相似地，ξmoφ也正规化到限定范围内。然后，微分器31执行计算ξnoφ-ξmoφ。根据状态，此差值的输出可以超过φ的限定范围。因此，通过sin-cos-atan2转换使输出回到限定范围内来执行正规化。在姿态角ψ、θ和φ中，ψ和φ的限定区域为-π到π，其与atan2处理的输出范围相同，所以执行atan2计算。同时，θ的限定区域为-π/2到π/2，其是atan2处理的输出范围的一半。因此，atan计算使用θ。atan的输出范围在-π/2到π/2与θ匹配。atan(y，x)＝tan-1(x，y)是使得在小的x区域，分母tan(y/x)小以便误差变大。因此，当与y相比x小时，使用cot-1(y，x)。
尽管迄今描述了本发明示例性实施例，本发明不限于这些示例性实施例。相反，各种修改是可能的。
例如，以图6中所示的结构，运动频率传感器52检测在例如两足行走的机器人移动中的特定频率，并且将其输出到低通滤波器46和48。然而，另一种选择是，以图1到图3中的结构，运动频率传感器52可以将此特定频率输出到低通滤波器18和28，并且低通滤波器18和28的截止频率可以设置为适应性地改变。
另外，以图1的结构，来自矩阵姿态角计算器(角速度系统)16的角速度系统姿态角被输出到低通滤波器18，并且来自矩阵姿态角计算器(加速度系统)26的加速度姿态角被输出到低通滤波器28。然而，另一种选择是，来自矩阵姿态角计算器16的角速度系统姿态角和来自矩阵姿态角计算器26的加速度姿态角也均可以直接输出到微分器30，然后来自微分器30的差值输入到单个低通滤波器，并且低范围分量作为偏移量输出到减法器32。这有利于减少LPF的数量。这同样也适于图2和3中的结构。
此外，模拟滤波器或数字滤波器也可以用作图1到图6中的低通滤波器。数字滤波器可以是FIR(有限时间冲激回应)滤波器或IIR(无限时间冲激回应)滤波器。
而且，在每个结构中，姿态角、角速度和加速度中的至少一个从输出装置34输出到机器人的主处理器。然而，另外，由矩阵加法计算器14计算出的姿态矩阵、由加速度矩阵计算器24计算出的姿态矩阵、由微分器30计算出的偏移量以及由四元数LPFs 63和67提取的四元数等，可以输出到输出装置34，然后从输出装置34输出到主处理器。
此外，图20、22和23中所示的结构也可以设置有运动频率传感器52，其输入来自角速度传感器10的角速度和来自加速度传感器20的加速度，并且检测在机器人，即移动体的运动中的特定频率。运动频率传感器从角速度和加速度中提取频繁出现的频率分量，并且将其输出到低通滤波器18和28。一个例子为当机器人，即移动体用两条腿行走时的两足移动中的特定频率。低通滤波器18和28根据从运动频率传感器输入的频率适应性地调整截止频率。运动频率越大，低通滤波器18和26越要增大截止频率。此外，运动频率传感器将运动频率输出到执行PID控制的乘法器38、积分器40和微分器42。乘法器38、积分器40和微分器42根据输入的运动频率调整乘法器系数。
迄今已经描述了本发明的示例性实施例。这些示例性实施例的每个的形式展示了如下独一无二的效果。即，根据利用姿态角的形式，输出为用在与机器人、车辆、船舶和飞机有关的控制等中的姿态角。姿态角本身为设备中的校正循环控制的对象，所以能够获得用于真实表现(actual phenomenon)的稳定的输出。此外，当通过合适的测量和控制来优化设备中的控制参数时，各种参数可以得到有效地优化以便与来自真实运动的姿态角具有良好的兼容性。因此，需要更少的时间和工作用于优化，而且可以获得适用于应用的特性。
因而，此形式适用于利用姿态角作为主要对象的测量和控制，并且能够进行过去不能或不足的车辆或机器人的姿态测量和控制。此外，以利用姿态矩阵的形式，输出为用于机器人、车辆、船舶和飞机有关的测量和控制等的姿态角。通过内部计算，姿态矩阵为校正循环的对象。因此，为避免特殊性等的特殊处理不是在内部计算中执行，这比较容易理解。进一步，姿态矩阵本身可以输出到外部部件。因此，此形式适于使用姿态矩阵的测量和控制。通过姿态矩阵，部件的个数比姿态角的个数多，所以有大量的数据。这不幸地意味着通信需要时间并且数据处理麻烦且复杂。然而，尽管有此缺点，大量的数据使得能够避免特殊的问题，并且能够扩展兼容性、压缩和修改坐标系，因而提供大的自由度。当有足够的通信速度、足够的测量和控制的计算速度、以及执行计算所需的大区域的存储器时可以容易使用此形式。因此，当有快速度通信网络时此形式特别有效，此形式应用于具有快速计算速度的高性能测量/控制处理系统，例如，在用于诸如核能设备和工业设备的空间和高水平测量/控制设备中使用的设备。
此外，利用四元数的形式，输出为用在与机器人、车辆、船舶和飞机有关的测量和控制等中的姿态角。对于内部计算，四元数为校正循环的对象。利用四元数的优点在于，其减少了计算所需的数据量，并且减少了计算步骤数。因此，可以利用设备中的具有中等计算速度和小存储器的处理器来实现此功能，因此可以减少成本。然而，因为四元数是其特征在实际空间中难于理解的表示方法，其在转换为姿态角后使用。因此，这是用于测量和控制的特性清楚的形式，并且适用于当有多个相似移动。因此，此形式适用于小尺寸工业设备、多用途机器人和批量生产的车辆的测量和控制。
权利要求
1.一种移动体姿态角检测设备，其特征在于，包括
检测移动体的角速度的角速度传感器；
检测所述移动体的加速度的加速度传感器；
角速度姿态角计算装置，用于由所述角速度计算所述移动体的姿态角，作为角速度姿态角；
加速度姿态角计算装置，用于由所述加速度计算所述移动体的姿态角，作为加速度姿态角；
角速度姿态角低范围分量提取装置，用于提取所述角速度姿态角的低范围分量；
加速度姿态角低范围分量提取装置，用于提取所述加速度姿态角的低范围分量；
差值计算装置，用于计算所述角速度姿态角的低范围分量和所述加速度姿态角的低范围分量之间的差，作为误差；
误差去除装置，用于从所述角速度姿态角去除所述误差；
输出装置，用于输出由所述误差去除装置去除了所述误差的角速度姿态角，作为所述移动体的姿态角；及
反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度姿态角反馈到所述角速度姿态角计算装置。
2.如权利要求1所述的设备，其特征在于
所述误差去除装置包括
按比例地计算由所述差值计算装置计算的所述差得到的所述误差的值的乘法器，
对由所述差值计算装置计算的所述差得到的所述误差的值进行积分计算的积分器，及
对由所述差值计算装置计算的所述差得到的所述误差的值进行微分计算的微分器；
及所述误差去除装置通过从所述角速度姿态角减去所述乘法器、所述积分器和所述微分器得到的所述计算值的和来去除所述误差。
3.如权利要求1或2所述的设备，其特征在于，进一步包括
用于独立于所述加速度传感器而检测绕所述移动体的垂直轴的姿态角并且将所述结果输出到所述加速度姿态角计算装置的装置。
4.如权利要求1-3中任意一项所述的设备，其特征在于
所述角速度姿态角计算装置包括
用于从所述角速度计算在经过一小段时间Δt之后定义小旋转角的小角度矩阵的装置；
用于从所述小角度矩阵和对应于从所述反馈装置反馈回来的所述角速度姿态角的姿态矩阵，计算新的角速度系统姿态矩阵的装置；
用于从所述新的角速度系统姿态矩阵计算所述角速度姿态角的装置；
及所述加速度姿态角计算装置包括
用于从所述加速度计算倾斜角的装置；
用于从所述倾斜角计算加速度系统姿态矩阵的装置；和
用于从所述加速度系统姿态矩阵计算所述加速度姿态角的装置。
5.如权利要求4所述的设备，其特征在于
所述角速度姿态角低范围分量提取装置具有由所述计算所述角速度姿态角的装置提取所述角速度姿态角的所述低范围分量的角速度系统低通滤波器；
所述加速度姿态角低范围分量提取装置具有由所述计算所述加速度姿态角的装置提取所述加速度姿态角的所述低范围分量的加速度系统低通滤波器；及
所述角速度系统低通滤波器和所述加速度系统低通滤波器具有相同的截止频率和相同的衰减因子。
6.如权利要求4所述的设备，其特征在于
所述角速度姿态角低范围分量提取装置具有由所述计算所述新的角速度系统姿态矩阵的装置提取所述新的角速度系统姿态矩阵的低范围分量的角速度系统低通滤波器；
所述加速度姿态角低范围分量提取装置具有由所述计算所述加速度系统姿态矩阵的装置提取所述加速度系统姿态矩阵的低范围分量的加速度系统低通滤波器；及
所述角速度系统低通滤波器和所述加速度系统低通滤波器具有相同的截止频率和相同的衰减因子。
7.如权利要求6所述的设备，其特征在于，进一步包括
用于正规化和正交化来自所述角速度系统低通滤波器的所述角速度系统姿态矩阵的装置；及
用于正规化和正交化来自所述加速度系统低通滤波器的所述加速度系统姿态矩阵的装置。
8.如权利要求5-7中任意一项所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述角速度系统低通滤波器和所述加速度系统低通滤波器的所述截止频率根据所述运动频率可变地设置。
9.如权利要求2所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述乘法器、所述积分器和所述微分器的系数根据所述运动频率可变地设置。
10.一种移动体姿态角检测设备，其特征在于，包括
检测移动体的角速度的角速度传感器；
检测所述移动体的加速度的加速度传感器；
角速度姿态角计算装置，用于从所述角速度计算所述移动体的姿态角，作为角速度姿态角；
加速度姿态角计算装置，用于从所述加速度计算所述移动体的姿态角，作为加速度姿态角；
差值计算装置，用于计算所述角速度姿态角和所述加速度姿态角之间的差；
低范围分量提取装置，用于提取所述差的低范围分量作为误差；
误差去除装置，用于从所述角速度姿态角去除所述误差；
输出装置，用于输出由所述误差去除装置去除了所述误差的角速度姿态角，作为所述移动体的姿态角；和
反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度姿态角反馈到所述角速度姿态角计算装置。
11.一种移动体姿态角检测设备，其特征在于，包括
检测移动体的角速度的角速度传感器；
检测所述移动体的加速度的加速度传感器；
用于从所述角速度计算在经过一小段时间ts之后定义小旋转角的小角度矩阵的装置；
用于从所述小角度矩阵和被反馈回来的姿态矩阵计算新的角速度系统姿态矩阵的装置；
用于从所述加速度计算倾斜角的装置；
用于从所述倾斜角计算加速度系统姿态矩阵的装置；
角速度系统低范围分量提取装置，用于提取所述角速度系统姿态矩阵的低范围分量；
加速度系统低范围分量提取装置，用于提取所述加速度系统姿态矩阵的低范围分量；
差值计算装置，用于计算所述角速度系统姿态矩阵的所述低范围分量和所述加速度系统姿态矩阵的所述低范围分量之间的差，作为误差；
误差去除装置，用于从所述角速度系统姿态矩阵去除所述误差；
输出装置，用于从由所述误差去除装置去除了所述误差的角速度系统姿态矩阵计算姿态角，并且输出所述结果作为所述移动体的姿态角；和
反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度系统姿态矩阵反馈到所述用于计算所述新的角速度系统姿态矩阵的装置。
12.如权利要求11所述的设备，其特征在于，所述误差去除装置包括
按比例地计算所述差值计算装置计算的所述差得到的所述误差值的乘法器，
对由所述差值计算装置计算的所述差得到的所述误差值进行积分计算的积分器，和
对由所述差值计算装置计算的所述差得到的所述误差值进行微分计算的微分器；
及所述误差去除装置通过从所述角速度系统姿态矩阵减去通过将来自所述乘法器、所述积分器和所述微分器的所述计算的值相加获得的矩阵，来去除所述误差。
13.一种移动体姿态角检测设备，其特征在于，包括
检测移动体的角速度的角速度传感器；
检测所述移动体的加速度的加速度传感器；
用于从所述角速度计算在经过一小段时间ts之后定义小旋转角的小角度四元数的装置；
用于从所述小角度四元数和被反馈回来的四元数计算新的角速度系统四元数的装置；
用于从所述加速度计算倾斜角的装置；
用于从所述倾斜角计算加速度系统四元数的装置；
角速度系统低范围分量提取装置，用于提取所述角速度系统四元数的低范围分量；
加速度系统低范围分量提取装置，用于提取所述加速度系统四元数的低范围分量；
差值计算装置，用于计算所述角速度系统四元数的所述低范围分量与所述加速度系统四元数的所述低范围分量之间的差，作为误差；
误差去除装置，用于从所述角速度系统四元数去除所述误差；
输出装置，用于从由所述误差去除装置去除了所述误差的角速度系统四元数计算姿态角，并且输出所述结果作为所述移动体的姿态角；知
反馈装置，用于将由所述误差去除装置去除了所述误差的所述角速度系统四元数反馈到所述用于计算所述新的角速度系统四元数的装置。
14.如权利要求13所述的设备，其特征在于，所述误差去除装置包括
按比例地计算由所述差值计算装置计算的所述差得到的所述误差值的乘法器，
对由所述差值计算装置计算的所述差得到的所述误差值进行积分计算的积分器，及
对由所述差值计算装置计算的所述差得到的所述误差值进行微分计算的微分器；
并且所述误差去除装置通过从所述角速度系统四元数减去通过将来自所述乘法器、所述积分器和所述微分器的所述计算的值相加获得的四元数来去除所述误差。
15.如权利要求11-14中任意一项所述的设备，其特征在于，进一步包括
用于独立于所述加速度传感器检测绕所述移动体的垂直轴的倾斜角并将所述结果输出到所述用于计算所述加速度系统姿态矩阵的装置的装置。
16.如权利要求11-15中任意一项所述的设备，其特征在于
所述角速度系统低范围分量提取装置具有角速度系统低通滤波器；
所述加速度系统低范围分量提取装置具有加速度系统低通滤波器；及
所述角速度系统低通滤波器和所述加速度系统低通滤波器具有相同的截止频率和相同的衰减因子。
17.如权利要求16所述的设备，其特征在于，进一步包括
用于正规化所述角速度系统低通滤波器的输出的装置；和
用于正规化所述加速度系统低通滤波器的输出的装置。
18.如权利要求16或17所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述角速度系统低通滤波器和所述加速度系统低通滤波器的截止频率根据所述运动频率可变地设置。
19.如权利要求12或14所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述乘法器、所述积分器和所述微分器的系数根据所述运动频率可变地设置。
20.如权利要求1所述的设备，其特征在于
所述角速度姿态角低范围分量提取装置和所述加速度姿态角低范围分量提取装置包括
用于sin转换和cos转换所述角速度姿态角的装置；
用于提取通过所述sin转换和所述cos转换获得的值的低范围分量的装置；和
用于对所述提取的低范围分量的值执行atan2计算或atan计算的装置。
21.如权利要求20所述的设备，其特征在于，所述误差去除装置包括
按比例地计算由所述差值计算装置计算的所述差得到的所述误差值的乘法器，
对由所述差值计算装置计算的所述差得到的所述误差值进行积分计算的积分器，及
对由所述差值计算装置计算的所述差得到的所述误差值进行微分计算的微分器；
及所述误差去除装置通过从所述角速度姿态角减去来自所述乘法器、所述积分器和所述微分器的所述计算值的和而去除所述误差。
22.如权利要求20或21所述的设备，其特征在于，进一步包括
用于独立于所述加速度传感器检测绕所述移动体的垂直轴的倾斜角并将所述结果输出到所述加速度姿态角计算装置的装置。
23.如权利要求20-22中任意一项所述的设备，其特征在于
所述角速度姿态角计算装置包括
用于从所述角速度计算在经过一小段时间ts之后定义小旋转角的小角度矩阵的装置；
用于从所述小角度矩阵和对应于从所述反馈装置反馈回来的所述角速度姿态角的姿态矩阵，计算新的角速度系统姿态矩阵的装置；和
用于从所述新的角速度系统姿态矩阵计算所述角速度姿态角的装置；
及所述加速度姿态角计算装置包括
用于从所述加速度计算倾斜角的装置；
用于从所述倾斜角计算加速度系统姿态矩阵的装置；和
用于从所述加速度系统姿态矩阵计算所述加速度姿态角的装置。
24.如权利要求20-23中任意一项所述的设备，其特征在于
所述角速度姿态角低范围分量提取装置具有提取所述sin转换和cos转换的值的所述低范围分量的角速度系统低通滤波器；
所述加速度姿态角低范围分量提取装置具有提取所述sin转换和cos转换的值的所述低范围分量的加速度系统低通滤波器；及
所述角速度系统低通滤波器和所述加速度系统低通滤波器具有相同的截止频率和相同的衰减因子。
25.如权利要求20-24中任意一项所述的设备，其特征在于
所述差值计算装置包括用于计算所述角速度姿态角的低范围分量在被sin转换和cos转换后，其结果再经atan2计算或atan计算而得到的结果的值，与所述加速度姿态角的低范围分量在被sin转换和cos转换后，其结果再经atan2计算或atan计算而得到的结果的值之间的差的装置。
26.如权利要求25所述的设备，其特征在于，进一步包括用于sin转换和cos转换通过计算所述差获得的值、并atan2计算或atan计算所述结果的装置。
27.如权利要求20-26中任意一项所述的设备，其特征在于
所述角速度姿态角和所述加速度姿态角的ψ或φ被定义为在-π和π之间，包括-π和π，的角度；
所述误差去除装置通过在所述误差小于-π时将所述误差替换为-π，以及在所述误差大于π时将所述误差替换为π，而从所述角速度姿态角去除所述误差；
所述角速度姿态角和所述加速度姿态角的θ被定义为在-π/2和π/2之间，包括-π/2和π/2，的角度；及
所述误差去除装置通过在所述误差小于-π/2时将所述误差替换为-π/2+δ(δ为实数)，以及在所述误差大于π/2时将所述误差替换为π/2-δ，而从所述角速度姿态角去除所述误差。
28.如权利要求24所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述角速度系统低通滤波器和所述加速度系统低通滤波器的截止频率根据所述运动频率可变地设置。
29.如权利要求21所述的设备，其特征在于，进一步包括
用于从所述角速度和所述加速度检测对于所述移动体特定的运动频率的装置，
其中所述乘法器、所述积分器和所述微分器的系数根据所述运动频率可变地设置。
全文摘要
由角速度传感器(10)检测到的角速度由小角度矩阵计算器(12)和矩阵加法计算器(14)积分，然后由矩阵姿态角计算器(16)还原为姿态角(角速度姿态角)。通过倾斜角计算器(22)和加速度矩阵计算器(24)计算姿态矩阵，并且由加速度传感器(20)检测到的加速度由矩阵姿态角计算器(26)还原为姿态角(加速度姿态角)。低通滤波器(18，28)的每个提取低范围成分，由微分器(30)计算这两者之间的差值，并且仅仅提取偏移量。减法器(32)将偏移量从角速度姿态角中去除，结果由输出装置(34)输出。另外，姿态角矩阵计算器(36)将所述结果转换为反馈到矩阵加法计算器(14)的姿态矩阵。
文档编号G01C15/00GK101232979SQ200680027981
公开日2008年7月30日 申请日期2006年8月1日 优先权日2005年8月1日
发明者杉原久义, 野野村裕, 藤吉基弘 申请人:丰田自动车株式会社