CN102160008A - 防摔倒控制装置及计算机程序 - Google Patents

Abstract

本发明提供一种防摔倒控制装置及计算机程序，该防摔倒控制装置结构简单，噪声、偏移等不会积累，能确保向前后方向的移动并高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。所述防摔倒控制装置包括车轮、以及在车轮的上方朝俯仰方向及摇摆方向摇动的主体，包括：前进/后退指示接收部(42)，该前进/后退指示接收部(42)接收车轮的前进或后退指示；目标俯仰角计算部(44)，该目标俯仰角计算部(44)基于根据接收到的指示、和检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算目标俯仰角；俯仰倾斜角推定部(47)，该俯仰倾斜角推定部(47)根据检测出的俯仰角速度、和基于目标俯仰角而生成的俯仰用转矩指令，推定从平衡状态的俯仰倾斜角；以及俯仰用转矩指令生成部(49)，该俯仰用转矩指令生成部(49)基于目标俯仰角和俯仰倾斜角，生成俯仰用转矩指令。

Description

防摔倒控制装置及计算机程序

技术领域

本发明涉及即使是单轮车、也能防止朝俯仰方向(pitch direction)摔倒的防摔倒控制装置及计算机程序。

背景技术

以往，对于双轮车、两足步行机器人等，由于在移动时，主体朝以大体前后方向为轴的旋转(roll：摇摆)方向摇动，因此，需要控制动作，使得通过控制摇摆方向的平衡，进行移动或停止而不摔倒。在防止朝摇摆方向摔倒时，需要正确检测摇摆方向的倾斜。作为为了控制摇摆方向的平衡而检测主体的倾斜的方法，具有通过利用角速度传感器检测角速度并对检测出的值进行积分以推定倾斜的方法、利用重物(砝码)检测倾斜的方法等。

但是，在利用角速度传感器并对角速度进行积分的方法中，在通过角速度传感器检测出的角速度输出中含有噪声、偏移的情况下，噪声、偏移等因积分而放大。由于放大后的噪声、偏移等不断积累，因此，存在与目标值的偏差增大、无法继续进行倾斜角推定及防摔倒控制的问题。

另一方面，在利用重物检测倾斜的方法中，由于受到重力的影响而无法瞬时反映倾斜，因此，响应性差，此外，由于检测偏离垂直方向的倾斜，因此，存在无法检测从平衡状态的倾斜的问题。例如，对于重心位置偏离摇摆方向的情况、受到侧风的情况等，垂直方向未必是平衡状态。为了使其不会摔倒，需要使倾斜状态返回至平衡状态，并检测从平衡状态的倾斜。

例如，在专利文献1中，提出了如下防摔倒控制装置，包括：使检测轴朝向主体的大体前后方向的角速度传感器、使旋转轴朝向主体的大体前后方向的电动机、检测电动机的旋转位置或旋转速度的旋转传感器、以及与电动机的旋转轴相连接的惯性转子，根据角速度传感器的角速度输出和提供给电动机的转矩指令，推定主体相对于平衡状态的倾斜角，利用所推定的倾斜角对主体的倾斜进行修正。

在专利文献1中，由于无需对角速度输出进行积分，因此，噪声、偏移等不会积累，此外，由于能推定相对于平衡状态的倾斜角，因此，能利用所推定的倾斜角来修正成左右方向的平衡状态。但是，对于双轮车等，只要能控制左右方向的平衡就能防止摔倒，而对于单轮车等，即使控制左右方向的平衡，若不控制前后方向的平衡，则还会朝前后方向摔倒。

为了推定单轮车的前后方向的倾斜，在非专利文献1中，利用了陀螺传感器和两个加速度传感器。由于加速度传感器越远离旋转轴，振动越会施加到加速度传感器上，因此，通过利用两个加速度传感器，消除振动所产生的加速度而推定正确值。将从两个加速度传感器计算出的角度θa、和由陀螺传感器得到的角速度的积分值的角度θz输出，为了消除θa和θz中包含的高频噪声而附加滤波器，设输入滤波后的值的偏差e并乘以观测增益(observer gain)后的输出为dθz，将利用dθz对角度θz进行了修正的推定值

用于控制。

在非专利文献1中，虽然单纯对由陀螺传感器得到的角速度进行积分而得到的角度θz随时间而发生偏差，但由于对角度θz进行了修正的推定值

可以保持固定值，因此，通过利用修正得到的推定值

能对根据由陀螺传感器得到的角速度而计算出的角度偏移的偏差进行修正。

专利文献1：国际公开2007/063665号

非专利文献1：笠井崇史著，“单轮车的姿势控制”，筑波大学研究生院博士课程系统信息工学研究科硕士论文，2005年1月，p.1-37

发明内容

但是，在非专利文献1的单轮车中，虽然通过加速度传感器测量相对于重力方向(垂直方向)的倾斜，但与上述左右方向相同，对于前后方向，垂直方向也未必是平衡状态。因此，在垂直方向不是平衡状态的情况下，存在无法修正成平衡状态的问题。此外，为了推定倾斜，由于除了陀螺传感器之外，还需要两个加速度传感器，因此，也存在难以使防摔倒控制装置结构简化并小型化的问题。

本发明是鉴于上述情况而完成的，其目的在于提供一种防摔倒控制装置及计算机程序，该防摔倒控制装置结构简单，噪声、偏移等不会积累，能确保向前后方向的移动并高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。

为了达到上述目的，第一发明所涉及的防摔倒控制装置的特征在于，该防摔倒控制装置包括朝前后方向旋转而移动的车轮、以及与该车轮的旋转轴连接且在该车轮的上方朝俯仰方向及摇摆方向摇动的主体，该主体包括：俯仰用角速度传感器，该俯仰用角速度传感器检测俯仰方向的倾斜角度的角速度即俯仰角速度；俯仰用电动机，该俯仰用电动机与所述车轮的旋转联动而使该车轮旋转；以及俯仰用旋转传感器，该俯仰用旋转传感器检测该俯仰用电动机的旋转位置或旋转速度，该防摔倒控制装置利用伴随所述车轮的旋转的反作用转矩，修正所述主体的俯仰方向的倾斜，该防摔倒控制装置包括：前进/后退指示接收部，该前进/后退指示接收部接收所述车轮的前进或后退指示；目标俯仰角计算部，该目标俯仰角计算部基于根据接收到的前进或后退指示、和由所述俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算作为目标的俯仰方向的倾斜角度即目标俯仰角；俯仰倾斜角推定部，该俯仰倾斜角推定部根据由所述俯仰用角速度传感器检测出的俯仰角速度、和基于所述目标俯仰角而生成并提供给所述俯仰用电动机的俯仰用转矩指令，推定所述主体相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角；以及俯仰用转矩指令生成部，该俯仰用转矩指令生成部基于所述目标俯仰角和所述俯仰倾斜角，生成所述俯仰用转矩指令。

此外，第二发明所涉及的防摔倒控制装置的特征在于，在第一发明中，所述前进/后退指示接收部接收所述车轮的前进或后退指示以作为旋转速度或旋转角度。

此外，第三发明所涉及的防摔倒控制装置的特征在于，在第一或第二发明中，包括目标俯仰角速度计算部，该目标俯仰角速度计算部根据从所述目标俯仰角减去所述俯仰倾斜角而得到的俯仰角度偏差，计算作为目标的俯仰方向的倾斜角度的角速度即目标俯仰角速度，所述俯仰用转矩指令生成部基于所述目标俯仰角速度和检测出的所述俯仰角速度的偏差，生成所述俯仰用转矩指令。

此外，第四发明所涉及的防摔倒控制装置的特征在于，在第一至第三发明的任一项发明中，包括：俯仰方向外部转矩推定部，该俯仰方向外部转矩推定部基于所述俯仰倾斜角，推定使所述主体朝俯仰方向倾斜的俯仰方向外部转矩；以及转矩修正部，该转矩修正部朝抵消所推定的所述俯仰方向外部转矩的方向修正所述俯仰用转矩指令。

此外，第五发明所涉及的防摔倒控制装置的特征在于，在第一至第四发明的任一项发明中，所述主体包括：摇摆用角速度传感器，该摇摆用角速度传感器检测摇摆方向的倾斜角度的角速度即摇摆角速度；朝摇摆方向旋转的惯性转子；摇摆用电动机，该摇摆用电动机与该惯性转子的旋转联动而使该惯性转子旋转；以及摇摆用旋转传感器，该摇摆用旋转传感器检测该摇摆用电动机的旋转位置或旋转速度，该防摔倒控制装置包括：目标摇摆角计算部，该目标摇摆角计算部基于由所述摇摆用旋转传感器检测出的摇摆方向的旋转速度，计算作为目标的摇摆方向的倾斜角度即目标摇摆角；摇摆倾斜角推定部，该摇摆倾斜角推定部根据由所述摇摆用角速度传感器检测出的摇摆角速度、和基于所述目标摇摆角而生成并提供给所述摇摆用电动机的摇摆用转矩指令，推定所述主体相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角；目标摇摆角速度计算部，该目标摇摆角速度计算部根据从所述目标摇摆角减去所述摇摆倾斜角而得到的摇摆角度偏差，计算作为目标的摇摆方向的倾斜角度的角速度即目标摇摆角速度；以及摇摆用转矩指令生成部，该摇摆用转矩指令生成部基于所述目标摇摆角速度和检测出的所述摇摆角速度的偏差，生成所述摇摆用转矩指令。

