角速度传感器的零点校正装置和方法

摘要

角速度传感器(10)设置在诸如机器人或类似物的移动体内。第一给定器(14)基于角速度的变化宽度是否小于或等于预定值来进行与静止状态有关的判定。静止状态判定器(20)判定所述静止状态是否已经持续超过判定时间。n－i总和平均单元(34)计算(n－i)个数据片的总和平均值，所述(n－i)个数据片是通过从n个在被判定为所述静止状态的延续的期间内输出的n个数据片中排除恰好在所述期间的结束正时之前输出的i个数据片来获取的，并且将其判定为零点偏移。零点校正器(36)进行所述角速度传感器(10)的输出值的零点校正，并且将校正后的值输出到输出单元(28)。

说明书

技术领域

[0001]本发明涉及角速度传感器，尤其涉及设置在诸如机器人等移动体内的角速度传感器的零点校正。

背景技术

[0002]加速度传感器和横摆率传感器用于诸如机器人等移动体的姿态控制。当三个正交轴分别为x轴、y轴和z轴时，轴的方向上的加速度通过三个加速度传感器检测得到，而关于轴的横摆率通过三个横摆率传感器检测得到。关于轴的角度，或者姿态角(侧倾角、俯仰角和横摆角)，通过对横摆率传感器的输出进行时间积分来获取。

[0003]公开号为2004-268730的日本专利申请公开了一种通过使用由陀螺传感器输出的加速度数据和姿态数据进行姿态控制的技术。

[0004]然而，由于姿态角通过角速度的积分建立，角速度传感器的偏移和漂移被逐渐累积。因此，如果偏移等过大，那么姿态角逐渐形成一个随时间增加和偏离的非常大的值。这可以通过使用偏移和漂移小的角速度传感器来轻易地避免。然而，这种角速度传感器大、重且昂贵。

发明内容

[0005]本发明的目的是提供一种能够检测移动体的姿态角同时能够避免积分误差的累积，并且采取简单结构而无需使用昂贵的高精度角速度传感器的装置和方法。

[0006]本发明的第一方案涉及设置在移动体内的角速度传感器的零点校正装置。所述零点校正装置包括：检测器件，其检测移动体的静止状态；计算器件，其计算在静止状态期间角速度传感器的输出值的平均值；以及校正器件，其将角速度传感器的零点校正到平均值。

[0007]在所述零点校正装置中，检测到设置有角速度传感器的移动体的静止状态，并且静止状态期间角速度传感器的输出值用于校正角速度传感器的零点。由于在移动体的静止状态期间，原则上角速度传感器的输出值应该为零，在静止状态期间从传感器输出的输出值形成零点偏移。因此，零点通过静止状态期间角速度传感器的输出值的平均值来进行校正，从而避免通过角速度的积分所获取的姿态角的误差累积。

[0008]移动体的静止状态可以通过来自角速度传感器的输出的变化宽度和设置在同一移动体内的加速度传感器的输出的变化宽度中的至少一个来检测。由于理想的是静止状态持续至少一定时间以确保零点校正良好的精确性，所以零点可以通过在持续至少预定判定时间的静止状态期间传感器输出值的平均值来进行校正。

[0009]本发明的第二方案涉及设置在移动体内的角速度传感器的零点校正方法。所述方法包括判定移动体是否处于静止状态的步骤；计算在所述静止状态期间所述角速度传感器的输出值的平均值的步骤；以及将所述角速度传感器的零点校正到所述平均值的步骤。

[0010]根据本发明，可以校正角速度传感器的零点，避免积分误差的累积，并且在采用简单结构的同时检测移动体的姿态角。

附图说明

[0011]通过下文中参照附图对示例性实施例的描述，本发明上述和其他的目的、特征和优点将变得明显，其中相同或相应的部分将由相同的附图标记表示，并且其中：

图1为一个实施例的结构框图；

图2为示出校正值计算方法的时间图；

图3为示出改进的校正值计算方法的时间图；

图4为示出进一步改进的校正值计算方法的时间图；

