JP5488095B2 - Inverted moving body and control method thereof - Google Patents

Description

  The present invention relates to an inverted moving body and a control method thereof.

  A moving body that moves by performing an inverted control has been developed. When embodying such an inverted mobile body, it is important to prevent overturning, runaway, etc. so that it can be operated safely. Here, the “inverted moving body” is a moving body having moving means such as wheels and a step of placing a load body such as a person or luggage, and the load body is driven by driving the wheels. A moving body that moves while maintaining an inverted state.

  FIG. 5 shows an example of an “inverted moving body”. The example shown in FIG. 5 is a two-wheel inverted robot that includes a wheel 501, a platform 502, a handle 503, and a grip 504, and conveys a user 505, and is a typical inverted mobile body.

  Patent Documents 1 to 4 disclose moving bodies that move by performing an inverted control. For example, in Patent Document 1, when the presence or absence of a passenger is detected using a proximity detector provided on an inverted moving body and the presence of the passenger cannot be confirmed, a safety switch is used. An inverted type moving body that prevents operation of a ground contact module having wheels is disclosed (page 41, FIG. 1 and the like).

Special table 2003-502002 gazette JP 2009-280132 A JP 2009-142127 A JP 2009-101899 A

For example, in the case where there is no occupant of an inverted mobile object, a technique for blocking the operation of the inverted mobile object with a safety switch for the purpose of enabling safe operation has been conventionally known (for example, Patent Document 1).
However, the dangerous behavior (falling, running away, etc.) of the inverted mobile body does not depend only on the presence or absence of the passenger, but is also caused by a plurality of other factors. As the dangerous state of the inverted moving body, for example, the following states can be considered.
(1) The state after the passenger jumps off the inverted moving body while traveling,
(2) The state in which the passenger supports the inverted moving body by hand before or after boarding,
(3) A state after the moment when the inverted moving body collides with an object during traveling,
(4) A state in which at least one wheel cannot be rotated in at least one direction because the wheels of the inverted moving body are sandwiched between grooves or steps.
(5) Friction with the road surface is small and the wheel is idling.
(6) A state in which one leg remains on the ground when the passenger is on board.
Therefore, in order to safely operate an inverted moving body, it is possible to determine whether or not it is in such a dangerous state, and if it is determined to be in a dangerous state, it prevents falling or running away There is a strong demand for safe control.

  Therefore, the present invention solves the above-described problems, determines whether it is in a dangerous state when the inverted control is performed, and can control the inverted mobile body safely and the control thereof. It aims to provide a method.

  An inverted moving body according to the present invention includes an at least one wheel and a load body rotatably connected to the wheel, and the inverted moving body that performs the inverted control of the load body by driving the wheel. A load angle detector that detects a load angle that is an inclination angle of the load body, and generates a torque command for driving the wheel based on the load angle detected by the load angle detector. A load angle detected by the load angle detector using a controller for performing the inversion control and an estimator derived based on a mathematical model of the inverted moving body, and generated by the controller A load angle estimation that calculates an estimated value of the load angle based on a torque command and calculates a load angle estimation error that is an error between the calculated estimated value and the load angle detected by the load angle detector Mistake When it is determined that the load angle estimation error is diverged by a calculation unit, a state discriminator that determines whether or not the load angle estimation error calculated by the load angle estimation error calculation unit diverges, and the state discriminator Includes a control switching unit that switches the control by the controller to perform predetermined safety ensuring control.

  Thereby, it is possible to calculate a load angle estimation error based on the load angle of the inverted moving body and the wheel torque command, and to determine whether or not the load angle estimation error diverges. Since the inverted moving body can be switched to the safety ensuring control, it is possible to determine a dangerous state and prevent a fall or a runaway.

  The load angle estimation error calculation unit calculates an estimator gain based on a load angle detected by the load angle detector and a torque command generated by the controller. Based on a calculator, a gain calculated by the estimator gain calculator using the estimator, a load angle detected by the load angle detector, and a torque command generated by the controller A load angle estimation error calculator for calculating the load angle estimation error.

  Furthermore, a wheel angle detector for detecting a rotation angle of the wheel is further provided, and the state discriminator discriminates whether or not the load angle estimation error calculated by the load angle estimation error calculation unit diverges. A plurality of states determined in advance for the inverted moving body based on the load angle estimation error, the load angle detected by the load angle detector, and the wheel angle calculated by the wheel angle detector. When the state discriminator determines that the load angle estimation error diverges, the control switcher switches the control by the controller to determine the discriminating state. If the state discriminator determines that the load angle estimation error does not diverge, the inversion control is started, continued, Any one of the control of the opening may be made to be implemented in the controller.

  Further, when the state discriminator determines that the load angle estimation error is diverging, based on the load angle estimation error, is the state after the boarding object jumps off during the movement of the inverted moving body? You may make it discriminate | determine. The control switching device is determined that the load angle estimation error is diverged by the state discriminator, and when it is determined that the boarded object is in a state after jumping while the inverted moving body is moving, It is preferable that the control by the controller is performed by switching to a control that gradually decelerates and stops at a predetermined pace after the boarding object jumps off.

  Furthermore, when the state discriminator determines that the load angle estimation error diverges, the inverted moving body is supported by an external object before or after boarding the boarding object based on the load angle. You may make it discriminate | determine whether it is in a state. The control switch is determined by the state discriminator that the load angle estimation error diverges, and the inverted moving body is supported by an external object before or after boarding the boarding object. When it is determined, it is preferable to switch the control by the controller to control that sets the torque command to 0.

  Further, when the state discriminator determines that the load angle estimation error diverges, the state discriminator is in a state after the moment when the inverted moving body collides with an object during movement based on the load angle estimation error. It may be determined whether or not. The control switcher is determined that the load angle estimation error is diverged by the state discriminator, and is determined to be in a state after the moment when the inverted moving body collides with an object during movement. It is preferable that the control by the controller is carried out by switching to a control in which the wheel is rotated for a predetermined short time in a direction opposite to that at the time of the collision and decelerated and stopped.

  Furthermore, when the state discriminator determines that the load angle estimation error diverges, the wheel is sandwiched by a groove or a step based on the wheel angle and cannot rotate in at least one direction. It may be determined whether or not there is. The control switch has been determined by the state discriminator that the load angle estimation error diverges, and is determined to be in a state in which the wheel cannot be rotated in at least one direction due to being sandwiched between grooves or steps. In this case, it is preferable that the control by the controller is switched to the control in which the torque command is set to zero.

  In addition, when the state discriminator determines that the load angle estimation error diverges, the friction with the road surface is small based on the wheel angle and the load angle estimation error, and the wheel is idling. You may make it discriminate | determine whether it is in a state. The control switch is determined when the state discriminator determines that the load angle estimation error diverges and the friction with the road surface is small and the wheel is idling. The wheel is rotated in the traveling direction of the inverted moving body with a relatively low motor torque until the load angle detected by the load angle detector approaches π / 2 [rad]. Thereafter, after the detected load angle approaches π / 2 [rad], it is preferable to switch to the control that continuously changes the motor torque to zero.

  Furthermore, when the state discriminator determines that the load angle estimation error diverges, a part of the boarding object is obtained when boarding the boarding object based on the load angle and the load angle estimation error. It may be determined whether or not is in a state of remaining on the ground. The control switch is determined that the load angle estimation error is diverged by the state discriminator, and it is determined that a part of the boarding object remains on the ground when the boarding object is boarded In addition, it is preferable that the control by the controller is switched to control that sets the control gain used by the controller to a relatively low value until the boarded object is completely boarded on the inverted moving body. It is.

  In addition, the state discriminator discriminates whether or not the boarding object is in a state of moving while on board based on the load angle when it is judged that the load angle estimation error does not diverge. May be. The control switch is controlled by the controller when it is determined by the state determiner that the load angle estimation error does not diverge and it is determined that the boarding object is moving while on board. It is preferable to switch to the control for causing the inverted moving body to run upside down.

  Furthermore, when the state discriminator discriminates that the load angle estimation error does not diverge, the inverted moving body is inverted on the spot without boarding an object based on the load angle. You may make it discriminate | determine whether it is in a state. The control switching unit determines that the load angle estimation error does not diverge by the state discriminator, and determines that the inverted moving body is inverted on the spot without boarding the object. In this case, it is preferable that the control by the controller is switched to the control that inverts the inverted moving body on the spot.

  An inverted moving body control method according to the present invention includes at least one wheel and a load body rotatably connected to the wheel, and the inverted body performs the inversion control of the load body by driving the wheel. A method for controlling a movable body, comprising: detecting a load angle that is an inclination angle of the load body; generating a torque command for driving the wheel based on the detected load angle; Performing the control and estimating the load angle based on the detected load angle and the generated torque command using an estimator derived based on the mathematical model of the inverted moving body A step of calculating a value, a step of calculating a load angle estimation error that is an error between the calculated estimated value of the load angle and the detected load angle, and whether the calculated load angle estimation error diverges A step of determining whether the result of the determination, when the load angle estimation error diverges, in which and a step of carrying out predetermined safety control by switching the inversion control.

  Thereby, it is possible to calculate a load angle estimation error based on the load angle of the inverted moving body and the wheel torque command, and to determine whether or not the load angle estimation error diverges. Since the inverted moving body can be switched to the safety ensuring control, it is possible to determine a dangerous state and prevent a fall or a runaway.

  According to the present invention, it is possible to provide an inverted moving body and a control method thereof that can determine whether it is in a dangerous state when the inverted control is performed and can safely control the inverted moving body.