此外，第六发明所涉及的防摔倒控制装置的特征在于，在第五发明中，在所述主体的重心位于与所述车轮的旋转轴交叉的垂直线上的状态下，所述摇摆用角速度传感器的检测轴朝向前后方向和水平方向。

此外，第七发明所涉及的防摔倒控制装置的特征在于，在第一至第六发明的任一项发明中，所述车轮的旋转的旋转比低于所述俯仰用电动机的旋转。

接下来，为了达到上述目的，第八发明所涉及的计算机程序的特征在于，该计算机程序可由装载于防摔倒控制装置的计算机执行，该防摔倒控制装置包括朝前后方向旋转而移动的车轮、以及与该车轮的旋转轴连接且在该车轮的上方朝俯仰方向及摇摆方向摇动的主体，该主体包括：俯仰用角速度传感器，该俯仰用角速度传感器检测俯仰方向的倾斜角度的角速度即俯仰角速度；俯仰用电动机，该俯仰用电动机与所述车轮的旋转联动而使该车轮旋转；以及俯仰用旋转传感器，该俯仰用旋转传感器检测该俯仰用电动机的旋转位置或旋转速度，该防摔倒控制装置利用伴随所述车轮的旋转的反作用转矩，修正所述主体的俯仰方向的倾斜，使所述计算机起到作为如下单元的功能：前进/后退指示接收单元，该前进/后退指示接收单元接收所述车轮的前进或后退指示；目标俯仰角计算单元，该目标俯仰角计算单元基于根据接收到的前进或后退指示、和由所述俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算作为目标的俯仰方向的倾斜角度即目标俯仰角；俯仰倾斜角推定单元，该俯仰倾斜角推定单元根据由所述俯仰用角速度传感器检测出的俯仰角速度、和基于所述目标俯仰角而生成并提供给所述俯仰用电动机的俯仰用转矩指令，推定所述主体相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角；以及俯仰用转矩指令生成单元，该俯仰用转矩指令生成单元基于所述目标俯仰角和所述俯仰倾斜角，生成所述俯仰用转矩指令。

此外，第九发明所涉及的计算机程序的特征在于，在第八发明中，使所述前进/后退指示接收单元起到作为接收所述车轮的前进或后退指示以作为旋转速度或旋转角度的单元的功能。

此外，第十发明所涉及的计算机程序的特征在于，在第八或第九发明中，使所述计算机起到作为目标俯仰角速度计算单元的功能，该目标俯仰角速度计算单元根据从所述目标俯仰角减去所述俯仰倾斜角而得到的俯仰角度偏差，计算作为目标的俯仰方向的倾斜角度的角速度即目标俯仰角速度，使所述俯仰用转矩指令生成单元起到基于所述目标俯仰角速度和检测出的所述俯仰角速度的偏差、生成所述俯仰用转矩指令的单元的功能。

此外，第十一发明所涉及的计算机程序的特征在于，在第八至第十发明的任一项发明中，使所述计算机起到作为如下单元的功能：俯仰方向外部转矩推定单元，该俯仰方向外部转矩推定单元基于所述俯仰倾斜角，推定使所述主体朝俯仰方向倾斜的俯仰方向外部转矩；以及转矩修正单元，该转矩修正单元朝抵消所推定的所述俯仰方向外部转矩的方向修正所述俯仰用转矩指令。

此外，第十二发明所涉及的计算机程序的特征在于，在第八至第十一发明的任一项发明中，所述主体包括：摇摆用角速度传感器，该摇摆用角速度传感器检测摇摆方向的倾斜角度的角速度即摇摆角速度；朝摇摆方向旋转的惯性转子；摇摆用电动机，该摇摆用电动机与该惯性转子的旋转联动而使该惯性转子旋转；以及摇摆用旋转传感器，该摇摆用旋转传感器检测该摇摆用电动机的旋转位置或旋转速度，使所述计算机起到作为如下单元的功能：目标摇摆角计算单元，该目标摇摆角计算单元基于由所述摇摆用旋转传感器检测出的摇摆方向的旋转速度，计算作为目标的摇摆方向的倾斜角度即目标摇摆角；摇摆倾斜角推定单元，该摇摆倾斜角推定单元根据由所述摇摆用角速度传感器检测出的摇摆角速度、和基于所述目标摇摆角而生成并提供给所述摇摆用电动机的摇摆用转矩指令，推定所述主体相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角；目标摇摆角速度计算单元，该目标摇摆角速度计算单元根据从所述目标摇摆角减去所述摇摆倾斜角而得到的摇摆角度偏差，计算作为目标的摇摆方向的倾斜角度的角速度即目标摇摆角速度；以及摇摆用转矩指令生成单元，该摇摆用转矩指令生成单元基于所述目标摇摆角速度和检测出的所述摇摆角速度的偏差，生成所述摇摆用转矩指令。

在第一发明及第八发明中，接收车轮的前进或后退指示，并基于根据接收到的前进或后退指示及由俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算出作为目标的俯仰方向的倾斜角度即目标俯仰角。根据由俯仰用角速度传感器检测出的俯仰角速度、和基于目标俯仰角而生成并提供给俯仰用电动机的俯仰用转矩指令，推定主体相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角。基于目标俯仰角和俯仰倾斜角，生成俯仰用转矩指令。由此，由于能高精度地推定主体相对于平衡状态朝俯仰方向倾斜的俯仰倾斜角，并且，未对俯仰用角速度传感器检测出的俯仰角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标俯仰角的计算误差，利用伴随车轮的旋转的反作用转矩，能高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。

这里，所谓“俯仰倾斜角”，是指将俯仰方向的平衡状态作为基本位置时的、在基本位置的主体与朝俯仰方向倾斜的主体的偏差角。所谓“平衡状态”，是指由主体等在俯仰方向负荷的重力所产生的转矩和逆风、顺风等所产生的转矩等外部转矩的总和成为0(零)的状态。

在第二发明及第九发明中，由于通过接收车轮的前进或后退指示以作为旋转速度或旋转角度，从而基于根据接收到的车轮的前进或后退指示及由俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算出目标俯仰角，因此，能确保用于进行所指示的移动的旋转速度，并修正俯仰方向的倾斜。此外，无需另外设置加速度传感器等，除了驱动车轮所需的电动机等之外，仅附加角速度传感器，以这样的简单结构，可使防摔倒控制装置简化并小型化。

在第三发明及第十发明中，根据从目标俯仰角减去俯仰倾斜角而得到的俯仰角度偏差，计算出作为目标的俯仰方向的倾斜角度的角速度即目标俯仰角速度，并基于目标俯仰角速度与检测出的俯仰角速度的偏差，生成俯仰用转矩指令，从而，能抑制误差的积累，并生成对主体的俯仰方向的倾斜进行适当修正的俯仰用转矩指令。

在第四发明及第十一发明中，由于基于俯仰倾斜角，推定使主体朝俯仰方向倾斜的俯仰方向外部转矩，并通过转矩修正部将俯仰用转矩指令朝抵消所推定的俯仰方向外部转矩的方向修正，从而，能考虑俯仰方向外部转矩所产生的影响而更适当地控制俯仰用电动机的旋转，因此，能更高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。

这里，所谓“俯仰方向外部转矩”，是指将因主体从平衡状态朝俯仰方向倾斜而引起的主体等负荷的重力所产生的转矩和逆风、顺风等所产生的主体倾斜方向(俯仰方向)的转矩相加而得到的转矩。此外，所谓“转矩修正部”，具有朝抵消俯仰方向外部转矩的方向修正俯仰用转矩指令的功能，后述的俯仰用电动机转矩指令电压计算部50是其一部分。

在第五发明及第十二发明中，基于由摇摆用旋转传感器检测出的摇摆方向的旋转速度，计算作为目标的摇摆方向的倾斜角度即目标摇摆角。根据由摇摆用角速度传感器检测出的摇摆角速度、和基于目标摇摆角而生成并提供给摇摆用电动机的摇摆用转矩指令，推定主体相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角。根据从目标摇摆角减去摇摆倾斜角的摇摆角度偏差，计算作为目标的摇摆方向的倾斜角度的角速度即目标摇摆角速度，基于目标摇摆角速度与检测出的摇摆角速度的偏差，生成摇摆用转矩指令。由此，由于能高精度地推定主体相对于平衡状态朝摇摆方向倾斜的摇摆倾斜角，并且，未对摇摆用角速度传感器检测出的摇摆角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标摇摆角的计算误差，利用伴随车轮的旋转的反作用转矩，能高精度地修正从平衡状态朝摇摆方向的倾斜，防止朝摇摆方向摔倒。