图5为示出进一步改进的校正值计算方法的时间图；

图6为示出进一步改进的校正值计算方法的时间图；

图7为另一个实施例的结构框图；

图8A和图8B示出又一个实施例的结构框图。

具体实施方式

[0012]下文将参照附图对本发明的实施例进行说明。

[0013]<第一实施例>

图1示出该实施例的结构框图。角速度传感器10设置在诸如机器人等移动体内的预定位置上，并且检测移动体的角速度。例如，角速度传感器10采用xyz正交坐标系作为传感器坐标系，并且检测关于轴，即，x轴、y轴和z轴的角速度。角速度传感器10将检测到的角速度输出到第一比较器12。

[0014]第一比较器12在量值上将输入角速度的变化宽度(波动宽度)与预定范围进行比较。该预定范围由第一给定器14设定。第一给定器14可设定固定范围作为预定范围。如图1所示，也可以提供将寄存器16设定的范围设定为预定范围的结构，使用者可以通过使用输入单元18在寄存器16中设定理想的范围。第一比较器12将角速度的变化宽度与预定范围在量值上的比较结果输出到静止判定器20。例如，当角速度的变化宽度在预定范围内时，第一比较器12将信号HI输出到静止判定器20，而当角速度的变化宽度超过预定范围时，第一比较器12将信号LOW输出到静止判定器20。

[0015]静止判定器20输入来自第一比较器12的比较结果，并且检测角速度传感器10的输出进入预定范围的时间(开始时间to)。静止判定器20也检测角速度传感器10的输出偏离预定范围的时间(结束时间te)。静止判定器20还测量自开始时间to起经过的时间(持续时间tok)。静止判定器20判定持续时间tok是否已经超过预定判定时间tos。如果持续时间tok已经超过判定时间tos，则静止判定器20将开始正时信号Tst输出到输出单元28。第二给定器22提供预定判定时间tos。在寄存器24处适当地设定第二给定器22的设定内容。输出开始正时信号后，静止判定器20在角速度传感器10的输出偏离预定范围的时刻(结束时间te)将结束正时信号Ten输出到输出单元28。此外，静止判定器20测量自开始正时信号Tst起经过的时间(持续时间tsk)。当持续时间tsk超过校正结束时间toe时，静止判定器20以强迫方式将结束正时信号Ten输出到输出单元28。

[0016]输出单元28将开始正时信号Tst和结束正时信号Ten输出到管理单元32。管理单元32接收开始正时信号Tst，并且在n-i总和平均单元34开始平均处理。此外，管理单元32接收结束正时信号Ten，并且在n-i总和平均单元34结束平均处理。此外，管理单元32参照寄存器30，将n-i总和平均单元34的-i处理时间toi指示给n-i总和平均单元34。另外，如果持续时间tsk大于或等于预定校正最小时间toh，则管理单元32经由n-i总和平均单元34向零点校正器36发出指令，用n-i总和平均值来更新零点校正值。如果持续时间tsk小于预定校正最小时间toh，则管理单元32判定没有获得足够的精确度；因此，零点校正器36处的零点校正值不更新。

[0017]n-i总和平均单元34具有寄存器，该寄存器保留与预定时间相对应的i个数据片，并且一旦接到来自管理单元32的指令便开始保留i个数据片。n-i总和平均单元34等待直至已经保留了i个数据片。当接收到第(i+1)个数据片时，n-i总和平均单元34以第一数据片为起始开始总和平均计算。以此方式，反复进行第一至第(n-i)个数据的总和平均计算。一旦接收到来自管理单元32的结束指令，n-i总和平均单元34将(n-i)总和平均值输出至零点校正器36。零点校正器36通过以输入的平均值更新零点来校正零点。采取第一至第(n-i)个数据片的总和平均计算的这种方式的原因如下。即，如果计算第一至第n个数据片的总和平均值，则所述计算包括了角速度实际开始变化后所获得的数据片，因此其精确度降低。因此，通过在获取总和平均值中使用第一至第(n-i)个数据片，直至先于te的预定时间toi的时间点，可以精确地计算零点校正值。

