Inverted two-wheeled robot

[0001] This is a continuation application of International Application No. PCT/JP2006/313579, filed Jul. 7, 2006.

[0002] The present invention relates to a two-wheeled inverted robot in which coaxial two wheels are used among robots which perform load carriage work, and particularly relates to the two-wheeled inverted robot in constraining the robot attitude.

[0003] In the robot which performs the load carriage work, generally the robot maintains its stability by necessarily bringing at least three points into contact like a three-wheel type robot or a four-wheel type robot. However, the three-wheel type robot or the four-wheel type robot occupies a large area and hardly has a small turning circle. Therefore, there has been developed a coaxial two-wheel type robot which operates based on an inverted pendulum model.

[0004] For example, there is known a coaxial two-wheel type moving robot described in Japanese Patent No. 2530652 and Japanese Unexamined Patent Publication No. 2004-74814.

[0005] FIG. 13 shows a conventional coaxial two-wheel type moving robot described in Japanese Patent No. 2530652.

[0006] The coaxial two-wheel type moving robot described in Japanese Patent No. 2530652 includes a body 104, a wheel drive motor 105, a control computer 106, and an angle detection means 107. The body 104 is rotatably supported on an axle shaft 101 which includes a pair of wheels 102 and 103 at both ends. The wheel drive motor 105 is attached to the body 104. The control computer 106 issues an operation command to the wheel drive motor 105. The angle detection means 107 detects inclination of the body 104. An inclination angle of the body 104 detected by the angle detection means 107 is sampled at short time intervals, the inclination angle of the body 104 is set at a state variable input value, and a feedback gain K is set at a coefficient. Then, a control torque of the wheel drive motor 105 is computed to perform state feedback control based on a control input computation equation which is previously inputted and set in the control computer 106. The control computer 106 issues an actuation command corresponding to the computed control torque to the wheel drive motor 105, which maintains the inverted attitude control.

[0007] FIG. 14 shows a conventional human-transport coaxial two-wheel type moving robot described in Japanese Publication No. 2004-74814.

[0008] In the human-transport coaxial two-wheel type moving robot described in Japanese Publication No. 2004-74814, the attitude control and running control are performed to maintain a fore-and-aft balance by controlling and driving right and left driving wheels 202 coaxially arranged on a platform 201 according to output of an attitude sensor. The human-transport coaxial two-wheel type moving robot includes an auxiliary wheel 203 which comes into contact with the ground on the front side and/or rear side of the driving wheels 202 and an auxiliary wheel drive unit 204 which extends and retracts the auxiliary wheel 203. The human-transport coaxial two-wheel type moving robot includes a control circuit which drives the auxiliary wheel drive unit 204 to extend and retract the auxiliary wheel 203 according to an obstacle detection sensor 205 and control states of running speed and attitude. Therefore, when an obstacle which tends to cause falling exists, because the auxiliary wheel 203 is extended to enhance safety, the risk of the falling can be decreased.

[0009] In the technique described in Japanese Patent No. 2530652, the state feedback control is always performed.

[0010] When the coaxial two-wheeled body 104 and the wheels 102 and 103 can relatively be rotated with no constraint, the torque generated by the wheel drive motor 105 contributes to the rotations of the wheels 102 and 103, and simultaneously a reaction torque acts on the body 104 to be able to maintain the inverted attitude control as a whole.

[0011] However, when a part of the body 104 comes into contact with a wall or the ground to receive a constraint force from the outside to such an extent that the body 104 cannot rotate any more, not only does the body 104 not rotate, but also the torque generated by the constraint force is transmitted to the wheels 102 and 103. Therefore, when compared with no constraint force, the excessive torque is applied to the wheels 102 and 103, which results in an issue that the wheel rotation is increased to rapidly enhance the running speed.

[0012] In the technique described in Japanese Publication No. 2004-74814, when the obstacle is detected by the obstacle detection sensor 205, because the human-transport coaxial two-wheel type moving robot has the mechanism in which the auxiliary wheel drive unit 204 is driven to extend and retract the auxiliary wheel 203, the extension of the auxiliary wheel 203 enables the three-point ground contact of the right and left driving wheels 202 and the auxiliary wheel 203 to decrease the risk of falling.

[0013] However, in the technique described in Japanese Publication No. 2004-74814, only the obstacle is recognized to extend the auxiliary wheel 203, and the speed and the attitude depend on the control of a user. Therefore, in the configuration described in Japanese Publication No. 2004-74814, as with Japanese Patent No. 2530652, there is also the issue that excessive torque is applied to the driving wheel 202 to rapidly enhance the running speed, when a user breaks down the balance to bring the body portion including the platform 201 into contact with the ground, or when the body portion including the platform 201 is brought into contact with the wall or the like to receive the constraint force to such an extent that the body portion cannot rotate. In this case, a stop command is transmitted to avoid the risk by the control of the user. However, there is the issue that a burden is placed on the user to the point that the operation depends on the user.

[0014] An object of the present invention is to provide a safe two-wheeled inverted robot and a control method thereof, in which the above issues are solved and the attitude control and the running state can be maintained and the burden is not placed on the user, when a part of the body of the coaxial two-wheeled robot which is moved while the inverted control is performed comes into contact with the ground or the wall to receive the constraint force.

[0015] The present invention is configured as follows in order to achieve the above object.

[0016] According to a first aspect of the present invention, there is provided a two-wheeled inverted robot comprising:

[0017] a body;

[0018] two wheels which are coaxially arranged on the body;

[0019] a drive device for driving the wheels respectively;

[0020] a first state detection means for detecting at least one of an inclination angle and an inclination angular speed of the body;

[0021] a second state detection means for detecting at least one of a rotation angle and a rotational angular speed of the wheel;

[0022] a body constraint recognizing means for receiving information detected by the first state detection means or the second state detection means, to detect whether or not rotation of the body in an inclination direction is constrained; and

[0023] a control means for determining a command value to the drive device, changing a plurality of running control methods by the determined command value to drive the drive device, and thereby performing running control of the body,

[0024] wherein the control means determines, based on detection result of the body constraint recognizing means, the command value to the drive device for changing the plurality of running control methods, the drive device is driven by the determined command value, and the running control of the body is performed to maintain a running state.

[0025] According to a tenth aspect of the present invention, there is provided a method of controlling a two-wheeled inverted robot, comprising:

[0026] detecting at least one of an inclination angle and an inclination angular speed of a body as first state detection information;

[0027] detecting at least one of a rotation angle and a rotational angular speed of a wheel as second state detection information;

