JP2010267193A - Device for control of inverted mobile - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an inverted mobile that overcomes an idle running of wheels, a collision, and a bumpy road surface by using a control considering nonlinear elements. <P>SOLUTION: A control device has a nonlinear control unit 150 which calculates a nonlinear torque applied to a mobile body 112 from nonlinear elements, based on detected signals from status sensor 113. A torque command calculator 111 calculates a torque command which is applied to the mobile body 112 based on a linear torque from a linear control unit 140 and a nonlinear torque from the nonlinear control unit 150. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to an inverted moving body control device. More specifically, the present invention relates to an inverted moving body control device that includes wheel driving means and a link-like load body and moves while performing balance control for inverting the link-like load body.

  2. Description of the Related Art A moving device that has a pair of left and right wheels provided coaxially and travels while maintaining an inverted state is known. For example, Patent Document 1 (Japanese Patent Laid-Open No. 2006-123014) discloses an inverted two-wheeled traveling robot that autonomously travels while maintaining an inverted state. Patent Document 2 (Japanese Patent Laid-Open No. 2006-315666) discloses a coaxial two-wheeled vehicle that travels while maintaining a balance while a person is on a step in a standing posture.

FIG. 9 is a diagram showing the configuration of the controller of the inverted two-wheeled traveling robot disclosed in Patent Document 1. In FIG.
In FIG. 9, 901 is a friction estimator, 902 is a target state generator, 903 is a state feedback gain, and 904 is an inverted robot.
An angular velocity command value is input to the friction estimator 901, and the friction estimator 901 calculates and outputs the friction of the motor and the friction between the wheel and the road surface as the estimated friction value.
The target state generator 902 receives the angular velocity command value and the estimated friction value, and the target state generator 902 calculates and outputs the target state of the inverted robot 904 that is the control target.
A signal obtained by subtracting the state variable of the inverted robot 904 from the target state is input to the state feedback gain 903, and the state feedback gain 903 receives a state feedback signal that causes the inverted robot 904 to perform a desired action based on the input signal. Calculate and output.
The inverted robot 904 is driven by an added value of the friction estimated value and the state feedback signal.
As described above, in the control of the inverted two-wheeled robot of the prior art, the operation of the inverted robot 904 is controlled based on the linearized model obtained by linearizing the inverted robot 904 to be controlled in the vicinity of a desired posture.

JP 2006-123014 A (Fig. 4)

In the conventional moving body control device, the operation of the two-wheel inverted moving body is controlled on the assumption that the vehicle travels on a horizontal and flat road surface. For this reason, for example, when the road surface is uneven, there have been problems such as the moving body toppling over, vibrating, and being unable to travel at a desired horizontal speed.
Further, in the conventional mobile control device, the control design is performed on the linearized model in the vicinity of the desired posture of the mobile body, but the mobile body may collide with a person or an object during traveling.
In such a case, since nonlinear torque affects the conventional linearization model, the conventional linearization model cannot cope with it, causing problems such as being uncontrollable or oscillating and falling.

  The present invention has been made in view of such problems, and even when the road surface is uneven or collides with a person or an object, the two-wheel inverted movement without falling down and without oscillating. It is an object of the present invention to provide a moving body control device that can move a body at a desired horizontal speed.

In order to solve the above problems, the present invention is configured as follows.
The control apparatus for an inverted moving body according to the present invention includes:
A control device that performs traveling control while inverting a moving body having a driving means having wheels and a load that is inverted and controlled on the wheels via a link,
A linear control unit that calculates linear torque for performing linear control according to a deviation between a command value from a command unit that commands a desired target state and a detection signal from a state sensor that detects the state of the moving body,
A non-linear controller that calculates non-linear torque due to non-linear elements applied to the moving body based on a detection signal from the state sensor;
A torque command calculator that calculates a torque command to be given to the moving body by the linear torque from the linear control unit and the nonlinear torque from the nonlinear control unit, the nonlinear control unit,
The angle formed between the vertical line and the straight line connecting the center of gravity of the load and the center of gravity of the wheel is the load angle,
A wheel horizontal speed estimator that estimates a wheel horizontal speed of the mobile body using the load angle;
A non-linear torque calculator that calculates non-linear torque, which is torque generated by a non-linear portion of the mobile body, using the estimated wheel horizontal speed.

