JP2013086557A - Moving body controller, method of controlling the same, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To improve safety of an inverted moving body which performs desired traveling while maintaining an inverted state according to a traveling operation by an occupant.SOLUTION: The moving body controller 110 includes: a state detection means 123 for detecting a state amount of the inverted moving body; a state estimation means 131 for calculating at least one of the state amount of the inverted moving body and the attitude angle of the occupant on the basis of the detected state amount, a disturbance torque estimated value generated at a driving wheel 410, an operation torque estimated value generated around the driving wheel and the driving torque of the driving wheel; a disturbance torque estimation means 132 for calculating a disturbance torque estimated value on the basis of at least one of the calculated state amount and the attitude angle of the occupant and the driving torque of the driving wheel; an operation torque estimation means 133 for calculating the operation torque estimated value on the basis of at least one of the calculated state amount and the attitude angle of the occupant; and a state determination means for determining whether the combination of the disturbance torque and operation torque of the driving wheel is in an instable state or not, on the basis of the disturbance torque estimated value, the operation torque estimated value, and the driving torque of the driving wheel.

Description

  The present invention relates to a moving body control device that stably controls traveling of an inverted moving body that performs a desired traveling while maintaining an inverted state in accordance with a traveling operation of a passenger, a control method thereof, and a program.

  There is known an inverted moving body that performs a desired traveling according to the inclination of a platform on which a passenger rides while maintaining an inverted state (see, for example, Patent Document 1).

JP 2003-502002 A

  Here, for example, when an inverted moving body that travels while maintaining an inverted state travels on an uneven road surface, disturbance torque is generated on the drive wheels due to the unevenness, and thus traveling becomes unstable. Further, due to the tilting operation of the platform by the occupant, an operating torque is generated in the drive wheel in accordance with the tilting operation.

  However, in the inverted type moving body shown in Patent Document 1, there is a state in which the disturbance torque from the road surface or the like and the operation torque according to the traveling operation are combined to increase instability, and such a state is sufficiently It cannot be said that stable control is taken into consideration, and there is a risk of problems in terms of safety.

  The present invention has been made to solve such problems, and it is a main object of the present invention to provide a mobile control device, a control method thereof, and a program capable of improving the safety of an inverted mobile body. And

One aspect of the present invention for achieving the above object is a moving body control device that controls an inverted moving body that performs a desired travel while maintaining an inverted state, wherein the state quantity of the inverted moving body is detected. State detecting means, state quantity detected by the state detecting means, disturbance torque estimated value which is an estimated value of torque due to disturbance generated in the driving wheel of the inverted moving body, and a traveling operation of the passenger At least one of the state quantity of the inverted mobile body and the occupant is calculated based on an operation torque estimated value that is an estimated value of torque generated around the driving wheel and a driving torque that drives the driving wheel. On the basis of at least one of the state quantity of the inverted moving body and the occupant calculated by the state estimating means, and the driving torque for driving the driving wheel, the disturbance torque Disturbance torque estimating means for calculating an estimated value, and operating torque estimation for calculating the operating torque estimated value based on at least one of the state quantity of the inverted moving body and the passenger calculated by the state estimating means The disturbance torque estimated value calculated by the disturbance torque estimating means, the operation torque estimated value calculated by the operation torque estimating means, and the driving torque for driving the driving wheel, And a state determination unit that determines whether or not a combination of disturbance torque and operation torque of the driving wheel is an unstable state that makes the inverted moving body unstable. .
In this one aspect, based on the determination result by the state determination unit, the state quantity of the inverted moving body calculated by the state estimation unit, and the disturbance torque estimation value calculated by the disturbance torque estimation unit Control means for calculating the drive torque of the drive wheel and controlling the drive of the drive wheel based on the calculated drive torque may be further provided.
In this aspect, when the state determination unit determines that the state is the unstable state, the control unit calculates a value obtained by multiplying the state amount obtained by modal coordinate conversion of the driving torque of the driving wheel by a control gain. You may perform the said control so that it may become the value which subtracted the said disturbance torque estimated value.
In this one aspect, when the state determination unit determines that the state is unstable, it may further include a warning unit that notifies the user to that effect.
In this aspect, the state determination means includes the state quantity that has undergone modal coordinate conversion, a drive torque that drives the drive wheel, a disturbance torque estimated value that is calculated by the disturbance torque estimation means, and the operation torque estimation. When the value based on the estimated operation torque calculated by the means is greater than a predetermined threshold value, the unstable state may be determined.
In this aspect, the state detection unit may detect at least one of rotation information of the driving wheel and posture angle information of the platform on which the passenger is boarded.
In this aspect, the state estimating means includes a driving torque of the driving wheel, a disturbance torque estimated value calculated by the disturbance torque estimating means, an operation torque estimated value calculated by the operating torque estimating means, The state quantity of the inverted mobile body and the occupant may be calculated based on a calculation result obtained by subtracting the estimated value of the state quantity from the state quantity and multiplying by the gain.
In this aspect, the state estimation means may calculate at least one of the angle of the driving wheel, the posture angle of the platform on which the passenger is boarded, and the posture angle of the passenger.
In this one aspect, the state estimation means may estimate the state quantity of the inverted mobile body and the occupant using the following equation (12).
In this aspect, the disturbance torque estimating means may calculate the disturbance torque estimated value using the following equation (13).
In this aspect, the operation torque estimating means may calculate the operation torque estimated value using the following equation (14).
In this one aspect, the state determination means determines that the state is unstable when any one of the following expressions (31), (32), (40), and (41) is satisfied. May be.
In this aspect, when the state determining unit determines that the inverted moving body is in the unstable state, the control unit determines that the driving torque of the driving wheel is calculated by the following equation (35). You may perform the said control so that it may be set to _m .
On the other hand, one aspect of the present invention for achieving the above object is a control method of a mobile control device that controls an inverted mobile body that performs a desired travel while maintaining an inverted state, wherein the inverted mobile body Detecting the state quantity, the detected state quantity, a disturbance torque estimated value that is an estimated value of the torque due to the disturbance generated in the driving wheel of the inverted moving body, and the traveling operation of the passenger At least one of the state quantity of the inverted moving body and the passenger is calculated based on an operation torque estimated value that is an estimated value of torque generated around the driving wheel and a driving torque that drives the driving wheel. Calculating the disturbance torque estimated value based on at least one of the calculated inverted moving body and the occupant state quantity and the driving torque for driving the driving wheel; Based on at least one of the calculated inverted moving body and the passenger's state quantity, calculating the operation torque estimated value, the calculated disturbance torque estimated value, and the calculated operation torque estimated Based on the value and the driving torque for driving the driving wheel, it is determined whether or not the combination of disturbance torque and operating torque of the driving wheel is an unstable state that makes the inverted moving body unstable. And a step of controlling the moving body control apparatus.
In this one aspect, the driving torque of the driving wheel is calculated based on the determination result, the detected state quantity of the inverted moving body, and the calculated disturbance torque estimated value, and the calculated driving The method may further include a step of controlling the driving of the driving wheel based on the torque.
This one aspect | mode WHEREIN: When it determines with it being the said unstable state, the step which notifies that to a user may further be included.
Furthermore, one aspect of the present invention for achieving the above object is a program of a moving body control device that controls an inverted moving body that performs a desired travel while maintaining an inverted state, the detected inverted type A state quantity of the moving body, a disturbance torque estimated value that is an estimated value of the torque caused by the disturbance generated in the driving wheel of the inverted moving body, and an estimated value of the torque generated around the driving wheel according to the traveling operation of the passenger A process for calculating at least one of the state quantity of the inverted mobile body and the passenger based on a certain operation torque estimated value and a drive torque for driving the drive wheel, and the calculated inverted movement A process for calculating the estimated disturbance torque based on at least one of the body and the state quantity of the passenger and the driving torque for driving the driving wheel, and the calculated inverted moving body and the passenger's A process for calculating the operation torque estimated value based on at least one of the attitudes, the calculated disturbance torque estimated value, the calculated operation torque estimated value, and a drive torque for driving the drive wheel And determining whether or not a combination of disturbance torque and operation torque of the driving wheel is an unstable state that makes the inverted moving body unstable, and causing the computer to execute It may be a program of the mobile control device.

  ADVANTAGE OF THE INVENTION According to this invention, the mobile body control apparatus which can improve the safety | security of an inverted mobile body, its control method, and a program can be provided.