[0018]下文将参照角速度传感器的输出值的时间图对所述实施例的零点校正值计算过程进行说明。

[0019]图2示出从角速度传感器10输出的角速度依时间的变化。在图2中，横轴表示时间(s)，纵轴表示角速度(rad/s)。对于基本计算，校正值的总和平均计算开始于开始时间to，并且校正值的总和平均计算结束于结束时间te。分别在时间to，te时输出开始正时信号Tst和结束正时信号Ten。来自n-i总和平均单元34的总和平均值Mean1输出到零点校正器36，从而更新零点校正值。

[0020]图3示出改进的零点校正值计算方法。与图2中的情况相同，考虑到校正的稳定性，当持续时间tok超过预定判定时间tos(即，在时间to+tos)，而不是在开始时间to时，开始正时信号Tst输出到输出单元28，以开始校正值的总和平均计算。在结束时间te，结束正时信号Ten输出到输出单元28，以结束校正值的总和平均计算。零点校正器36的校正值更新为计算后的校正值Mean2。由于平均过程中没有包括紧随着静止状态的开始之后的不稳定数据，因此，可以避免不必要的校正的发生。

[0021]图4示出进一步改进的零点校正值计算方法。与图3中的情况基本相同，就作为预定时间的时间(即，时间te-toi)处的数据而言，所述预定时间即结束时间te之前的-i处理时间toi，进行总和平均计算(n-i总和平均计算)，并且n-i总和平均值Mean3被输出到零点校正器36。图4中的Tn-i是管理单元32作为指令给n-i总和平均单元34的正时信号。因此，可以从总和平均值中排除在结束时间te之前的具有超低精确度的数据群，并且可以改进精确度。可以通过n-i总和平均单元34改变时间toi或与时间toi相对应的从角速度传感器10输出的输出值的数目。

[0022]图5示出进一步改进的零点校正值计算方法。与图2至图4中的情况相同，测量持续时间tsk，并且当持续时间tsk等于或大于校正最小时间toh时，利用总和平均值来更新零点校正值的命令经由n-i总和平均单元34给到零点校正器36。因此，可以确保保持良好的精确度所需样本的数量，并且使以良好的精确度更新校正值成为可能。

[0023]图6示出进一步改进的零点校正值计算方法。与图2至图5中的情况相同，当持续时间tsk达到预定时间时，即校正结束时间toe，静止判定器20以强迫方式输出结束正时信号Ten，从而结束静止校正。因此，可以在预定时间内获得具有必要精确度的零点校正值，并且从而能够更新所述零点校正值。在这些计算方法中，判定时间tos约为0.1s至1s，例如，可以设定为约0.3s。尽管判定时间越短，转换到静止校正越早，但判定时间的设定取决于移动体的稳定响应。需要相对较长时间达到静止状态的移动体的判定时间被设定得相对较长。然而，如果判定时间被设定得过长(例如，5s或更长)，则转换到静止校正的频率下降，从而使精确度总体上下降。

[0024]此外，校正最小时间toh约为1s至10s是适当的。校正最小时间toh可以约为8s。将校正最小时间toh设定为尽可能长的时间实现了校正值精确度的改进。然而，如果校正最小时间toh被设定得过长，则执行静止校正的频率下降，从而使精确度总体上下降。因此，校正最小时间toh的设定也与移动体的动作一致。

[0025]-i处理时间toi约为0.1s至1s，并且可以约为0.3s。尽管较短的-i处理时间toi通常增加静止校正数据的数量，但是-i处理时间toi的设定取决于移动体的激活响应。对于需要相对较长的时间来激活的移动体，-i处理时间toi被设定得相对较长。考虑到将其设定为过长的时间(例如，约5s)不便于减少用于静止校正的数据的数量，并且带来了与初衷相反的校正值的精确度的下降，-i处理时间toi可以设定为适当的值。