[0028] inputting the first state detection information or the second state detection information to detect whether or not rotation of the body in an inclination direction is constrained as body constraint recognizing information; and

[0029] changing a plurality of running control methods with control means based on the detected body constraint recognizing information, determining a command value to a drive device which drives each of the two wheels coaxially arranged in the body, changing the plurality of running control methods to drive the drive device by the determined command value, and performing the running control of the body to maintain a running state.

[0030] According to the configuration of the present invention, when a part of the body of the two-wheeled inverted robot which runs while the inverted control is performed comes into contact with the ground or the wall to constrain the rotation, the constraint in the rotational direction of the body is recognized by the body constraint recognizing means or the body constraint recognizing operation. For example, a change in ratio of the torque contributing to the body rotation according to the recognized constraint state can solve the issue that the attitude control cannot be maintained but the running speed is rapidly increased due to the application of the excessive torque to the wheel.

[0031] Consequently, according to the configuration of the present invention, in the inverted coaxial two-wheeled robot which is moved while the inverted control is performed, the attitude control and the running state can be maintained and the robot can be moved while the burden is not placed on the user, even if a part of the body comes into contact with the ground or the wall to constrain the rotation.

[0048] Before the description of the present invention proceeds, it is to be noted that like parts are designated by like reference numerals throughout the accompanying drawings.

[0049] Preferred embodiments of the present invention will be described in detail below with reference to the drawings.

[0050] A two-wheeled inverted robot according to a first embodiment of the present invention and a control method thereof will be described below with reference to the drawings.

[0051] FIG. 1A is a side view showing a state in which the two-wheeled inverted robot of the first embodiment of the present invention is inclined, and FIG. 2 is a front view showing the state in which the two-wheeled inverted robot is inclined. Two wheels 2a and 2b are substantially coaxially arranged for a body 1 of the robot. In this case, the two wheels 2a and 2b are respectively set as the left wheel 2a and the right wheel 2b with respect to the forward direction shown by an arrow A in the body 1 of FIG. 1A. FIG. 2 is a front view showing the two-wheeled inverted robot when viewed from the forward direction of the body 1. Actuators 3a and 3b function as an example of the drive device, and the actuators 3a and 3b independently generate drive forces between the wheels 2a and 2b and the body 1 respectively. The actuators 3a and 3b are attached near the wheels 2a and 2b of the body 1, and the actuators 3a and 3b are coupled to the wheels 2a and 2b respectively. For the sake of explanation, it is assumed that the actuators 3a and 3b are motors. Speed reducers 4a and 4b may be attached to the motors 3a and 3b so that these devices 4a, 4b, 3a, 3b may constitute an example of the drive device. Encoders 5a and 5b, which can measure rotation angles of the body 1 and wheels 2a and 2b, are also attached to the motors 3a and 3b. The encoders 5a and 5b function as an example of the second state detection means for detecting at least one of the rotation angle and a rotational angular speed in each of the wheels 2a and 2b as the second state detection information. The rotation angle is measured at a constant sampling time (for example, every 1 ms), and the difference is divided by the sampling time to determine the rotational angular speed, so that the rotational angular speed can be dealt with as the second state detection information.

[0052] Two sensors of a uniaxial gyroscope 6a and a biaxial acceleration sensor 6b are attached to an upper portion of the body 1. The two sensors are an example of an attitude sensor 6 for detecting the attitude in a vertical plane in which an axle shaft direction of the body 1 is set at normal, namely, the two sensors function as an example of the first state detection means for detecting at least one of an inclination angle and an inclination angular speed of the body as the first state detection information. The uniaxial gyroscope 6a is attached to the body 1 in the vertical plane in which the axle shaft direction is set at the normal such that the inclination angular speed in the axle shaft direction can be detected as the first state detection information, and the uniaxial gyroscope 6a can also detect the inclination angular speed (shown by arc 6a1) in the vertical plane in which the axle shaft direction of the body 1 is set at the normal. The biaxial acceleration sensor 6b is attached so as to be able to detect acceleration with respect to two given directions (6b1 and 6b2) in the vertical plane in which the axle shaft direction is set at the normal, and the biaxial acceleration sensor 6b can detect a gravitational direction (6b3). The attitude angle (inclination angle) of the body 1 which is of the first state detection information is determined as follows. An integration value of the uniaxial gyroscope 6a and a value from the biaxial acceleration sensor 6b are processed by a highpass filter and a lowpass filter in a control computer unit 9 respectively, and the processed values are combined by the control computer unit 9 to determine the information.

[0053] Contact sensors 10a and 10b are provided in lower portions of the front and rear of the body 1. The contact sensors 10a and 10b can measure contact force from another object to the lower portions of the front and rear of the body 1 as the contact information.

[0054] Motor drivers 7a and 7b and a battery 8 and the control computer unit 9 are mounted in the body 1. The motor drivers 7a and 7b and the battery 8 drive the motors 3a and 3b, and the control computer unit 9 functions as an example of the control means and the body constraint recognizing means. As shown in FIG. 1B, the control computer unit 9 includes an A/D converter 9a, a D/A converter 9b, an encoder counter unit 9c, and an arithmetic unit 9d. The values of the encoders 5a and 5b are input into the arithmetic unit 9d through the encoder counter unit 9c. The values of the uniaxial gyroscope 6a, the biaxial acceleration sensor 6b, and the contact sensors 10a and 10b are input into the arithmetic unit 9d through the A/D converter 9a. Based on the inputted values from the contact sensors 10a and 10b, the arithmetic unit 9d detects whether or not the rotation in the inclination direction of the body 1 is constrained, and the arithmetic unit 9d can produce the body constraint recognizing information. Examples of the produced body constraint recognizing information include information on the constraint force to the body 1 which is judged by the inputs of the contact sensors 10a and 10b (see the first embodiment), information on the presence or absence of the contact which is judged by the inputs of the contact sensors 10a and 10b (see the second embodiment), information on the inclination angle of the body 1 (see the third embodiment), and the like. As described later, the body constraint recognizing information can be used in changing the control method. The arithmetic unit 9d of the control computer unit 9 computes the torques necessary for the attitude control and running control from the values inputted to the arithmetic unit 9d, and the D/A converter 9b issues torque command values to the motor drivers 7a and 7b. The motor drivers 7a and 7b drive the motors 3a and 3b based on the command values, respectively. The method of computing the output value from the input value will be described later.