In addition, the control device for an inverted moving body of the present invention includes:
A control device that performs traveling control while inverting a moving body having a driving means having wheels and a load that is inverted and controlled on the wheels via a link,
A linear control unit that calculates linear torque for performing linear control according to a deviation between a command value from a command unit that commands a desired target state and a detection signal from a state sensor that detects the state of the moving body,
A non-linear controller that calculates non-linear torque due to non-linear elements applied to the moving body based on a detection signal from the state sensor;
A torque command calculator that calculates a torque command to be given to the moving body by the linear torque from the linear control unit and the nonlinear torque from the nonlinear control unit, the nonlinear control unit,
The angle formed between the vertical line and the straight line connecting the center of gravity of the load and the center of gravity of the wheel is the load angle,
A wheel vertical acceleration estimator that estimates the wheel vertical acceleration of the mobile body using the load angle;
A non-linear torque calculator that calculates a non-linear torque, which is a torque generated by a non-linear portion of the mobile body, using the estimated wheel vertical acceleration value.

  According to the present invention as described above, a two-wheel inverted moving body that does not fall over and does not oscillate even when there is unevenness on the road surface or when it collides with a person or an object due to control that takes nonlinear elements into account. Can be driven at a desired horizontal speed.

The figure which shows 1st Embodiment which concerns on the inverted moving body of this invention. The figure which modeled the mobile body. The figure which shows the simulation result of a load angle. The figure which shows the simulation result of a wheel horizontal speed. The figure which shows the modification 1. FIG. The figure which shows a coaxial two-wheeled vehicle as an inverted moving body. The figure which shows an inverted autonomous traveling robot as an inverted moving body. The figure which shows the inverted type mobile body provided with the link mechanism so that rocking | fluctuation was possible on the wheel drive means of 4 wheels. The figure which shows the structure of the controller of the conventional inverted two-wheeled traveling robot.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
(First embodiment)

FIG. 1 is a diagram showing a first embodiment according to the inverted moving body of the present invention.
The inverted moving body 100 includes a moving body main body 112 as a control target, a state sensor 113 that detects the state of the moving body main body 112, a command unit 120 that commands a desired target state, and a detection result by the state sensor 113. And a control device 130 that controls the moving body main body 112 based on a command value from the command unit 120.

Common examples of the mobile body 112 include a coaxial two-wheeled vehicle (FIG. 6) and an inverted autonomous traveling robot (FIG. 7).
However, the present invention is not limited to this, and any means may be used as long as it includes a driving means using wheels and a link-shaped load body and performs balance control for inverting the link-shaped load body.
For example, a configuration as shown in FIG. 8 may be used.
FIG. 8 shows a configuration in which a link mechanism 802 is provided on a four-wheel drive means 801 so as to be swingable.
For example, the upper portion of the link mechanism 802 may be formed in a cage shape 803 so that articles can be carried thereon.

In the following description, such a moving body main body 112 is modeled as shown in FIG.
Here, in FIG. 2, 201 is a load, 202 is a wheel, and 203 is a road surface.
As shown in FIG. 2, it is assumed that the moving body main body 112 travels upside down.
The load 201 is a robot body, a person riding on the mobile body 112, or a luggage.
The wheel 202 moves by using a frictional force with the road surface 203 when the load 201 is placed and rotates.