在第六发明中，由于在主体的重心位于与车轮的旋转轴交叉的垂直线上的状态下，通过使摇摆用角速度传感器的检测轴朝向前后方向和水平方向，从而摇摆用角速度传感器能检测出摇摆方向的正确角速度，因此，能高精度地修正摇摆方向的倾斜。使用了振子的摇摆用角速度传感器通过检测对应于与振子的振动方向正交的方向的力的信号，来求出角速度。由于摇摆用角速度传感器的检测轴朝向大体前后方向而安装于主体，因此，摇摆用角速度传感器检测出的力的方向成为与大体前后方向的检测轴正交的大体垂直方向。由于在以摇摆用角速度传感器的检测轴朝向前后方向和水平方向的状态检测角速度的情况下，能检测垂直方向的力，因此，能正确求出在摇摆方向实际产生的角速度ω。但是，由于在主体的重心有偏斜、以摇摆用角速度传感器的检测轴例如前侧比水平方向要低而前倾的状态检测角速度的情况下，检测出与垂直方向偏离预定角度θ的方向的力，求出ωcosθ，因此，检测出比实际角速度要慢的角速度。

在第七发明中，使得车轮的旋转的旋转比低于俯仰用电动机的旋转，例如从俯仰用电动机通过锥形齿轮、滑轮、传送带来驱动车轮，使俯仰用电动机侧的滑轮的减速比比车轮侧的滑轮高三倍，从而能以俯仰用电动机所产生的转矩的1/3转矩来控制车轮。此外，能以比俯仰用电动机所产生的转矩要低的转矩比来控制车轮，从而，实质上的电动机的临界转矩变高，可增大能防止摔倒的倾斜角的容许角。

根据上述结构，由于能高精度地推定主体相对于平衡状态朝俯仰方向倾斜的俯仰倾斜角，并且，未对俯仰用角速度传感器检测出的俯仰角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标俯仰角的计算误差，利用伴随车轮的旋转的反作用转矩，能高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。此外，通过接收车轮的前进或后退指示，并基于根据接收到的指示及由俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算出目标俯仰角，从而能确保用于进行所指示的移动的旋转速度，并修正俯仰方向的倾斜。此外，无需另外设置加速度传感器等，除了驱动车轮所需的电动机等之外，仅附加角速度传感器，以这样的简单结构，可使防摔倒控制装置简化并小型化。

附图说明

图1是示意性表示采用了本发明的实施方式所涉及的防摔倒控制装置的单轮车机器人的结构的主视图及侧视图。

图2是说明俯仰方向、摇摆方向、以及侧转(yaw)方向的示意图。

图3是表示防止单轮车机器人朝俯仰方向摔倒的控制的一个示例的控制框图。

图4是从侧面观察单轮车机器人的模型的示意图。

图5是表示从俯仰用电动机通过锥形齿轮、滑轮、传送带来驱动车轮的结构的示意图。

图6是表示从俯仰用电动机通过齿轮来直接驱动车轮的结构的一个示例的示意图。

图7是表示从俯仰用电动机通过齿轮来直接驱动车轮的结构的其他示例的示意图。

图8是表示从俯仰用电动机通过锥形齿轮、齿轮来驱动车轮的结构的示意图。

图9是采用图5所示的驱动结构的现实中的单轮车机器人的例示图。

图10是表示防止单轮车机器人朝摇摆方向摔倒的控制的一个示例的控制框图。

图11是从正面观察单轮车机器人的示意图。

图12是说明利用摇摆用陀螺传感器根据主体的重心位置检测出的角速度的灵敏度的示意图。

图13是表示单轮车机器人的控制基板的控制器所进行的俯仰方向的防摔倒处理步骤的流程图。

图14是表示单轮车机器人的控制基板的控制器所进行的摇摆方向的防摔倒处理步骤的流程图。

标号说明

1 单轮车机器人(防摔倒控制装置)

2 车轮

3 主体

4 框架

5 锥形齿轮

6 传送带

31 俯仰用陀螺传感器(俯仰用角速度传感器)

32 俯仰用电动机

33 俯仰用编码器(encoder)(俯仰用旋转传感器)

35 控制基板

36 电池

41 俯仰用计数器部

42 前进/后退指示接收部

43 俯仰用旋转速度计算部

44 目标俯仰角计算部

45 俯仰用AD转换器部

46 俯仰角速度计算部

47 俯仰倾斜角推定部

48 目标俯仰角速度计算部

49 俯仰用转矩指令生成部

50 俯仰用电动机转矩指令电压计算部

51 俯仰用DA转换器部

52 俯仰方向外部转矩推定部

O、O₁、O₂ 旋转中心

m₁ 主体质量

m₂ 惯性转子质量

I_1p 绕O的主体的惯性力矩

I_2p 绕O的车轮的惯性力矩

I_1r 绕O₁的主体的惯性力矩

I_2r 绕O₂的惯性转子的惯性力矩

θ_1p 主体相对于垂直轴的俯仰方向的倾斜角度

θ_2p 车轮相对于主体的旋转角度

θ_1r 主体相对于垂直轴的摇摆方向的倾斜角度

θ_2r 惯性转子相对于主体的旋转角度

τ_1p 作用于主体的绕O的扰动转矩

τ_2p 作用于车轮的转矩

l_Gp 从O到主体重心位置的距离

l_r 从O₁到O₂的距离

l_Gr 从O₁到主体重心位置的距离

r 车轮的半径

g 重力加速度

具体实施方式

下面，对于本发明的实施方式所涉及的防摔倒控制装置，基于附图具体说明适用于装载于单轮车上方的主体一边摇动、一边前后移动而不会因单轮车的旋转引起摔倒的单轮车机器人的示例。

图1是示意性表示采用了本发明的实施方式所涉及的防摔倒控制装置的单轮车机器人的结构的主视图及侧视图。图1(a)表示正视图，图1(b)表示右侧视图。本实施方式所涉及的防摔倒控制装置起到防止单轮车机器人主体朝俯仰方向及摇摆方向摔倒的功能。

如图1(a)、图1(b)所示，单轮车机器人1由朝前后方向旋转而移动的车轮2、以及与车轮2的旋转轴连接且在车轮2的上方朝俯仰方向及摇摆方向摇动的主体3构成。虽然在图1(a)、图1(b)的示例中，设主体3为人形机器人，但并不限于此。

这里，预先明确俯仰方向及摇摆方向。图2是说明俯仰方向、摇摆方向、以及侧转方向的示意图。如图2所示，单轮车机器人1在xy平面上沿x轴的(+)方向前进或沿x轴的(-)方向后退而移动的情况下，绕y轴的旋转方向是俯仰方向。在面对y轴的(+)方向绕逆时针旋转的情况下，主体3向前方倾斜，在面对y轴的(+)方向绕顺时针旋转的情况下，主体3向后方倾斜。此外，绕x轴的旋转方向是摇摆方向，是主体3朝左右方向摇动的情况的旋转方向。此外，绕z轴的旋转方向是侧转方向，是使车轮2的方向与x轴方向倾斜的情况的旋转方向。

如图1(a)、图1(b)所示，主体3包括检测俯仰方向的倾斜角度的角速度即俯仰角速度的俯仰用陀螺传感器(俯仰用角速度传感器)31、与车轮2的旋转联动而使车轮2旋转的俯仰用电动机32、以及检测俯仰用电动机32的旋转位置或旋转速度的俯仰用编码器(俯仰用旋转传感器)33。俯仰用陀螺传感器31使检测俯仰角速度的未图示的检测轴朝向大体左右方向而安装于主体3。这里，所谓大体左右方向，是指相对于严格的左右方向也可以有上下若干角度偏差。主体3和车轮2通过支承车轮2而使车轮2可旋转自如的框架4连接，俯仰用电动机32产生的旋转通过主体3所具有的锥形齿轮5和传送带6传送到车轮2。框架4是主体3的一部分，在图1(a)、图1(b)的示例中，框架4成为主体3即人形机器人的腿。另外，俯仰用角速度传感器只要能检测俯仰角速度即可，并不限于陀螺传感器。

此外，在主体3即人形机器人的背部，装备有控制俯仰用电动机32的控制基板35及电池36。在控制基板35上，装载有对俯仰用电动机32进行旋转驱动的驱动器、A/D转换器、D/A转换器、计数器、控制器等。具体而言，控制器是微处理器、CPU、LSI等。为了取得俯仰方向的平衡，单轮车机器人1进行控制，从而利用驱动车轮2时的反作用而取得平衡。图3是表示防止单轮车机器人1朝俯仰方向摔倒的控制的一个示例的控制框图。