It is a block diagram which shows the schematic system configuration | structure of the moving body control apparatus which concerns on one embodiment of this invention. It is a flowchart which shows an example of the control processing flow by the moving body control apparatus which concerns on one embodiment of this invention. It is a figure which shows an example of the time change of a passenger | crew's attitude | position angle in the simulation of the mobile body control apparatus which concerns on one embodiment of this invention. It is a figure which shows an example of the time change of the disturbance torque in the simulation of the mobile body control apparatus which concerns on one embodiment of this invention. It is a perspective view which shows schematic structure of the inverted type mobile body which concerns on one embodiment of this invention.

  Embodiments of the present invention will be described below with reference to the drawings. A moving body control apparatus 100 according to an embodiment of the present invention is an apparatus that stably controls the traveling of an inverted moving body 120 that is a control target as shown in FIG. The inverted moving body 120 according to the present embodiment includes a platform 421 on which a passenger rides, a pair of left and right drive wheels 410 rotatably provided on both sides of the platform 421, and a pair that drives the pair of drive wheels 410. Motor 121 (FIG. 1), and a handle 422 that is swingably provided on the platform 421 and is gripped and operated by a passenger.

  The inverted moving body 120 is operated according to a traveling operation (for example, an operation of tilting the platform 421 or the handle 422, input of operation information (movement speed, movement direction, etc.) to a command input device (not shown)). It is configured to perform a desired travel while maintaining the inverted state. For example, the inverted moving body 120 moves forward when the platform 421 is tilted forward, and the inverted moving body 120 moves backward when the platform 421 is tilted backward. In the present embodiment, the configuration of the inverted moving body 120 is an example. For example, the inverted moving body 120 has a configuration having a pair of drive wheels 410, but is not limited thereto, and may have a configuration having one wheel or three or more wheels, and any configuration is applicable.

  FIG. 1 is a block diagram showing a schematic system configuration of the mobile control device according to the present embodiment. The mobile body control device 110 according to the present embodiment is based on a state detector 123 that detects a state quantity of the inverted mobile body 120 and a state quantity of the inverted mobile body 120 detected by the state detector 123. Based on the estimation device 130 for estimating the disturbance torque and the operation torque, the state quantity detected by the state detector 123 and the disturbance torque estimation value and the operation torque estimation value estimated by the estimation device 130, each motor 121 is controlled. And a control device 140 for controlling.

  Each motor 121 is connected to the drive shaft of each drive wheel 410 via a speed reduction mechanism or the like, and independently drives each drive wheel 410 in accordance with a control signal (motor current) from the control device 140. . Accordingly, the inverted mobile body 120 can perform desired traveling such as forward / reverse, left / right turning, acceleration / deceleration, and stop while maintaining the inverted state according to the traveling operation of the passenger.

  The state detector 123 is a specific example of the state detection unit, and detects the state amount of the inverted moving body 120 such as rotation information of each driving wheel 410 and posture angle information of the platform 421. The state detector 123 detects the rotation angle, rotation angular velocity, and rotation angle acceleration of each driving wheel 410 using, for example, a rotation sensor provided on each driving wheel 410, and the angle provided on the platform 421 and the handle 422. The attitude angle of the platform 421 and the handle 422 can be detected using a sensor or the like.

  An estimation device 130 and a control device 140 are connected to the state detector 123, and the detected state quantity is output to the estimation device 130 and the control device 140 as a state detection value.

  Based on the state quantity of the inverted moving body 120 from the state detector 123 and the motor torque (drive torque) from the control device 140, the estimation device 130 is a disturbance torque generated in each drive wheel 410 by the external environment described later. And the operation torque generated in each drive wheel 410 by the operation of the passenger. The estimation device 130 includes a state estimator 131, a disturbance torque estimator 132, and an operation torque estimator 133.

  The state estimator 131 is a specific example of the state estimator, and the state detection value from the state detector 123, the disturbance torque estimation value from the disturbance torque estimator 132, and the operation torque estimation from the operation torque estimator 133. Based on the value and the motor torque of each motor 121 from the stability controller 142, for example, the estimated position of the inverted mobile body 120 and the occupant that cannot be detected by the state detector 123, such as the posture angle of the occupant Calculate the value.

  A disturbance torque estimator 132 and an operation torque estimator 133 are connected to the state estimator 131, and the disturbance torque estimator 132 uses the calculated estimated values of the inverted moving body 120 and the occupant as the state estimated values. And output to the operation torque estimator 133.

  The state estimator 131 is not input from the state detector 123 among the preset parameters (for example, parameters necessary for calculating the estimated disturbance torque value and the estimated operation torque value as will be described later). The parameter (for example, the posture angle of the occupant) that cannot be detected by the detector 123 may be automatically calculated, and otherwise, the state quantities from the state detector 123 may be output as state estimation values.

  The disturbance torque estimator 132 is a specific example of the disturbance torque estimation means, and is based on the state estimated value from the state estimator 131 and the motor torque of each motor 121 from the stability controller 142, and the inverted movement. Disturbance torque generated in each drive wheel 410 by the input from the external environment such as an uneven road surface while the body 120 is traveling is calculated. The disturbance torque estimator 132 is connected to the control device 140 and the state estimator 131 and outputs the calculated disturbance torque to the control device 140 and the state estimator 131 as a disturbance torque estimated value.

  The operation torque estimator 133 is a specific example of the operation torque estimator. The operation torque estimator 133 calculates the operation torque generated around each drive wheel 410 according to the traveling operation by the occupant based on the estimated state value from the state estimator 131. To do. A control device 140 and a state estimator 131 are connected to the operation torque estimator 133, and the calculated operation torque is output to the control device 140 and the state estimator 131 as an operation torque estimated value.

  The control device 140 controls the drive of each motor 121 based on the estimated disturbance torque and operation torque estimated value of the inverted moving body 120 from the estimation device 130 and the estimated state value from the state estimator 131. The control device 140 includes a state determiner 141 and a stability controller 142.

  The state determination unit 141 is a specific example of the state determination unit. The disturbance torque estimation value from the disturbance torque estimator 132 of the estimation device 130, the operation torque estimation value from the operation torque estimator 133, and the stability controller 142. Based on the motor torque of each motor 121, whether or not the combination of the disturbance torque and the operating torque is in an unstable state that makes the inverted moving body 120 unstable is determined. A stability controller 142 is connected to the state determiner 141, and the determination result is output to the stability controller 142 as a state determination result.

  When the state determiner 141 determines that the state is unstable, for example, a warning device (one specific example of a warning unit) 143 is used to warn the user. Examples of the warning device 143 include a speaker that outputs a warning sound, a light that lights / flashes a warning light, a display device that displays a warning, a vibration device that generates a warning vibration on the platform 421, the handle 422, and the like. A communication device that notifies the user via wireless or the like is included.

  The stability controller 142 is a specific example of a control unit, and includes a state estimated value from the state estimator 131, a state determination result from the state determiner 141, a disturbance torque estimated value from the disturbance torque estimator 132, Based on the above, when it is determined that the combination of the disturbance torque and the operation torque is in an unstable state, the torque of each motor 121 is calculated so that the inverted moving body 120 can continue the stable travel, and the calculated The motor torque of the motor 121 is output to the state estimator 131, the disturbance torque estimator 132, and the state determiner 141, and the motor current corresponding to the calculated torque of the motor 121 is sent to each motor 121 via a drive circuit or the like. Output for. Each motor 121 rotationally drives each drive wheel 410 according to the motor current from the safety controller 142. As a result, the inverted moving body 120 can continue to travel stably even when the combination of the disturbance torque and the operation torque is in an unstable state.

  Here, the estimation device 130 and the control device 140 can be realized as, for example, an ASIC (Application Specific Integrated Circuit) or a programmable electronic system.

  Next, the operation principle of the mobile control device 110 according to the present embodiment will be described in detail using a specific example.

