CN101233413A - 加速度传感器校正设备及加速度传感器的输出值校正方法 - Google Patents

Abstract

姿态角计算单元(14)自加速度传感器(10)的输出值计算机器人的姿态角。姿态角比较单元(16)将设置在寄存器(20)内的处于特定姿态的姿态角与检测到的姿态角相比较，并将其差值输出到校正值计算单元(18)。所述校正值计算单元(18)输出校正单元到零点校正单元(26)或灵敏度校正单元(28)，以消除这些差值。从输入单元(22)对设置在所述寄存器(20)内的姿态角进行设置也是可以接受的。

Description

加速度传感器校正设备及加速度传感器的输出值校正方法

技术领域

[0001]本发明涉及用于校正安装到诸如机器人或类似物的移动体上的加速度传感器的输出值的技术。

背景技术

[0002]加速度传感器和横摆率传感器用于诸如机器人或类似物的移动体的姿态控制。以X轴、Y轴和Z轴作为三个正交轴，则这三个轴向上的加速度由三个加速度传感器检测，并且绕着这三个轴的横摆率由三个横摆率传感器检测。绕着这些轴的角度，即，姿态角(侧倾角(roll angle)、俯仰角(pitch angle)和横摆角(yaw angle))通过对这些横摆率传感器的输出进行时间积分而获得。

在专利公开号为JP-A-2004-268730的日本公开专利中，公开了通过利用从陀螺仪传感器输出的加速度数据和姿态数据进行姿态控制的技术。

[0003]加速度传感器具有零点漂移，所以当移动体静止时有必要对该零点漂移进行校正；但是因为由重力产生的加速度即使在静止时也存在，所以不可能确定零点。当然，利用具有零点稳定性的高精度加速度传感器是可能的，但是在这种情况下，不仅费用变高，而且尺寸和重量也会增加。

发明内容

[0004]因而，本发明的目的是提供具有简单结构的可以校正加速度传感器的输出值的技术，并且该技术能够以高精度检测移动体的加速度，进而检测其姿态角。

[0005]本发明的第一方面涉及一种加速度传感器的校正设备，包括：用于以来自设于移动体上的加速度传感器的输出值为基础，计算所述移动体的姿态角数据的装置；及用于通过将所述姿态角数据和参考姿态角数据相比较，来校正所述加速度传感器的所述输出值的装置。

[0006]依照这种校正设备，诸如机器人或类似物的移动体的姿态角数据由所述加速度传感器的所述输出值计算得到。将与由该姿态角数据的加速度传感器进行的检测分开检测的参考姿态角，或设置的参考姿态角，和计算得到的所述姿态角数据一起比较。并且，如果零点漂移或灵敏度反常地出现在所述加速度传感器的所述输出值中，那么基于这些输出值而计算出的所述姿态角具有不同于所述参考姿态角数据的值。因而，通过比较这两种姿态角数据，检测出所述加速度传感器的所述输出值中的反常，并且校正它们的量是可行的。因为，以上述校正设备，并不是比较由所述加速度传感器检测到的所述加速度本身，而是比较由所述加速度得到的所述姿态角数据，因此以高精度执行校正而不受来自重力加速度的任何影响是可行的。

[0007]依照本发明，以简单的结构来校正所述加速度传感器的所述输出值是可行的，而且以高精度检测移动体的加速度，进而检测其姿态角也是可行的。

[0008]本发明的第二方面涉及一种校正加速度传感器的输出值的方法。该方法包括以来自加速度传感器的输出值为基础，计算移动体的姿态角数据的步骤；将所述姿态角数据和参考姿态角数据进行比较的步骤；以及以所述姿态角数据和所述参考姿态角数据的比较结果为基础来校正所述加速度传感器的所述输出值的步骤。

附图说明

[0009]本发明的前述的和进一步的目标、特征和优点从结合附图对于优选具体实施方式的下述描述而变得更清楚，其中相似附图标记用于代表相似部件，并且其中：

图1为本发明的一个具体实施方式的结构框图；

图2为另一具体实施方式的结构框图；

图3为又一具体实施方式的结构框图；

图4为显示了本发明的一个具体实施方式中的校正处理的控制流程的流程图；

图5为显示了参考坐标系(XYZ)和传感器坐标系(xyz)之间关系的图形；