It is a figure which shows the structure of the control system of the inverted moving body concerning Embodiment 1. FIG. FIG. 3 is a schematic side view of the inverted moving body according to the first embodiment. 3 is a flowchart illustrating a method for controlling an inverted moving body according to the first embodiment; It is a figure which shows the simulation result which confirms the effect which determines the danger of the inverted type mobile body concerning Embodiment 1, or a safe state. It is a figure which shows the inverted type mobile body relevant to this invention.

Embodiment 1 FIG.
Embodiments of the present invention will be described below with reference to the drawings.
FIG. 1 is a configuration diagram of an inverted moving body control system according to the present embodiment. The control system 100 includes a command input device 101, a controller 102, an inverted moving body 103, a load angle detector 104, a load angle estimation error calculator 105, a state discriminator 108, a control switch 109, It is equipped with.

  The command input device 101 is configured so that the moving body 103 is in a desired running state (forward / backward, acceleration / deceleration, left / right turn, stop, etc.) in accordance with a driving operation (handle operation, switch operation, center of gravity movement operation, etc.) by the passenger A command value is generated. The command input device 101 generates, for example, a load angle command or a moving speed command of the inverted moving body 103 as a command input.

  The controller 102 performs the inversion control of the inverted moving body 103 so that the load angle of the inverted moving body 103 follows the input command. In addition, the controller 102 performs control for ensuring predetermined safety such as retreat travel or stop in response to a control switching instruction from the control switch 109. The controller 102 generates a torque command in order to perform these controls.

  The load angle detector 104 detects an inclination angle of the inverted moving body 103 with respect to the vertical line as a load angle used for the inversion control by the controller 102. The load angle detector 104 is installed on the inverted moving body 103. The load angle detector 104 is an attitude sensor configured using, for example, a gyro sensor or an acceleration sensor.

  The load angle estimation error calculator 105 includes an estimator gain calculator 105 and a load angle estimation error calculator 106. The load angle estimation error calculation unit 105 uses the estimator (observer) derived from the mathematical model of the inverted moving body 103, the torque command output from the controller 102, and the load detected by the load angle detector 104. An estimated value of the load angle is calculated based on the angle, and a load angle estimation error that is an error between the calculated estimated value of the load angle and the detected load angle is calculated.

  The estimator gain calculator 106 uses an estimator (observer) based on the torque command output from the controller 102 and the load angle detected by the load angle detector 104, and calculates the load angle estimated value and the load. Estimate angular velocity estimated value, load angular acceleration estimated value, etc. Further, the estimator gain calculator 105 calculates an estimator gain based on the torque command output from the controller 102, the load angle detected by the load angle detector 104, and the load angle estimated value.

  The load angle estimation error calculator 107 uses an estimator (observer) to calculate and detect the estimated value of the load angle based on the torque command, the detected load angle, and the calculated estimator gain. A load angle estimation error that is an error from the determined load angle is calculated.

  The state discriminator 108 discriminates whether or not the calculated load angle estimation error diverges. Further, the state discriminator 108 determines which of the plurality of states of the inverted mobile body 103 is based on the calculated waveform shape of the load angle estimation error.

  The control switch 109 switches the inversion control by the controller 102 based on the determination result by the state determiner 108 and causes the controller 102 to perform predetermined safety ensuring control such as retreating or stopping. When it is determined by the state discriminator 108 that the load angle estimation error diverges, the control switch 109 further switches the control by the controller 102 according to the determined state, and enters the determined state. The controller 102 performs corresponding safety ensuring control. Further, when the state discriminator 108 determines that the load angle estimation error does not diverge, the control switching unit 109 further controls the controller 102 to perform normal inversion control (start and continue) according to the determined state. , Any one control to be resumed) is performed.

  Next, with reference to FIG. 2, a mechanism in which the inverted moving body switches the inversion control according to the state will be described in detail. FIG. 2 is a schematic side view of the inverted moving body according to the present embodiment.

  As shown in FIG. 2, the inverted moving body 103 includes two wheels 202 arranged coaxially and a load body 201 connected to the wheels. The load body 201 is rotatably connected with the rotation axis of the wheel 202 as the center of rotation. The load body 201 is configured to include a boarding object such as a passenger or luggage, and further a step of supporting the boarding object. The load angle indicates an inclination angle of the load body 201 with respect to the vertical direction. The inverted moving body 103 rotates the wheels 202 so as to maintain the inverted state of the load body 201 and move at a desired speed in the traveling direction (for example, the left-right direction in the figure). The moving body 103 is configured to move forward or backward depending on, for example, the tilt angle when the load body 201 is tilted forward or backward. Although not shown, the inverted moving body 103 includes a speed reducer that transmits a motor torque that rotates the wheel 202, a motor that generates the motor torque, a servo amplifier that drives the motor, A wheel angle detector for detecting the rotation angle and an input device for inputting a desired rotation speed command to the motor are provided.