ただし、上記（１）式乃至（３）式における各記号の意味は以下の通りである。
Ｊ_１：駆動車輪４１０の慣性モーメント[ｋｇ・ｍ^２]、
Ｊ_３：搭乗者の慣性モーメント[ｋｇ・ｍ^２]、
θ_１：駆動車輪４１０の角度［ｒａｄ］、
θ_２：プラットホーム４２１の姿勢角度［ｒａｄ］、
θ_３：搭乗者の姿勢角度［ｒａｄ］、
ｍ_１：駆動車輪４１０の質量[ｋｇ]、
ｍ_３：搭乗者の質量[ｋｇ]、
ｒ_１：駆動車輪４１０の半径[ｍ]、
ｌ_ｃ：搭乗者の重心距離（搭乗者の重心位置と駆動車輪４１０の回転軸（駆動軸）との間の距離）[ｍ]、
Ｔ_ｍ：モータ１２１のモータトルク（モータ１２１に対するトルク指令）[Ｎ・ｍ]、
Ｔ_ｄ：外乱トルク[Ｎ・ｍ]、
Ｔ_ｉｎ：操作トルク[Ｎ・ｍ]
倒立型移動体１２０が通常走行中において、下記（４）式乃至（６）式が成立する。 The equation of motion combining the passenger and the inverted moving body 120 is expressed as the following formulas (1) to (3).

However, the meaning of each symbol in the above formulas (1) to (3) is as follows.
J ₁ : moment of inertia of the driving wheel 410 [kg · m ² ],
J ₃ : Passenger moment of inertia [kg · m ² ],
θ ₁ : angle [rad] of the drive wheel 410,
θ ₂ : posture angle [rad] of the platform 421,
θ ₃ : Passenger posture angle [rad],
m ₁ : mass [kg] of the drive wheel 410,
m ₃ : Mass of passenger [kg],
r ₁ : radius [m] of the drive wheel 410,
l _c : the center of gravity distance of the occupant (the distance between the position of the center of gravity of the occupant and the rotation axis (drive axis) of the drive wheel 410) [m],
T _m : Motor torque of the motor 121 (torque command to the motor 121) [N · m],
T _d : disturbance torque [N · m],
T _in : Operating torque [N · m]
While the inverted moving body 120 is traveling normally, the following formulas (4) to (6) are established.

上記（４）式乃至（６）式を用いて、上記（１）式乃至（３）式を線形化すると、下記（７）式乃至（９）式が得られる。

When the above equations (1) to (3) are linearized using the above equations (4) to (6), the following equations (7) to (9) are obtained.

上記（７）式乃至（９）式を、状態空間で表現すると下記（１０）式および（１１）式が得られる。

When the above expressions (7) to (9) are expressed in the state space, the following expressions (10) and (11) are obtained.

ただし、上記（１０）式および（１１）式における記号の意味は以下の通りである。
ｘ：状態
Ａ：システム行列、
ｂ：モータ１２１のモータトルクの入力行列、
ｄ：操作入力行列、
Ｍ：慣性行列、
Ｋ：剛性行列、
ｈ：モータ１２１のモータトルクの入力ベクトル、
ｆ：操作トルク入力ベクトル、
Ｃ：出力行列、
ｙ：出力ベクトル
ここで、例えば、状態検出器１２３で検出できない搭乗者の姿勢角度θ_３を推定するため、状態推定器１３１に下記（１２）式を導入する。また、倒立型移動体１２０の外乱トルク推定値Ｔ_ｄハットを推定するため、外乱トルク推定器１３２に下記（１３）式を導入し、倒立型移動体１２０の操作トルク推定値Ｔ_ｉｎハットを推定するため、操作トルク推定器１３３に下記（１４）式を導入する。