[0055] Then, a method of controlling the two-wheeled inverted robot having the above configuration will be described with reference to FIG. 3. FIG. 3 shows the robot when viewed from above in a vertical direction. It can be thought that the mechanism is divided into two kinds of control of translational control 11 (attitude control+fore-and-aft movement control) and turning control 12. The translational control 11 relates to the attitude in the vertical plane (B-B′ plane) in which the axle shaft direction is set at the normal and the horizontal speed in the vertical plane. The turning control 12 relates to the operation in the turning direction. The former performs the attitude control and running control to maintain the running state so as to follow up a speed ν in the translational direction. The latter performs the control so as to follow up an angular speed ω in the turning direction. As will be described later, the movement control (running control) can be performed to maintain the running state by combining these two kinds of control.

[0056] Then, the movement control will sequentially be described.

[0057] First, the translational control 11 will be described with reference to FIG. 4 which shows mechanism parameters. Because the operation is performed in the vertical plane in which the axle shaft direction is set at the normal, the translational control 11 will be dealt with as a two-dimensional model. As shown in FIG. 4, the parameters are set as follows. The wheels 2a and 2b, the motors 3a and 3b, the speed reducers 4a and 4b, and the encoders 5a and 5b are regarded as one component respectively, and those are set at a wheel 2, a motor 3, a speed reducer 4, and an encoder 5 respectively. A gravity center of the body 1 is set at a body gravity center 13, the axle shaft of the wheel 2 is set at an axle shaft 14, and a gravity center of the wheel 2 is set at a wheel gravity center 15.

[0058] M_b: mass [kg] of the body 1 of the robot

[0059] M_w: mass (total of two wheels) [kg] of the wheel 2

[0060] I_b: inertia moment [kgm²] about the body gravity center 13 of the body 1 of the robot

[0061] I_w: inertia moment (total of two wheels) [kgm²] about the wheel gravity center 15 of the wheel 2

[0062] I_m: inertia moment [kgm²] of the motor 3

[0063] r: radius [m] of the wheel 2

[0064] L_g: distance [m] from the axle shaft 14 to the body gravity center 13 of the robot

[0065] μ_s: viscous friction coefficient [Nm/(rad/s)] among the motor 3, the speed reducer 4, and the axle shaft 2

[0066] μ_g: viscous friction coefficient [Nm/(rad/s)] between the wheel 2 and the ground 800

[0067] τ_t: torque constant [Nm/A] of the motor 3

[0068] η: reduction gear ratio of a gear portion in the speed reducer 4

[0069] g: gravitational acceleration

[0070] φ: inclination [rad] of the body 1 of the robot

[0071] θ: rotation angle [rad] of the wheel 2 to the ground 800

[0072] u: current [A] inputted to the motor 3

• • [0073] Signs are as follows.

[0074] T: kinetic energy, U: potential energy, D: friction loss, z₁: body gravity center height, z₂: wheel gravity center height, s₁: body gravity center translational position, s₂: axle shaft translational position.

[0075] An equation of motion of the two-wheeled inverted robot is derived using a Lagrangian method. In the Lagrangian equation of motion expressed by the following Equations 1 and 2, each value and its differential value are expressed by the following Equations 3-10.

[0076] $\begin{array}{cc}\frac{d}{dt}\left(\frac{\partial T}{\partial \stackrel{.}{\varphi }}\right)-\frac{\partial T}{\partial \varphi }+\frac{\partial U}{\partial \varphi }+\frac{\partial D}{\partial \varphi }=Q_1& \left(\mathrm{Equation}1\right)\\ \frac{d}{dt}\left(\frac{\partial T}{\partial \stackrel{.}{\theta }}\right)-\frac{\partial T}{\partial \theta }+\frac{\partial U}{\partial \theta }+\frac{\partial D}{\partial \theta }=Q_2& \left(\mathrm{Equation}2\right)\\ s_1=r\theta +L_g\sin \varphi & \left(\mathrm{Equation}3\right)\\ s_2=r\theta & \left(\mathrm{Equation}4\right)\\ {\stackrel{.}{s}}_1=r\stackrel{.}{\theta }+L_g\stackrel{.}{\varphi }\cos \stackrel{.}{\varphi }& \left(\mathrm{Equation}5\right)\\ {\stackrel{.}{s}}_2=r\stackrel{.}{\theta }& \left(\mathrm{Equation}6\right)\\ z_1=r+L_g\cos \varphi & \left(\mathrm{Equation}7\right)\\ z_2=r& \left(\mathrm{Equation}8\right)\\ {\stackrel{.}{z}}_1=-L_g\stackrel{.}{\varphi }\sin \varphi & \left(\mathrm{Equation}9\right)\\ {\stackrel{.}{z}}_2=0& \left(\mathrm{Equation}10\right)\end{array}$
Each piece of energy is expressed by Equations 11-13.

[0077] $\begin{array}{cc}T=\frac{1}{2}{M}_w\left({\stackrel{.}{s}}_2^2+{\stackrel{.}{z}}_2^2\right)+\frac{1}{2}{M}_b\left({\stackrel{.}{s}}_1^2+{\stackrel{.}{z}}_1^2\right)+\frac{1}{2}{I}_w{\stackrel{.}{\theta }}^2+\frac{1}{2}{I}_b{\stackrel{.}{\varphi }}^2+\frac{1}{2}{I}_m{{\eta }^2\left(\stackrel{.}{\theta }-\stackrel{.}{\varphi }\right)}^2& \left(\mathrm{Equation}11\right)\\ U=M_{w}gr+M_{b}g\left(r+l\cos \varphi \right)& \left(\mathrm{Equation}12\right)\\ D=\frac{1}{2}\left({{\mu }_s\left(\stackrel{.}{\theta }-\stackrel{.}{\varphi }\right)}^2+u_g{\stackrel{.}{\theta }}^2\right)& \left(\mathrm{Equation}13\right)\end{array}$
In a simple model, when no external force is applied to the body 1, the Lagrangian equations of motion are expressed by Equations 14-15,
Q₁=−ητ₁u  (Equation 14)
Q₂=ητ₁u  (Equation 15)
Therefore, kinetic energy T is obtained by the following Equation 16,

[0078] $\begin{array}{cc}T=\frac{1}{2}{M}_w\left(r^2{\stackrel{.}{\theta }}^2\right)+\frac{1}{2}{M}_b\left(r^2{\stackrel{.}{\theta }}^2+2r\stackrel{.}{\theta }{L}_g\stackrel{.}{\varphi }\cos \varphi +L_g^2{\stackrel{.}{\varphi }}^2\right)+\frac{1}{2}{I}_w{\stackrel{.}{\theta }}^2+\frac{1}{2}{I}_b{\stackrel{.}{\varphi }}^2+\frac{1}{2}{I}_m{{\eta }^2\left(\stackrel{.}{\theta }-\stackrel{.}{\varphi }\right)}^2& \left(\mathrm{Equation}16\right)\end{array}$