如图3所示，在俯仰用计数器部41中，对俯仰用编码器33的输出脉冲进行计数。在前进/后退指示接收部42中，接收车轮2的前进指示或后退指示以作为旋转速度或旋转角度。在将前进指示或后退指示作为旋转角度输出的情况下，在俯仰用旋转速度计算部43中，从前进指示或后退指示的脉冲信号的脉冲数减去利用俯仰用计数器部41计数得到的脉冲数，将进行减法运算得到的脉冲数转换成旋转角度，之后，进行微分，求出俯仰用电动机32的旋转速度。也可以装备用于去除噪声的LPF(低通滤波器)。

在目标俯仰角计算部44中，根据利用俯仰用旋转速度计算部43求出的俯仰用电动机32的旋转速度，对俯仰用电动机32的旋转速度乘以比例系数来求出目标俯仰角，从而，在俯仰用电动机32的旋转是朝车轮2前进的方向旋转的情况下，使目标俯仰角成为车轮2前进的方向，在俯仰用电动机32的旋转是朝车轮2后退的方向旋转的情况下，使目标俯仰角成为车轮2后退的方向。由此，能确保用于进行所指示的移动的旋转速度，并修正俯仰方向的倾斜。

另一方面，在俯仰用AD转换器部45中，测定俯仰用陀螺传感器31的俯仰角速度输出。在俯仰角速度计算部46中，对俯仰角速度输出乘以转换系数，计算俯仰角速度ω_1p。

在俯仰倾斜角推定部47中，根据俯仰角速度ω_1p和后述的俯仰用转矩指令τ_2p，利用基于包含主体3和车轮2的系统的倾斜角方向(俯仰方向)的运动方程式所导出的后述(式18)，计算俯仰倾斜角。此外，为了具有适当的推定速度而使环路稳定，串联附加一阶延迟要素，从而计算俯仰倾斜角的推定值。具体而言，对利用了(式18)的计算值串联附加例如1/(0.1S+1)作为一阶延迟要素，但是，并不限于此，可以附加成为适当的推定速度那样的任意延迟要素。

在俯仰方向外部转矩推定部52中，对俯仰倾斜角的推定值乘以转换系数，计算作用于主体3的俯仰方向外部转矩的推定值，生成俯仰用修正转矩(相当于俯仰方向外部转矩的推定值)τ_3p。

在目标俯仰角速度计算部48中，对从目标俯仰角减去俯仰倾斜角的推定值而得到的俯仰角度偏差乘以比例增益，计算目标俯仰角速度ω_2p。在俯仰用转矩指令生成部49中，对目标俯仰角速度ω_2p和俯仰角速度ω_1p的偏差进行例如PI控制，从而生成俯仰用转矩指令τ_0p。在俯仰用电动机转矩指令电压计算部50中，对将俯仰用转矩指令τ_0p和俯仰用修正转矩τ_3p相加而得到的俯仰用转矩指令τ_2p乘以转换系数，生成指令电压。最后，在俯仰用DA转换器部51中，对驱动器输出指令电压，控制俯仰用电动机32的旋转。

这里，对于利用(式18)表示的俯仰倾斜角的计算公式的导出方法，在下面进行说明。图4是从侧面观察单轮车机器人1的模型的示意图。在图4中，仅示意性示出车轮2、主体3、以及安装于主体3的俯仰用陀螺传感器31，箭头方向是前进方向，示出主体3前倾的状态。首先，根据拉格朗日方程式，导出运动方程式。将主体3和车轮2组合后的整体的动能T及势能U如下所示。

$T=\frac{1}{2}{I}_{1p}{\stackrel{\·}{\mathrm{\θ}}}_{1p}^2+\frac{1}{2}{I}_{2p}{({\stackrel{\·}{\mathrm{\θ}}}_{1p}+{\stackrel{\·}{\mathrm{\θ}}}_{2p})}^2$ ...(式1)

U＝m₁l_Gp g cosθ_1p+m₂rg          ...(式2)

其中，I_1p：绕旋转中心O的主体的惯性力矩，

θ_1p：主体相对于垂直轴的俯仰方向的倾斜角度，

I_2p：绕旋转中心O的车轮的惯性力矩，

θ_2p：车轮相对于主体的旋转角度，

m₁：主体质量，

l_Gp：从旋转中心O到主体重心位置的距离，

g：重力加速度，r：车轮的半径

m₂：惯性转子质量。

根据广义坐标和广义速度的微分量如下所示。

$\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_{1p}}=I_{1p}{\stackrel{\·}{\mathrm{\θ}}}_{1p}+I_{2p}({\stackrel{\·}{\mathrm{\θ}}}_{1p}+{\stackrel{\·}{\mathrm{\θ}}}_{2p})$ ...(式3)

$\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_{2p}}=I_{2p}({\stackrel{\·}{\mathrm{\θ}}}_{1p}+{\stackrel{\·}{\mathrm{\θ}}}_{2p})$ ...(式4)

$\frac{\∂T}{{\∂\mathrm{\θ}}_{1p}}=0$ ...(式5)

$\frac{\∂T}{{\∂\mathrm{\θ}}_{2p}}=0$ ...(式6)

$\frac{\∂U}{{\∂\mathrm{\θ}}_{1p}}=-m_1{l}_{\mathrm{Gp}}g\sin {\mathrm{\θ}}_{1p}$ ...(式7)

$\frac{\∂U}{{\∂\mathrm{\θ}}_{2p}}=0$ ...(式8)

将(式3)～(式8)代入拉格朗日方程式(式9)、(式10)。

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{{\stackrel{\·}{\mathrm{\θ}}}_{1p}}\right)-\frac{\∂T}{{\∂\mathrm{\θ}}_{1p}}+\frac{\∂U}{{\∂\mathrm{\θ}}_{1p}}={\mathrm{\τ}}_{1p}$ ...(式9)

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{{\stackrel{\·}{\mathrm{\θ}}}_{2p}}\right)-\frac{\∂T}{{\∂\mathrm{\θ}}_{2p}}+\frac{\∂U}{{\∂\mathrm{\θ}}_{2p}}={\mathrm{\τ}}_{2p}$ ...(式10)

其中，τ_1p：作用于主体的绕旋转中心O的转矩，τ_2p：作用于车轮的转矩。

其结果是，作为运动方程式，得到下面的(式11)、(式12)。

$I_{1p}{\stackrel{\·\·}{\mathrm{\θ}}}_{1p}+I_{2p}({\stackrel{\·\·}{\mathrm{\θ}}}_{1p}+{\stackrel{\·\·}{\mathrm{\θ}}}_{2p})-m_1{l}_{\mathrm{Gp}}g\sin {\mathrm{\θ}}_{1p}={\mathrm{\τ}}_{1p}$ ...(式11)

$I_{2p}({\stackrel{\·\·}{\mathrm{\θ}}}_{1p}+{\stackrel{\·\·}{\mathrm{\θ}}}_{2p})={\mathrm{\τ}}_{2p}$ ...(式12)

若将(式12)进行变形，则成为(式13)。

${\stackrel{\·\·}{\mathrm{\θ}}}_{2p}=\frac{{\mathrm{\τ}}_{2p}}{I_{2p}}-{\stackrel{\·\·}{\mathrm{\θ}}}_{1p}$ ...(式13)

若将(式13)代入(式11)，并用θ_1p来对sinθ_1p进行近似，则得到(式14)。根据(式14)，主体3的运动变成与车轮2的角度及角速度无关。

$I_{1p}{\stackrel{\·\·}{\mathrm{\θ}}}_{1p}-m_1{l}_{\mathrm{Gp}}g{\mathrm{\θ}}_{1p}={\mathrm{\τ}}_{1p}-{\mathrm{\τ}}_{2p}$ ...(式14)

—俯仰倾斜角的推定—

虽然俯仰倾斜角也可以通过对俯仰用陀螺传感器31的输出进行积分来求出，但由于偏差累积而变得不正确，所以，需要通过其他方法来求出。因此，利用图4所示的模型的运动方程式，根据俯仰用陀螺传感器31输出的俯仰角速度ω_1p和俯仰用转矩指令τ_2p，来推定俯仰倾斜角。若将运动方程式(式14)进行变形，则成为(式15)。

${\mathrm{\θ}}_{1p}+\frac{{\mathrm{\τ}}_{1p}}{m_1{l}_{\mathrm{Gp}}g}=\frac{{\mathrm{\τ}}_{2p}+I_{1p}{\stackrel{\·\·}{\mathrm{\θ}}}_{1p}}{m_1{l}_{\mathrm{Gp}}g}$ ...(式15)

另一方面，通过(式16)来表示俯仰用陀螺传感器31输出的俯仰角速度ω_1p。

${\stackrel{\·\·}{\mathrm{\θ}}}_{1p}\≅{\stackrel{\·}{\mathrm{\ω}}}_{1p}$ ...(式16)

此外，在主体3因逆风、顺风等而产生倾斜方向(俯仰方向)的转矩τ_1p的情况下，外观上的平衡倾斜角θ_0p成为(式17)。