State sensor 113, the angle (theta ₁₎ of the load 201, and detects the wheel angle 202 (θ _2).

The command unit 120 includes a wheel horizontal speed command generator 121 and a load angle command calculator 122.
The wheel horizontal speed command generator 121 generates and outputs a wheel horizontal speed command that is a desired horizontal movement speed of the wheel 202 of the moving body 112.
The load angle command calculator 122 receives the wheel horizontal speed command, and when the road surface 203 on which the mobile body 112 travels is horizontal, the load angle command calculator 122 is a load angle that is such that the wheel horizontal speed follows the wheel horizontal speed command. Calculates and outputs an angle command.

  The control device 130 includes a case-dependent linear control unit 140, a non-linear control unit 150, and a torque command calculator 111.

  The case-specific linear control unit 140 includes an attenuation range calculator 141, an attenuation parameter calculator 142, a case-dependent linear torque calculator 143, and a control switch 144.

The attenuation range calculator 141 receives the load angle command from the load angle command calculator 122 and the load angle (θ ₁ ) and wheel angle (θ ₂ ) that are detected values by the state sensor 113.
Based on the input signal, the attenuation range calculator 141 calculates and outputs the range of the load angle to which only attenuation using viscous friction is performed in the operation control of the moving body 112 as the attenuation range.

The attenuation parameter calculator 142 includes a load angle command (θ ₁ ^* ) from the load angle command calculator 122, a load angle (θ ₁ ) and a wheel angle (θ ₂ ) that are detected values by the state sensor 113. Entered. The attenuation parameter calculator 142 calculates and outputs an attenuation parameter used for control in the attenuation range based on the input signal.

The case-specific linear torque calculator 143 includes a load angle command (θ ₁ ^* ) from the load angle command calculator 122, an attenuation parameter from the attenuation parameter calculator 142, and a load angle (a value detected by the state sensor 113). θ ₁ ) and wheel angle (θ ₂ ) are input. The case-by-case linear torque calculator 143 is a linear feedback obtained by multiplying one or more of a position deviation, a speed deviation, and an acceleration deviation by a predetermined gain by a damping torque obtained by adding a negative sign to a multiplication value of a load speed and a damping parameter. Torque is calculated and output.

  The control switch 144 receives the attenuation range calculated by the attenuation range calculator 141, the detection value by the state sensor 113, and the case-dependent linear torque calculated by the case-by-case linear torque calculator 143. ing. The control switch 144 switches the case-specific linear torque calculated by the case-specific linear torque calculator 143 and outputs it.

The nonlinear controller 150 includes a wheel vertical acceleration estimator 151, a wheel horizontal speed estimator 152, and a nonlinear torque calculator 153.
A detection signal from the state sensor 113 is input to the wheel vertical acceleration estimator 151, and the wheel vertical acceleration estimator 151 estimates the vertical acceleration of the wheel 202 based on the input signal and outputs it as a wheel vertical acceleration estimated value. .

A detection signal from the state sensor 113 is input to the wheel horizontal speed estimator 152, and a torque command from the torque command calculator 111 is branched and input. The wheel horizontal speed estimator 152 estimates the horizontal speed of the wheel 202 based on the input signal and outputs it as a wheel horizontal speed estimated value.
The non-linear torque calculator 153 receives the estimated wheel vertical acceleration value and the estimated wheel horizontal speed value, and the non-linear torque calculator 153 calculates and outputs a non-linear torque indicating non-linear dynamics of the moving body 112.

  The torque command calculator 111 receives the case-by-case linear torque switched and output by the control switch 144 and the nonlinear torque output from the nonlinear torque calculator 153, and the torque command calculator 111 receives these input signals. A torque command obtained by dividing the added value by the radius of the wheel 202 is output.

  The mobile body 112 is driven by the torque command.

  Hereinafter, the details of the mechanism by which the control device according to the first embodiment controls the operation of the mobile body 112 will be described.

In FIG. 2, parameters are set as follows.
m ₁ is the load mass,
J ₁ is the load moment of inertia,
m ₂ is the wheel mass,
J ₂ is the wheel moment of inertia,
l is the distance between the center of gravity of the load wheel, which is the distance between the center of gravity of the load and the wheel,
r is the wheel radius,
θ ₁ is the load angle,
θ ₂ is the wheel angle,
T _ref is a torque command.
Furthermore, when the wheel horizontal position is x ₂ and the wheel vertical position is y ₂ , the load horizontal position x ₁ and the load vertical position y ₁ are expressed by the following equations (1) and (2), respectively.

  Using equations (1) and (2), the kinetic energy T and potential energy V of the moving body 112 are expressed by equations (3) and (4) as follows.

  Then, using the Euler-Lagrange equation, equations of motion of the moving body 112 are obtained as equations (5) to (8).

However, g is a gravitational acceleration.
Further, when the viscous friction between the wheel 202 and the road surface 203 is taken into consideration, the equations (6) and (7) can be rewritten as the equations (9) and (10).
Here, D is a viscous friction coefficient.

  Using Expression (9) and Expression (10), Expression (11) is obtained.

  When Expression (11) is subtracted from Expression (5), Expression (12) is obtained.

However, N _x is a nonlinear term with the wheel horizontal position x ₂ as a parameter, and N _y is a nonlinear term with the wheel vertical position y ₂ as a parameter.

Assuming that the load angle θ ₁ changes sufficiently later than the wheel horizontal position x ₂ , Equation (11) can be rewritten as Equation (13).