[0079] The equation of motion is derived for the inclination φ of the body 1 of the robot by the Lagrangian method.
M_b(r{umlaut over (θ)}L_g cos φ+L_g²{umlaut over (φ)})+I_bφ−I_mη²({umlaut over (θ)}−{umlaut over (φ)})−M_bgl sin φ−(μ_s({dot over (θ)}−{dot over (φ)}))=(M_bL_g²+I_b+I_mη²){umlaut over (φ)}+(M_brL_g cos φ−I_mη²){umlaut over (φ)}−M_bgl sin φ−μ_s{dot over (θ)}+μ_s{dot over (φ)}=−ητ₁u  (Equation 17)

[0080] The equation of motion is derived for the rotation angle θ of the ground 800 of the wheel 2 by the Lagrangian method.
M_w(r²{umlaut over (θ)})+M_b(r²{umlaut over (θ)}+rL_g{umlaut over (φ)} cos φ−rL_g{dot over (φ)}² sin φ)+I_w{umlaut over (θ)}+I_mη²({umlaut over (θ)}−{umlaut over (φ)})+(μ_s({dot over (θ)}−{dot over (φ)})+u_g{dot over (θ)})=(M_brL_g cos φ−I_mη²){umlaut over (φ)}+((M_b+M_w)r²+I_w+I_mη²){umlaut over (θ)}−M_brL_g{dot over (φ)}² sin φ+μ_s{dot over (θ)}−μ_s{dot over (φ)}+u_g{dot over (θ)}=ητ₁u  (Equation 18)
When (Equation 17) and (Equation 18) are added to each other, Equation 19 is obtained.
(M_brL_g cos φ+M_bL_g²+I_b){umlaut over (φ)}+((M_b+M_w)r²+I_w+M_brL_g cos φ){umlaut over (θ)}−M_bgl sin φ−M_brL_g{dot over (φ)}² sin φ+u_g{dot over (θ)}=0  (Equation 19)

[0081] Therefore, (Equation 17) and (Equation 19) are approximated around φ=0, {dot over (φ)}=0 to obtain the following Equations.
(sin φ=φ, cos φ=1, {dot over (φ)}²=0)
(M_bL_g²+I_b+η²I_m){umlaut over (φ)}+(M_brL_g cos φ−η²I_m){umlaut over (θ)}−M_bglφ−μ_s{dot over (θ)}+μ_s{dot over (φ)}=−ητ₁u  (Equation 20)
(M_brL_g+M_bL_g²+I_b){umlaut over (φ)}+((M_b+M_w)r²+M_brL_g+I_w){umlaut over (θ)}−M_bglφ+u_g{dot over (θ)}=0  (Equation 21)

[0082] When the variables are set as follows,

[0083] $\begin{array}{cc}\left(\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right)=\left(\begin{array}{cc}{M}_b{L}_g^2+I_b+{\eta }^2{I}_m& M_b{\mathrm{rL}}_g\cos \varphi -{\eta }^2{I}_m\\ M_b{\mathrm{rL}}_g+M_b{L}_g^2+I_b& \left(M_b+M_w\right)r^2+M_b{\mathrm{rL}}_g+I_w\end{array}\right)& \left(\mathrm{Equation}22\right)\\ \Delta =a_{11}{a}_{22}-a_{12}{a}_{21}& \left(\mathrm{Equation}23\right)\\ \left(\begin{array}{cc}{a}_1& a_2\\ a_3& a_4\\ a_5& a_6\end{array}\right)=\frac{1}{\Delta }\left(\begin{array}{cc}\left(a_{22}-a_{12}\right)M_b{\mathrm{gL}}_g& \left(a_{11}-a_{21}\right)M_b{\mathrm{gL}}_g\\ -{\mu }_s{a}_{22}& {\mu }_s{a}_{21}\\ {\mu }_s{a}_{22}+{\mu }_g{a}_{12}& -\left({\mu }_s{a}_{21}+{\mu }_g{a}_{11}\right)\end{array}\right)& \left(\mathrm{Equation}24\right)\\ \left(\begin{array}{c}{b}_1\\ b_2\end{array}\right)=\frac{1}{\Delta }\left(\begin{array}{c}-a_{22}{\mathrm{\eta \tau }}_t\\ a_{21}{\mathrm{\eta \tau }}_t\end{array}\right)& \left(\mathrm{Equation}25\right)\end{array}$
the following forms of the state equation are obtained.

[0084] $\begin{array}{cc}\begin{array}{c}\frac{d}{dt}\left(\begin{array}{c}\varphi \\ \stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\end{array}\right)=\stackrel{.}{x}\\ =\left(\begin{array}{ccc}0& 1& 0\\ a_1& a_3& a_5\\ a_2& a_4& a_6\end{array}\right)\left(\begin{array}{c}\varphi \\ \stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\end{array}\right)+\left(\begin{array}{c}0\\ b_1\\ b_2\end{array}\right)u\\ =\mathrm{Ax}+\mathrm{Bu}\end{array}& \left(\mathrm{Equation}26\right)\\ A=\left(\begin{array}{ccc}0& 1& 0\\ a_1& a_3& a_5\\ a_2& a_4& a_6\end{array}\right)B=\left(\begin{array}{c}0\\ b_1\\ b_2\end{array}\right)x=\left(\begin{array}{c}\varphi \\ \stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\end{array}\right)& \left(\mathrm{Equation}27\right)\end{array}$

[0085] At this point,

[0086] $x=\left(\begin{array}{c}\varphi \\ \stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\end{array}\right)$
is a state vector.

[0087] The above modeling is an approximate model when each value of the state vector is zero. Therefore, in order to consider follow-up capability with respect to the position (speed command) in the fore-and-aft direction, a model control system which follows up a stepwise target input is formed to the model. It is assumed that the target input is a constant speed target value. It is assumed that a speed y (={dot over (θ)}) is observable, and the speed y={dot over (θ)} is expressed by y={dot over (θ)}=Cx=(0,0,1)x.

[0088] The control system is formed by an integrator such that a speed y(t) is caused to follow up a stepwise target angular speed {dot over (θ)}_d=ν/r with no steady-state deviation.

FIG. 5 shows the control block at that time.

In this case,

K(1×3), G(1×1) are feedback vectors which stabilize the closed loop system.