Here, the state of the inverted moving body specifically includes the following states. Of the following states 1 to 8, state 1 and state 2 show examples of safe states for the passenger. States 3 to 8 show examples of dangerous states.
State 1: State in which the boarding object is traveling State 2: State in which the boarding object is not boarded and the inverted mobile body is inverted on the spot State 3: The boarding object is upside down while traveling State after jumping off 103 State 4: State where inverted moving body 103 is supported by an external object such as a passenger's hand before or after boarding the object State 5: Inverted moving body 103 while traveling The state after the moment when the vehicle collides with the object State 6: The state where the wheel 202 of the inverted moving body is sandwiched between grooves or steps and cannot rotate in at least one direction State 7: The friction with the road surface is small, and the wheel 202 A state where the vehicle is idle State 8: A state where a part of the passenger's foot, etc. remains on the ground when boarding the boarding object

Details of the dangerous state (state 3 to state 8) will be described below.
In state 3, after the passenger jumps off, there is a possibility that the inverted mobile body 103 and the passenger come into contact with each other. For example, there is a possibility of coming into contact with a handle that has bounced up (for example, the handle 503 in FIG. 5).
In state 4, when the passenger supports the inverted mobile body 103 at an angle at which a handle (for example, the handle 503 in FIG. 5) tilts toward the passenger, the inverted mobile body 103 travels toward the passenger. May be in contact with the passenger.
In the state 5, the control by the passenger cannot be performed, and the inverted mobile body 103 may run away.
In state 6, the inverted mobile body 103 tries to rotate in the direction in which the wheel 202 can no longer rotate, so that the wheel 202 rotates with the maximum torque at the moment when the wheel 202 slightly deviates from the groove or step. There is a possibility that the inverted moving body 103 will fall greatly.
In state 7, since the wheel 202 can receive only a small reaction force from the floor, the wheel 202 is rotated at the maximum torque in order to set the load angle to a desired angle. As a result, when the road surface friction changes, the wheel 202 is inverted. There is a possibility that the moving body 103 shakes greatly.
In state 8, as in state 4, depending on the angle of the handle, there is a possibility that the foot remaining on the occupant's ground may come into contact with the inverted moving body 103 and its wheels 202.