${\mathrm{\θ}}_{0p}=-\frac{{\mathrm{\τ}}_{1p}}{m_1{l}_{\mathrm{Gp}}g}$ ...(式17)

因而，根据上述(式15)、(式16)、(式17)，导出并计算(式18)，从而能推定当前的俯仰方向的倾斜角θ_1p相对于外观上的平衡倾斜角的偏差角(俯仰倾斜角)。但是，为了具有适当的推定速度而使环路稳定，可以串联附加一阶延迟要素。另外，(式18)是推定俯仰倾斜角的计算公式的一个示例，根据对象模型，推定俯仰倾斜角的计算公式有时会不同。

${\widetilde{\mathrm{\θ}}}_{1p}\≡{\mathrm{\θ}}_{1p}-(-\frac{{\mathrm{\τ}}_{1p}}{m_1{l}_{\mathrm{Gp}}g})\≅\frac{{\mathrm{\τ}}_{2p}+I_{1p}{\stackrel{\·}{\mathrm{\ω}}}_{1p}}{m_1{l}_{\mathrm{Gp}}g}$ ...(式18)

根据由俯仰用陀螺传感器31输出的俯仰角速度ω_1p、和基于目标俯仰角而生成并提供给俯仰用电动机32的俯仰用转矩指令τ_2p，推定主体3相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角，从而能高精度地推定俯仰倾斜角。此外，由于未对俯仰用陀螺传感器31的俯仰角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标俯仰角的计算误差，利用伴随车轮2的旋转的反作用转矩，能高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。

—俯仰方向外部转矩前馈—

根据通过(式18)推定的偏差角(俯仰倾斜角)，补偿俯仰方向外部转矩。

${\widetilde{\mathrm{\τ}}}_{2p}=m_1{l}_{\mathrm{Gp}}g{\widetilde{\mathrm{\θ}}}_{1p}$ ...(式19)

采用(式19)，加到转矩上。

${\mathrm{\τ}}_{2p}={\widehat{\mathrm{\τ}}}_{2p}+{\widetilde{\mathrm{\τ}}}_{2p}$ ...(式20)

若采用(式20)，则运动方程式(式14)成为(式21)，从而能补偿俯仰方向外部转矩。由于通过推定主体3从平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角的(式18)，能推定因从平衡状态朝俯仰方向的倾斜角度而产生的俯仰方向外部转矩，因此，能计算出与推定的俯仰方向外部转矩相抵消的修正转矩。因而，由于能考虑俯仰方向外部转矩所产生的影响而更适当地控制俯仰用电动机32的旋转，因此，能更高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。特别是，即使在倾斜角环路、倾斜角速度环路的响应频率较低的情况下，也能利用前馈控制对俯仰方向外部转矩进行补偿，从而继续进行防摔倒控制，因此，可进行稳定的控制。

$I_{1p}{\stackrel{\·\·}{\mathrm{\θ}}}_{1p}=-{\widehat{\mathrm{\τ}}}_{2p}$ ...(式21)

修正后的俯仰用转矩指令通过俯仰用DA转换器部51输出到驱动器，控制俯仰用电动机32的旋转。俯仰用电动机32产生的旋转传送到车轮2。图5是表示从俯仰用电动机32通过锥形齿轮5、滑轮、传送带6来驱动车轮2的结构的示意图。图5(a)表示正视图，图5(b)表示右侧视图。图5仅示出单轮车机器人1中与车轮2的驱动相关的车轮2及主体3的车轮2侧部分。

如图5(a)、图5(b)所示，俯仰用电动机32产生的旋转依次通过电动机轴321、锥形齿轮5、5、主体3侧的滑轮7、传送带6、车轮2侧的滑轮8传送到车轮2。在单轮车机器人1中，使得车轮2的旋转的旋转比低于俯仰用电动机32的旋转。具体而言，使得俯仰用电动机32侧、即主体3侧的滑轮7与车轮2侧的滑轮8相比要提高减速比。

例如，通过将减速比提高三倍，能利用俯仰用电动机32产生的转矩、即进行上述修正后的俯仰用转矩指令所产生的转矩的1/3转矩对车轮2进行控制。因而，能提高实质上的俯仰用电动机32的临界转矩，可增大能防止摔倒的倾斜角的容许角。

图6是表示从俯仰用电动机32通过齿轮来直接驱动车轮2的结构的一个示例的示意图。图6(a)表示正视图，图6(b)表示右侧视图。如图6(a)、图6(b)所示，俯仰用电动机32产生的旋转通过与齿轮323同轴地共同旋转的齿轮324直接传送到车轮2的轮胎325，而该齿轮323与俯仰用电动机32的电动机轴321的齿轮322相啮合。

图7是表示从俯仰用电动机32通过齿轮来直接驱动车轮2的结构的其他示例的示意图。图7(a)表示正视图，图7(b)表示右侧视图。另外，在图7(a)的正视图中，为了说明起见，利用截面来示出车轮2。如图7(a)、图7(b)所示，与图6的结构相同，俯仰用电动机32产生的旋转通过与齿轮323同轴地共同旋转的齿轮324直接传送到车轮2，而该齿轮323与俯仰用电动机32的电动机轴321的齿轮322相啮合，但与图6的结构的不同点在于：不像图6的结构那样传送到车轮2的轮胎325，而是传送到形成于车轮2的轮子326的齿轮。

在单轮车机器人1中，也可以采用图6或图7所示的驱动结构。但是，在图6所示的驱动结构中，由于将齿轮324直接按压在车轮2的轮胎而传送旋转，因此，若发生打滑等，则难以高精度地控制车轮2的旋转。

图8是表示从俯仰用电动机32通过锥形齿轮5、5、齿轮9来驱动车轮2的结构的示意图。图8(a)表示正视图，图8(b)表示右侧视图。如图8(a)、图8(b)所示，俯仰用电动机32产生的旋转经由俯仰用电动机32的电动机轴321从锥形齿轮5通过与车轮2的旋转轴共同旋转的齿轮9传送到车轮2。由于在图8所示的驱动结构中，与图5所示的驱动结构相同，仅有齿轮5或齿轮9的啮合发生所谓松动部分的齿隙(backlash)的程度，不会发生图6及图7所示的驱动结构那样的打滑等，因此，能高精度地控制车轮2的旋转。此外，图8所示的驱动结构通过进一步使齿轮介于锥形齿轮5与齿轮9之间而变更齿轮比(减速比)，从而也可像图5所示的驱动结构那样提高实质上的俯仰用电动机32的临界转矩。

图9是采用图5所示的驱动结构的现实中的单轮车机器人1的例示图。图9(a)表示正视图，图9(b)表示右侧视图。由于图9(a)、图9(b)所示的驱动结构与图5相同，因此，省略结构的说明。如图9所示，能将锥形齿轮5、滑轮7、8、以及传送带6收纳在框架4内，使得从侧面无法看到，与图8所示的驱动结构那样的会看到齿轮9等的结构相比，还可得到独具匠心的效果。因而，在单轮车机器人1中，虽然可以采用图5至图8所示的任一种驱动结构，但特别优选采用可得到上述各种效果的图5所示的驱动结构。

由于单轮车机器人1像上述那样起到防止朝俯仰方向及摇摆方向摔倒的功能，因此，下面说明防止朝摇摆方向摔倒的结构。

如图1(a)、图1(b)所示，除了上述防止朝俯仰方向摔倒的结构之外，主体3包括检测摇摆方向的倾斜角度的角速度即摇摆角速度的摇摆用陀螺传感器(摇摆用角速度传感器)61、朝摇摆方向旋转的惯性转子64、与惯性转子64的旋转联动而使惯性转子64旋转的摇摆用电动机62、以及检测摇摆用电动机62的旋转位置或旋转速度的摇摆用编码器(摇摆用旋转传感器)63。摇摆用陀螺传感器61使检测摇摆角速度的未图示的检测轴朝向大体前后方向而安装于主体3。这里，所谓大体前后方向，是指相对于严格的前后方向也可以有上下若干角度偏差。另外，摇摆用角速度传感器61只要能检测摇摆角速度即可，并不限于陀螺传感器。

此外，装备于主体3即人形机器人的背部的上述控制基板35不仅控制俯仰用电动机32，还控制摇摆用电动机62。图10是表示防止单轮车机器人1朝摇摆方向摔倒的控制的一个示例的控制框图。

如图10所示，在摇摆用计数器部71中，对摇摆用编码器63的输出脉冲进行计数。在摇摆用旋转速度计算部73中，将摇摆用计数器部71的输出转换成旋转角度，之后，进行微分，求出摇摆用电动机62的旋转速度。也可以装备用于去除噪声的LPF(低通滤波器)。