[0089] In this configuration, Equation 28 is obtained from z=∫({dot over (θ)}_d−{dot over (θ)})dt.
ż=−Cx+{dot over (θ)}_d  (Equation 28)
The control input is expressed by Equation 29.
u=−Kx+Gz  (Equation 29)
Assuming that I_n is a 3×3 unit matrix and I_m=1, the Equation 30 is obtained.

[0090] $\begin{array}{cc}\begin{array}{c}\stackrel{.}{u}=-K\stackrel{.}{x}+G\stackrel{.}{z}\\ =-K\left(\mathrm{Ax}+\mathrm{Bu}\right)+G\left(-\mathrm{Cx}+{\theta }_d\right)\\ =-\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+G{\theta }_d\end{array}& \left(\mathrm{Equation}30\right)\end{array}$

[0091] The Equation 31 and Equation 32 are obtained from (Equation 26) and (Equation 30).

[0092] $\begin{array}{cc}\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{u}\end{array}\right]=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d& \left(\mathrm{Equation}31\right)\\ y={\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{cc}{x}^T& u^T\end{array}\right]}^T& \left(\mathrm{Equation}32\right)\end{array}$
At this point, the steady-state values are obtained as follows.

[0093] $\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}x\left(\infty \right)\\ u\left(\infty \right)\end{array}\right]=-{{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d\\ =-{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{cc}{I}_n& 0\\ -G^{-1}K& -G^{-1}\end{array}\right]\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d\\ ={\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ I_m\end{array}\right]{\stackrel{.}{\theta }}_d\end{array}& \left(\mathrm{Equation}33\right)\\ y\left(\infty \right)={\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{cc}{x\left(\infty \right)}^T& {u\left(\infty \right)}^T\end{array}\right]}^T& \left(\mathrm{Equation}34\right)\end{array}$
Here,

[0094] $\begin{array}{cc}\begin{array}{c}y\left(\infty \right)={\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{cc}{x\left(\infty \right)}^T& {u\left(\infty \right)}^T\end{array}\right]}^T\\ ={\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ I_m\end{array}\right]{\stackrel{.}{\theta }}_d\\ ={\stackrel{.}{\theta }}_d\end{array}& \left(\mathrm{Equation}35\right)\end{array}$
Therefore, it is found that the steady-state deviation does not exist.

[0095] Then, an error system is considered from the steady-state value.

It is assumed that
x_e=x−x(∞), u_e=u−u(∞), e={dot over (θ)}−{dot over (θ)}_d,
and the formula deformation is performed, then the Equation 36 is obtained.

[0096] $\begin{array}{cc}\begin{array}{c}\left[\begin{array}{c}{\stackrel{.}{x}}_e\\ {\stackrel{.}{u}}_e\end{array}\right]=\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{u}\end{array}\right]\\ =\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d\\ =\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e+x\left(\infty \right)\\ u_e+u\left(\infty \right)\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d\\ =\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left\{\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ I_m\end{array}\right]u_r\right\}+\\ \left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{.}{\theta }}_d\\ =\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]\\ =\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+\left[\begin{array}{cc}0& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]\\ =\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]-\left[\begin{array}{c}0\\ I_m\end{array}\right]\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]\end{array}& \left(\mathrm{Equation}36\right)\end{array}$

[0097] The Equation 39 is obtained from the Equations 37 and 38.

[0098] $\begin{array}{cc}H=\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]& \left(\mathrm{Equation}37\right)\\ w=-H\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]& \left(\mathrm{Equation}38\right)\\ \begin{array}{c}\left[\begin{array}{c}{\stackrel{.}{x}}_e\\ {\stackrel{.}{u}}_e\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]-\left[\begin{array}{c}0\\ I_m\end{array}\right]H\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]\\ =\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+\left[\begin{array}{c}0\\ I_m\end{array}\right]w\end{array}& \left(\mathrm{Equation}39\right)\end{array}$
When the output is set at e, the Equation 40 is obtained.
e=[C0][x_e^Tu_e^T]^T  (Equation 40)
The error system is regarded as a regulator in which the state feedback of w is performed to (Equation 39) and (Equation 40). Therefore, a feedback gain H of the error system is derived by an optimum regulator method, a pole placement method, and other various design methods. When the gain is used, an optimum servo system can be designed with a small transient response error of the deviation e(t).
The Equation 41 is obtained from (Equation 37).

[0099] $\begin{array}{cc}\left[\begin{array}{cc}K& G\end{array}\right]={H\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]}^{-1}=\left[k_1,k_2,k_3,k_4\right]& \left(\mathrm{Equation}41\right)\end{array}$
Therefore, assuming that the Equation 42 is the torque command value τ₁ outputted to the motor 3 using the Gain [k₁, k₂, k₃, k₄],
τ₁=ητ₁u=−ητ₁[k₁,k₂,k₃,k₄]x₁  (Equation 42)

[0100] the inverted attitude control and the speed control can simultaneously be realized.

[0101] However,

[0102] $\begin{array}{cc}{x}_1=\left(\begin{array}{c}\varphi \\ \stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\\ z_1\end{array}\right)& \left(\mathrm{Equation}43\right)\\ z_1=-\int \left({\stackrel{.}{\theta }}_d-\stackrel{.}{\theta }\right)dt=-z& \left(\mathrm{Equation}44\right)\end{array}$
At this point, k₁<0, k₂<0, k₃<0, k₄<0
In the configuration, there are two wheels 2.
Therefore, when a half of the torque command value τ₁ is outputted to each wheel 2 (2a and 2b), the attitude control and speed control can be performed in the translational direction.
Assuming that the torque command values of the wheels 2a and 2b are the left wheel τ_1L and the right wheel τ_1R respectively, the Equations 45 and 46 are obtained.
τ_1L=0.5τ₁  (Equation 45)
τ_1R=0.5τ₁  (Equation 46)

[0103] Then, the turning control 12 will be described with reference to FIG. 6 which is a schematic view showing the turning control.

[0104] When the body 1 of the robot is viewed from above, it is assumed that a length between the wheels 2 of the body 1 of the robot is set at L_w.

The moving speed of the body 1 of the robot is set at ν in the horizontal plane,

the rotational speed of the left wheel 2a is set at ν_L,

the rotational speed of the right wheel 2b is set to ν_R,

the turning radius is set at R, and the turning is set at ω.

The rotational angular speed of the left wheel 2a is set at {dot over (θ)}_L and

the rotational angular speed of the right wheel 2b is set at {dot over (θ)}_R.

Then, the Equation 47 is obtained.