However, the meanings of symbols in the above equations (10) and (11) are as follows.
x: state A: system matrix,
b: Input matrix of motor torque of the motor 121,
d: operation input matrix,
M: inertia matrix
K: stiffness matrix,
h: motor torque input vector of the motor 121,
f: operation torque input vector,
C: output matrix,
y: Output vector Here, for example, the following equation (12) is introduced to the state estimator 131 in order to estimate the posture angle θ ₃ of the passenger that cannot be detected by the state detector 123. Further, in order to estimate the disturbance torque estimation value _Td hat of the inverted mobile body 120, the following equation (13) is introduced into the disturbance torque estimator 132 to estimate the operation torque estimation value T _in hat of the inverted mobile body 120. Therefore, the following equation (14) is introduced to the operation torque estimator 133.

上記（１２）式乃至（１４）式の安定性を示すため、下記（１６）式に示すリアプノフ関数候補を導入する。

In order to show the stability of the above equations (12) to (14), Lyapunov function candidates shown in the following equation (16) are introduced.

ただし、上記（１６）式において、各記号の意味は以下の通りである。
ｑ_ｄ：外乱トルクＴ_ｄの重み、
ｑ_ｉｎ：操作トルクＴ_ｉｎの重み
上記（１６）式に対するリアプノフ方程式は、下記（１７）式として表される。

However, in the above equation (16), the meaning of each symbol is as follows.
q _d : weight of disturbance torque T _d ,
q _in : Weight of the operating torque T _in The Lyapunov equation for the above equation (16) is expressed as the following equation (17).

ただし、上記（１７）式において、Ｑは正定値行列である。
上記（１６）式の１階時間微分は、下記（１８）式として表される。

However, in the above equation (17), Q is a positive definite matrix.
The first-order time differentiation of the above equation (16) is expressed as the following equation (18).

上記（１８）式（Ｖの微分値）が全てのｘ~≠０に対して負となる十分条件は、下記（１９）式および（２０）式として導出できる。

The sufficient condition for the above expression (18) (the differential value of V) to be negative with respect to all x˜ ≠ 0 can be derived as the following expressions (19) and (20).

さらに、上記（８）式および（７）式を書き換えると、下記（２１）式および（２２）式が得られる。

Furthermore, when the above equations (8) and (7) are rewritten, the following equations (21) and (22) are obtained.

上記状態推定器１３１における（１２）式の帯域を十分に広く設計することにより、トルク外乱推定誤差Ｔ_ｄ~および操作トルク推定誤差Ｔ_ｉｎ~よりも状態推定誤差ｘ~の方が早く０に収束すると仮定できる。したがって、上記（１３）式及び上記（２１）式を用いて、上記（１９）式が得られ、上記（１４）式及び上記（２２）式を用いて、上記（２０）式が得られる。

By designing sufficiently wide band (12) in the state estimator 131 converges, the torque disturbance estimation error T _{d ~} and operating torque estimation error T _in the state estimation error x 0 faster towards ~ than _~ You can assume that. Therefore, the equation (19) is obtained using the equation (13) and the equation (21), and the equation (20) is obtained using the equation (14) and the equation (22).

Based on the above equation (12), the estimated state value x hat converges to the detected state value x. Based on the above equation (13), the estimated disturbance torque T _d hat converges to the disturbance torque T _d . Based on the above equation (14), the operation torque estimated value T _in hat converges to the operation torque T _in .

As described above, the state estimator 131 includes the state x, the disturbance torque estimated value T _d hat from the disturbance torque estimator 132, the operation torque estimated value T _in hat from the operation torque estimator 133, and the stability controller 142. The state estimated value x hat is calculated using the above equation (12) based on the motor torque T _m from the above. As a result, it is possible to estimate an arbitrary inverted moving body 120 and a passenger's state quantity that cannot be detected by the state detector 123. Therefore, for example, the number of sensors provided in the inverted movable body 120 can be reduced, and the size and weight can be efficiently reduced.

The disturbance torque estimator 132 uses the above equation (13) based on the state estimation values θ ₁ , θ ₂ , θ ₃ from the state estimator 131 and the motor torque T _m from the stability controller 142. The disturbance torque estimated value _Td hat is calculated. The operating torque estimator 133 calculates the operating torque estimated value T _in hat using the above equation (14) based on the state estimated values θ ₁ , θ ₂ , and θ ₃ from the state estimator 131.

Next, the disturbance torque T _d caused in the drive wheels 410 by the external environment such as uneven road surface, and the operating torque T _in occurring around each drive wheel 410 by the travel operation by the rider 110, the combination of the inverted vehicle 120 The operation of the moving body control device 110 when becomes unstable will be described in detail.

ただし、上記（２３）式において、Σはモーダル行列（システム行列Ａの固有ベクトルを並べた正方行列）である。
上記（２３）式を上記（１０）式へ代入すると、下記（２４）式が得られる。 First, modal coordinate conversion of the state detection value x is performed using the following equation (23).

However, in the above equation (23), Σ is a modal matrix (a square matrix in which the eigenvectors of the system matrix A are arranged).
Substituting the above equation (23) into the above equation (10) yields the following equation (24).

上記（２４）式のシステム行列（ΣＡΣ^−１）は、対角行列とジョーダン正準系の対角ブロック行列となる。したがって、上記（２４）式の対角行列に対応するサブシステムおよび、ジョーダン正準系に対応するサブシステムの最後の式は、下記（２５）式として書き換えられ、ジョーダン正準系に対応するサブシステムの最後の式以外の式は、下記（２６）式として書き換えられる。

The system matrix (ΣAΣ ⁻¹ ) in the above equation (24) is a diagonal block matrix and a diagonal block matrix of the Jordan canonical system. Therefore, the subsystem corresponding to the diagonal matrix of the above equation (24) and the last equation of the subsystem corresponding to the Jordan canonical system are rewritten as the following equation (25), and the subsystem corresponding to the Jordan canonical system: Expressions other than the last expression of the system are rewritten as the following expression (26).

ただし、上記（２５）式および（２６）式における各記号の意味は以下の通りである。
λ_ｉ：システム行列Ａの第ｉ番目の固有値［ｒａｄ／ｓ］、
β_ｉ：Σｂの第ｉ要素、
δ_ｉ：Σｄの第ｉ要素
上記（２５）式および（２６）式において、インデックスｉは重複することはないものとする。また、上記（２５）式および（２６）式を合わせて、１から６の整数が全て割り当てられる。
上記（１０）式が不安定と成るための十分条件は、下記（２７）式又は（２８）式として表される。