图6为显示了所述参考坐标系中的姿态角(侧倾角、俯仰角和横摆角)的图形；

图7为显示了传感器坐标系n的时变的图形；

图8为显示了所述传感器坐标系中的小旋转角的图形；及

图9为显示倾斜角的图形。

具体实施方式

[0010]在下文中，将参照附图说明本发明的具体实施方式。

[0011]第一具体实施方式

图1为该第一具体实施方式的结构框图。加速度传感器10设置在以预定姿态的诸如机器人或类似物的移动体的预定位置上，并且检测该移动体的加速度并将其输出到校正计算单元12。

[0012]该校正计算单元12基于将在下文中描述的来自零点校正单元26和来自灵敏度校正单元28的校正数据，校正所述加速度传感器10的输出值，并且将结果输出到输出单元24。而且，所述校正计算单元12将所述经过校正的输出值输出到姿态角计算单元14。

[0013]该姿态角计算单元14基于来自所述校正计算单元12的输出值计算倾斜角，基于这些倾斜角计算姿态矩阵，并且基于该姿态矩阵计算所述移动体的姿态角。在下文中将对由所述加速度计算所述倾斜角，以及由所述倾斜角计算所述姿态角进行描述。所述姿态角计算单元14将通过这种计算获得的姿态角输出到姿态角比较单元16。

[0014]所述姿态角比较单元16将由所述输出值获得的姿态角(所述加速度姿态角)和设置于寄存器20中的姿态角(所述参考姿态角)进行比较，并判定它们之间的差是否大于预定允许值。如果所述加速度姿态角和所述参考姿态角之间的差大于或等于所述预定允许值，那么判定有必要对所述输出值进行校正，并且所述加速度姿态角和所述参考姿态角之间的差被输出到校正值计算单元18。

[0015]该校正值计算单元18利用输入的这些差值来计算校正所述输出值的零点和灵敏度所需的校正值，将校正所述输出值的零点所需的校正值输出到所述零点校正单元26，并将校正所述输出值的灵敏度所需的校正值输出到灵敏度校正单元28。所述零点校正单元26向校正计算单元12输出由校正计算单元12进行零点校正所需的零点漂移值。所述校正计算单元12通过从所述输出值消除所述零点漂移从而校正所述输出值。此外，所述灵敏度校正单元28向校正计算单元12输出由校正计算单元12进行灵敏度校正所需的系数(增益)。这种对所述输出值的校正可以仅包括由所述零点校正单元26进行的零点校正。

[0016]如上所述，有待于与所述加速度姿态角相比较的参考姿态角设置于所述寄存器20内。设置于所述寄存器20内的该参考姿态角是所述机器人被保持在预先指定的姿态时的姿态角，但是，假设所述精度有保证，如下布置也是可以接受的：经由输入单元22从独立于所述加速度传感器10而设置在所述机器人上的姿态角传感器提供参考姿态角。当将所述加速度姿态角与处于预定姿态的参考姿态角比较时，将固定值设置到所述寄存器20内就足够了，所述输入单元22不是必需的。利用光纤陀螺仪(optical fiber gyro，FOG)或类似物作为独立设置的姿态角传感器是可行的。通过对由所述光纤陀螺仪检测到的角速度进行时间积分而测得所述姿态角，并且这些姿态角被提供给所述输入单元22并被设置在所述寄存器20内。所述加速度传感器10检测垂直方向上的加速度，并且，如果作为移动体的机器人直立时是静止的，那么这样直立时的参考角被设置在所述寄存器20内，并且与所述加速度姿态角相比较。如果所述加速度传感器10精确地输出“1G”，那么所述加速度姿态角和所述参考姿态角在预定允许值的范围内相互一致，但如果不是这种情况，那么所述加速度传感器10的输出值根据这些差值进行校正。如果所述机器人倾斜，会存在不在所述垂直轴上的加速度分量；但是，这时通过比较所述加速度姿态角和所述参考姿态角，校正所述加速度传感器10的输出值是可行的。