在目标摇摆角计算部74中，对摇摆用电动机62的旋转速度乘以比例系数来求出目标摇摆角，从而，在摇摆用电动机62的旋转从单轮车机器人1的正面观察是向左旋转的情况下，使得目标摇摆角成为从单轮车机器人1的正面观察的向右方向，在摇摆用电动机62的旋转从单轮车机器人1的正面观察是向右旋转的情况下，使得目标摇摆角成为从单轮车机器人1的正面观察的向左方向。另外，优选附加积分器，从而使惯性转子64中不保留稳定旋转。

另一方面，在摇摆用AD转换器部75中，获取摇摆用陀螺传感器61的摇摆角速度输出。在摇摆角速度计算部76中，对摇摆角速度输出乘以转换系数，计算摇摆角速度ω_1r。

在摇摆倾斜角推定部77中，根据摇摆角速度ω_1r和后述的摇摆用转矩指令τ_2r，计算由基于包含主体3(惯性转子64以外的部分)和惯性转子64的系统的倾斜角方向(摇摆方向)的运动方程式所导出的后述(式25)来表示的摇摆倾斜角。此外，为了具有适当的推定速度而使环路稳定，串联附加一阶延迟要素，从而计算摇摆倾斜角的推定值。具体而言，对利用(式25)计算出的计算值串联附加例如1/(0.1S+1)作为一阶延迟要素，但是，并不限于此，可以附加成为适当的推定速度那样的任意延迟要素。

在摇摆方向外部转矩推定部82中，对摇摆倾斜角的推定值乘以转换系数，计算作用于主体3的摇摆方向外部转矩的推定值，生成摇摆用修正转矩(相当于摇摆方向外部转矩的推定值)τ_3r。

在目标摇摆角速度计算部78中，对从目标摇摆角减去摇摆倾斜角的推定值而得到的摇摆角度偏差乘以比例增益，生成目标摇摆角速度ω_2r。在摇摆用转矩指令生成部79中，对目标摇摆角速度ω_2r和摇摆角速度ω_1r的偏差进行例如PI控制，从而生成摇摆用转矩指令τ_0r。在摇摆用电动机转矩指令电压计算部80中，对将摇摆用转矩指令τ_0r和摇摆用修正转矩τ_3r相加而得到的摇摆用转矩指令τ_2r乘以转换系数，生成指令电压。最后，在摇摆用DA转换器部81中，对驱动器输出指令电压，控制摇摆用电动机62的旋转。

这里，对于利用(式25)表示的摇摆倾斜角的计算公式的导出方法，在下面进行说明。图11是从正面观察单轮车机器人1的示意图。在图11中，仅示意性示出主体3以及安装于主体3的惯性转子64。首先，根据拉格朗日方程式，导出运动方程式。将主体3(惯性转子64以外的部分)和惯性转子64组合后的整体的动能T及势能U如下所示。

$T=\frac{1}{2}{I}_{1r}{\stackrel{\·}{\mathrm{\θ}}}_{1r}^2+\frac{1}{2}{I}_{2r}{({\stackrel{\·}{\mathrm{\θ}}}_{1r}+{\stackrel{\·}{\mathrm{\θ}}}_{2r})}^2+\frac{1}{2}{m}_2{l}_r^2{\stackrel{\·}{\mathrm{\θ}}}_{1r}^2$ ...(式22)

U＝(m₁l_Gr+m₂l_r)g cosθ_1r    ...(式23)

其中，I_1r：绕旋转中心O₁的主体的惯性力矩，θ_1r：主体相对于垂直轴的摇摆方向的倾斜角度，I_2r：绕旋转中心O₂的惯性转子的惯性力矩，θ_2r：惯性转子相对于主体的旋转角度，l_r：从旋转中心O₁到O₂的距离，l_Gr：从旋转中心O₁到主体重心位置的距离。

像导出上述计算俯仰倾斜角的计算公式那样，根据(式22)、(式23)，可得到(式24)。根据(式24)，主体3的运动变成与惯性转子64的角度及角速度无关。

$(I_{1r}+m_2{l}_r^2){\stackrel{\·\·}{\mathrm{\θ}}}_{1r}-(m_1{l}_{\mathrm{Gr}}+m_2{l}_r)g{\mathrm{\θ}}_{1r}={\mathrm{\τ}}_{1r}-{\mathrm{\τ}}_{2r}$ ...(式24)

其中，τ_1r：作用于主体的绕旋转中心O₁的转矩，τ_2r：作用于惯性转子的转矩。

—摇摆倾斜角的推定—

与上述俯仰倾斜角相同，虽然摇摆倾斜角也可以通过对摇摆用陀螺传感器61的输出进行积分来求出，但由于偏差累积而变得不正确，因此，需要通过其他方法来求出。与上述推定俯仰倾斜角的情况相同，利用图11所示的模型的运动方程式，根据摇摆用陀螺传感器61输出的摇摆角速度ω_1r和摇摆用转矩指令τ_2r，推定摇摆倾斜角。

在因侧风、曲线行驶所产生的离心力等而导致主体3产生倾斜方向(摇摆方向)的转矩τ_1r的情况下，根据将运动方程式(式24)进行变形后的公式、和由摇摆用陀螺传感器61输出的摇摆角速度ω_1r，能通过(式25)推定当前的摇摆方向的倾斜角θ_1r与外观上的平衡倾斜角的偏差(摇摆倾斜角)。另外，为了具有适当的推定速度而使环路稳定，可以串联附加一阶延迟要素。此外，(式25)是推定摇摆倾斜角的计算公式的一个示例，根据对象模型，推定摇摆倾斜角的计算公式有时会不同。

${\widetilde{\mathrm{\θ}}}_{1r}\≡{\mathrm{\θ}}_{1r}-(-\frac{{\mathrm{\τ}}_{1r}}{(m_1{l}_{\mathrm{Gr}}+m_2{l}_r)g})\≅\frac{{\mathrm{\τ}}_{2r}+(I_{1r}+m_2{l}_r^2){\stackrel{\·}{\mathrm{\ω}}}_{1r}}{(m_1{l}_{\mathrm{Gr}}+m_2{l}_r)g}$ ...(式25)

根据由摇摆用陀螺传感器61输出的俯仰角速度ω_1r、和基于目标摇摆角而生成并提供给摇摆用电动机62的摇摆用转矩指令τ_2r，推定主体3相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角，从而与俯仰倾斜角相同，能高精度地推定摇摆倾斜角。此外，由于未对摇摆角速度传感器产生的摇摆角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标摇摆角的计算误差，利用伴随车轮2的旋转的反作用转矩，能高精度地修正从平衡状态朝摇摆方向的倾斜，防止朝摇摆方向摔倒。

—摇摆方向外部转矩前馈—

根据通过(式25)推定的偏差角度，补偿摇摆方向外部转矩。

${\widetilde{\mathrm{\τ}}}_{2r}=(m_1{l}_{\mathrm{Gr}}+m_2{l}_r)g{\widetilde{\mathrm{\θ}}}_{1r}$ ...(式26)

采用(式26)，加到转矩上。

${\mathrm{\τ}}_{2r}={\widehat{\mathrm{\τ}}}_{2r}+{\widetilde{\mathrm{\τ}}}_{2r}$ ...(式27)

若采用(式27)，则运动方程式(式24)成为(式28)，从而能补偿摇摆方向外部转矩。因而，与俯仰方向相同，由于能考虑摇摆方向外部转矩所产生的影响而更适当地控制摇摆用电动机62的旋转，因此，能更高精度地修正从平衡状态朝摇摆方向的倾斜，防止朝摇摆方向摔倒。

$(I_{1r}+m_2{l}_r^2){\stackrel{\·\·}{\mathrm{\θ}}}_{1r}=-{\widehat{\mathrm{\τ}}}_{2r}$ ...(式28)

图12是说明利用机器人用陀螺传感器61根据主体3的重心位置检测出的角速度的灵敏度的示意图。图12是从单轮车机器人1的右侧面观察到的图，粗箭头a表示前进方向，表示单轮车机器人1的俯仰方向的平衡状态。摇摆用陀螺传感器61是使用了振子的角速度传感器，通过检测对应于与振子的振动方向正交的方向的力的信号，来求出角速度。另外，检测出的力的方向成为与大体前后方向的检测轴正交的大体垂直方向。如图12所示，由于主体3的重心G不在主体3的轴38上，而位于主体3的靠后方，因此，对于单轮车机器人1，在俯仰方向的平衡状态下，主体3的轴38不是垂直方向而前倾。摇摆用陀螺传感器61安装于主体3，使得在主体3的轴38成为垂直方向的状态下，检测轴朝向前后方向和水平方向，并检测与垂直方向的力相对应的信号。

在图12所示的俯仰方向的平衡状态下，摇摆用陀螺传感器61的检测轴朝向箭头b且前侧比水平方向要低而前倾。在检测轴前倾的状态下，虽然摇摆用陀螺传感器61检测出与箭头d所示的力相对应的信号，但在摇摆方向实际产生的角速度与箭头c所示的力相对应。若将与箭头c所示的力相对应的角速度设为ω，则与箭头d所示的力相对应的角速度成为ωcosθ，摇摆用陀螺传感器61检测出的角速度比实际的角速度要慢。若摇摆用陀螺传感器61无法正确检测出实际的角速度，则无法高精度地推定摇摆倾斜角，从而无法高精度地修正摇摆方向的倾斜。