[0026]校正结束时间toe约为1s至300s，并且可以约为3s。通常，较长的校正结束时间toe改进校正值的精确度。然而，如果校正结束时间toe被设定得过长，则执行静止校正的频率下降，从而使精确度总体上下降。因此，可取的是将校正结束时间toe设定得与移动体的运动一致。校正结束时间toe被设定为比校正最小时间toh长至少-i处理时间toi的时间。

[0027]附带地，第一比较器12、静止判定器20、管理单元32、n-i总和平均单元34、零点校正器36等可以由微处理器形成。

[0028]<第二实施例>

图7示出该实施例的结构框图。在图1的实施例中，将来自角速度传感器10的角速度用于与机器人的静止状态有关的判定。然而，在第二实施例中，来自设置在同一机器人中的加速度传感器的加速度用于与静止状态有关的判定。

[0029]加速度传感器40检测在机器人轴向即x轴、y轴和z轴方向上的加速度，并将所述加速度输出到第二比较器42。

[0030]第二比较器42在量值上将第三给定器44内设定的预定范围与加速度的变化宽度进行比较，并且将比较结果输出至第三比较器48。所述预定范围由第三给定器44设定。第三给定器44可以设定固定范围作为预定范围。如图7所示，也可以提供将寄存器46设定的范围设定为预定范围的结构，使用者可以通过使用输入单元在寄存器46中设定理想的范围。

[0031]第三比较器48判定开始正时和结束正时时的加速度的绝对值是否基本等于重力加速度(例如，9.797072m/s²，在京都大学的国际基准点。术语“基本”含盖了重力加速度±允许误差，并且通过对预期在加速度传感器40内出现的偏移分解因式来设定允许误差)，或判定所述期间内的重力加速度是否基本等于重力加速度。如果这些加速度不是基本等于重力加速度，则第三比较器48判定机器人不处于静止状态。另一方面，如果这些加速度基本等于重力加速度，则第三比较器48判定机器人处于静止状态，并且将判定结果输出到静止判定器20。后续程序与第一实施例中相应的程序相似。使用加速度进行与静止状态有关的判定有助于平移运动和振动运动的检测，并且改进了角速度传感器10的零点校正的精确度。

[0032]<第三实施例>

图8A和8B示出该实施例的结构框图。在所述实施例中，角速度和加速度二者均被用作与静止状态有关的判定。

[0033]使用角速度来判定静止状态的结构与图1示出的结构相似。然而，第一比较器12将比较结果输出至组合比较器50，而不是静止判定器20。

[0034]另一方面，使用加速度来判定静止状态的结构与图7示出的结构相似。然而，第三比较器48将比较结果输出至组合比较器50，而不是静止判定器20。

[0035]组合比较器50输入来自第一比较器12的比较结果，以及来自第三比较器48的比较结果，如果两个比较结果均在一定范围内，则判定机器人处于静止状态，并且将判定结果输出至静止判定器20。后续程序与第一实施例中相应的程序相似。

[0036]在第三实施例中，基于角速度和加速度的组合来进行与静止状态有关的判定，从而使机器人的平移和旋转运动可以被监控。因此，可以以甚至更高的精确度来判定静止状态的出现，从而可以非常顺利地校正角速度传感器10的零点。

[0037]虽然上文对本发明的实施例进行了说明，但是本发明不局限于此，而是可以进行多种改变。

[0038]例如，尽管在图1至图8B中，使用者可以通过经由输入单元18在寄存器24中设定理想值来调节判定存在显著的静止状态的判定时间tos，判定时间tos可以自动地设定，来代替使用者手动设定。此外，可以改变组合比较器50处的组合判定标准，而不是判定时间。例如，在即使经过了预定时间之后也不进行零点校正的情况下，如果判定角速度和加速度中的一个处于静止状态，则可以判定机器人处于静止状态，而不是如果判定角速度和加速度二者均处于静止状态才判定机器人处于静止状态。因此，可以放宽判定条件以确保零点校正的早期执行。寄存器16、24、30、46可以由一个器件形成。