However, constants c ₁ , c ₂ , and c ₃ are portions that change sufficiently later than the wheel horizontal position x ₂ .
From equation (13), the estimated wheel horizontal speed is expressed by equation (14).

s is a Laplace operator, L ⁻¹ is an inverse Laplace transform, and l ₁ and l ₂ are parameters of the wheel horizontal speed estimator 152.
The wheel horizontal speed estimator 152 calculates the wheel horizontal speed estimated value using the equation (14).

On the other hand, when equation (8) is solved for wheel vertical acceleration (d ² y ₂ / dt ² ), equation (15) is obtained.

  The wheel vertical acceleration estimator 151 calculates the wheel vertical acceleration estimated value using Equation (15).

Assuming that the wheel horizontal speed command which is a desired wheel horizontal speed is v ₂ ^* (= x ₂ * first order differential), the wheel horizontal speed dx ₂ / dt is the wheel horizontal speed command when the road surface 203 is horizontal and flat. The load angle command θ ₁ ^*, which is the load angle θ ₁ that becomes v ₂ *, is expressed by Expression (16). That is, the load angle command is an arc tangent of a value obtained by dividing the wheel horizontal acceleration command by the gravitational acceleration.

The wheel horizontal speed command generator 121 outputs the wheel horizontal speed command v ₂ ^* (= first derivative of x ₂ *), and the load angle command calculator 122 uses the equation (16) to calculate the load angle command θ _1. ^* Calculate and output.

  Substituting Equation (14) and Equation (15) into Equation (12) yields Equation (17).

  Rewriting equation (17) gives equation (18).

However, u is a case-by-case linear torque.
Then, as the load angle theta ₁ is converged to a load angle command theta ₁ ^*, consider a linear torque u divided case shown in equation (19).

Where e = θ ₁ ^* −θ ₁ is the load angle tracking deviation,
β is the speed proportional control gain,
κ is the position proportional control gain,
γ is the attenuation parameter,
sgn (•) is a signum function that takes a value of +1 when • is a positive number, −1 when a negative number, and 0 when it is zero.

Further, h = c | θ ₁ ^* | is an attenuation range that is a load angle range provided to suppress chattering by feedback control, and c is a parameter of the attenuation range h.

Here, the case-by-case linear torque u represented by the equation (19) means that when the deviation between the load angle θ ₁ and the load angle command θ ₁ ^* is small, specifically, the absolute value of the load angle θ ₁ . When the angular deviation between the load angle θ ₁ and the load angle command θ ₁ ^* is within the range of the attenuation range h in the direction in which the load decreases, the load 201 is caused to perform an operation in which the viscous friction is the attenuation parameter γ. It represents that the angle θ ₁ can be stably converged to the load angle command θ ₁ ^* .
When the deviation between the load angle θ ₁ and the load angle command θ ₁ ^* is other than the above, the load 201 is subjected to feedback control in which the rigidity is the position proportional control gain κ and the viscous friction is the speed proportional control gain β. This indicates that the load angle θ ₁ can be converged to the load angle command θ ₁ ^* .

Here, it is preferable to set the attenuation parameter γ as a function of the load angle command θ ₁ ^* and the load angle tracking deviation e, for example, as shown in the equation (20).

In other words, when the case-specific linear control arithmetic unit 140 receives the load angle command θ ₁ ^* from the load angle command arithmetic unit 122, the load angle command θ ₁ ^* is converted into the attenuation range calculator 141, the attenuation parameter calculator 142, and the case-specific linear unit. It is input to the torque calculator 143.
Then, the attenuation range calculator 141 calculates an attenuation range where h = c | θ ₁ ^* | using the load angle command θ ₁ ^* and the parameter c.
The obtained attenuation range h is output to the control switch 144.

  Further, the damping parameter calculator 142 calculates the damping parameter γ according to the above equation (20), and outputs it to the linear torque calculator 143 according to the case.

The case-specific linear torque calculator 143 uses the attenuation parameter γ given from the attenuation parameter calculator 142, the preset speed proportional control gain β, and the position proportional control gain κ, and calculates the case-specific linear torque u of Equation (19). calculate.
The calculated case-specific linear torque u is output to the control switch 144.