In order to model the inverted moving body illustrated in FIG. 2 using a mathematical model, an equation of motion of the inverted moving body will be described. In the following, equations of motion are derived using analytical mechanics techniques.
First, the kinetic energy T and the potential energy V are expressed using the following (Equation 1) and (Equation 2), respectively.

The meanings of the symbols used in (Equation 1) and (Equation 2) are as follows.
θ ₁ : Load angle θ ₂ : Wheel angle m ₁ : Load mass J ₁ : Load inertia moment m ₂ : Wheel mass J ₂ : Wheel inertia moment l: Distance between load center of gravity and axle r: Wheel radius g: Gravity acceleration

  Next, Lagrangian L = TV is calculated using (Equation 1) and (Equation 2), and using the Euler-Lagrange equation, the equation of motion is obtained as (Equation 3) and (Equation 4) below. It is done.

なお、（数３）で用いた記号の意味は次の通りである。
Ｔ_ｍ：モータトルク

In addition, the meaning of the symbol used by (Equation 3) is as follows.
T _m : Motor torque

Next, the principle of detecting the state 8 from the state 1 described above and switching the inversion control according to each detected state will be described. First, from (Equation 3) and (Equation 4), the second-order time differential value of the wheel angle θ ₂ (in the equation, the variable indicated by adding two dots above θ ₂ ) is deleted, and the following (Equation 5) is obtained.

An estimator (observer) for estimating the load angle θ ₁ is designed based on (Equation 5) with the following (Equation 6) configuration. The estimator (observer) can estimate a load angle estimated value, a load angular velocity estimated value, a load angular acceleration estimated value, and the like from the detected load angle θ ₁ and the torque command T _m . Although details will be described later, in (Equation 6), by setting an estimator gain L shown in (Equation 9) described later, the estimated load angle converges to the load angle.

The meanings of the symbols used in (Equation 6) are as follows.
L: Estimator gain sgn (·): Signum function (sign function)
θ ₁ ^{^} : Load angle estimation value (in the equation, a variable indicated by adding a hat (^) to the top of θ ₁ )
θ ₁ ^˜ : Load angle estimation error (in the equation, a variable indicated by adding a tilde (˜) above θ ₁ )
s: Intermediate variable c: Speed of convergence of load angle estimation error

Next, a sufficient condition of the estimator gain L for converging the load angle estimation error θ ₁ ^to 0 is derived using the estimator of (Equation 6). In order to converge the load angle estimation error θ ₁ ^to 0 in (Equation 6), the intermediate variable s should be converged to 0. Therefore, the following (Equation 7) is used as a Lyapunov function candidate.

  In (Equation 7), V> 0 for all s ≠ 0. If the first-order time derivative of the Lyapunov function candidate V is negative for all s ≠ 0, the intermediate variable s converges to zero. That is, the following (Equation 8) may be established.

The sufficient condition of (Equation 8) is as follows.

The meanings of symbols used in (Equation 9) are as follows.
δ: Indicator for determining the state of the inverted moving body 103

By setting the detector gain L as in (Equation 9), (Equation 8) is established, and the intermediate variable s converges to 0. When s converges to 0, the load angle estimation error θ ₁ ^˜ converges at the speed of c from (Equation 6), and the load angle estimation value θ ₁ ^{^} converges to the load angle θ ₁ .

Here, a situation is assumed in which an external force not included in the equations of motion shown in (Equation 3) and (Equation 4) is applied to the inverted moving body 103. In such a situation, if the absolute value of the external force is greater than the state determination index δ (Equation 9) no longer satisfied (9), the load angle estimation error theta ₁ ^~ diverges.

Therefore, it is possible to determine whether or not an external force greater than the state determination index δ is applied to the inverted moving body 103 according ^to the divergence or convergence state of the load angle estimation error θ ₁ . Among the plurality of dangerous states described above, in the case of the state 1 and the state 2, the load angle estimation error θ ₁ ^to converges to 0, whereas in the case of the state 3 to the state 8, the load angle estimation error θ ₁ ^to diverge.

Based on the principle described above, the estimator gain calculator 106 shown in FIG. 1 calculates the estimator gain L using (Equation 9). The load angle estimation error calculator 107 calculates the load angle estimation error θ ₁ ^to based on (Equation 5) and (Equation 6). The state discriminator 108 discriminates whether or not the calculated load angle estimation errors θ ₁ ^to diverge. As a result of the determination, while the load angle estimation error θ ₁ ^˜ converges to 0, it is determined that the inverted moving body 103 is in a safe state and the inversion control is continued. On the other hand, if the load angle estimation error θ ₁ ^˜ diverges as a result of the determination, it is a dangerous state such as the state 3 to the state 8 described above, so that the inverted moving body 103 is controlled to retreat or stop. To implement. Furthermore, as will be described later, the state discriminator 108 can also determine whether the inverted moving body 103 is in the state 1 to the state 8 described above.

Using the load angle estimation error θ ₁ ^˜ , the state 1 to the state 8 described above are determined based on the following conditions. This determination is performed by the state determiner 108. In the following determination conditions, d is a threshold value for determining convergence / divergence of the load angle estimation error θ ₁ ^to c ₅₁ , c ₅₂ , c ₆₁ , c ₆₂ , c ₇₁ , and c ₇₂ are constant values. .

状態１の判別条件（数１０）は、走行中の倒立型移動体１０３の負荷角度θ_１は、進行方向に非零値を取ることに基づいて導出した。 (Determination condition of state 1)