[0105] $\begin{array}{cc}\left(\begin{array}{c}v\\ \omega \end{array}\right)=\left(\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ \frac{-r}{L_w}& \frac{r}{L_w}\end{array}\right)\left(\begin{array}{c}{\stackrel{.}{\theta }}_L\\ {\stackrel{.}{\theta }}_R\end{array}\right)& \left(\mathrm{Equation}47\right)\end{array}$

[0106] The turning speed target value is set at ω_d. The feedback (PD control or the like) is performed to the difference between the turning speed target value ω_d and the actual turning speed ω to determine the torque τ₂ in the turning direction.

[0107] It is assumed that the torque τ₂ in the turning direction may be the difference in torque generated in the right and left wheels 2a and 2b.

[0108] A half of the torque in the turning direction is added to and subtracted from the right and left wheels 2a and 2b where the target torque in the turning direction can be computed respectively, which allows the control to be realized in the turning direction.

[0109] FIG. 7 shows a control block diagram in which the translational control 11 and the turning control 12 are integrated.

[0110] Assuming that the torque command values issued the command to the left wheel 2a and right wheel 2b are τ_L and τ_R, the Equation 48 is obtained.

[0111] $\begin{array}{cc}\left(\begin{array}{c}{\tau }_L\\ {\tau }_R\end{array}\right)=\left(\begin{array}{cc}\frac{1}{2}& -\frac{1}{2}\\ \frac{1}{2}& \frac{1}{2}\end{array}\right)\left(\begin{array}{c}{\tau }_1\\ {\tau }_2\end{array}\right)& \left(\mathrm{Equation}48\right)\end{array}$

[0112] Thus, in the robot, the turning and linear running can be performed while the inverted attitude is maintained, by determining the torque.

[0113] When the body 1 is separated from the ground 800 as shown in FIG. 1A, the running can be performed while the inverted attitude is maintained based on the control rule described above.

[0114] Then, in the first embodiment, as shown in FIG. 8, the case where the body 1 is inclined and thereby the lower portion of the body 1 comes into contact with the ground 800 is shown as one example of the case in which a part of the body 1 receives the constraint in the rotational direction. It is assumed that the frictional force is zero between the body 1 and the ground 800, and only vertical drag F₁ is generated as the constraint force at a contact point which is horizontally moved from the axle shaft 14 by the distance r₁. The contact sensor 10b (because the contact sensor 10a is similar to the contact sensor 10b, the description thereof is represented by the description of the contact sensor 10b) is a sensor which can measure the contact force from another object, the ground 800, or the wall against the body 1. The contact force sensor 10b detects the constraint force F₁ in the vertical direction. The distance r₁ is already known due to the geometric condition.

[0115] At this point, the torque τ_a=F₁r₁ is generated in the body 1 by the constraint force. When a positional displacement amount is in the state expressed by s₃>0, it is considered a case where the contact shown in FIG. 8 is generated (inclination angle is φ_a<0).

[0116] In this case, the torque command value of the motor 3 becomes Equation 49 based on Equation 42. In Equation 49, the direction in which the wheel 2 is normally rotated is set at positive (the direction in which the body 1 of the robot is moved in the right direction of FIG. 8 and the body 1 is rotated leftward about the axle shaft 14).
τ₁=ητ₁[−k₁,−k₂,−k₃,−k₄]x₁=−ητ₁(k₁φ+k₂{dot over (φ)}+k₃{dot over (θ)}+k₄z₁)  (Equation 49)

[0117] Because the vertical drag F₁>0, the rotation in the inclination direction is constrained, leading to φ=φ_a and {dot over (φ)}=0. In this case, Equation 50 is obtained.
τ₁=−ητ₁(k₁φ_a+k₃{dot over (θ)}+k₄z₁)  (Equation 50)
Because a torque τ_b applied to the body 1 becomes a sum of a reaction force (−τ₁) of a torque from the motor 3 and a torque r caused by the contact, Equation 51 is obtained.
τ_b=τ_a−τ₁=F₁r₁+ητ₁(k₁φ_a+k₃{dot over (θ)}+k₄z₁)  (Equation 51)
τ_b can become either τ_b>0 or τ_b<0 depending on values of {dot over (θ)} and z₁. In Equation 51, in the case of τ_b>0, because the torque which rotates the body 1 rightward is applied to the body 1, the body 1 is rotated rightward about the axle shaft 14, the contact is eliminated, which allows the body 1 to return to the inverted state. However, in the case of τ_b<0, the leftward rotational torque is applied to the body 1, and the rightward rotational torque is generated as a reaction force in the wheel 2. At this point, because the body 1 is constrained by the external force F₁, the torque τ_a=F₁r₁ generated by the external force F₁ is also transmitted as the rotational torque to the wheel 2. Therefore, the excessive torque is applied to the wheel 2 to generate an excessively rotational phenomenon in the wheel 2, which causes the robot to fall in a runaway phenomenon.

[0118] Therefore, in the method of controlling the robot according to the first embodiment of the present invention, the contact force sensor 10b detects the contact information, and the detected contact force direction and the torque τ₁ are compared to each other. In the case where the rotational directions are different from each other, the torque command value τ₁ to the motor 3 is set at τ₁ expressed by Equation 52 in which the torque τ_a=F₁r₁ generated by the contact force is added.
τ₁=−ητ₁(k₁φ_a+k₃{dot over (θ)}+k₄z₁)+F₁r₁  (Equation 52)

[0119] Accordingly, in the method of controlling the robot according to the first embodiment of the present invention, an influence of the contact force from the ground 800 can be removed by computing the torque of the motor 3 based on Equation 52, and the safe two-wheeled inverted robot can be realized while the burden is not placed on the user.

[0120] In the configuration, when the frictional force F₂ is generated between the body 1 and the ground 800, as shown in FIG. 9, the same effect is obtained by the running while the torque in the direction opposite the rotation generated by the resultant force F₃ is compensated.
τ₁=−ητ₁(k₁φ_a+k₃{dot over (θ)}+k₄z₁)+F₃r₃  (Equation 53)
The disturbance force is estimated with no contact sensor by using a disturbance observer, and thereby the control similar to Equation 52 and Equation 53 may be performed with the estimated contact force.

[0121] A running operation flowchart in which change control is used in the first embodiment will be described with reference to FIG. 12.

[0122] In an initial state (Step S1), the body 1 of the robot is stopped while inclined. An initial setting is performed as shown in

(z₁νω)=(0,0,0) (=Step S2).

Then, the inverted stopping control

[0123] ({dot over (θ)}_d=ν/r=0) expressed by Equation 50 (Step S3) is performed to raise the body 1 of the robot. After a stable time (for example, about five seconds) elapses (Step S4) since the inverted stopping control (Step S3), it is judged whether or not the body 1 is in contact with the ground 800 or the like (Step S5).