因而，为了正确检测出在摇摆方向实际产生的角速度，优选将摇摆用陀螺传感器61安装于主体3，在图12那样的俯仰方向的平衡状态下，即在主体3的重心G位于与车轮2的轴交叉的垂直线上的状态下，使得摇摆用陀螺传感器61的检测轴朝向前后方向和水平方向。由此，摇摆用陀螺传感器61能在俯仰方向的平衡状态下正确检测出在摇摆方向实际产生的角速度，即使是在从俯仰方向的平衡状态朝俯仰方向摇动微小角度的状态下，也能以与实际的摇摆角速度较小的误差高灵敏度地检测出摇摆角速度。

接下来，基于流程图，说明由上述图3所示的控制框图构成的单轮车机器人1的控制动作。为了简化说明，将俯仰方向的防摔倒控制和摇摆方向的防摔倒控制分开说明。图13是表示单轮车机器人1的控制基板35的控制器所进行的俯仰方向的防摔倒处理步骤的流程图。

如图13所示，控制基板35的控制器对检测俯仰用电动机32的旋转的俯仰用编码器33的输出(脉冲信号)的脉冲数进行计数(步骤S1201)。控制器接收车轮2的前进(或后退)指示以作为旋转速度的脉冲信号(步骤S1202)。

控制器根据从前进(或后退)指示的脉冲信号的脉冲数减去俯仰用编码器33的输出(脉冲信号)的脉冲数而得到的脉冲数，计算出俯仰方向的旋转速度偏差(步骤S1203)。具体而言，在将进行减法运算而得到的脉冲数转换成旋转角度之后，进行微分，求出旋转速度偏差。控制器基于俯仰方向的旋转速度偏差，计算出作为目标的俯仰方向的倾斜角度即目标俯仰角(步骤S1204)。

控制器从计算出的目标俯仰角减去由后述的步骤S1212推定出的俯仰倾斜角，计算出俯仰角度偏差(步骤S1205)，并对计算出的俯仰角度偏差乘以比例增益，计算出目标俯仰角速度ω_2p(步骤S1206)。

控制器计算出目标俯仰角速度ω_2p与由后述的步骤S1211计算出的俯仰角速度ω_1p的俯仰角速度偏差(步骤S1207)，并通过对计算出的俯仰角速度偏差进行PI控制等，生成俯仰用转矩指令τ_0p(步骤S1208)。

控制器利用由后述的步骤S1213推定的俯仰方向外部转矩τ_3p来修正所生成的俯仰用转矩指令τ_0p，生成俯仰用转矩指令τ_2p(步骤S1209)。

控制器进行A/D转换而获取由俯仰用陀螺传感器31输出的俯仰角速度的输出(步骤S1210)。控制器对所获取的俯仰角速度的输出乘以转换系数，计算出俯仰角速度ω_1p(步骤S1211)。

控制器利用(式18)，根据计算出的俯仰角速度ω_1p和由上述步骤S1209生成的俯仰用转矩指令τ_2p，推定主体3相对于平衡状态朝俯仰方向倾斜的俯仰倾斜角(步骤S1212)。控制器基于所推定的俯仰倾斜角，推定使主体3朝俯仰方向倾斜的俯仰方向外部转矩(步骤S1213)。

控制器判断在步骤S1209是否生成了俯仰用转矩指令τ_2p(步骤S1214)。

在控制器判断为生成了俯仰用转矩指令τ_2p的情况下(步骤S1214：是)，控制器对所生成的俯仰用转矩指令τ_2p乘以转换系数，计算出指令电压(步骤S1215)。控制器将计算出的指令电压进行D/A转换，并输出到对俯仰用电动机32进行旋转驱动的驱动器(步骤S1216)。控制器将处理返回至步骤S1201及步骤S1210，重复上述处理。

另一方面，在控制器判断为没有生成俯仰用转矩指令τ_2p的情况下(步骤S1214：否)，主体3在平衡状态且处于也没有前进/后退指示的状态，控制器结束处理。虽然上述示例示出了接收旋转角度作为前进指示或后退指示的情况的处理步骤，但即使在接收旋转速度作为前进指示或后退指示的情况下，也可通过求出角速度偏差，根据相同的处理步骤进行俯仰方向的防摔倒控制。

图14是表示单轮车机器人1的控制基板35的控制器所进行的摇摆方向的防摔倒处理步骤的流程图。如图14所示，控制基板35的控制器对检测摇摆用电动机62的旋转的摇摆用编码器63的输出(脉冲信号)的脉冲数进行计数(步骤S1301)。

控制器根据计数得到的脉冲数，计算出摇摆方向的旋转速度(步骤S1302)。具体而言，在将计数得到的脉冲数转换成旋转角度之后，进行微分，计算出旋转速度。控制器基于摇摆方向的旋转速度，计算出作为目标的摇摆方向的倾斜角度即目标摇摆角(步骤S1303)。

控制器从计算出的目标摇摆角减去由后述的步骤S1311推定出的摇摆倾斜角，计算出摇摆角度偏差(步骤S1304)，并对计算出的摇摆角度偏差乘以比例增益，计算出目标摇摆角速度ω_2r(步骤S1305)。

控制器计算出目标摇摆角速度ω_2r与由后述的步骤S1310计算出的摇摆角速度ω_1r的摇摆角速度偏差(步骤S1306)，并通过对计算出的摇摆角速度偏差进行PI控制等，生成摇摆用转矩指令τ_0r(步骤S1307)。

控制器利用由后述的步骤S1312推定的摇摆方向外部转矩τ_3r来修正所生成的摇摆用转矩指令τ_0r，生成摇摆用转矩指令τ_2r(步骤S1308)。

控制器进行A/D转换而获取由摇摆用陀螺传感器61输出的摇摆角速度的输出(步骤S1309)。控制器对所获取的摇摆角速度的输出乘以转换系数，计算出摇摆角速度ω_1r(步骤S1310)。

控制器利用(式25)，根据计算出的摇摆角速度ω_1r和由上述步骤S1308生成的摇摆用转矩指令τ_2r，推定主体3相对于平衡状态朝摇摆方向倾斜的摇摆倾斜角(步骤S1311)。控制器基于所推定的摇摆倾斜角，推定使主体3朝摇摆方向倾斜的摇摆方向外部转矩(步骤S1312)。

控制器判断在步骤S1308是否生成了摇摆用转矩指令τ_2r(步骤S1313)。

在控制器判断为生成了摇摆用转矩指令τ_2r的情况下(步骤S1313：是)，控制器对所生成的摇摆用转矩指令τ_2r乘以转换系数，计算出指令电压(步骤S1314)。控制器将计算出的指令电压进行D/A转换，并输出到对摇摆用电动机62进行旋转驱动的驱动器(步骤S1315)。控制器将处理返回至步骤S1301及步骤S1309，重复上述处理。

另一方面，在控制器判断为没有生成摇摆用转矩指令τ_2r的情况下(步骤S1313：否)，主体3是平衡状态，控制器结束处理。

如上所述，根据本实施方式，由于能高精度地推定主体相对于平衡状态朝俯仰方向倾斜的俯仰倾斜角，并且，未对俯仰用角速度传感器产生的俯仰角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标俯仰角的计算误差，利用伴随车轮的旋转的反作用转矩，能高精度地修正从平衡状态朝俯仰方向的倾斜，防止朝俯仰方向摔倒。此外，通过接收车轮的前进或后退指示，并基于根据接收到的指示及由俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算出目标俯仰角，从而能确保用于进行所指示的移动的旋转速度，并修正俯仰方向的倾斜。此外，无需另外设置加速度传感器等，除了驱动车轮所需的电动机等之外，仅附加角速度传感器，以这样的简单结构，可使防摔倒控制装置简化并小型化。

此外，由于能高精度地推定主体相对于平衡状态朝摇摆方向倾斜的摇摆倾斜角，并且，未对摇摆用角速度传感器产生的摇摆角速度进行积分，因此，不会因噪声、偏移等的积累而导致产生目标摇摆角的计算误差，利用伴随车轮的旋转的反作用转矩，能高精度地修正从平衡状态朝摇摆方向的倾斜，防止朝摇摆方向摔倒。

另外，上述实施方式当然可以在不脱离本发明的要点范围内进行变更。上述实施方式所涉及的防摔倒控制装置并不限定适用于单轮车机器人，也可以适用于例如两足步行机器人、以左右方向的一个轴为中心朝前后方向旋转而移动的双轮车等，能起到与上述效果相同的效果。

Claims (12)

1.一种防摔倒控制装置，其特征在于，