The condition 1 discrimination condition (Equation 10) was derived based on the fact that the load angle θ ₁ of the inverted moving body 103 during traveling takes a non-zero value in the traveling direction.

状態２の判別条件（数１１）は、搭乗者が搭乗せずに倒立型移動体１０３がその場で倒立している場合には、負荷角度θ_１が０であることに基づいて導出した。 (Determination condition of state 2)

The determination condition (Formula 11) for the state 2 is derived based on the fact that the load angle θ ₁ is 0 when the inverted mobile body 103 is inverted on the spot without the passenger getting on.

状態３の判別条件（数１２）は、搭乗者が飛び降りた場合には、負荷慣性モーメントが突然減少して、図２に例示した負荷体２０１を起き上がらせる方向に、負荷角度推定誤差θ_１ ^〜がオーバーシュートすることに基づいて導出した。 (Determination condition of state 3)

Determination conditions state 3 (Equation 12), when the rider jumped is moment of inertia is suddenly reduced, in the direction in which Okiagara the load body 201 illustrated in FIG. 2, the load angle estimation error theta ₁ ^~ Derived based on overshoot.

状態４の判別条件（数１３）は、負荷体２０１を外から手で支えている場合には、負荷角度θ_１が一定値を取り、モータトルクＴ_ｍが飽和して一定値を取ることに基づいて導出した。 (Conditions for status 4)

The determination condition (Formula 13) for state 4 is that when the load body 201 is supported by the hand from the outside, the load angle θ ₁ takes a constant value, and the motor torque T _m saturates and takes a constant value. Derived based on.

状態５の判別条件（数１４）は、倒立型移動体１０３が一定速度で移動中に物体と衝突した場合には、衝突の直後の所定の短い時間の間、一定速度で移動し、モータトルクＴ_ｍが飽和することに基づいて導出した。 (Conditions for status 5)

The determination condition (Formula 14) for state 5 is that when the inverted moving body 103 collides with an object while moving at a constant speed, it moves at a constant speed for a predetermined short time immediately after the collision, and the motor torque Derived based on the saturation of _Tm .

状態６の判別条件（数１５）は、図２に例示した車輪２０２が少なくとも片方に動かなくなった場合には、車輪速度ｄθ_２／ｄｔが０となり、モータトルクＴ_ｍが飽和し、負荷体２０１が自由落下することに基づいて導出した。 (Conditions for state 6)

The determination condition (Formula 15) for the state 6 is that when the wheel 202 illustrated in FIG. 2 stops moving to at least one side, the wheel speed dθ ₂ / dt becomes 0, the motor torque T _m is saturated, and the load body 201 Was derived on the basis of free fall.

状態７の判別条件（数１６）は、車輪２０２が空転する場合には、車輪速度ｄθ_２／ｄｔが非零となり、負荷体２０１を起き上がらせるためにモータトルクＴ_ｍが飽和し、路面からの反力が得られないために負荷体２０１が自由落下することに基づいて導出した。 (Determination condition of state 7)

The determination condition of state 7 (Equation 16) is that when the wheel 202 is idling, the wheel speed dθ ₂ / dt becomes non-zero, the motor torque T _m is saturated in order to raise the load body 201, and from the road surface Since the reaction force could not be obtained, the load 201 was derived based on the free fall.

状態８の判別条件（数１７）は、搭乗者が倒立型移動体１０３に片足を乗せており、且つ、倒立型移動体１０３が搭乗者側に傾いている状態では、路面に着いている足に搭乗者の体重がより多く掛かっており、搭乗者から遠い側に傾いている状態では、倒立型移動体１０３に載せている足に搭乗者の体重がより多く掛かっていることに基づいて導出した。 (Conditions for status 8)

The discrimination condition (Formula 17) for state 8 is that the foot that is on the road surface when the rider puts one foot on the inverted moving body 103 and the inverted moving body 103 is tilted toward the passenger. Is derived based on the fact that the weight of the occupant on the inverted mobile body 103 is heavier when the weight of the occupant is greater and the vehicle is leaning farther from the occupant. did.

  The state discriminator 108 discriminates the state based on the discrimination condition described above, and the discrimination result is output to the control switch 109. When the state input from the state discriminator 108 is the state 1 or the state 2, the control switch 109 switches so that the controller 102 starts / continues / restarts the inverted control. When the state input from the state discriminator 108 is one of the state 3 to the state 8, the control switching unit 109 performs control for the controller 102 to retreat or stop according to the state. Switch to

  A specific example of the retreat travel or stop by the controller 102 and the effect when it is determined that any of the dangerous states of the state 3 to the state 8 will be described.

(In the case of state 3)
In a state after the passenger jumps off the inverted moving body 103 during traveling, the vehicle is gradually decelerated at a predetermined pace and stopped. Thereby, the danger that the passenger who jumped down and the inverted mobile body 103 contact can be reduced.

(In the case of state 4)
In the state where the passenger supports the inverted moving body 103 by hand before or after boarding, the motor torque _{Tm is set} to zero. Thereby, the danger that the inverted moving body 103 may run away and come into contact with surrounding objects can be reduced.

(In the case of state 5)
In a state after the moment when the inverted moving body 103 collides with an object during traveling, the wheel 202 is rotated in a direction opposite to that at the time of the collision for a predetermined short time, and then decelerated and stopped. Thereby, damage to a passenger and the collided object can be reduced.

(In the case of state 6)
In a state where the wheel 202 of the inverted moving body is sandwiched between grooves or steps and cannot rotate in at least one direction, the motor torque _{Tm is set} to zero. Thereby, when the wheel 202 is suddenly removed from the groove or the like, it is possible to reduce the risk that the inverted moving body 103 starts suddenly.