[0017]尽管所述加速度姿态角和所述参考姿态角在所述姿态角比较单元16中进行比较，但是如下这些布置也是可以接受的：比较由所述姿态角计算单元14计算得到的倾斜角和设置在所述寄存器20内的参考姿态角，或比较由所述姿态角计算单元14计算得到的姿态矩阵和设置在所述寄存器20内的参考姿态矩阵。而且，由所述姿态角计算单元14计算姿态角的四元数，并将该四元数和设置在所述寄存器20内的参考四元数相比较也是可以的。在该具体实施方式中，“姿态数据”用作姿态角、倾斜角、姿态矩阵或四元数的通称。

[0018]在下文中，将说明由所述加速度计算所述倾斜角的方法，由所述倾斜角计算姿态矩阵的方法，及由所述姿态矩阵计算姿态角的方法。

[0019]首先，将说明所述姿态矩阵。在作为传感器坐标系的符号系(notational system)的参考坐标系XYZ中，其被表示为在一离散时间n时的姿态矩阵。所述姿态矩阵T(n)由4×4的元素组成，如等式(1)所示：

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(1\right)$

[0020]该矩阵T(n)的含义为：第一列(a，b，c)、第二列(d，e，f)和第三列(g，h，i)分别为从所述参考坐标系看到的传感器坐标系n的x轴、y轴和z轴的方向矢量。并且第四列给出了传感器坐标系在所述参考坐标系内的原点位置(通常，如果有平移，平移量在该第四列中给出)。如果所述原点不移动，所述第四列的前三个元素(给出了该位置的变换)为零。如图5中所示，所述传感器坐标系n的原点位置On在所述参考坐标系的(0，0，0)位置处，并且所述x轴矢量在所述参考坐标系内具有(a，b，c)分量，所述y轴矢量在所述参考坐标系内具有(d，e，f)分量，所述z轴矢量在所述参考坐标系内具有(g，h，i)分量。

[0021]现在将在下文中说明由所述姿态角(侧倾角、俯仰角和横摆角)获得所述姿态矩阵T(n)的方法。在根据用于表示所述姿态矩阵T(n)的矩阵进行的旋转变换中，有必要考虑所述旋转轴的顺序。如图6中所示，当使用通常用于机器人的侧倾角、俯仰角和横摆角时，定义发生三种旋转；最初，绕z轴的旋转φ；然后，在该旋转后，绕所述y轴的旋转θ；最后，在此旋转后，绕x轴的旋转ψ(必须注意这一点，即所述轴的旋转顺序是固定的)。

[0022]起因于所述侧倾角、俯仰角和横摆角的变换矩阵将被称为RPY(φ，θ，ψ)。RPY(φ，θ，ψ)是所述旋转变换矩阵从左到右相乘的矩阵乘积，由等式(2)给出：

RPY(φ，θ，ψ)＝Rot(x，φ)·Rot(y，θ)·Rot(z，ψ)    (2)

[0023]在具体条件下，等式(2)可以表示为等式(3)：