该防摔倒控制装置包括朝前后方向旋转而移动的车轮、以及与该车轮的旋转轴连接且在该车轮的上方朝俯仰方向及摇摆方向摇动的主体，

该主体包括：

俯仰用角速度传感器，该俯仰用角速度传感器检测俯仰方向的倾斜角度的角速度即俯仰角速度；

俯仰用电动机，该俯仰用电动机与所述车轮的旋转联动而使该车轮旋转；以及

俯仰用旋转传感器，该俯仰用旋转传感器检测该俯仰用电动机的旋转位置或旋转速度，

该防摔倒控制装置利用伴随所述车轮的旋转的反作用转矩，修正所述主体的俯仰方向的倾斜，

该防摔倒控制装置包括：

前进/后退指示接收部，该前进/后退指示接收部接收所述车轮的前进或后退指示；

目标俯仰角计算部，该目标俯仰角计算部基于根据接收到的前进或后退指示、和由所述俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算作为目标的俯仰方向的倾斜角度即目标俯仰角；

俯仰倾斜角推定部，该俯仰倾斜角推定部根据由所述俯仰用角速度传感器检测出的俯仰角速度、和基于所述目标俯仰角而生成并提供给所述俯仰用电动机的俯仰用转矩指令，推定所述主体相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角；以及

俯仰用转矩指令生成部，该俯仰用转矩指令生成部基于所述目标俯仰角和所述俯仰倾斜角，生成所述俯仰用转矩指令。

2.如权利要求1所述的防摔倒控制装置，其特征在于，

所述前进/后退指示接收部接收所述车轮的前进或后退指示以作为旋转速度或旋转角度。

3.如权利要求1或2所述的防摔倒控制装置，其特征在于，

包括目标俯仰角速度计算部，该目标俯仰角速度计算部根据从所述目标俯仰角减去所述俯仰倾斜角而得到的俯仰角度偏差，计算作为目标的俯仰方向的倾斜角度的角速度即目标俯仰角速度，

所述俯仰用转矩指令生成部基于所述目标俯仰角速度和检测出的所述俯仰角速度的偏差，生成所述俯仰用转矩指令。

4.如权利要求1至3的任一项所述的防摔倒控制装置，其特征在于，包括：

俯仰方向外部转矩推定部，该俯仰方向外部转矩推定部基于所述俯仰倾斜角，推定使所述主体朝俯仰方向倾斜的俯仰方向外部转矩；以及

转矩修正部，该转矩修正部朝抵消所推定的所述俯仰方向外部转矩的方向修正所述俯仰用转矩指令。

5.如权利要求1至4的任一项所述的防摔倒控制装置，其特征在于，

所述主体包括：

摇摆用角速度传感器，该摇摆用角速度传感器检测摇摆方向的倾斜角度的角速度即摇摆角速度；

朝摇摆方向旋转的惯性转子；

摇摆用电动机，该摇摆用电动机与该惯性转子的旋转联动而使该惯性转子旋转；以及

摇摆用旋转传感器，该摇摆用旋转传感器检测该摇摆用电动机的旋转位置或旋转速度，

该防摔倒控制装置包括：

目标摇摆角计算部，该目标摇摆角计算部基于由所述摇摆用旋转传感器检测出的摇摆方向的旋转速度，计算作为目标的摇摆方向的倾斜角度即目标摇摆角；

摇摆倾斜角推定部，该摇摆倾斜角推定部根据由所述摇摆用角速度传感器检测出的摇摆角速度、和基于所述目标摇摆角而生成并提供给所述摇摆用电动机的摇摆用转矩指令，推定所述主体相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角；

目标摇摆角速度计算部，该目标摇摆角速度计算部根据从所述目标摇摆角减去所述摇摆倾斜角而得到的摇摆角度偏差，计算作为目标的摇摆方向的倾斜角度的角速度即目标摇摆角速度；以及

摇摆用转矩指令生成部，该摇摆用转矩指令生成部基于所述目标摇摆角速度和检测出的所述摇摆角速度的偏差，生成所述摇摆用转矩指令。

6.如权利要求5所述的防摔倒控制装置，其特征在于，

在所述主体的重心位于与所述车轮的旋转轴交叉的垂直线上的状态下，所述摇摆用角速度传感器的检测轴朝向前后方向和水平方向。

7.如权利要求1至6的任一项所述的防摔倒控制装置，其特征在于，

所述车轮的旋转的旋转比低于所述俯仰用电动机的旋转。

8.一种计算机程序，其特征在于，该计算机程序可由装载于防摔倒控制装置的计算机执行，

该防摔倒控制装置包括朝前后方向旋转而移动的车轮、以及与该车轮的旋转轴连接且在该车轮的上方朝俯仰方向及摇摆方向摇动的主体，

该主体包括：

俯仰用角速度传感器，该俯仰用角速度传感器检测俯仰方向的倾斜角度的角速度即俯仰角速度；

俯仰用电动机，该俯仰用电动机与所述车轮的旋转联动而使该车轮旋转；以及

俯仰用旋转传感器，该俯仰用旋转传感器检测该俯仰用电动机的旋转位置或旋转速度，

该防摔倒控制装置利用伴随所述车轮的旋转的反作用转矩，修正所述主体的俯仰方向的倾斜，

使所述计算机起到作为如下单元的功能：

前进/后退指示接收单元，该前进/后退指示接收单元接收所述车轮的前进或后退指示；

目标俯仰角计算单元，该目标俯仰角计算单元基于根据接收到的前进或后退指示、和由所述俯仰用旋转传感器检测出的旋转角度而导出的俯仰方向的旋转速度偏差，计算作为目标的俯仰方向的倾斜角度即目标俯仰角；

俯仰倾斜角推定单元，该俯仰倾斜角推定单元根据由所述俯仰用角速度传感器检测出的俯仰角速度、和基于所述目标俯仰角而生成并提供给所述俯仰用电动机的俯仰用转矩指令，推定所述主体相对于平衡状态朝俯仰方向倾斜的角度即俯仰倾斜角；以及

俯仰用转矩指令生成单元，该俯仰用转矩指令生成单元基于所述目标俯仰角和所述俯仰倾斜角，生成所述俯仰用转矩指令。

9.如权利要求8所述的计算机程序，其特征在于，

使所述前进/后退指示接收单元起到作为接收所述车轮的前进或后退指示以作为旋转速度或旋转角度的单元的功能。

10.如权利要求8或9所述的计算机程序，其特征在于，

使所述计算机起到作为目标俯仰角速度计算单元的功能，该目标俯仰角速度计算单元根据从所述目标俯仰角减去所述俯仰倾斜角而得到的俯仰角度偏差，计算作为目标的俯仰方向的倾斜角度的角速度即目标俯仰角速度，

使所述俯仰用转矩指令生成单元起到基于所述目标俯仰角速度和检测出的所述俯仰角速度的偏差、生成所述俯仰用转矩指令的单元的功能。

11.如权利要求8至10的任一项所述的计算机程序，其特征在于，使所述计算机起到作为如下单元的功能：

俯仰方向外部转矩推定单元，该俯仰方向外部转矩推定单元基于所述俯仰倾斜角，推定使所述主体朝俯仰方向倾斜的俯仰方向外部转矩；以及

转矩修正单元，该转矩修正单元朝抵消所推定的所述俯仰方向外部转矩的方向修正所述俯仰用转矩指令。

12.如权利要求8至11的任一项所述的计算机程序，其特征在于，

所述主体包括：

摇摆用角速度传感器，该摇摆用角速度传感器检测摇摆方向的倾斜角度的角速度即摇摆角速度；

朝摇摆方向旋转的惯性转子；

摇摆用电动机，该摇摆用电动机与该惯性转子的旋转联动而使该惯性转子旋转；以及

摇摆用旋转传感器，该摇摆用旋转传感器检测该摇摆用电动机的旋转位置或旋转速度，

使所述计算机起到作为如下单元的功能：

目标摇摆角计算单元，该目标摇摆角计算单元基于由所述摇摆用旋转传感器检测出的摇摆方向的旋转速度，计算作为目标的摇摆方向的倾斜角度即目标摇摆角；

摇摆倾斜角推定单元，该摇摆倾斜角推定单元根据由所述摇摆用角速度传感器检测出的摇摆角速度、和基于所述目标摇摆角而生成并提供给所述摇摆用电动机的摇摆用转矩指令，推定所述主体相对于平衡状态朝摇摆方向倾斜的角度即摇摆倾斜角；

目标摇摆角速度计算单元，该目标摇摆角速度计算单元根据从所述目标摇摆角减去所述摇摆倾斜角而得到的摇摆角度偏差，计算作为目标的摇摆方向的倾斜角度的角速度即目标摇摆角速度；以及

摇摆用转矩指令生成单元，该摇摆用转矩指令生成单元基于所述目标摇摆角速度和检测出的所述摇摆角速度的偏差，生成所述摇摆用转矩指令。

Images