(In the case of state 7)
Small friction with the road surface, in the state in which the wheel 202 is idle, until the load angle theta ₁ which is detected approaches the π / 2 [rad], at relatively low motor torque T _m, the inverted vehicle When the wheel 202 is rotated in the traveling direction 103 and the load angle θ ₁ approaches π / 2 [rad], the motor torque T _m is continuously changed to zero. Thereby, when the inverted mobile body 103 falls down, there is a possibility that the impact on the passenger can be reduced.

(In the case of state 8)
When one of the legs remains on the ground when the passenger is on board, the control gain is set to a relatively low value until both legs of the passenger get on the inverted mobile body 103. For example, the control band is set to about 10 [Hz]. As a result, the risk of the passenger's feet remaining on the road surface coming into contact with the inverted moving body 103 can be reduced, and at the same time, the rider can easily balance, so that the rider can ride safely. There is an effect of becoming.

With reference to FIG. 3, a process for switching between the inverted control of the inverted moving body and the control for retracting or stopping will be described. FIG. 3 is a flowchart of the control method of the inverted moving body.
First, the passenger inputs a command such as a desired load angle and moving speed using the command input device 101 (S301). The load angle detector 104 detects the load angle at a constant cycle (S302).

The load angle estimation error calculator 105 (the estimator gain calculator 106 and the load angle estimation error calculator 107) calculates the load angle estimation error θ ₁ ^to as described above (S303). The state discriminator 108 determines whether or not the load angle estimation error θ ₁ ^˜ is divergent, and based on the waveform shape of the load angle estimation error θ ₁ ^˜ , in any of the above state 1 to state 8 It is determined whether or not there is (S304).

When the load angle estimation error θ ₁ ^˜ converges (state 1 or state 2), the control switch 109 switches the control of the controller 102 so as to start / continue / restart the inverted control (S305). On the other hand, the control switch 109, when the load angle estimation error theta ₁ ^~ diverges (either from the state 3 of the state 8), stops the control according to the command input, according to the determined state, The control of the controller 102 is switched so as to make the retreat travel or stop (S306). In order to cause the controller 102 to stop the control according to the command input, for example, the control switching unit 109 uses the gain used when the controller 102 calculates the control command based on the input command from the command input device 101. The gain value may be set to 0. Thereafter, the processing from S301 to S306 is repeated again.

Next, the effect of the present invention will be described with reference to FIG. Here, a simulation for determining a safe or dangerous state of the inverted mobile body 103 is performed using the estimator of (Equation 6), and the result is shown in FIG. The numerical values used for the simulation are as follows.
m ₁ = 70 [kg]
J ₁ = 25.2 [kg · m ^ 2]
m ₂ = 15 [kg]
J ₂ = 0.075 [kg · m ^ 2]
l = 0.9 [m]
r = 0.1 [m]
g = 9.8 [m / s ^ 2]
D = 1 × 10 ^ −6 [N · s / m] (D: Coefficient of viscous friction between wheel and road surface)
c = 500 [rad / s]
δ = 0.01
Kp = 50 [s ^ -1] (Kp: load angle proportional control gain)
Kv = 50 (2π) [s ^ −1] (Kv: load speed proportional control gain)
T = 1 × 10 ^ −3 [s] (T: control cycle)

In this simulation, the controller 102 proportionally controls the load angle θ ₁ and the wheel speed θ ₂ using the gains (Kp, Kv). Then, the road wheels 202 are in contact, if the changes to the state (state 7) slip from a state in which no slip (state 1), based on ^~ load angle estimation error theta ₁ which the estimator calculates the (number 6) Thus, the state of the inverted moving body 103 of the inverted moving body 103 is determined.

As shown in FIG. 4A, at time 2 [s], the road surface is changed from a state in which the wheels 202 and the road surface do not slip (a stick indicated by a value 1 on the vertical axis) to a state in which the wheel 202 slips (a slip indicated by 0). The state changes.
In FIG. 4B, the applied load angle command is indicated by a broken line, and the detected load angle θ ₁ is indicated by a solid line. In this case, in a situation shown in FIG. 4 (a), when given load angle command as shown in FIG. 4 (b), Ni will be indicated by the load angle theta ₁ is detected, and slipped It can be seen that the load body 201 is diverging.

Figure 4 (c), the shows ^~ load angle estimation error theta ₁ which the load angle estimation error calculator 107 outputs a solid line. Under the conditions shown in FIGS. 4A and 4B, the load angle estimation error θ ₁ ^to be output from the load angle estimation error calculator 107 changes as shown in FIG. 4C. That is, when the above-described equation of motion is not established due to the external force, the load angle estimation error θ ₁ ^˜ changes beyond the threshold value.

In FIG. 4 (c), the state determination unit 108, among the time interval the load angle estimation error theta ₁ ^~ is within a predetermined range, the load angle theta _{1 =} 0 a is the interval (0 [s] from 1 [ s]), it is determined that the state is 2, and in the section where the load angle θ ₁ ≠ 0 (after 1 [s]), it is determined that the state is 1. The state determination unit 108, the load angle estimation error theta ₁ ^~ is in the time interval is beyond the scope of the broken line, the load angle estimation error theta ₁ ^~ are divergent quadratic curve manner, and the wheel angular velocity Based on the fact that (the second-order time differential value of the wheel angle θ ₂ ) is non-zero (not shown), it is determined that the state is 7. In FIG. 4C, a predetermined range is indicated by a broken line. The predetermined range is determined based on δ described above, and an appropriate value is set in advance for the threshold δ by the user.