$\mathrm{RPY}(\mathrm{\φ},\mathrm{\θ},\mathrm{\ψ})=\left(\begin{array}{cccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& 0& 0\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}\cos \mathrm{\θ}& 0& \sin \mathrm{\θ}& 0\\ 0& 1& 0& 0\\ -\sin \mathrm{\θ}& 0& \cos \mathrm{\θ}& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ 0& \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

[0024]当等式(3)完全写出时，得到等式(4)：

$\mathrm{RPY}(\mathrm{\φ},\mathrm{\θ},\mathrm{\ψ})=\left(\begin{array}{cccc}\cos \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\φ}\cos \mathrm{\θ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& 0\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

[0025]应当理解，也可以利用欧拉角(Euler angle)作为所述姿态角代替所述侧倾角、俯仰角和横摆角。利用欧拉角，当最初绕z轴的旋转φ，接下来，在该旋转后，绕所述y轴的旋转θ，最后，在此旋转后，绕z轴的旋转ψ发生时的变换矩阵表示为Euler(Eφ，Eθ，Eψ)，并且由等式(5)给出：

Euler(Eφ，Eθ，Eψ)＝Rot(x，Eφ)·Rot(y，Eθ)·Rot(z，Eψ)    (5)

[0026]在具体条件下，等式(5)可以表示为等式(6)：

$\mathrm{Euler}(\mathrm{E\φ},\mathrm{E\θ},\mathrm{E\ψ})=\left(\begin{array}{cccc}\cos \mathrm{E\φ}& -\sin \mathrm{E\φ}& 0& 0\\ \sin \mathrm{E\φ}& \cos \mathrm{E\φ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}\cos \mathrm{E\θ}& 0& \sin \mathrm{\θ}& 0\\ 0& 1& 0& 0\\ -\sin \mathrm{E\θ}& 0& \cos \mathrm{\θ}& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}\cos \mathrm{E\ψ}& -\sin \mathrm{E\ψ}& 0& 0\\ \sin \mathrm{E\ψ}& \cos \mathrm{E\ψ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(6\right)$

[0027]当等式(6)完全写出时，得到等式(7)：

$\mathrm{Euler}(\mathrm{E\φ},\mathrm{E\θ},\mathrm{E\ψ})=\left(\begin{array}{cccc}\cos E\mathrm{\φ}\cos E\mathrm{\θ}\cos \mathrm{E\ψ}-\sin E\mathrm{\φ}\sin \mathrm{E\ψ}& -\cos E\mathrm{\φ}\cos E\mathrm{\θ}\sin \mathrm{E\ψ}-\sin E\mathrm{\φ}\cos \mathrm{E\ψ}& \cos E\mathrm{\φ}\sin \mathrm{E\θ}& 0\\ \sin E\mathrm{\φ}\cos E\mathrm{\θ}\cos \mathrm{E\ψ}+\cos E\mathrm{\φ}\sin \mathrm{E\ψ}& -\sin E\mathrm{\φ}\cos E\mathrm{\θ}\sin \mathrm{E\ψ}+\cos E\mathrm{\φ}\cos \mathrm{E\ψ}& \sin E\mathrm{\φ}\sin \mathrm{E\θ}& 0\\ -\sin E\mathrm{\θ}\cos \mathrm{E\φ}& \sin E\mathrm{\θ}\sin \mathrm{E\ψ}& \cos \mathrm{E\θ}& 0\\ 0& 0& 0& 1\end{array}\right)---\left(7\right)$

[0028]参考坐标系记为O-XYZ，并且所述初始传感器坐标系记为O₀-x₀y₀z₀。所述坐标变换A(0)在t＝0时刻使所述参考坐标系和所述坐标系O₀-x₀y₀z₀间产生联系。在t＝t_n时刻的坐标系记为O_n-x_ny_nz_n。并且假设该坐标系的原点未移动，所以它们是相同的。从那时以后，如图7中所示，由于所述移动体姿态的变化，所述坐标系从O_(n-1)-x_(n-1)y_(n-1)z_(n-1)变化为O_n-x_ny_nz_n，那么O_(n-1)-x_(n-1)y_(n-1)z_(n-1)和O_n-x_ny_nz_n由从所述输出值获得的矩阵A(n)相联系。从所述参考坐标系所见的传感器坐标系T(n)通过从右顺次应用所述变换A(n)由等式(8)得到。当所述传感器坐标系的原点随着时间移动时，则随着时间变化的坐标顺次插入所述矩阵A的第四列的前三个元素。所述矩阵A的该第四列将不在此特别进行详述，因为它不会由于所述传感器坐标系的旋转而受到影响。

T(n)＝A(0)A(1).....A(n-1)A(n)    (8)

[0029]然后，将说明由光纤陀螺仪或类似物的角速度输出值得到微旋转矩阵A(n)的方法。三个角速度传感器设置在所述传感器坐标系的不同轴上，并且，如图8中所示，它们测量绕所述传感器的x、y和z轴的角速度。这时，在等式(4)中，旋转角Δφ、Δθ和Δψ足够小，则为这种情况：

sin≈θ    (9)

cos≈1    (10)

因此，利用绕传感器z轴的微旋转角Δφ、绕传感器y轴的微旋转角Δθ和绕传感器x轴的微旋转角Δψ表示等式(11)是可行的。因为，作为等式(11)的结果，所述矩阵的每个元素由一个独立的微旋转角组成，近似地，不依赖于所述旋转的顺序。

$A\left(i\right)=\left(\begin{array}{cccc}1& -\mathrm{\Δ\φ}& \mathrm{\Δ\θ}& 0\\ \mathrm{\Δ\φ}& 1& -\mathrm{\Δ\ψ}& 0\\ -\mathrm{\Δ\θ}& \mathrm{\Δ\ψ}& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(11\right)$

[0030]在所述微角度和所述输出值之间，即所述微旋转角Δφ、Δθ和Δψ，与来自所述角度传感器的输出值ωx、ωy和ωz，和所述采样周期t_s之间，具有由等式(12)至(14)给定的关系。因为所述采样周期t_s为关于所述旋转运动足够快的周期，从而在所述采样周期t_s内所述旋转足够小，并且可以认为是微旋转角。

Δφ＝ω_x·t_s    (12)

Δθ＝ω_y·t_s    (13)

Δψ＝ω_z·t_s    (14)

[0031]因此，所述矩阵A(n)由等式(15)给出：

$A\left(n\right)=\left(\begin{array}{cccc}1& -{\mathrm{\ω}}_x\·t_s& {\mathrm{\ω}}_y\·t_s& 0\\ {\mathrm{\ω}}_x\·t_s& 1& -{\mathrm{\ω}}_z\·t_s& 0\\ -{\mathrm{\ω}}_y\·t_s& {\mathrm{\ω}}_z\·t_s& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(15\right)$

[0032]然后，将描述由所述姿态矩阵获得所述姿态角的技术。

[0033]所述姿态矩阵T(n)由等式(16)给出：

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(16\right)$

[0034]所述横摆角φ为：

φ＝atan2(b，a)    (17)

[0035]所述横摆角φ(其为一个姿态角)的范围是-π＜φ≤π。

[0036]所述俯仰角θ为：

θ＝atan2(-c，cosφ·a+sinφ·b)    (18)

[0037]所述俯仰角θ(其为一个姿态角)的范围是-π/2≤θ≤π/2。

[0038]所述侧倾角ψ为：

ψ＝atan2(sinφ·g-cosφ·h，-sinφ·d+cosφ·e)    (19)

[0039]所述侧倾角ψ(其为一个姿态角)的范围是-π＜ψ≤π。

[0040]如果使用欧拉角，则应用等式(20)至(23)：

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(20\right)$

[0041]

Eφ＝atan2(b，a)    (21)

Eθ＝atan2(cosφ·g+sinφ·h，i)    (22)

Eψ＝atan2(-sinφ·a+cosφ·b，-sinφ·d+cosφ·e)    (23)

[0042]下面，将说明所述矩阵的正规化。

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(24\right)$

[0043]在所述计算后，因为所述姿态矩阵T(n)的每个列有时不是单位矢量，因而以等式(25)对所述姿态矩阵进行正规化，所以等式(24)中的每列矢量的大小变为1。

$\mathrm{Normalized}\left[T\right]=\left(\begin{array}{cccc}{p}_{1}a& p_{2}d& p_{1}b{p}_{2}f-p_{1}c{p}_{2}e& 0\\ p_{1}b& p_{2}e& p_{1}c{p}_{2}d-p_{1}a{p}_{2}f& 0\\ p_{1}c& p_{2}f& p_{1}a{p}_{2}e-p_{1}b{p}_{2}d& 0\\ 0& 0& 0& 1\end{array}\right)---\left(25\right)$

[0044]在此，p₁和p₂由等式(26)和(27)给出。

$p_1=\frac{1}{\sqrt{a^2+b^2+c^2}}---\left(26\right)$

$p_2=\frac{1}{\sqrt{d^2+e^2+f^2}}---\left(27\right)$

[0045]在正规化后观察所述元素，T(n)变为：

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& g& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(28\right)$

[0046]现在，将说明所述矩阵的正交。

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(29\right)$

[0047]关于所述姿态矩阵T(n)，因为在所述计算后，所述姿态矩阵的列有时不是正交轴，因而执行正交处理，以使所述矢量正交(在这种情况下，所述z轴作为参照)。为了获得正交于所述z轴和所述y轴的一个新的x’轴，得到a′、b′和c′：

a′＝ei-fh    (30)

b′＝fg-di    (31)

c′＝dh-eg    (32)

[0048]然后，为了获得正交于所述z轴和所述x’轴的一个新的y’轴，得到d′、e′和f：

d′＝hc′-ib′    (33)

e′＝ia′-gc′    (34)

f＝gb′-ha′    (35)

[0049]由这些所获得的a’至f’得到正交姿态矩阵T(n)：

$T\left(n\right)\left(\begin{array}{cccc}{a}^{\′}& d^{\′}& g& 0\\ b^{\′}& e^{\′}& h& 0\\ c^{\′}& f^{\′}& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(36\right)$

[0050]现在，将说明函数atan2。atan2(y，x)是用于电脑的函数，具有两个变量x和y。其适用范围比通常使用的atan函数更广。

ξ＝atan2(y，x)    (37)

(-π＜ξ≤π)

当x＞0且y＞0时，则

ξ＝tan^-1(y/x)    (38)

当x＞0且y＜0时，则

ξ＝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

[0051]下面，说明所述倾斜角的计算。这是以来自所述加速度传感器10的加速度为基础，用所述姿态角计算单元14计算所述倾斜角的方法。所述倾斜角为所述传感器x、y和z轴与所述参考Z轴之间的角度λx、λy和λz。也就是说，

λx为所述x轴与所述Z轴之间的角度；

λy为所述y轴与所述Z轴之间的角度；及

λz为所述z轴与所述Z轴之间的角度。

λx、λy和λz的范围是0≤(λx，λy，λz)≤π。图9显示了所述倾斜角和所述重力矢量。如下所述，所述倾斜角从沿着所述传感器坐标设置的加速度传感器获得。所述加速度Gx、Gy和Gz使用等式(40)至(42)进行正规化，并且从而得到正规化后的加速度Gx′、Gy′和Gz′。

${G_x}^{\′}=\frac{G_x}{\sqrt{{G_x}^2+{G_y}^2+{G_z}^2}}---\left(40\right)$

${G_y}^{\′}=\frac{G_y}{\sqrt{{G_x}^2+{G_y}^2+{G_z}^2}}---\left(41\right)$

${G_z}^{\′}=\frac{G_z}{\sqrt{{G_x}^2+{G_y}^2+{G_z}^2}}---\left(42\right)$

[0052]所述倾斜角λx、λy和λz由所述加速度Gx、Gy和Gz通过利用等式(43)至(45)获得。

λ_x＝Arc cos(-G_x′)    (43)

λ_y＝Arc cos(-G_y′)    (44)

λ_z＝Arc cos(-G_z′)    (45)

[0053]下面，将描述由所述倾斜角λx、λy和λz获得所述姿态矩阵T(n)的技术。所述姿态矩阵是基于所述倾斜角由所述姿态角计算单元14通过计算获得的。

c＝cos(λx)                            (46)

a＝+(1-c2)                           (47)

b＝0                                   (48)

f＝cos(λy)                            (49)

d＝-cf/a                               (50)

e＝+(1-f2-d2)(其中0≤λ z＜π/2))    (51)

e＝-(1-f2-d2)(其中π/2＜λz≤π)     (52)

e＝0  (其中λz＝π/2)                  (53)

g＝-ce(54)

h＝cd-af                               (55)

i＝ae                                  (56)

[0054]所述姿态矩阵T(n)由以上结果获得。

[0055]应当理解，当由所述姿态矩阵T(n)获得所述倾斜角λx、λy和λz时，应用以下等式：

$T\left(n\right)=\left(\begin{array}{cccc}a& d& g& 0\\ b& e& h& 0\\ c& f& i& 0\\ 0& 0& 0& 1\end{array}\right)---\left(57\right)$

λx＝acos(c)     (58)

λy＝acos(f)     (59)

λz＝acos(i)     (60)

[0056]如此，以该具体实施方式，通过将由所述加速度传感器10获得的姿态角和由独立设置的姿态角传感器获得的参考姿态角进行比较，以简单的方式来校正所述加速度传感器的输出值是可行的。

[0057]第二具体实施方式

图2为该第二具体实施方式的结构框图。与图1的不同点为设置了三个加速度传感器10a、10b和10c作为所述加速度传感器10，它们检测沿着所述三个轴x、y和z方向上的加速度；并且此外，三个校正计算单元12a、12b和12c分别相应于这些加速度传感器10a、10b和10c而设置。

[0058]由这三个加速度传感器10a、10b和10c检测所述加速度，通过由其输出值计算所述姿态角而唯一指定所述移动体的姿态是可行的。在改变所述移动体的姿态时顺次执行指定的姿态，并且在这些指定的姿态下检测的姿态角与设置在所述寄存器20内的参考姿态角相比较。例如，所述机器人的姿态可以被顺次改变，以使所述x轴、所述y轴和所述z轴在所述Z轴的方向(也就是，在垂直方向上)，并且利用在这些时候的加速度姿态角和所述参考姿态角的差，所述加速度传感器10a、10b和10c的输出值可以顺次得到校正。可以不仅提供所述加速度传感器10a和10b，而是一般地多达n个加速度传感器(其中n≥2)。

[0059]应当理解，为了方便，在附图中，来自所述零点校正单元26的校正信号仅显示为输出到所述校正计算单元12a，但是它也可以输出到所述其它校正计算单元12b和12c。这同样适用于所述灵敏度校正单元28。

[0060]第三具体实施方式

图3显示了该第三具体实施方式的结构。在上文所述的具体实施方式中，对于所述输出值的校正是当所述机器人静止于指定姿态下时执行的。因而，例如在所述加速度传感器的校正单元根据来自用户或所述机器人的主处理器经由输入单元50输入的命令执行校正的结构的情况下，当收到校正执行命令时，有必要作出所述机器人这时是否静止的判定，以便可执行校正。如此，一旦从外部收到校正执行命令，图3中的静止判定单元30作出所述机器人是否处于静止状态的判定。

[0061]所述静止判定单元30检测来自所述姿态角计算单元14的姿态角的变化量，并判定是否这些变化量小于或等于预定值。如果所述姿态角的变化量小于或等于所述预定值，则判定所述机器人处于静止状态，将校正允许信号输出到所述校正值计算单元18。所述校正值计算单元18收到该校正允许信号时计算校正值，并将其输出到所述零点校正单元26等。也可以使所述静止判定单元30不是去检测所述姿态角的变化量，而是去检测来自所述加速度传感器10自身的输出值的变化量，并将这些变化量与预定值相比较，由此判定所述静止状态。如果所述机器人不是静止而是运动的，那么平移加速度与离心加速度彼此相叠加，而且因为待校正的输出值随时间变化，所以所述校正的精度显著降低。在所述机器人处于静止状态时，通过对所述输出值执行所述校正而确保所述校正的精度是可行的。

[0062]图4为显示了该具体实施方式的处理的流程图。首先，输入来自所述用户的校正命令(或者来自已从用户处接收到命令的主处理器)，并且输入侧倾角ψi、俯仰角θi和横摆角φi(在步骤S101中)作为所述参考姿态角。由所述用户输入或按照其命令输入的这些参考姿态角(ψi，θi，φi)设置在所述寄存器20内。所述静止判定单元30一旦收到该校正命令，则检测所述加速度传感器10a、10b和10c的输出值，或者来自姿态角校正计算单元12a、12b和12c的姿态角的变化量(时间波动宽度)，并判定它们是否小于预定值(在步骤S102中)。如果这些变化量小于或等于所述预定值，则所述静止判定单元30判定所述机器人处于静止状态。应当理解也可以将所述变化量小于或等于所述预定值的时间段与预定阈值时间段相比较，并且仅在所述时间段大于或等于所述预定阈值时间段时才判定所述机器人处于静止状态。例如，该预定阈值时间段可以设置为三秒，从而可能检测到有校正意义的所需的静止状态。

[0063]如果所述静止判定单元30已判定所述机器人处于静止状态，则如上所述所述静止判定单元30输出所述校正允许信号到所述校正值计算单元18。并且，一旦收到该校正允许信号，以此时的加速度姿态角和所述参考姿态角的差为基础，所述校正值计算单元18计算并输出校正值以使该差值减小或消除。所述校正计算单元12a、12b和12c通过使用这些校正值执行(在步骤S103中)对所述输出值的零点校正或灵敏度校正。

[0064]然后，作出是否重复所述校正的决定(在步骤S104中)，并且，如果有必要多次执行所述校正，那么所述机器人的姿态被改变(在步骤S105中)，并且再次输入所述参考姿态角(ψj，θj，φj)，并执行相同的校正处理。应当对所有所述三个加速度传感器10a、10b和10c执行校正，那么，在这种情况下，所述校正处理被重复至少三次。例如，在姿态(0，0，0)下，对所述加速度传感器10c在所述z轴方向上执行灵敏度校正；然后，在姿态(π/4，0，0)下，对所述加速度传感器10a在所述y轴方向上执行灵敏度校正；以及，最后，在姿态(0，π/4，0)下，对所述加速度传感器10b在所述x轴方向上执行灵敏度校正。关于三个加速度传感器，如果通过对所述两个加速度传感器(10a，10b)在所述x轴方向上和在所述y轴方向上执行校正可以获得足够的精度，那么即使不对所述z轴方向执行校正，也可以仅执行两次校正。这预示了所述机器人的姿态不是非常倾斜的情况等等。

[0065]在图示的具体实施方式中，所述控制器用普通用途的处理器实现。本领域技术人员应当理解，所述控制器可以使用单一用途的集成电路(举例来说，ASIC)实现，所述集成电路具有用于全面的系统级控制的主处理器或中心处理器部分，及在所述中心处理器部分的控制下专用于执行各种不同的特定计算、函数和其它程序的单独的部分。所述控制器可以为多个单独专用的或可编程集成的或其它电子电路或单元(举例来说，硬接线电子或逻辑电路诸如分离元件电路，或可编程逻辑单元诸如PLD、PLA、PAL或类似物)。所述控制器可以为用于一般用途的计算机例，如微处理器、微控制器或其它处理单元(CPU或MPU)而适当编程，它们可以单独使用或者与一个以上外围(举例来说，集成电路)数据和信号处理单元结合使用。通常，在能够执行此处所述的程序的有限状态机上的各种单元的任何单元或部件都可以用作所述控制器。分布式处理结构可以用于实现最大数据/信号处理能力和速度。

[0066]虽然本发明参照其优选具体实施方式进行了描述，但应当理解本发明不限于所述优选具体实施方式或结构。相反地，本发明应该包含各种修改和等同配置。另外，虽然所述优选具体实施方式的各种部件以示例性的各种组合和结构给出，但其它组合和结构，包括更多、更少或仅单个部件也在本发明的构思和范围内。

Claims (10)

1.一种加速度传感器的校正设备，包括：

计算单元，其用于以来自设于移动体上的加速度传感器的输出值为基础，计算所述移动体的姿态角数据；和

校正单元，其用于通过将所述姿态角数据和参考姿态角数据相比较，来校正所述加速度传感器的所述输出值。

2.根据权利要求1所述的校正设备，进一步包括设置单元，其用于将所述参考姿态角设置为当所述移动体处于特定姿态时的姿态角。

3.根据权利要求1或2所述的校正设备，其中：

设有多达n个(此处n≥2)的所述加速度传感器；并且

所述校正单元对用于所述移动体的n个不同特定姿态的输出值进行校正。

4.根据权利要求1至3中任意一项所述的校正设备，进一步包括检测单元，其用于检测所述移动体的静止状态，并且其中所述校正单元校正处于所述静止状态的所述输出值。

5.根据权利要求4所述的校正设备，其中所述检测单元根据所述加速度传感器的所述输出值的变化量或来自所述计算单元的所述姿态角数据的变化量是否小于或等于预定值，来检测所述静止状态。

6.根据权利要求4所述的校正设备，其中所述检测单元根据来自所述计算单元的所述姿态角数据的变化量是否小于或等于预定值，来检测所述静止状态。

7.根据权利要求4至6中任意一项所述的用于加速度传感器的校正设备，进一步包括输入单元，其用于输入用于校正所述输出值的校正命令信号，并且其中当所述校正命令信号已被输入时，所述检测单元检测所述静止状态。

8.根据权利要求1至7中任意一项所述的校正设备，其中所述校正单元校正所述输出值的零点和灵敏度中的至少一个。

9.一种校正加速度传感器的输出值的方法，包括：

以来自加速度传感器的输出值为基础，计算移动体的姿态角数据；

将所述姿态角数据和参考姿态角数据相比较；以及

以所述姿态角数据和所述参考姿态角数据的比较结果为基础，对所述加速度传感器的所述输出值进行校正。

10.一种加速度传感器的校正设备，包括：

计算装置，其以来自设于移动体上的加速度传感器的输出值为基础，计算所述移动体的姿态角数据；和

校正装置，其通过将所述姿态角数据和参考姿态角数据相比较，来校正所述加速度传感器的所述输出值。

Images