The control switch 144 switches and selects the case-specific linear torque u obtained by the case-specific linear torque calculator 143 with reference to the load angle tracking deviation e and the attenuation range h.
The case-dependent linear torque selected by the control switch 144 is output to the torque command calculator 111.

  Further, the non-linear torque calculator 153 calculates Nx + Ny in Expression (12) based on the estimated wheel vertical acceleration value calculated using Expression (15) and the estimated wheel horizontal speed value calculated using Expression (14). Calculate and output some nonlinear torque.

  The torque command calculator 111 calculates and outputs a torque command Tref according to equation (21) using the control switching value u and the nonlinear torque Nx + Ny.

Where r is the wheel radius.
The mobile body 112 is driven and controlled by a torque command _Tref .

According to the first embodiment having such a configuration, the following effects can be achieved.
(1) In the present embodiment, since the moving body main body 112 is controlled using the estimated wheel horizontal speed value of the equation (14), the wheel level of the moving body main body 112 can be adjusted even when the wheel 202 and the road surface 203 slide relative to each other. speed v ₂ (= first derivative of x ₂₎ it is possible to match the ^* wheel horizontal speed instruction v ₂ ^(first derivative of x ₂ *).

Conventionally, since a control design assuming a simple linear model has been adopted, no special consideration has been given to a non-linear element in which a wheel and a road surface slip.
In the past, gains were set so that non-linear elements such as wheel slipping were also suppressed as one of the disturbances, but the wheels slipped due to slippage and collision with the road surface, and the load posture was In the case of a large deviation, it is impossible to return to a stable state even with a control design that considers robustness.
In this regard, in the present embodiment, the wheel horizontal velocity estimate of equation (14) estimated by the wheel horizontal velocity estimator 152, in the nonlinear torque unit 153, a wheel horizontal position x ₂ of the formula (12) as a parameter A nonlinear term Nx is obtained and added to the torque command. Therefore, it corresponds to a non-linear element in which the wheel 202 and the road surface 203 slide relatively, and the wheel horizontal speed v ₂ (= first derivative of x ₂ ) of the moving body 112 is set to the wheel horizontal speed command v ₂ ^* (x ₂ * first derivative).

(2) In this embodiment, since the moving body main body 112 is controlled using the estimated wheel vertical acceleration value of the equation (15), the wheel 202 is in contact with the road surface 203 even if the road surface 203 is uneven. The load angle θ ₁ can be stably controlled.
Conventionally, it is assumed that the vehicle travels on a horizontal road surface, and non-linear elements that get over unevenness are not considered. For this reason, there are problems such as oscillation when the load is greatly inclined when overcoming the road surface irregularities, and falling because the control becomes impossible.
In this respect, in the present embodiment, the wheel vertical acceleration estimated value of Expression (15) is calculated by the wheel vertical acceleration estimator 151, and the nonlinear torque calculator 153 uses the wheel vertical position y2 of Expression (12) as a parameter. The term Ny is obtained and added to the torque command.
Accordingly, it is possible to stably control the load angle θ1 in response to a nonlinear element that overcomes the road surface unevenness.

(3) In the first embodiment, as shown in Expression (19), the load angle θ ₁ is divided into three ranges, and an optimum torque command is calculated in each case.
Then, the control switch 144 switches the control between inside and outside the attenuation range.
Thus, without the load angle theta ₁ is vibrated in the vicinity of the load angle command theta ₁ ^*, can be made to smoothly converge the load angle command theta ₁ ^*.
As a result, stable horizontal travel can be realized.

(4) comprising a damping parameter calculator 142, so set the damping parameter γ by Equation (20), compared with the case of fixing the damping parameter γ a constant value, the load smoothly fast load angle theta ₁ angle command theta ₁ ^* Can be converged.

(Experimental example)
Next, experimental examples demonstrating the effects of the present invention will be shown.
As an experimental example, the simulation result of the first embodiment is shown.
Here, the numerical values used in the simulation are as follows.

m ₁ = 70 [kg],
J ₁ = 25.2 [kg · m ² ],
m ₂ = 15 [kg],
J ₂ = 0.075 [kg ・ m ² ],
l = 0.9 [m],
r = 0.1 [m],
D = 0.1 [N · s / m],
g = 9.8 [m / s ² ],
T = 1 × 10 ^-3 [s],
κ = 40 [s ^-1],
J ₁₀ = m ₁ × l ² + J ₁ [kg · m ² ],
β ₀ = 2πκ [s ^-1 ],
β = β ₀ × J ₁₀ [N · m · s / rad],
γ = 0.1 [N · m · s / rad],
pcl = [-49.9, -201.4] [rad / s],
td = 0.5 [s]

However,
m ₁ is the load mass,
J _1, the load moment of inertia,
m ₂ is the wheel mass,
J ₂ is the wheel moment of inertia,
l is the load wheel center of gravity distance,
r is the wheel radius,
D is the viscous friction between wheel surfaces,
g is the acceleration of gravity,
T is the control cycle,
κ is a position proportional control gain in the present invention,
J ₁₀ is the nominal moment of inertia,
β ₀ is a normalized speed proportional control gain in the present invention,
β is a speed proportional control gain in the present invention,
γ is a friction parameter in the present invention,
pcl is a closed loop pole in the prior art,
td is an impulse disturbance time.

In addition,
The viscous friction D between wheel road surfaces is a viscous friction acting between the wheel 202 and the road surface 203 in FIG.
The nominal moment of inertia J ₁₀ is a parameter that normalizes the speed loop in the present invention,
pcl is a closed-loop pole disposed by state feedback control in the prior art.
Further, consider a case where there is an impulse-like acceleration disturbance input in the vertically upward direction of the wheel 202 at the impulse disturbance time td.

3 and 4 are diagrams showing simulation results.
FIG. 3 shows changes in the load angle.
3, a broken line L ₁₁ is the load angle command (Load angular position reference input), the solid line L ₁₀ is the load angle by the present invention (Load angular position with proposed control) , one-dot chain line L ₁₂ represents the load angle of the prior art.
Up to 0.5 [s] when the acceleration disturbance is input, both the conventional technology and the present invention follow the load angle command to the same extent. However, after 0.5 [s], the load angle oscillates in the conventional technology. On the other hand, it can be seen that the present invention follows the load angle command without oscillating even after the acceleration disturbance input.

  The reason why the load angle according to the present invention changes in a polygonal time is because control is performed to add only attenuation in the attenuation range of Equation (19), and by providing the attenuation range, It was shown that the load position is less likely to vibrate in the vicinity of the load position command.

  Further, the wheel vertical acceleration estimator 151 calculates the wheel vertical acceleration estimated value of the equation (15), and the nonlinear torque calculator 153 calculates the nonlinear torque Ny. Since the acceleration disturbance in the vertical direction can be compensated, the present invention has been shown to stabilize the load angle even in the presence of the acceleration disturbance.

FIG. 4 shows the change in the horizontal speed of the wheel.
In FIG. 4, a broken line L ₂₁ is a desired wheel horizontal speed, a solid line L ₂₀ is a wheel horizontal speed according to the present invention, and a one-dot chain line L ₂₂ is a wheel horizontal speed according to the prior art.
Both the prior art and the present invention follow the desired wheel horizontal speed up to 0.5 [s] when the acceleration disturbance is input, whereas the prior art becomes vibrational after 0.5 [s]. It can be seen that does not vibrate and follows the desired wheel horizontal speed.

(Modification 1)
In the first embodiment, the case where the case-by-case linear control unit 140 is provided as the most preferable embodiment has been described, but as shown in FIG. 5, a linear torque calculator 501 is used instead of the case-by-case linear control unit, Conventional linear feedback control may be applied.
Even in this case, since the non-linear control unit 150 is provided, even if there are non-linear elements such as wheel slip, collision, road surface unevenness, etc., these can be dealt with and stable running can be realized.

In addition, this invention is not limited to the said embodiment, The deformation | transformation in the range which can achieve the objective of this invention, improvement, etc. are included in this invention.
For example, in the equation (19), the position P / speed P control may be replaced with an arbitrary control law such as position P / speed PI control, position P / speed IP control, position PID control, etc. is there.

  The present invention can move a two-wheel inverted moving body at a desired horizontal speed without tipping over and oscillating, even when the road surface is uneven or collides with a person or an object. It can be widely applied to robots that run with two wheels upside down, electric wheelchairs, automatic conveyance devices, robots that work in narrow places such as lifesaving in disasters, and assembly devices for electronic components that are vulnerable to vibration.

DESCRIPTION OF SYMBOLS 100 ... Inverted type mobile body, 111 ... Torque command calculator, 112 ... Mobile body main body, 113 ... State sensor, 120 ... Command part, 121 ... Wheel horizontal speed command generator, 122 ... Load angle command calculator, 130 ... Control Device: 140 ... linear control unit according to case, 141 ... attenuation range calculator, 142 ... attenuation parameter calculator, 144 ... control switching unit, 150 ... nonlinear controller, 151 ... wheel vertical acceleration estimator, 152 ... wheel horizontal speed estimation 153 ... Nonlinear torque calculator, 201 ... Load, 202 ... Wheel, 203 ... Road surface, 901 ... Friction estimator, 902 ... Target state generator, 903 ... State feedback gain, 904 ... Inverted robot.

Claims (7)

A control device that performs traveling control while inverting a moving body having a driving means having wheels and a load that is inverted and controlled on the wheels via a link,
A linear control unit that calculates linear torque for performing linear control according to a deviation between a command value from a command unit that commands a desired target state and a detection signal from a state sensor that detects the state of the moving body,
A non-linear controller that calculates non-linear torque due to non-linear elements applied to the moving body based on a detection signal from the state sensor;
A torque command calculator that calculates a torque command to be given to the moving body by the linear torque from the linear control unit and the nonlinear torque from the nonlinear control unit, the nonlinear control unit,
The angle formed between the vertical line and the straight line connecting the center of gravity of the load and the center of gravity of the wheel is the load angle,
A wheel horizontal speed estimator that estimates a wheel horizontal speed of the mobile body using the load angle;
A non-linear torque calculator that calculates a non-linear torque, which is a torque generated by a non-linear portion of the main body of the moving body, using the estimated wheel horizontal speed value. In the inverted mobile control device according to claim 1,
The wheel horizontal speed estimator calculates a wheel horizontal speed estimated value according to the following equation, and is an inverted moving body control device.

However,

here,
θ ₁ is the load angle,
m ₁ is the load mass,
m ₂ is the wheel mass,
l is the distance between the center of gravity of the load wheel, which is the distance between the center of gravity of the load and the wheel,
r is the wheel radius
θ ₁ is the load angle,
T _ref is a torque command,
s is a Laplace operator,
l ₁ and l ₂ are the parameters of the wheel horizontal speed estimator,
L ⁻¹ represents Laplace inversion. In the control device for an inverted moving body according to claim 2,
The non-linear torque calculator calculates a non-linear term N _x using a wheel horizontal position x ₂ as a parameter according to the following equation.

A control device that performs traveling control while inverting a moving body having a driving means having wheels and a load that is inverted and controlled on the wheels via a link,
A linear control unit that calculates linear torque for performing linear control according to a deviation between a command value from a command unit that commands a desired target state and a detection signal from a state sensor that detects the state of the moving body,
A non-linear controller that calculates non-linear torque due to non-linear elements applied to the moving body based on a detection signal from the state sensor;
A torque command calculator that calculates a torque command to be given to the moving body by the linear torque from the linear control unit and the nonlinear torque from the nonlinear control unit, the nonlinear control unit,
The angle formed between the vertical line and the straight line connecting the center of gravity of the load and the center of gravity of the wheel is the load angle,
A wheel vertical acceleration estimator that estimates the wheel vertical acceleration of the mobile body using the load angle;
A non-linear torque calculator that calculates a non-linear torque, which is a torque generated by a non-linear part of the main body of the moving body, using the estimated wheel vertical acceleration value. In the control device for an inverted moving body according to claim 4,
The wheel vertical acceleration estimator calculates a wheel vertical acceleration estimated value by the following formula.

However,
θ ₁ is the load angle,
m ₁ is the load mass,
m ₂ is the wheel mass,
l is the distance between the center of gravity of the load wheel, which is the distance between the center of gravity of the load and the wheel. In the inverted mobile control device according to claim 5,
The non-linear torque calculator calculates the non-linear term N _y with the wheel vertical position y ₂ as a parameter using the following equation:

θ ₁ is the load angle,
m ₁ is the load mass,
m ₂ is the wheel mass,
l is the distance between the center of gravity of the load wheel, which is the distance between the center of gravity of the load and the wheel,
r is the wheel radius
J ₂ is the wheel inertia moment,
θ ₂ is the wheel angle. In the control device for an inverted mobile body according to any one of claims 1 to 6,
The torque command computing unit uses as a torque command a value obtained by dividing an addition value obtained by adding the linear torque from the linear control unit and the nonlinear torque from the nonlinear control unit by the wheel radius of the moving body. A control device for an inverted moving body.

Images