However, the meaning of each symbol in the above formulas (25) and (26) is as follows.
λ _i : i-th eigenvalue [rad / s] of the system matrix A,
β _i : i-th element of Σb,
δ _i : i-th element of Σd In the above formulas (25) and (26), the index i is assumed not to overlap. Further, all the integers from 1 to 6 are assigned by combining the above formulas (25) and (26).
A sufficient condition for the above expression (10) to be unstable is expressed as the following expression (27) or (28).

ただし、上記（２７）式および（２８）式において、α_ｉを正の定数とする。
本実施の形態において、モータトルクが飽和しない範囲で制御可能な外乱トルクと操作トルクを対象としており、上記（２７）式および（２８）式は上記（１０）式の不安定性が不安定極の存在に起因する場合を示している。したがって、外乱トルクや操作トルクが大きくモータトルクが飽和して不安定となる場合は、本実施の形態において対象外とする。
上記（２７）式および（２８）式に、ｓｇｎ（ｚ_ｉ）を乗算し、｜ｚ_ｉ｜で除算すると下記（２９）式および（３０）式が得られる。

However, in the above equations (27) and (28), α _i is a positive constant.
In the present embodiment, disturbance torque and operation torque that can be controlled within a range in which the motor torque does not saturate are targeted, and the above formulas (27) and (28) are based on the instability of the above formula (10). The case due to existence is shown. Therefore, when the disturbance torque and the operation torque are large and the motor torque is saturated and unstable, it is excluded from the present embodiment.
When the above equations (27) and (28) are multiplied by sgn (z _i ) and divided by | z _i |, the following equations (29) and (30) are obtained.

状態判定器１４１は、下記（３１）式及び（３２）式を用いて、外乱トルクと操作トルクとの組合せが倒立型移動体１２０をより不安定にする不安定状態であるか否かを判定する。より具体的には、状態判定器１４１は、モーダル座標変換を行った状態検出値ｘと、外乱トルク推定器１３２からの外乱トルク推定値Ｔ_ｄハットと、操作トルク推定器１３３からの操作トルク推定値Ｔ_ｉｎハットと、安定制御器１４２からのモータトルクＴ_ｍと、に基づいて、下記（３１）式又は（３２）式が成立したと判断したとき、外乱トルクと操作トルクとの組合せが倒立型移動体１２０をより不安定にする不安定状態であると判断する。

The state determiner 141 determines whether or not the combination of the disturbance torque and the operation torque is an unstable state that makes the inverted moving body 120 more unstable using the following equations (31) and (32). To do. More specifically, the state determiner 141 is a state detection value x obtained by performing modal coordinate transformation, a disturbance torque estimation value _Td hat from the disturbance torque estimator 132, and an operation torque estimation from the operation torque estimator 133. When it is determined that the following formula (31) or (32) is established based on the value T _in hat and the motor torque T _m from the stability controller 142, the combination of disturbance torque and operation torque is inverted. It is determined that the mold moving body 120 is in an unstable state that makes it more unstable.

  Here, the state determiner 141 determines whether or not the following expression (31) or (32) is satisfied, thereby converting the plurality of modes of the inverted mobile body 120 into expressions on the coordinates of modal forms. Among the dynamics corresponding to the equations (25), (26), etc.), when the combination of the disturbance torque and the operation torque for at least one mode destabilizes the mode, it is determined as the unstable state. In addition, the real part of the eigenvalue of the equation of motion representing each dynamics is a negative value, and the greater the definite value of the negative value, the faster the tracking deviation converges.

  When the state determiner 141 determines that the state is unstable, for example, the state determiner 141 notifies the passenger using the warning device 143. As a result, the passenger can perform preliminary operations such as a deceleration operation, a stop operation, and a get-off operation for the unstable state, so that safety is improved.

ただし、上記（３１）式および（３２）式において、所定閾値ｃ_ｉは状態判定器１４１に予め設定された正の定数であり、ユーザが任意の値に設定変更できるように構成されている。
上記（３１）式および（３２）式において、外乱トルクＴ_ｄ及び操作トルクＴ_ｉｎは、状態検出器１２３などにより直接検出することが出来ないため、夫々、外乱トルク推定器１３２により算出した外乱トルク推定値Ｔ_ｄハット及び操作トルク推定器１３３により算出した操作トルク推定値Ｔ_ｉｎハットに置き換えている。
さらに、安定制御器１４２は、外乱トルクＴ_ｄ及び操作トルクＴ_ｉｎにより安定性が減少した下記（３３）式のモードの極を、ラプラス平面の左奥へ移動させるような安全制御を実施する。

However, in the above equations (31) and (32), the predetermined threshold c _i is a positive constant preset in the state determiner 141, and is configured so that the user can change the setting to an arbitrary value.
In the above formulas (31) and (32), the disturbance torque T _d and the operation torque T _in cannot be directly detected by the state detector 123 or the like, and therefore the disturbance torque calculated by the disturbance torque estimator 132, respectively. It is replaced with the operation torque estimate T _in hat calculated by the estimated value T _d hat and operating torque estimator 133.
Further, stability control 142, the disturbance torque T _d and the operating torque T _in the mode of electrode below which stability is reduced (33), to implement the safety control such as moving to the left back of the Laplace plane.

アッカーマンの公式により制御ゲイン行列ｋは下記（３４）式として導出できる。

ただし、上記（３４）式において、Ｃは上記（３３）式の可制御性行列であり、α_ｃ（・）は所望の極を持つ特性多項式である。
上記（３４）式に示す制御ゲイン行列ｋを用いて、外乱トルクＴ_ｄ及び操作トルクＴ_ｉｎにより安定性が減少した上記（３３）式に示すモードのダイナミクスを安定化するモータトルクＴ_ｍは、下記（３５）式として導出できる。

The control gain matrix k can be derived as the following equation (34) by Ackermann's formula.

However, in the above equation (34), C is the controllability matrix of the above equation (33), and α _c (•) is a characteristic polynomial having a desired pole.
Using the control gain matrix k shown in the above equation (34), the motor torque T _m that stabilizes the dynamics of the mode shown in the above equation (33) whose stability has been reduced by the disturbance torque T _d and the operating torque T _in is: It can be derived as the following equation (35).