Based on the determination result of the state, the control switch 109 causes the controller 102 to perform the proportional control using the above-described control gain while in the state 2 and the state 1, so that the inverted moving body 103 is inverted. Maintain state. And after changing to the state 7, until the load angle θ ₁ approaches π / 2 [rad], the wheel 202 is rotated in the traveling direction of the inverted moving body 103 with a relatively low motor torque T _m , after the load angle theta ₁ is close to π / 2 [rad] changes the motor torque T _m continuously 0. By thus changing the motor torque T _m, by a small friction (viscous friction coefficient D) between the wheel 202 and the road surface, to become a force in the direction in which Okiagara the load body 201 acts, inverted moving Even when the body 103 falls, the impact on the passenger can be reduced.

As described above, the inverted moving body 103 is described using two nonlinear equations of motion (Equations 3 and 4). By deforming these two equations of motion, one nonlinear differential equation (Equation 5) including only the load angle is obtained. Based on this nonlinear differential equation, a nonlinear estimator (Equation 6) for estimating the load angle is designed.
Then, the difference between the load angle and the load angle estimated value is defined as a load angle estimation error, a first-order differential function representing the desired dynamics of the load angle estimation error is defined as an intermediate variable, and Lyapunov, which is a quadratic function of the intermediate variable. Function candidates (Equation 7) are introduced.
The Lyapunov function candidate is positive when the intermediate variable is non-zero, and the estimator gain of the nonlinear estimator is set so that the first-order time derivative (Equation 8) of the Lyapunov function candidate is negative (Equation 9 ). Thereby, when the inverted moving body 103 does not receive a large external force that is not included in the nonlinear equation of motion, the load angle estimation error converges to 0, and when such an external force is received, Angle estimation error diverges.

While the load angle estimation error converges to 0, it can be determined that the inverted moving body 103 is in a safe state, and thus the inverted control is continued during this time. On the other hand, when the load angle estimation error diverges, it can be determined that the inverted moving body 103 is in a dangerous state, and thus control for ensuring safety such as retreating or stopping is performed.
Further, when the load angle estimation error diverges, a plurality of dangerous states can be determined based on the characteristics of the waveform of the load angle estimation error at that time. Implement the control.

In the conventional inverted type moving body, it is difficult to automatically determine whether or not it is in a dangerous state.
On the other hand, in the present invention, by detecting the divergence or convergence of the estimation error of the nonlinear estimator using only the detected load angle, it is possible to determine whether or not it is a dangerous state. Can be controlled safely.
Furthermore, based on the characteristics of the waveform of the load angle estimation error, it is possible to accurately determine the dangerous state, and it is possible to perform safe control according to each dangerous state.
Therefore, according to the present invention, it is possible to prevent a fall or runaway by determining a dangerous state and performing control so as to be safe.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, the above-described state 8 is an example, and other states may be determined as a safe state and a dangerous state for the passenger. The dangerous state may be a state in which the behavior of the inverted moving body becomes dangerous for the occupant due to an external force that was not assumed when modeling the inverted moving body.

  In the first embodiment described above, the inverted moving body including the two wheels 202 has been illustrated. However, the present invention is not limited to this, and the inverted moving body that performs the inverted control by driving one or more wheels 202 is used. Any mobile body may be used.

  Each element of the control system is not limited to hardware such as logic elements that operate as various arithmetic units, but is configured by a computer having a CPU (central processing unit), a memory (storage device), and the like. It may be. Then, a predetermined program is incorporated in this computer so as to realize each function of the controller 102, the estimator gain calculator 106, the load angle estimation error calculator 107, the state discriminator 108, the control switch 109, and the like. Good.

100: control system,
101: Command input device,
102: Controller
103: Inverted type moving body,
104: Load angle detector,
105: Load angle estimation error calculation unit,
106: Estimator gain calculator,
107: Load angle estimation error calculator,
108: State discriminator,
109: Control switch,
201: load body,
202: wheels,
501: wheel,
502: Platform
503: Handle,
504: Grip,
505: User

Claims (20)