[0124] When the body 1 is not in contact with the ground 800 or the like in Step S5, the setting of the speed expressed by speed (νω) is performed (Step S8). Then, after the inverted running control (Step S9) expressed by (Equation 49) is performed to maintain the running state, the flow returns to Step S5.

[0125] When the body 1 is in contact with the ground 800 or the like in Step S5, the setting of the speed expressed by speed (νω) is performed (Step S6). Then, after the inverted running control (Step S7) expressed by Equation 52 or Equation 53 is performed to maintain the running state, the flow returns to Step S5.

[0126] In Steps S6 and S8, the differential torque is corrected regardless of the contact of the body with the ground, so that the same running can be performed in both the cases.

[0127] Thus, according to the first embodiment, the following control can be performed under the control of the control computer unit 9.

[0128] The inverted stopping control (Equation (49), Steps S2 and S3) is performed from the initial state in which the body 1 is stopped and in contact with the ground 800, and thereby the body 1 is raised to the inverted position to maintain the inverted state (however the running is not performed). Then, after the predetermined time (stable time) elapses to stabilize the inverted state (Step S4), the contact sensors 10a and 10b start to detect whether or not the body 1 is in contact with the ground or the like (Step S5). When the body 1 is not in contact with the ground or the like in Step S5 (in other words, when the body is not constrained), the inverted stopping control is transferred to the inverted running control (Equation (49), Steps S8 and S9) to maintain the running state, and the contact sensors 10a and 10b detect whether or not the body 1 is in contact with the ground or the like again (Step S5). When the body 1 is in contact with the ground or the like in Step S5 (in other words, when the body is constrained), the inverted stopping control is transferred to the inverted running control (Equation (52) or Equation (53), Steps S6 and S7) to maintain the running state, and the contact sensors 10a and 10b detect whether or not the body 1 is in contact with the ground or the like again (Step S5).

[0129] Thus, the inverted two-wheel running is performed while the inverted stopping control (Step S3), the inverted running control (Step S9), and the inverted running control (Step S7) are appropriately switched. Therefore, even if the body 1 is in contact with the ground or the like, the torque ratio contributing to the body rotation can be changed according to the contact state (namely, body constraint state) to prevent the runaway that the running speed is rapidly increased because the excessive torque is applied to the wheel 2 to hardly maintain the attitude control, and the safety running can be continued while the burden is not placed on the user.

[0130] A second embodiment of the present invention will be described with reference to FIG. 8 which is used in the first embodiment. In the first embodiment, the contact sensors are provided to detect the contact forces. On the other hand, in the second embodiment, the control method will be described in the case where the contact sensor 10b is a sensor which detects only the presence or absence of the contact while not being able to measure the force, or in the case where only the presence or absence of the contact is utilized although the sensor can measure the force.

[0131] When a part of the body 1 is in contact with the ground 800 or the like, the body 1 cannot be rotated, and the body 1 is in contact with the ground 800 at least at three points, i.e., at the two wheels 2a and 2b and at least at one other point. In the conventional movement control of the independently two-wheeled drive truck which is in contact with the ground 800 by the auxiliary wheel or the like, the running control is performed to the rotation speed of the wheel 2 by PID control or the like to maintain the running state. Therefore, it can be regarded that the second embodiment has the same configuration as the conventional movement control in the case where the body 1 is in contact with the ground 800. In the case where the body 1 is in contact with the ground 800, the second embodiment has the speed control configuration in which the PI control is substantially performed to the rotation speed as shown by Equation 54.
τ₁=k₅({dot over (θ)}−{dot over (θ)}_d)+k₆∫({dot over (θ)}−{dot over (θ)}_d)dt=k₅({dot over (θ)}−{dot over (θ)}_d)+k₆z₁  (Equation 54)
Accordingly, when the body 1 is in contact with the ground 800 or the like, the control is performed by changing the control to Equation 54. When the body 1 is not in contact with the ground 800 or the like, the control is performed by changing the control to Equation 50.

[0132] A running operation flowchart in which the change control is used in the second embodiment will be described with reference to FIG. 10.

[0133] In the initial state (Step S11), the body 1 of the robot is stopped while inclined. The initial setting is performed as shown in

(z₁νω)=(0,0,0) (=Step S12). Then, the inverted stopping control

({dot over (θ)}_d=ν/r=0) expressed by (Equation 50) (Step S13) is performed to raise the body 1 of the robot.

[0134] After the stable time (for example, about five seconds) elapses (Step S14) since the inverted stopping control (Step S13), it is judged whether or not the body 1 is in contact with the ground or the like (Step S15). When the body 1 is not in contact with the ground or the like in Step S15, after the setting of the speed (νω) is performed (Step 20), the inverted running control (Step S21) expressed by Equation 49 is performed to maintain the running state. An integral term reset historical bit bt1 is provided as the condition that the control method is transferred from the inverted running control (Step S21) based on Equation 49 to the speed control (Step S18) based on Equation 54 during the contact. The integral term shall mean integration z₁ of a speed difference (positional displacement amount of the positional displacement by the break-down of the balance of the body 1 to the current position of the body 1). During the inverted running control (Step S21), it is judged whether or not the body 1 is in contact with the ground or the like (Step S22).

[0135] When the body 1 is not in contact with the ground 800 or the like in Step S22, the integral term reset historical bit bt1 is reset (bt1=0) (Step S26) (namely, while the integral term is not reset), and the flow returns to Step S20.

[0136] When the body 1 is in contact with the ground 800 or the like in Step S22, it is judged whether or not the integral term reset history is turned on, namely, it is judged whether or not the integral term reset historical bit bt1 is one (bt1=1) (Step S23). Only in the case of integral term reset historical bit bt1=0 (integral term has been not reset after the contact), the integral term z₁ is reset z₁=0 (the integral term is reset and historical information on the reset of the integral term is left) (Step S27), and the flow returns to Step S20. The reason why the integral term is reset is that the positional displacement (integration z₁ of the speed difference) generated before the contact is set to zero at one to perform the PI control from the contact. Thus, the positional displacement amount prior to the contact can be deleted by resetting the integral term, and the runaway phenomenon of the body 1 can be prevented because the operation control is performed only by the post-contact positional displacement amount of the body 1.

[0137] In the case where the integral term reset historical bit bt1 is one (bt1=1) in Step S23, it is judged whether or not the body 1 is in contact with the ground or the like (Step S24).

[0138] When the body 1 is not in contact with the ground 800 or the like in Step S24, the integral term reset historical bit bt1 is reset (bt1=0) (Step S28) (namely, while the integral term is not reset), and then, the flow returns to Step S20.