上記（３５）式を用いることで、外乱トルクＴ_ｄと操作トルクＴ_ｉｎにより安定性が減少した上記（３３）式に示すモードのダイナミクスが安定化される。

By using the above equation (35), the dynamics of the mode shown in the above equation (33) whose stability is reduced by the disturbance torque T _d and the operation torque T _in are stabilized.

When the unstable controller 142 receives an unstable state determination result indicating that it is unstable from the state determiner 141, the stability controller 142 estimates the state estimated value z hat from the state estimator 131 and the disturbance torque estimator 132 from the disturbance torque estimator 132. Based on the value _Td hat, the motor torque _Tm is calculated using the above equation (35). The stability control unit 142, the motor current so as to output the motor torque T _m of the motor 121 is calculated, and output to each motor 121.

As described above, in the moving body control device 110 according to the present embodiment, the state determination unit 141 performs the disturbance torque generated in each driving wheel 410 by the input from the external environment such as the uneven road surface and the traveling operation by the passenger. Accordingly, an unstable state in which the combination of the operation torque generated around each drive wheel 410 and the inverted moving body 120 becomes more unstable is determined with high accuracy, for example, a warning device that indicates the unstable state. By notifying the passenger through 143, the safety can be improved. Furthermore, the stability controller 142 calculates the motor torque _Tm that allows the inverted moving body 120 to continue the stable travel, and controls each motor 121 based on the calculated motor torque _Tm , so that the inverted movement The traveling of the body 120 can be stabilized.
In particular, as described above, since the minimum necessary stabilization can be performed only for the unstable state (unstable mode) that affects the stabilization of traveling, no extra motor current is generated, and thus saving is possible. Electric power can be efficiently achieved. In this case, the inverted moving body 120 can travel a longer distance with the same battery capacity. Further, since saturation of the motor torque is difficult to occur, the stability is increased and the fall caused by the stability can be suppressed.

  Next, a modification of the above embodiment will be described. In the above-described embodiment, the inverted mobile body 120 may be configured to have, for example, a complex eigenvalue. In this case, the subsystem equations having the complex eigenvalues shown in the above (24) are expressed as the following equations (36) and (37).

ただし、上記（３６）式および（３７）式において、ω_ｉは減衰固有周波数である。
上記（３６）式および（３７）式で表されるモードは、減衰固有周波数ω_ｉで振動しながら収束度λ_ｉで収束する。
上記（３６）式および（３７）式のサブシステムが不安定となる十分条件は、上記（２７）式および（２８）式と同様に導出でき、下記（３８）式又は（３９）式として表される。

However, in the above formulas (36) and (37), ω _i is an attenuation natural frequency.
The modes represented by the above formulas (36) and (37) converge at the convergence λ _i while oscillating at the attenuation natural frequency ω _i .
A sufficient condition for the subsystems of the above formulas (36) and (37) to be unstable can be derived in the same manner as the above formulas (27) and (28), and is expressed as the following formula (38) or (39). Is done.

上記（２５）式、（２６）式、（３８）式、及び（３９）式において、インデックスｉは重複することはなく、上記（２５）式、（２６）式、（３８）式、及び、（３９）式を合わせて、１から６の整数が全て割り当てられる。
状態判定器１４１は、上記実施の形態と同様に、下記（４０）式又は（４１）式が成立したと判定したとき、搭乗者に対して不安全状態である旨を、警告装置１４３を介して通知する。

In the above formulas (25), (26), (38), and (39), the index i does not overlap, and the above formulas (25), (26), (38), and (39) All the integers from 1 to 6 are assigned together.
Similarly to the above-described embodiment, when the state determination unit 141 determines that the following expression (40) or (41) is established, the state determination unit 141 indicates via the warning device 143 that the passenger is in an unsafe state. To notify.

  When the unstable controller 142 receives an unstable state determination result indicating that it is unsafe from the state determiner 141, the stability controller 142 supplies each motor 121 with a motor current that causes stable operation to continue as in the above embodiment. Output.

  In this way, the stability controller 142 performs the determination by replacing the above equations (31) and (32) with the above equations (40) and (41), whereby the disturbance torque and the operation torque are complex eigenvalues. Even when the combination having an unstable state is in an unstable state, the unstable state can be appropriately determined, and the stable operation of the inverted moving body 120 can be continued.

  Next, a control method by the mobile control device according to the present embodiment will be described in detail. FIG. 2 is a flowchart showing an example of a control processing flow by the mobile control device according to the present embodiment.

  The state detector 123 of the inverted moving body 120 detects the state amount of the inverted moving body 120 (step S101), and outputs the detected state amount to the state estimator 131 of the estimation device 130 as a state detection value. .

  The state estimator 131 includes a state detection value from the state detector 123, a disturbance torque estimation value from the disturbance torque estimator 132, an operation torque estimation value from the operation torque estimator 133, and each of the stability controller 142. Based on the motor torque of the motor 121, the estimated value of the state quantity of the inverted mobile body 120 and the occupant 120 is calculated using the above equation (12) (step S102), and the calculated inverted mobile body 120 and boarding are calculated. The estimated value of the person's state quantity is output to the disturbance torque estimator 132, the operation torque estimator 133, and the stability controller 142 as the state estimated value.

  The disturbance torque estimator 132 is based on the estimated state value from the state estimator 131 and the motor torque of each motor 121 from the stability controller 142, and receives the input from the external environment using the above equation (13). The disturbance torque generated in each drive wheel 410 is calculated (step S103), and the calculated disturbance torque is output as a disturbance torque estimated value to the state determiner 141, the stability controller 142, and the state estimator 131.

  Based on the state estimated value from the state estimator 131, the operation torque estimator 133 calculates the operation torque generated around each drive wheel 410 in accordance with the traveling operation by the occupant using the equation (14) ( In step S104, the calculated operation torque is output to the state determiner 141 and the state estimator 131 as an operation torque estimated value.

  The state determiner 141 is based on the disturbance torque estimation value from the disturbance torque estimator 132, the operation torque estimation value from the operation torque estimator 133, and the motor torque of each motor 121 from the stability controller 142. The combination of the disturbance torque and the operating torque satisfying any one of the above formulas (31), (32), (40) and (41) makes the inverted moving body 120 unstable. It is determined whether or not it is in a stable state (step S105), and the determination result is output to the stability controller 142 as a state determination result.

  When the state determiner 141 determines that the combination of disturbance torque and operation torque is in an unstable state (YES in step S105), the warning device 143 issues a warning to the user (step S106). Moreover, the stability controller 142 is based on the state estimated value from the state estimator 131, the state determination result from the state determiner 141, and the disturbance torque estimated value from the disturbance torque estimator 132 (35). ) Is used to calculate motor torque that allows the inverted moving body 120 to continue stable travel (step S107), and the calculated motor torque is used as a state estimator 131, a disturbance torque estimator 132, and a state determiner 141. The motor current corresponding to the calculated motor torque is output to each motor 121 via a drive circuit or the like. Each motor 121 rotationally drives each drive wheel 410 in accordance with the motor current from the safety controller 142. Thereby, the inverted mobile body 120 can continue a stable driving | running | working.

  Next, a simulation result by the moving body control device according to the present embodiment will be described. In this simulation, each parameter is set as follows, for example.