An inverted mobile body comprising at least one wheel and a load body rotatably connected to the wheel, and performing an inverted control of the load body by driving the wheel,
A load angle detector for detecting a load angle that is an inclination angle of the load body;
A controller for generating a torque command for driving the wheel based on the load angle detected by the load angle detector and performing the inversion control;
Using the estimator derived based on the mathematical model of the inverted mobile body, the load angle detected by the load angle detector and the torque command generated by the controller, the load A load angle estimation error calculation unit that calculates an estimated value of an angle and calculates a load angle estimation error that is an error between the calculated estimated value and the load angle detected by the load angle detector;
A state discriminator for discriminating whether or not the load angle estimation error calculated by the load angle estimation error calculation unit diverges;
An inverted moving body comprising: a control switch that switches the control by the controller to perform predetermined safety ensuring control when the state determiner determines that the load angle estimation error is diverged. The load angle estimation error calculator is
An estimator gain calculator that calculates a gain of the estimator based on a load angle detected by the load angle detector and a torque command generated by the controller;
Using the estimator, based on the gain calculated by the estimator gain calculator, the load angle detected by the load angle detector, and the torque command generated by the controller, the load An inverted mobile object according to claim 1, further comprising a load angle estimation error calculator that calculates an angle estimation error. A wheel angle detector for detecting a rotation angle of the wheel;
The state discriminator is
It is determined whether or not the load angle estimation error calculated by the load angle estimation error calculation unit diverges, the load angle estimation error, the load angle detected by the load angle detector, and the wheel angle detection On the basis of the wheel angle calculated by the vessel, it is determined which of the plurality of states predetermined for the inverted moving body,
The control switch is
When it is determined by the state discriminator that the load angle estimation error is diverging, the control by the controller is switched, and predetermined safety ensuring control according to the determined state is performed,
The controller is configured to cause the controller to perform any one of start, continuation, and resumption of the inversion control when the state discriminator determines that the load angle estimation error does not diverge. The inverted mobile object according to 1. The state discriminator is
When it is determined that the load angle estimation error diverges, it is determined based on the load angle estimation error whether the boarded object is in a state after jumping down during the movement of the inverted moving body. The inverted moving body according to claim 3. The control switch is
When it is determined by the state discriminator that the load angle estimation error diverges, and when it is determined that the boarded object has jumped off during the movement of the inverted moving body, the control by the controller is performed. 5. The inverted moving body according to claim 4, wherein after switching off the boarding object, the control is switched to a control that gradually decelerates and stops at a predetermined pace. The state discriminator is
When it is determined that the load angle estimation error diverges, it is determined whether the inverted moving body is supported by an external object before or after boarding the boarding object based on the load angle. The inverted moving body according to claim 3, wherein The control switch is
When it is determined by the state discriminator that the load angle estimation error diverges, and it is determined that the inverted moving body is supported by an external object before or after boarding the boarding object, The inverted moving body according to claim 6, wherein the control by the controller is switched to control that sets the torque command to 0. The state discriminator is
When it is determined that the load angle estimation error diverges, it is determined based on the load angle estimation error whether or not the inverted moving body is in a state after the moment when the inverted moving body collides with an object. The inverted moving body according to claim 3, wherein The control switch is
When the state discriminator determines that the load angle estimation error diverges, and when it is determined that the inverted moving body is in a later state from the moment of collision with the object during movement, the control by the controller The inverted moving body according to claim 8, wherein the wheel is rotated by switching to a control for decelerating and stopping by rotating the wheel in a direction opposite to that at the time of the collision for a predetermined short time. The state discriminator is
When it is determined that the load angle estimation error diverges, based on the wheel angle, it is determined whether or not the wheel is sandwiched by a groove or a step and cannot rotate in at least one direction. The inverted moving body according to claim 3, wherein The control switch is
When it is determined by the state discriminator that the load angle estimation error diverges and the wheel is sandwiched between grooves or steps and cannot be rotated in at least one direction, the controller The inverted moving body according to claim 10, wherein the control according to is switched to control that sets the torque command to 0. The state discriminator is
When it is determined that the load angle estimation error diverges, it is determined whether or not the wheel is idling based on the wheel angle and the load angle estimation error with little friction with the road surface. The inverted moving body according to claim 3, wherein The control switch is
When the state discriminator determines that the load angle estimation error diverges and the friction with the road surface is small and the wheel is idling, the control by the controller Until the load angle detected by the angle detector approaches π / 2 [rad], the wheel is rotated in the traveling direction of the inverted moving body with a relatively low motor torque, and then detected. The inverted moving body according to claim 12, wherein after the load angle approaches π / 2 [rad], the control is performed by switching to control that continuously changes the motor torque to zero. The state discriminator is
When it is determined that the load angle estimation error diverges, a part of the boarding object remains on the ground when the boarding object is boarded based on the load angle and the load angle estimation error. It is discriminate | determined whether It is. The inverted type moving body of Claim 3 characterized by the above-mentioned. The control switch is
When it is determined by the state discriminator that the load angle estimation error diverges, and when the boarding object is boarded, it is determined that a part of the boarding object remains on the ground. 15. The control is performed by switching to a control in which a control gain used by the controller is set to a relatively low value until the boarding object is completely boarded on the inverted moving body. The inverted moving body described in 1. The state discriminator is
4. When it is determined that the load angle estimation error does not diverge, it is determined based on the load angle whether or not the boarding object is in a state of moving while on board. Inverted moving body. The control switch is
When it is determined by the state discriminator that the load angle estimation error does not diverge and it is determined that the boarding object is moving while on board, the control by the controller is performed by the inverted movement. The inverted moving body according to claim 16, wherein the inverted moving body is implemented by switching to a control for causing the body to run upside down. The state discriminator is
When it is determined that the load angle estimation error does not diverge, it is determined based on the load angle whether the inverted moving body is in an inverted state without the boarding object being boarded. The inverted moving body according to claim 3, wherein The control switch is
When it is determined that the load angle estimation error does not diverge by the state discriminator, and when it is determined that the inverted moving body is in an inverted state without boarding an object, 19. The inverted moving body according to claim 18, wherein the control by the controller is switched to a control for inverting the inverted moving body on the spot. An inverted moving body control method comprising at least one wheel and a load body rotatably connected to the wheel, wherein the load body is inverted by driving the wheel,
Detecting a load angle that is an inclination angle of the load body;
Generating a torque command for driving the wheel based on the detected load angle, and performing the inversion control;
Calculating an estimated value of the load angle based on the detected load angle and the generated torque command using an estimator derived based on the mathematical model of the inverted mobile body; ,
Calculating a load angle estimation error that is an error between the calculated estimated value of the load angle and the detected load angle;
Determining whether or not the calculated load angle estimation error diverges;
And a step of switching the inverted control to perform predetermined safety ensuring control when the load angle estimation error diverges as a result of the determination, and a method for controlling the inverted moving body.

Images