[0139] In the case where the body 1 is still in contact with the ground 800 or the like in Step S24 (bt1=1), when the stable time (for example, one second) does not elapse (Step S25), the flow returns to Step S20 to continue the inverted running (Step S21).

[0140] On the other hand, when the stable time (for example, one second) elapses, the inverted running (Step S21) is transferred to the speed control (Steps S30 to S32).

[0141] When the body 1 is in contact with the ground 800 or the like in Step S15, or when the stable time elapses in Step S25, the speed (νω) is set (Step S30).

[0142] Then, it is judged whether or not the speed (νω) is (0,0) (Step S31). In the case of (νω)=(0,0), it is judged that the body 1 is not raised, and then, the flow is ended (Step S19). In the case where the speed (νω) is not (0,0) in Step S31, namely, (νω)≠(0,0),
the robot runs under the speed control (Step S32) shown by Equation 54, and then, the flow returns to Step S14. Then, it is judged whether or not the stable time (for example, five seconds) of the speed control (Step S32) elapses (Step S14).

[0143] Thus, according to the second embodiment, the following control can be performed under the control of the control computer unit 9.

[0144] The inverted stopping control (Equation (49), Steps S12 and S13) is performed from the initial state in which the body 1 is stopped and in contact with the ground 800, and thereby the body 1 is raised to the inverted position to maintain the inverted state (however the running is not performed). Then, after the predetermined time (stable time) elapses to stabilize the inverted state (Step S14), the contact sensors 10a and 10b start to detect whether or not the body 1 is in contact with the ground or the like (Step S15). When the body 1 is not in contact with the ground or the like in Step S15 (in other words, when the body is not constrained), the inverted stopping control is transferred to the inverted running control (Equation (49), Steps S20 and S21) to maintain the running state. When the body 1 is in contact with the ground or the like in Step S15 (in other words, when the body is constrained), the inverted stopping control is transferred to the speed control (Steps S30 to S32). Then, after the predetermined time (stable time) elapses to stably perform the speed control (Step S14), the contact sensors 10a and 10b detect whether or not the body 1 is in contact with the ground or the like again (Step S15). When the body 1 is not in contact with the ground or the like in Step S15 (when the body 1 is successful in standing up), the speed control is transferred to the inverted running control (Equation (49), Steps S20 and S21) to maintain the running state. When the body 1 is in contact with the ground or the like in Step S15 (when the body 1 fails to stand up), the speed control (Steps S30 to S32) is continued. In performing the speed control (Steps S30 to S32), when the speed and the angular speed are zero, it is judged that the body 1 is in contact with the ground 800 or the like, namely the body is not raised, and the raising drive of the body 1 is stopped to maintain the contact state.

[0145] In performing the inverted running control (Equation (49), Steps S20 and S21), When the body 1 is in contact with the ground or the like (Step S22), the positional displacement amount until the contact is reset to be zero, and then, the operation control is performed only by the positional displacement amount from the contact. Therefore, the runaway phenomenon of the body 1 can be prevented. In the inverted running control, when the contact is continued after the stable time elapses since the positional displacement amount until the contact is reset to be zero, it is judged that the inverted state of the body 1 is broken down, and then, the operation control can be performed such that the body 1 is returned to the inverted state by changing the inverted running control to the speed control.

[0146] Thus, the inverted two-wheel running is performed while the inverted stopping control (Step S13), the inverted running control (Step S21), and the running control (Step S32) are appropriately switched. Therefore, even if the body 1 is in contact with the ground or the like, the torque ratio contributing to the body rotation can be changed according to the contact state (namely, body constraint state) to prevent the runaway that the running speed is rapidly increased because the excessive torque is applied to the wheel 2 to hardly maintain the attitude control, and the safety running can be continued while the burden is not placed on the user.

[0147] In the second embodiment, the presence or absence of the contact is detected by the actual inputs of the contact sensors 10a and 10b to change the control method. On the other hand, in a third embodiment of the present invention, it is found that the ground 800 on which the body runs is a flat surface, the control method is changed by the inclination angle of the body 1 with no contact sensor, and the change control is performed when the inclination angle of the body 1 exceeds a predetermined angle. That is, as shown in FIG. 11, when the body 1 is inclined and always in contact with the ground 800, the inclination angle of the body 1 is set as follows: φ>φ_b>0 φ<φ_c<0. When the body 1 is inclined but not in contact with the ground 800, namely, in the case of

[0148] (φ_b≧φ≧φ_c, the inverted running control shown by Equation 49 is performed to maintain the running state. When the body 1 is inclined and comes into contact with the ground 800, namely, in the case of (φ>φ_b or φ<φ_c), the control is performed based on Equation 54.

[0149] Here, as one example, it is assumed that when, as shown in FIG. 11, the body 1 is inclined to the inclination position (II or III shown by dot line) by 10° with respect to the inverted stopping or the running state (I shown by an alternate long and short dash line), the body 1 is in contact with the ground 800. In consideration of a tolerance of 0.5°, when the body is inclined not lower than 9.5°, it may be judged that the body 1 is in contact with the ground 800. That is, in the case of φ>φ_b=9.5°>0 or φ<φ_c=−9.5°<0, it may be judged that the body 1 is in contact with the ground 800. Therefore, when the body 1 is inclined but not in contact with the ground 800, namely, in the case of (φ_b=9.50°≧φ≧φ_c=9.5°), the inverted running control shown by Equation 49 is performed to maintain the running state. When the body 1 is inclined and comes into contact with the ground 800, namely, in the case of (φ>φ_b=9.5° or φ<φ_c=−9.5°), the speed control is performed based on Equation 54.

[0150] Thus, the robot runs while the control method is changed according to the inclination angle of the body 1. Therefore, even if the body 1 comes into contact with the flat surface, the runaway is not generated and the robot can continue the safe running safely while the burden is not placed on the user.

[0151] By properly combining the arbitrary embodiments of the aforementioned various embodiments, the effects possessed by the embodiments can be produced.

[0152] As described above, the present invention can provide a two-wheeled inverted robot and the control method thereof, in which the attitude control and the running state can be maintained and the safe movement can be performed while the burden is not placed on the user, even when the body comes into contact with the ground or the wall to constrain the rotation.

[0153] Although the present invention has been fully described in connection with the preferred embodiments thereof with reference to the accompanying drawings, it is to be noted that various changes and modifications are apparent to those skilled in the art. Such changes and modifications are to be understood as included within the scope of the present invention as defined by the appended claims unless they depart therefrom.