m ₁ = 5 [kg], m ₃ = 70 [kg], J ₁ = 0.025 [kg · m ² ], J ₃ = 25.2 [kg · m ² ], r ₁ = 0.1 [m ], L _c = 0.8 [m], g = 9.8 [m / s ² ], q _d = 1, q _in = 1, θ _1ic (initial value of angle of driving wheel 410) = 0 [rad ], _2ic (initial value of posture angle of platform 421) = 0.12 [rad], _θ3ic (initial value of passenger posture angle) = 0.12 [rad], differential value of _θ1ic = 22. 2 [rad / s], θ _2ic differential value = 0 [rad / s], θ _3ic differential value = 0 [rad / s], T (control period) = 1 × 10 ⁻³ [s]

The above numerical values used in this simulation are examples of realistic numerical values of the inverted moving body 120. In this simulation, for example, a sawtooth wave disturbance torque _Td having a period of 0.5 [s] and an amplitude of 0.15 [N · m] due to road surface unevenness is received and a passenger is received at 1 [s]. There simulates a case where the operation torque _{T in} such a 0.5 [rad / s] to alpha _i of the equation (27) is input by traveling operation of the rider. Further, in this simulation, the weight matrix Q is, for example, a unit matrix.

  FIG. 3 is a diagram illustrating an example of a temporal change in the posture angle of the occupant in the simulation of the mobile control device according to the embodiment of the present invention. In FIG. 3, a dotted line (a) shows a state where the passenger has fallen (state where the posture angle of the passenger is π / 4 [rad]), and a broken line (b) shows a conventional mobile control device (for example, The time variation of the posture angle of the occupant by the state feedback control in which the pole of the closed loop system excluding the disturbance torque term and the operating torque term in the above equation (10) is set to -1 [rad / s] is shown as a solid line (C) has shown the time change of a passenger | crew's attitude | position angle by the mobile body control apparatus 110 which concerns on this Embodiment.

  In FIG. 3, when an operating torque that makes the state space shown in the above equation (10) unstable is input by the traveling operation of the passenger at an elapsed time of approximately 1 [s], In the operation control by the control device, it can be seen that the occupant cannot continue to ride on the inverted moving body, and has fallen for approximately 5 [s].

  On the other hand, in the above equation (35), in the mobile control device 110 according to the present embodiment in which the closed loop system pole of the above equation (33) is arranged at -1 [rad], the occupant has an elapsed time of approximately 1 [ s], after the operation torque that makes the state space shown in the above equation (10) unstable is input by the driving operation, the state determiner 141 satisfies the above equation (31) and is in an unstable state. It is determined that Then, the stability controller 142 controls the motor torque using the above equation (35) based on the determination result from the state determiner 141.

  As a result, it is understood that the posture angle of the passenger is maintained at 0.1 [rad] and the stable running is continued. Thus, it turns out that the mobile body control apparatus 110 which concerns on this Embodiment can control the inverted mobile body 120 stably compared with the past. Furthermore, the state determiner 141 can also notify the passenger 110 via the warning device 143 that the state is unstable based on the determination that the condition of the expression (31) is satisfied. Thereby, the safety | security of an inverted type mobile body can be improved more.

  FIG. 4 is a diagram showing a temporal change in disturbance torque in the simulation of the mobile control device according to the embodiment of the present invention. In FIG. 4, the broken line (d) indicates the estimated disturbance torque value, and the solid line (e) indicates the disturbance torque.

  As shown in FIG. 4, since the estimated disturbance torque value according to the above equation (13) converges to the disturbance torque, the broken line (d) of the estimated disturbance torque value and the solid line (e) of the disturbance torque overlap. At this time, the operation torque estimated value (not shown) according to the above equation (14) also converges to the operation torque. Therefore, it can be seen that the disturbance torque estimator 132 and the operation torque estimator 133 according to the present embodiment can estimate the disturbance torque and the operation torque with high accuracy, respectively.

As described above, in the moving body control device 110 according to the present embodiment, the state determiner 141 performs each driving according to the disturbance torque generated in each driving wheel 410 by an input from the external environment such as an uneven road surface and the traveling operation by the passenger. An unstable state in which the combination of the operation torque generated around the wheel 410 and the inverted moving body 120 becomes more unstable is determined with high accuracy. For example, the warning device 143 informs the passenger of the unstable state. By notifying, the safety can be improved. Furthermore, the stability controller 142 calculates the motor torque _Tm that allows the inverted moving body 120 to continue the stable travel, and controls each motor 121 based on the calculated motor torque _Tm , so that the inverted movement The traveling of the body 120 can be stabilized. That is, the safety of the inverted moving body 120 can be improved.

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.
In the above embodiment, the present invention has been described as a hardware configuration, but the present invention is not limited to this. In the present invention, for example, the processing executed by the estimation device 130 and the control device 140 can be realized by causing a CPU to execute a computer program.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included. The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  The present invention, for example, in the fields of automobiles, electric wheelchairs, aircrafts, ships, railways, heavy machinery, etc., is an inverted mobile body that moves according to a human operation while maintaining an inverted state, a pipe inspection robot, and a telesurgical robot It can be widely applied to all service robots remotely operated by humans, such as remote care robots.

DESCRIPTION OF SYMBOLS 110 Mobile body control apparatus 120 Inverted type mobile body 121 Motor 123 State detector 130 Estimation apparatus 131 State estimator 132 Disturbance torque estimator 133 Operation torque estimator 140 Control apparatus 141 State determination device 142 Stability controller 410 Drive wheel 421 Platform 422 handle

Claims (17)

A moving body control device that controls an inverted moving body that performs a desired travel while maintaining an inverted state,
State detecting means for detecting a state quantity of the inverted moving body;
A state amount detected by the state detecting means, a disturbance torque estimated value which is an estimated value of torque caused by a disturbance generated in the driving wheel of the inverted moving body, and generated around the driving wheel in accordance with a traveling operation of a passenger State estimation means for calculating at least one of the state quantity of the inverted mobile body and the occupant based on an operation torque estimated value that is an estimated value of the torque and a driving torque that drives the driving wheel;
Disturbance torque estimation for calculating the estimated disturbance torque value based on at least one of the inverted moving object and the occupant state quantity calculated by the state estimation means and the drive torque for driving the drive wheel Means,
Operating torque estimating means for calculating the operating torque estimated value based on at least one of the inverted moving body and the passenger's state quantity calculated by the state estimating means;
Based on the disturbance torque estimation value calculated by the disturbance torque estimation means, the operation torque estimation value calculated by the operation torque estimation means, and the drive torque for driving the drive wheel, State determination means for determining whether a combination of disturbance torque and operation torque is an unstable state that makes the inverted moving body unstable; and
A moving body control apparatus comprising: The moving body control device according to claim 1,
Driving the driving wheel based on the determination result by the state determination unit, the state quantity of the inverted moving body calculated by the state estimation unit, and the disturbance torque estimation value calculated by the disturbance torque estimation unit A moving body control device further comprising control means for calculating torque and controlling driving of the drive wheel based on the calculated drive torque. The moving body control device according to claim 2,
When the state determining means determines that the state is unstable, the control means calculates the disturbance torque estimated value from a value obtained by multiplying the state quantity obtained by modal coordinate conversion of the driving torque of the driving wheel by a control gain. The moving body control device is characterized in that the control is performed so as to obtain a value obtained by subtracting. The mobile body control device according to any one of claims 1 to 3,
A moving body control device, further comprising warning means for notifying a user to that effect when the state determination means determines that the state is unstable. The mobile body control device according to any one of claims 1 to 4,
The state determination means is calculated by the operation torque estimation means, the state quantity that has been subjected to modal coordinate conversion, a drive torque that drives the drive wheel, a disturbance torque estimation value that is calculated by the disturbance torque estimation means, and the operation torque estimation means. A mobile body control device, wherein when the value based on the estimated operation torque is larger than a predetermined threshold value, the unstable state is determined. The mobile body control device according to any one of claims 1 to 5,
The moving body control device, wherein the state detection means detects at least one of rotation information of the drive wheel and posture angle information of a platform on which a passenger is boarded. The mobile body control device according to any one of claims 1 to 6,
The state estimating unit is configured to calculate the state based on the driving torque of the driving wheel, the disturbance torque estimated value calculated by the disturbance torque estimating unit, the operation torque estimated value calculated by the operation torque estimating unit, and the state quantity. A moving body control apparatus, wherein the state quantity of the inverted moving body and a passenger is calculated based on a calculation result obtained by subtracting an estimated value of a quantity and multiplying by a gain. The mobile body control device according to any one of claims 1 to 7,
The state estimating means calculates at least one of an angle of the driving wheel, a posture angle of a platform on which a passenger rides, and a posture angle of a passenger, and the mobile body control device. The mobile body control device according to any one of claims 1 to 8,
The said state estimation means estimates the state quantity of an inverted-type moving body and a passenger using the following formula, The moving body control apparatus characterized by the above-mentioned.

The mobile body control device according to any one of claims 1 to 9,
The said disturbance torque estimation means calculates the said disturbance torque estimated value using a following formula, The moving body control apparatus characterized by the above-mentioned.

In the above equation, A is a system matrix of the state equation of the inverted moving body, L is a gain matrix of the state estimating means, and Q is a positive definite matrix. It is a moving body control apparatus of any one of Claims 1 thru | or 10, Comprising:
The operating torque estimating means calculates the operating torque estimated value by using the following formula, and the moving body control device:

It is a moving body control apparatus of any one of Claims 1 thru | or 11, Comprising:
The mobile unit control apparatus, wherein the state determination unit determines that the state is unstable when any one of the following expressions is established.

The mobile body control device according to any one of claims 1 to 12,
When the state determining unit determines that the inverted moving body is in the unstable state, the control unit controls the control unit so that the driving torque of the driving wheel becomes a torque T _m calculated by the following equation: A moving body control device characterized by:

A control method for a moving body control device that controls an inverted moving body that performs a desired travel while maintaining an inverted state,
Detecting a state quantity of the inverted moving body;
The detected state quantity, a disturbance torque estimated value that is an estimated value of torque due to a disturbance generated in the driving wheel of the inverted moving body, and an estimated value of the torque generated around the driving wheel in accordance with the traveling operation of the passenger Calculating at least one of the inverted movable body and the occupant's state quantity based on the estimated operation torque value and the driving torque for driving the driving wheel;
Calculating the disturbance torque estimated value based on at least one of the calculated inverted moving body and the occupant state quantity and a driving torque for driving the driving wheel;
Calculating the operation torque estimated value based on at least one of the calculated inverted moving body and the passenger's state quantity;
Based on the calculated disturbance torque estimated value, the calculated operation torque estimated value, and the drive torque for driving the drive wheel, a combination of the disturbance torque and the operation torque of the drive wheel is the inverted movement. Determining whether the body is unstable or not,
A control method for a moving body control device comprising: It is a control method of the mobile control device according to claim 14,
The driving torque of the driving wheel is calculated based on the determination result, the calculated state quantity of the inverted moving body, and the calculated disturbance torque estimated value, and the driving torque of the driving wheel is calculated based on the calculated driving torque. A control method for a moving body control device, further comprising a step of controlling driving of the driving wheel. A control method for a mobile body control device according to claim 14 or 15,
A control method for a mobile control apparatus, further comprising a step of notifying a user to that effect when it is determined that the state is unstable. A program of a moving body control device that controls an inverted moving body that performs a desired traveling while maintaining an inverted state,
The detected state quantity of the inverted moving body, a disturbance torque estimated value that is an estimated value of the torque caused by the disturbance generated in the driving wheel of the inverted moving body, and around the driving wheel according to the traveling operation of the passenger A process of calculating at least one of the state quantity of the inverted mobile body and the occupant based on an operation torque estimated value that is an estimated value of generated torque and a driving torque for driving the driving wheel;
Processing for calculating the estimated disturbance torque value based on at least one of the calculated inverted moving body and the occupant state quantity and driving torque for driving the driving wheel;
A process of calculating the operation torque estimated value based on at least one of the calculated inverted movable body and the passenger's state quantity;
Based on the calculated disturbance torque estimated value, the calculated operation torque estimated value, and the drive torque for driving the drive wheel, a combination of the disturbance torque and the operation torque of the drive wheel is the inverted movement. A process of determining whether or not an unstable state that makes the body unstable,
A program for a moving body control apparatus that causes a computer to execute the above.

Images