WO2015092913A1 - Two-wheeled mobile object, and method of controlling same - Google Patents

Abstract

Provided is a two-wheeled mobile object and the like that achieve stable posture during movement. The two-wheeled mobile object is provided with: an upper body (7); a pair of rotatable legs (5R, 5L); wheels (1R, 1L) mounted to the legs (5R, 5L); wheel drive motors (2R, 2L); steer units (3R, 3L); steer drive motors (4R, 4L); a posture detection unit (8) that detects posture information of the upper body (7); and a control unit (9). In accordance with the rotating speed of the wheels (1R, 1L), the control unit (9) implements an intermediate mode in which the weighting for feedback gain used for first control and second control is smoothly varied.

Description

Two-wheel moving body and control method thereof

The present invention relates to a two-wheeled vehicle and a control method thereof.

A two-wheel moving body that has two wheels and moves by rotating each wheel is known. The two-wheel moving body has an advantage that the area when viewed in plan can be reduced as compared with a moving body including three or more wheels.

For example, Patent Document 1 describes a standing two-wheeled parallel two-wheeled vehicle on which a person rides. The parallel two-wheeled vehicle with a pedal is configured to be switchable between a parallel two-wheel mode in which axles of two wheels are arranged on the same straight line and a bicycle mode in which two wheels are shifted and arranged like a bicycle.

Patent Document 2 describes a control method for stabilizing the movement in the parallel two-wheel mode described above. That is, Patent Document 2 describes that the wheel driving motor is controlled based on the inclination angle of the vehicle body detected by the angle detecting means to stabilize the attitude of the coaxial two-wheeled vehicle.

In addition, the applicant of the present application has developed a two-wheel moving body that executes an oblique two-wheel mode, which is an intermediate form between the parallel two-wheel mode and the bicycle mode, and discloses an invention relating to the two-wheel moving body in Patent Document 3. . That is, Patent Document 3 describes a two-wheel moving body that stabilizes the posture by adjusting the degree of opening of two legs, the steering angle of wheels, and the like.

JP 2010-274715 A JP 63-305082 A PCT / JP2013 / 056566

By the way, the parallel two-wheel mode described in Patent Documents 1 and 2 is effective during low-speed movement, but when one wheel contacts an obstacle (for example, a step) during high-speed movement, a large overturning force is generated. There is a problem that it is easy to fall. Moreover, although the bicycle mode described in Patent Documents 1 and 2 is effective during high-speed movement, there is a problem in that it cannot be stabilized by steering in a stopped state and is likely to fall over.

Patent Document 3 proposes a two-wheel moving body that overcomes the two conflicting disadvantages described above by transitioning the parallel two-wheel mode and the bicycle mode via the oblique two-wheel mode. However, there is room for further improvement in the stabilization of the oblique two-wheel mode when transitioning from one of the parallel two-wheel mode and the bicycle mode to the other.

Therefore, an object of the present invention is to provide a two-wheeled moving body that stabilizes the posture during movement and a control method thereof.

In order to solve the above-described problem, the two-wheel moving body according to the present invention, when shifting from one of the parallel two-wheel mode and the bicycle mode to the other, the first control that stabilizes the posture by accelerating and decelerating the wheel drive motor; In addition to the second control for stabilizing the posture by adjusting the rotation angle of the steering drive motor, the weighting of the feedback gain used for the first control and the second control is made according to the rotational speed of the wheel. An intermediate mode for smoothly changing is executed.

According to the present invention, it is possible to provide a two-wheel moving body that stabilizes a posture during movement and a control method thereof.

BRIEF DESCRIPTION OF THE DRAWINGS It is an external view of the two-wheel moving body which concerns on 1st Embodiment of this invention, (a) is a right view of a two-wheel moving body, (b) is a front view of a two-wheel moving body, (c) is a two-wheeled vehicle. It is a top view of a moving body. It is a block diagram of the control part with which a two-wheel moving body is provided. It is a block diagram of the moving system control part with which a control part is provided. It is a flowchart which shows the process which a control part performs. It is explanatory drawing which shows the relationship between a target moving speed and a target leg opening angle. It is explanatory drawing which shows typically the positional relationship of each structure of a two-wheel mobile body, (a) is a top view, (b) is a side view. It is a flowchart which shows the flow of a process when an intermediate mode execution part performs intermediate mode. It is an external view of the two-wheeled mobile body which concerns on 2nd Embodiment of this invention, (a) is a right view of a two-wheeled mobile body, (b) is a front view of a two-wheeled mobile body, (c) is a two-wheeled vehicle. It is a top view of a moving body. It is a block diagram of the control part with which a two-wheel moving body is provided.

<< First Embodiment >>
<Configuration of two-wheeled vehicle>
Fig.1 (a) is a right view of a two-wheeled mobile body, FIG.1 (b) is a front view, FIG.1 (c) is a top view. In FIG. 1, the two legs 5R and 5L are shown open, but the legs 5R and 5L can be opened and closed as will be described later.
The two-wheel moving body M is a moving body that moves by rotating the wheels 1R and 1L installed on the distal ends of the two legs 5R and 5L, and is used for, for example, a service robot. The two-wheel moving body M includes wheels 1R, 1L, wheel drive motors 2R, 2L, steer portions 3R, 3L, steer drive motors 4R, 4L, legs 5R, 5L, leg drive motor 6, and upper body 7. A posture detection unit 8 and a control unit 9.

As shown in FIGS. 1 (a) and 1 (b), the right wheel 1R is installed on the lower side of the leg 5R (on the side opposite to the rotation axis of the leg 5R), and its steer angle δ _R by the steer portion 3R. (See FIG. 6A) is adjusted. Note that the center of gravity X _G (see FIG. 6B) of the two-wheel moving body M is located above the wheels 1R and 1L.

The wheel drive motor 2R shown in FIGS. 1B and 1C is a motor that rotates the wheel 1R in response to a command from the control unit 9, and is accommodated in the steering unit 3R while being connected to the axle of the wheel 1R. Has been.
The wheel drive motor 2R includes an encoder (not shown) that detects a rotation angle ζ _R of the wheel 1R (see FIG. 6B) and a rotation speed dζ _R indicating a temporal change of the rotation angle ζ _R. Is installed.

The steer portion 3R adjusts the steer angle δ _R (see FIG. 6A) of the wheel 1R, and is installed near the lower end of the leg 5R. The steer portion 3R is disposed on the inner side of the wheel 1R in the left-right direction (see FIG. 1B), and accommodates the wheel drive motor 2R therein. Note that the steer portion 3R may be disposed outside the wheel 1R in the left-right direction.

The steer drive motor 4R is a motor that adjusts the steer angle δ _R (see FIG. 6A) of the wheel 1R in accordance with a command from the control unit 9. For example, the steer drive motor 4R is housed in the lower portion of the leg 5R (see FIG. 1B), and the rotation shaft thereof is installed in the steer portion 3R.
The steer drive motor 4R is provided with an encoder (not shown) that detects the steer angle δ _R of the wheel 1R.

Thus, the right wheel 1R is rotated by the wheel drive motor 2R, the steering angle [delta] _R is adjusted by a steering drive motor 4R. On the other hand, the left wheel 1L is rotated by a wheel drive motor 2L different from the right side, and the steering angle δ _L (see FIG. 6A) is adjusted by the steering drive motor 4L.
Since the left wheel 1L, the wheel drive motor 2L, the steer portion 3L, and the steer drive motor 4L are the same as those on the right side, description thereof is omitted.

The legs 5 </ b> R are installed so as to be rotatable (open / closed legs) in the front-rear direction around a shaft (not shown) disposed inside the upper body 7. The leg 5R has an upper end portion (rotating shaft) installed on the upper body 7 and a lower end portion installed on the steer portion 3R. The left leg 5L has the same configuration as the right leg 5R.
In this way, the pair of legs 5R and 5L are installed on the upper body 7, and are configured to be rotatable about the upper end thereof.

The leg drive motor 6 is a motor that rotates (opens / closes) the legs 5R and 5L in response to a command from the control unit 9, and is accommodated in the upper body 7. In this embodiment, the right leg 5R and the left leg 5L are rotated by one leg drive motor 6. The legs 5R and 5L may be rotated by separate leg drive motors.
The leg drive motor 6 is provided with an encoder (not shown) that detects an open leg angle ψ of the legs 5R and 5L and an open leg angular velocity dψ that is a temporal change of the open leg angle ψ.

The upper body 7 is a housing that houses equipment including the leg drive motor 6, the posture detection unit 8, and the control unit 9. As described above, the rotation shafts (not shown) of the legs 5R and 5L are disposed inside the upper body 7.

The posture detection unit 8 is, for example, a gyroscope, and is a sensor that acquires information related to the posture of the two-wheel moving body M.
The information described above includes the inclination angle θ in the traveling direction of the two-wheeled moving body M (see FIG. 6B), the inclination angular velocity dθ indicating the temporal change of the inclination angle θ, and the inclination of the two-wheeled moving body M in the left-right direction. The angle φ, the inclination angular velocity dφ indicating a temporal change of the inclination angle φ, the angle ω indicating the turning direction of the two-wheeled moving body M, and the turning direction angular velocity dω indicating the temporal change of the angle ω are included. The posture detection unit 8 outputs each detection result to the control unit 9.

The control unit 9 executes arithmetic processing in accordance with information input from the posture detection unit 8, an operation by an operator, and the like, and the wheel drive motors 2R and 2L, the steer drive motors 4R and 4L, and the leg drive motor 6 are operated. Control. The control unit 9 is, for example, a microcomputer (not shown), reads a program stored in a ROM (Read Only Memory), develops it in a RAM (Random Access Memory), and a CPU (Central Processing Unit) performs various processes. Is supposed to run.

<Configuration of control unit>
FIG. 2 is a configuration diagram of a control unit included in the two-wheel moving body. As shown in FIG. 2, the control unit 9 includes a target value generation unit 91, a leg control unit 92, a mechanism information calculation unit 93, and a moving system control unit 94.
The target value generating unit 91 moves the two-wheeled mobile body M based on the current position / orientation of the two-wheeled mobile body M, the coordinates of the target position that is the destination, the speed upper limit value, the acceleration upper limit value, etc. _0t , _y0t , _ω0t ) and the target moving speed _v0t . Here, x _0t and y _0t are the coordinates of the center of gravity X _G (see FIG. 6) of the two-wheel moving body M, and ω _0t is an angle representing the turning direction of the two-wheel moving body M.

The aforementioned target position, speed upper limit value, and acceleration upper limit value are stored in advance in a storage unit (not shown). The current position and direction of the two-wheeled moving body M may be calculated based on the past movement history, or may be input from the outside and stored in the storage unit.
The target value generation unit 91 sets the trajectory described above based on a plurality of times (current time + kΔt: Δt is a control cycle, k is a natural number) and the coordinates of the two-wheeled moving body M at each time. The target moving speed v _0t is set similarly.

The target value generation unit 91 outputs the calculated target movement speed v _0t to the leg control unit 92. Further, the target value generation unit 91 outputs the trajectory (x _0t , y _0t , ω _0t ) and the target moving speed v _0t to the moving system control unit 94.

The leg control unit 92 calculates the target leg opening angle ψ ₀ of the legs 5R and 5L corresponding to the target moving speed v _0t input from the target value generating unit 91. The target leg opening angle ψ ₀ is a target angle when the legs 5R and 5L are rotated (see FIG. 1A). Leg control unit 92 controls the leg drive motor 6 according to the target open leg angle [psi ₀ calculated, and outputs the target open leg angle [psi ₀ to mechanism information calculation section 93.

Based on the target leg opening angle ψ ₀ input from the leg control unit 92, the mechanism information calculation unit 93 is information indicating the positional relationship of the wheels 1R, 1L with respect to the upper body 7 (distance h, displacement q described later: FIG. 6). (See (a) and (b)) and the center of gravity X _G (see FIGS. 6A and 6B) of the two-wheeled moving body M excluding the wheels 1R and 1L.
Further, the mechanism information calculation unit 93 calculates the distance h (see FIG. 6B) between the line segment T (see FIG. 6A) connecting the centers of the wheels 1R and 1L and the center of gravity X _G and the center of gravity X _G. moment of inertia I _G about the axis perpendicular to the street travel direction, is calculated.
The mechanism information calculation unit 93 outputs the calculated values to the moving system control unit 94.

The moving system control unit 94 is based on the information input from the target value generation unit 91 and the mechanism information calculation unit 93 and the information input from the encoders (not shown) and the attitude detection unit 8 described above. Wheel drive motors 2R and 2L and steer drive motors 4R and 4L are controlled.

<Configuration of mobile system controller>
FIG. 3 is a configuration diagram of a mobile system control unit included in the control unit. The moving system control unit 94 includes a speed / position calculation unit 941, a travel mode determination unit 942, a parallel two-wheel mode execution unit 943, a bicycle mode execution unit 944, and an intermediate mode execution unit 945.

The speed / position calculation unit 941 moves based on information input from the posture detection unit 8 and the mechanism information calculation unit 93 and information input from an encoder (not shown) installed in each motor. The actual velocity (dx _c , dy _c ) and position (x _c , y _c ) of the body M are calculated.
The travel mode determination unit 942 determines a travel mode to be executed in the next control cycle based on information input from the mechanism information calculation unit 93.
This traveling mode includes a parallel two-wheel mode, an intermediate mode, and a bicycle mode. Each travel mode will be described later.

The parallel two-wheel mode execution unit 943 executes the two-wheel parallel mode so that the current position (x, y, ω) approaches the trajectory (x _0t , y _0t , ω _0t ) input from the target value generation unit 91. Here, the two-wheel parallel mode is a travel mode in which the inverted pendulum control is executed in a state where the wheels 1R and 1L are positioned substantially coaxially (that is, the legs 5R and 5L are closed). The above-described inverted pendulum control is control that stabilizes the posture of the two-wheeled moving body M by using the inertial force generated in association with the acceleration / deceleration of the wheel drive motors 2R and 2L.
As the inverted pendulum control, for example, a method described in Japanese Patent Application Laid-Open No. 2007-319991 can be used.

The bicycle mode execution unit 944 executes the bicycle mode so that the current position (x, y, ω) approaches the trajectory (x _0t , y _0t , ω _0t ) input from the target value generation unit 91. Here, the bicycle mode is a traveling mode in which the axles of the wheels 1R and 1L are shifted and the surface directions thereof are substantially matched, and the posture is stabilized using centrifugal force.
As the bicycle mode, for example, Marumo et al., “Research on collision avoidance system for motorcycles”, Transactions of the Japan Society of Mechanical Engineers (C), September 2011, Vol. 77, No. 781, p. The method described in 3300-3331 can be used.

The intermediate mode execution unit 945 executes the intermediate mode so that the current position (x, y, ω) approaches the trajectory (x _0t , y _0t , ω _0t ) input from the target value generation unit 91. Here, the intermediate mode is an intermediate running mode that is executed when shifting from one of the parallel two-wheel mode and the bicycle mode to the other mode.
As shown in FIG. 3, the intermediate mode execution unit 945 includes a weight calculation unit 945a, an inverted pendulum control execution unit 945b, and an attitude stabilization control execution unit 945c. The configuration of the intermediate mode execution unit 945 will be described later.

<Processing content of control unit>
FIG. 4 is a flowchart illustrating processing executed by the control unit. Note that the series of processing shown in FIG. 4 is executed in one control cycle of the control unit 9.
In step S101, the control unit 9 causes the target value generation unit 91 to calculate the trajectory (x _0t , y _0t , ω _0t ) and the target moving speed v _0t of the two-wheel moving body M. For example, when the current position (x, y, ω) and the target position (x ₀ , y ₀ , ω ₀ ) of the movement destination are given, the target value generation unit 91 creates a straight line connecting these two points. A trajectory of the two-wheeled moving body M is assumed.
Further, the target value generation unit 91 determines a target moving speed v _0t at each time in the future based on, for example, a trapezoidal speed pattern (acceleration → constant speed → deceleration). Note that the method of calculating the trajectory (x _0t , y _0t , ω _0t ) and the target moving speed v _0t is not limited to the above example.

In step S102, the control unit 9 causes the leg control unit 92 to calculate a target leg opening angle ψ ₀ corresponding to the target moving speed v _0t based on the following (Equation 1). Note that the speed thresholds v _ra and v _rb and the upper limit value ψ _max of the target leg opening angle ψ ₀ are preset constants.

FIG. 5 is an explanatory diagram showing the relationship between the target moving speed and the target leg opening angle. The broken line shown in FIG. 5 corresponds to (Formula 1). When the target moving speed v _0t is less than the threshold value v _ra , the leg control unit 92 sets the target leg opening angle ψ ₀ to zero (that is, closes the legs 5R and 5L).
When the target moving speed v _0t is not less than the threshold value v _{ra and not} more than the threshold value v _rb , the leg control unit 92 sets the target leg opening angle ψ ₀ so that the legs 5R and 5L are opened _{wider as} the target moving speed v _0t increases. Set.

When the target moving speed v _0t is larger than the threshold value v _rb , the leg control unit 92 drives the leg drive motor 6 so as to open the legs 5R and 5L by the angle ψ _max (that is, maximize the legs 5R and 5L). open). In this manner, the leg control unit 92 changes the positional relationship between the wheels 1R and 1L according to the target moving speed v _0t of the two-wheel moving body M.

In step S <b> 103, the control unit 9 uses the mechanism information calculation unit 93 to calculate the center of gravity X _G and the moment of inertia I _{G of} the two-wheel moving body M.
That is, mechanism information calculation unit 93, shown below, based on the (Equation 2), calculates the position vector of the center of gravity X _G in a two-wheeled mobile M (excluding the wheels 1R, and 1L). A vector X _i shown in (Expression 2) is a position vector of the devices (the steer portions 3R and 3L, the legs 5R and 5L, the upper body 7 and the like) constituting the two-wheeled moving body M. The vector X _i is based on the connection relationship and shape of the devices described above, the target leg opening angle ψ ₀ input from the leg control unit 92, information input from the attitude detection unit 8 and encoders (not shown) of each motor, and the like. Calculated based on The mass W _i shown in (Formula 2) is the mass of each device described above, and is stored in advance in a storage unit (not shown).

FIGS. 6A and 6B are explanatory views schematically showing the positional relationship of each component of the two-wheel moving body, where FIG. 6A is a plan view and FIG. 6B is a side view. In FIG. 6, the simplified as a point mass located the body 7 to the center of gravity X _G. Further, the XY plane shown in FIG. 6 is along the horizontal direction, and the Z axis is along the vertical direction.
The mechanism information calculation unit 93 calculates the distance h between the line segment T connecting the centers of the left and right wheels 1R and 1L and the center of gravity X _G of the two-wheel moving body M based on the following (Formula 3). In addition, the vector a shown in (Formula 3) is a position vector of the center of the wheel 1R, and the vector b is a position vector of the center of the wheel 1L.

Further, the mechanism information calculation unit 93 determines the displacement q () of the wheels 1R and 1L based on the state in which the wheels 1R and 1L are coaxially positioned and the steering angles δ _R and δ _L (see FIG. 6A) are zero. The front-rear direction) and the width w (left-right direction) are calculated. The displacement q and the width w are obtained from the geometric positional relationship shown in FIG. 6 based on the target leg opening angle ψ ₀ or the like input from the leg control unit 92.
In this way, the mechanism information calculation unit 93 calculates information (displacement q, distance h, width w) indicating the positional relationship between the body 7 and the wheels, and 1R and 1L.

Further, the mechanism information calculation unit 93 calculates the moment of inertia I _G around the center of gravity X _G (around the axis parallel to the line segment T shown in FIG. 6A). The inertia moment _IG is calculated based on the positional relationship and mass of the devices (the steer portions 3R, 3L, the legs 5R, 5L, the upper body 7, etc.) constituting the two-wheeled moving body M.

In addition, the control unit 9 calculates the speed v and the position (x, y, z) of the two-wheeled moving body M by the speed / position calculation unit 941. For the calculation process of the velocity v and the position (x, y, z), the distance h, the displacement q, information input from the attitude detection unit 8 and each encoder (not shown) are used.

The speed / position calculation unit 941 calculates the speed (dx _R , dy _R ) of the wheel 1R on the XY plane based on (Formula 4) using each acquired information, and the wheel based on (Formula 5). The speed (dx _L , dy _L ) of 1 _L is calculated.
Here, r is the radius of the wheels 1R, 1L, ζ _R is the rotation angle of the wheel 1R, θ is the inclination angle in the traveling direction of the two-wheel moving body M, and ω is the angle indicating the turning direction, [delta] _R is steering angle of the wheel 1R.

Subsequently, the speed / position calculation unit 941 calculates the center of gravity X _{G of the} body 7 by the following (Formula 6) based on the speeds (dx _R , dy _R ), (dx _L , dy _L ) of the wheels 1R, 1L. The speed (dx _C , dy _C ) is calculated.

Further, the speed / position calculation unit 941 calculates the height z _C of the center of gravity _{XG in} the z direction (vertical direction) based on the following (Formula 7).

Also, the speed / position calculation unit 941 integrates (dx _R + dx _L ) / 2 and (dy _R + dy _L ) / 2 with time, and calculates the current position (x, y) of the center of gravity X _G. . The speed-position calculating unit 941 described below on the basis of (Equation 8), to calculate the velocity v of the center of gravity X _G.

The current position (x, y) and speed v of the two-wheeled moving body M calculated in this way are used for controlling the wheel drive motors 2R, 2L and the steer drive motors 4R, 4L based on feedback control.

Next, in step S104 of FIG. 4, the control unit 9 determines whether or not the displacement q (see FIG. 6A) is less than the threshold value D1 by the travel mode determination unit 942. The threshold value D1 is a value that serves as a criterion for determining whether or not the posture can be sufficiently stabilized only in the parallel two-wheel mode, and is set in advance. When the displacement q is less than the threshold value D1 (S104 → Yes), the traveling mode determination unit 942 determines to execute the parallel two-wheel mode.

In step S <b> 105, the control unit 9 controls the posture of the two-wheel moving body M by the parallel two-wheel mode execution unit 943.
For example, the parallel two-wheel mode execution unit 943 provides predetermined feedback gains K1, K2, K3, and K4 for the rotation angle ζ, the rotation speed dζ, the inclination angle θ in the traveling direction, and the inclination angular velocity dθ of the wheels 1R and 1L, respectively. A value obtained by multiplication and addition is set as a target value of the average torque τ _c of the wheel drive motors 2R and 2L.

The parallel two-wheel mode execution unit 943 calculates the turning torque τ _d based on the following (Equation 9). K _d is a turning gain set in advance, ω _ot is a target value indicating the turning direction, and ω is an angle indicating the current turning direction.

The parallel two-wheel mode execution unit 943 outputs the target torque (τ _c −τ _d ) to the right wheel drive motor 2R and outputs the target torque (τ _c + τ _d ) to the left wheel drive motor 2L. As described above, τ _c is a target value of the average torque of the wheels 1R and 1L. Further, the parallel two-wheel mode execution unit 943 sets the target steer angle of the steer drive motors 4R and 4L to zero.
Thus, the parallel two-wheel mode execution unit 943 uses the inertial force generated with the acceleration / deceleration of the wheel drive motors 2R and 2L, and stabilizes the posture of the two-wheel moving body M by the inverted pendulum control.

In FIG. 4, when the displacement q is greater than or equal to the threshold value D1 (S104 → No), the process of the control unit 9 proceeds to step S106. In step S106, the control unit 9 determines whether the displacement q is less than the threshold value D2 by the travel mode determination unit 942. The threshold value D2 is a value that serves as a criterion for determining whether or not the posture stability can be sufficiently secured only in the bicycle mode, and is set in advance.
When the displacement q is less than the threshold value D2 (S106 → Yes), the control unit 9 outputs a command signal to the intermediate mode execution unit 945.

In step S107, the control unit 9 determines to execute the intermediate mode. Here, the intermediate mode is a travel mode that is executed when transitioning from one of the parallel two-wheel mode and the bicycle mode to the other. In the intermediate mode, the control of the wheel drive motors 2R and 2L based on the inverted pendulum control and the control of the steer drive motors 4R and 4L based on the posture stability control are executed together.

As described above, the inverted pendulum control (first control) is based on the inertial force (acting in a direction perpendicular to the line segment T: see FIG. 6A) accompanying acceleration / deceleration of the wheel drive motors 2R and 2L. This is control that stabilizes the posture of the two-wheeled moving body M by using it.
In addition, the posture stabilization control (second control) acts in a direction parallel to the line segment T generated by the two-wheel moving body M turning with the rotation of the steering drive motors 4R and 4L: 6 (a)) is used to stabilize the posture of the two-wheeled moving body M.

In the intermediate mode, the transition from one of the parallel two-wheel mode and the bicycle mode to the other is smoothly performed by smoothly changing the weight of the gain used for the inverted pendulum control and the posture stabilization control.

FIG. 7 is a flowchart showing a flow of processing when the intermediate mode execution unit executes the intermediate mode.
In step S1071, the intermediate mode execution unit 945 calculates the ratio (that is, the division weight of each control) between the gain used in the inverted pendulum control and the gain used in the posture stabilization control by the weight calculation unit 945a (see FIG. 3). .

Weight calculator 945a, using the following (Equation 10), calculates a weighting factor E ₁ which focuses on torque efficiency of the inverted pendulum control. K shown in (Expression 10) is a coefficient for adjusting the ratio between E ₁ and E ₂ and is set in advance. ξ is an angle formed by a line segment T (see FIG. 6) connecting the centers of the wheels 1R and 1L and the front direction of the upper body 7 (forward direction in FIG. 6). δ is an angle indicating the current turning direction, and is an average value of the steer angles δ _R and δ _L. As described above, when the inverted pendulum control is performed, since the steer portions 3R and 3L are not rotated (the steer is not cut), the angle δ is constant.

As the angle (ξ + δ) shown in FIG. 6 approaches π / 2, the degree to which the torque of the wheels 1R, 1L is reflected in the progress of the two-wheel moving body M increases. In other words, the larger the weighting factor E _1, also increases the torque efficiency in rotating the wheels 1R, and 1L.

When the line segment T (see FIG. 6) connecting the centers of the wheels 1R and 1L is perpendicular to the traveling direction of the two-wheel moving body M (angle δ in FIG. 6), the weighting element E ₁ is maximum (E ₁ = K) On the other hand, when the line segment T and the traveling direction of the two-wheeled moving body M are on a straight line, the weighting element E ₁ is minimum (E ₁ = 0).

The weight calculation section 945a, using the following (Equation 11), calculates a weighting factor E ₂ which focuses on the centrifugal force generation efficiency of the posture stabilization control.
M ₁ shown in (Formula 11) is the mass of the two-wheel moving body M excluding the wheels 1R and 1L, and r is the radius of the wheels 1R and 1L. h is the distance between the line segment T connecting the centers of the wheels 1R and 1L and the center of gravity X _G , ζ is the rotation angle of the wheel 1R, and L is the length of the line segment T (between the centers of the wheels 1R and 1L) Distance) (see FIG. 6A).

The weighting element E ₂ is proportional to the square of the rotational speed of the wheels 1R, 1L (dζ / dt) ² , and the centrifugal force generation efficiency increases as the weighting element E ₂ increases. Instead of the product of r ² and (dζ / dt) ² described in (Equation 11), the square of the speed v of the two-wheel moving body M may be used.
The angle ξ included in the weighting elements E ₁ and E ₂ , the distance h included in the weighting element E ₂ , the rotational speed (dζ / dt), and the length L change continuously over time. Accordingly, the magnitudes of the weighting elements E ₁ and E ₂ also change continuously with the passage of time. Incidentally, when the length L than a predetermined value set in advance is small, the weight calculation section 945a, it is preferable to set the value of weighting factor E ₂ to zero. Thus, it is possible to avoid a situation that there is no solution for the weighting factor E _2.

The weight calculation unit 945a outputs the weighting elements E ₁ and E ₂ to the inverted pendulum control execution unit 945b and outputs the weighting elements E ₁ and E ₂ to the posture stabilization control execution unit 945c.

In step S1072 in FIG. 7, the intermediate mode execution unit 945 calculates the target torque τ ₀ of the wheel drive motors 2R and 2L by the inverted pendulum control execution unit 945b.
That is, the inverted pendulum control execution unit 945b calculates a feedback gain when controlling the wheel drive motors 2R and 2L using the first weight value (1 / E ₁ + E ₂ ) input from the weight calculation unit 945a. .

For example, the inverted pendulum control execution unit 945b uses the first weight value (1 / E as the feedback gain used in the parallel two-wheel mode execution unit 943 as the feedback gain of the inclination angle θ and the inclination angular velocity dθ of the body 7 along the traveling direction. ₁ + E ₂ ) is used. As the feedback gain, a value obtained by multiplying the first weighting value (1 / E ₁ + E ₂ ) by a predetermined constant may be used.
In addition, the inverted pendulum control execution unit 945b performs gain scheduling based on the already calculated distance h (see FIG. 6), inertia moment I _G , mass of each device, and the like, and target torques of the wheel drive motors 2R and 2L. τ ₀ is calculated.

Note that the target torque τ _o is calculated based on the following (Equation 12) using the average torque τ _c calculated by the parallel two-wheel mode execution unit 943, taking into consideration only the influence of the wheels 1R and 1L not being coaxial. It may be calculated.

In step S1073 of FIG. 7, the intermediate mode execution unit 945 calculates the posture stabilization steer angle δ _os by the posture stabilization control execution unit 945c. This steering angle δ _os for stabilizing the posture is a steering angle for stabilizing the posture of the two-wheeled moving body M by generating a centrifugal force by cutting the steering.
For example, the posture stabilization control execution unit 945c uses the second weighting value (E ₂ / E) for the feedback gain used in the bicycle mode execution unit 944 as the feedback gain of the inclination angle θ and the inclination angular velocity dθ of the body 7 along the traveling direction. A steering angle δ _os for posture stabilization is calculated using a product of ₁ + E ₂ ).

The posture stabilization control execution unit 945c reverses the sign of each steer angle δ _os with respect to the steer drive motors 4R and 4L. As a result, a centrifugal force in the vertical direction is generated with respect to the line segment T connecting the centers of the wheels 1R and 1L, and the posture of the two-wheel moving body M can be stabilized.

In step S1074 in FIG. 7, the posture stabilization control execution unit 945c calculates the follower steering angles δ _of and δ _oa . The tracking steer angles δ _of and δ _oa are steer angles for causing the traveling path of the two-wheeled vehicle M to follow the trajectory (x _0t , y _0t , ω _0t ) generated by the target value generation unit 91. It is.
The steer angle δ _of is calculated by multiplying the distance between the trajectory (x _0t , y _0t , ω _0t ) and the current position of the two-wheel moving body M by the feedback gain K _f . Note that the posture stabilization control execution unit 945c makes the positive and negative _of the respective steering angles δ _of regarding the steering drive motors 4R and 4L.

Further, the posture stabilization control executing unit 945C, and the angle omega that indicates the current turning direction, an angle omega ₀ indicating a target turning direction, the difference a multiplied by the feedback gain K _a in the, steering angle for turning [delta] _oa Set as. The posture stabilization control execution unit 945c reverses the sign of each steer angle δ _oa with respect to the steer drive motors 4R and 4L.

In step S1075 in FIG. 7, the posture stabilization control execution unit 945c calculates target steer angles δ _R and δ _L of the steer drive motors 4R and 4L based on the following (Formula 13).

The above (Formula 13) is a formula for calculating the target steer angles δ _R and δ _L when the left leg 5L is ahead of the body 7. Note that when the right leg 5R precedes, the signs of the steer angles δ _os and δ _oa are opposite to those in (Formula 13).
The target torque τ ₀ calculated in step S1071 is output to the wheel drive motors 2R and 2L, and the target steer angles δ _R and δ _L calculated in step S1074 are output to the steer drive motors 4R and 4L.

Again, returning to FIG. When displacement q (refer to Drawing 6 (a)) is more than threshold D2 in Step S106 (S106-> No), processing of control part 9 progresses to Step S108. In step S <b> 108, the control unit 9 executes the bicycle mode by the bicycle mode execution unit 944.

The bicycle mode execution unit 944 outputs a target rotational speed dζ ₀ obtained by dividing the target moving speed v _ot by the radius r of the wheels 1R, 1L to the left and right wheel drive motors 2R, 2L. Further, the bicycle mode execution unit 944 calculates a target steer angle _δot and drives the left and right steer drive motors 4R and 4L.
Thus, the posture of the two-wheeled moving body M can be stabilized by executing the bicycle mode with the legs 5R and 5L opened relatively large (q ≧ D2). In addition, although description is abbreviate | omitted about the detail of a bicycle mode, it is preferable to change a steering angle gain according to the moving speed of the two-wheel mobile body M. FIG.

<Effect>
According to this embodiment, when shifting from one of the parallel two-wheel mode and the bicycle mode to the other, the intermediate mode is executed, and the weighting elements E ₁ and E ₂ are reflected in the feedback gains of the inverted pendulum control and the posture stabilization control.

As a result, in the process of opening the legs 5R and 5L according to the speed increase, the degree of influence of the inverted pendulum control using the inertial force is gradually reduced, and the degree of influence of the posture stabilization control using the centrifugal force is gradually increased. . Since the variables (angle ξ, distance h, rotational speed (dζ / dt), length L) of the weighting elements E ₁ and E ₂ change continuously, the above-described changes also occur continuously. Therefore, the running mode can be smoothly shifted in the order of parallel two-wheel mode → intermediate mode → bicycle mode.
The same applies to the case where the traveling mode is shifted in the order of bicycle mode → intermediate mode → parallel two-wheel mode in the deceleration process.

If a transition is made from one of the bicycle mode and the parallel two-wheel mode to the other without going through the intermediate mode, the target torques of the wheel drive motors 2R, 2L and the angles of the steer drive motors 4R, 4L may become discontinuous, Compensation may occur, and the driving mode may not be smoothly shifted and the vehicle may fall over.
On the other hand, in the present embodiment, the ratio of the feedback gain is continuously changed over time while executing the inverted pendulum control and the posture stabilization control together in the intermediate mode. Therefore, as described above, the travel mode can be smoothly shifted.

<< Second Embodiment >>
The second embodiment differs from the first embodiment in that the stereo camera 10 is mounted on the upper body 7 and the configuration of the control unit 9, but is otherwise the same as in the first embodiment. It is. Therefore, a different part from 1st Embodiment is demonstrated and description of the overlapping part is abbreviate | omitted.

FIG. 8 is a right side view of the two-wheel moving body according to this embodiment, FIG. 8B is a front view, and FIG. 8C is a plan view.
As shown in FIG. 8, the two-wheeled moving body M includes a stereo camera 10 (imaging unit) that images the outside world including at least a travel destination route. The stereo camera 10 is configured so that an object can be imaged simultaneously from a plurality of different directions.

FIG. 9 is a configuration diagram of a control unit included in the two-wheel moving body. As shown in FIG. 9, the control unit 9 includes an image recognition unit 95, a target value generation unit 91, a leg control unit 92, and a moving system control unit 94.
Based on the image information input from the stereo camera 10, the image recognition unit 95 recognizes the state of the route scheduled to travel (tilt, presence / absence of obstacles, unevenness, etc.). Note that details of the processing executed by the image recognition unit 95 are omitted.
The target value generation unit 91, based on information input in advance by the operator and information input from the image recognition unit 95, a trajectory (x _0t , y _0t , ω _0t ) and a target for moving to the destination. A moving speed v _0t is generated.
The leg control unit 92 calculates the target leg opening angle ψ ₀ of the legs 5R and 5L corresponding to the target moving speed v _0t input from the target value generating unit 91.

The moving system control unit 94 includes the information input from the image recognition unit 95, the trajectory (x _0t , y _0t , ω _0t ) and the target moving speed v _0t input from the target value generation unit 91, and the leg control unit 92. Each motor is controlled on the basis of the target leg opening angle ψ ₀ of the legs 5R and 5L input from. The configuration of the mobile system control unit 94 is the same as that described in the first embodiment (see FIG. 3).

The travel mode determination unit 942 (see FIG. 3) determines the travel mode according to the size of the target leg opening angle ψ ₀ input from the leg control unit 92, for example.
For example, the weighting element E ₁ used in the weight calculation unit 945a (see FIG. 3) is a fixed value, and a function that increases in proportion to the square of the rotational speeds (dζ / dt) of the wheels 1R and 1L is used as the weighting element E _2. It may be used. Even in this case, the first weighting value (1 / E ₁ + E ₂ ) and the second weighting value (E ₂ / E ₁ + E ₂ ) are continuously set according to the rotational speeds (dζ / dt) of the wheels 1R and 1L. Since it changes, the running mode can be smoothly shifted.

When the unevenness is recognized on the path by the image recognition unit 95 during the execution of the intermediate mode, the moving system control unit 94 preferably performs the following process. That is, the mobile system control unit 94 multiplies the first weighting value (1 / E ₁ + E ₂ ) corresponding to the inverted pendulum control by a predetermined value α (<1) by the weight calculation unit 945a (see FIG. 3). The first weighting value is made smaller than when there is no unevenness. On the other hand, the weight value for posture stabilization control is divided by the predetermined value α, and the second weight value is made larger than when there is no unevenness.
Thereby, the gain of the inverted pendulum control that is greatly affected by the unevenness of the floor surface is suppressed, and the posture of the two-wheeled moving body M can be stabilized.

When the unevenness is recognized by the image recognition unit 95, the target value generation unit 91 outputs the target moving speed v _0t to the leg control unit 92 so as to widen the legs 5R and 5L compared to the normal time without the unevenness. . If the wheel 1R (or wheel 1L) on one side rides on the convex part during execution of the parallel two-wheel mode (that is, the inverted pendulum control), a large overturning force is generated and the posture of the two-wheeled moving body M becomes unstable. Cheap.
As described above, the legs 5R and 5L are opened relatively large by the leg control unit 92, whereby the bicycle mode can be quickly shifted from the parallel two-wheel mode to the intermediate mode. Even when riding on the convex part in the bicycle mode, the posture can be quickly stabilized by utilizing the centrifugal force.

<Effect>
According to the present embodiment, the weighting elements E ₁ and E ₂ are calculated based on the rotational speeds (dζ / dt) of the wheels 1R and 1L even if the control unit 9 does not include the mechanism information calculation unit 93. did. Thereby, it is possible to smoothly shift from one of the parallel two-wheel mode and the bicycle mode to the other through the intermediate mode.

When the unevenness is recognized on the floor surface scheduled to travel from the imaging result of the stereo camera 10, the control unit 9 suppresses the gain of the inverted pendulum control to increase the gain of the posture stabilization control, Open 5L promptly and shift to bicycle mode. Therefore, even when the floor surface is uneven, the posture of the two-wheel moving body M can be kept stable.

≪Modification≫
The two-wheeled moving body M according to the present invention has been described above, but the present invention is not limited to the above-described embodiments, and can be changed as appropriate.
For example, in the first embodiment, the case where the weighting element E ₁ is calculated based on (Formula 10) and the weighting element E ₂ is calculated based on (Formula 11) is described, but the present invention is not limited to this. That is, if the weighting of the feedback gain used for the inverted pendulum control (first control) and the posture stability control (second control) can be smoothly changed according to the rotation speed of the wheels 1R and 1L, A functional form may be used.

Moreover, in 1st Embodiment, the control part 9 is based on the rotational speed of wheel 1R, 1L and the information (displacement q, distance h, width w) which shows the positional relationship of the upper body 7 and wheel 1R, 1L. Although the case of executing the intermediate mode has been described, the present invention is not limited to this.
That is, the intermediate mode may be executed based only on the rotational speeds of the wheels 1R and 1L. In this case, the wheels 1R, with increasing rotational speed of 1L, a smaller weighting factor E ₁ of the inverted pendulum control, increasing the weighting factor E ₂ of the posture stabilization control.

In the second embodiment, the case where the “imaging unit” that images the outside world is the stereo camera 10 has been described. However, the present invention is not limited to this. For example, a CCD (Charge-Coupled Device) camera or the like may be used as the “imaging means”.

In each embodiment, the feedback gain of the inverted pendulum control is calculated by multiplying the feedback gain of the parallel two-wheel mode by the first weighting value (1 / E ₁ + E ₂ ), and the second weight is assigned to the feedback gain of the bicycle mode. Although the case where the feedback gain of the posture stabilization control is calculated by multiplying the value (E ₂ / E ₁ + E ₂ ) has been described, the present invention is not limited to this.
For example, each weight value may be set so that the ratio between the first weight value and the second weight value is 1: E ₂ . In this case, according to the weighting factor E ₂ increases or decreases, it is possible to smoothly change the weighting of the feedback gain used in the inverted pendulum control and posture stabilization control.

M Two- wheel moving body 1R, 1L Wheel 2R, 2L Wheel drive motor 3R, 3L Steer unit 4R, 4L Steer drive motor 5R, 5L Leg 6 Leg drive motor 7 Upper body 8 Attitude detection unit 9 Control unit 10 Stereo camera (imaging means)
91 Target Value Generation Unit 92 Leg Control Unit 93 Mechanism Information Calculation Unit 94 Moving System Control Unit 95 Image Recognition Unit 941 Speed / Position Calculation Unit 942 Traveling Mode Determination Unit 943 Parallel Two Wheel Mode Execution Unit 944 Bicycle Mode Execution Unit 945 Intermediate Mode Execution Unit 945a Weight calculation unit 945b Inverted pendulum control execution unit 945c Attitude stabilization control execution unit

Claims (12)

1. Upper body,
   A pair of legs rotatable about one end installed on the upper body;
   Wheels respectively installed on the other ends of the pair of legs;
   A wheel drive motor installed on each of the wheels;
   Steer portions for adjusting the steer angles of the wheels, and
   A steer drive motor installed in each of the steer portions;
   A posture detection unit for detecting posture information of the upper body;
   A control unit,
   The controller is
   A parallel two-wheel mode in which each of the wheels is substantially coaxial, and a bicycle mode in which the axis of the wheel is shifted and the surface directions thereof substantially coincide with each other can be executed.
   When shifting from one of the parallel two-wheel mode and the bicycle mode to the other,
   The first control for stabilizing the posture by accelerating / decelerating the wheel drive motor and the second control for stabilizing the posture by adjusting the rotation angle of the steer drive motor are performed together, and the rotation of the wheel is performed. A two-wheeled vehicle that executes an intermediate mode in which the weighting of the feedback gain used in the first control and the second control is smoothly changed according to speed.
2. The controller is
   Information indicating the positional relationship between the upper body and the wheel is calculated based on the connection relationship of the devices including the upper body, the leg, the wheel, and the steer portion, and the target leg opening angle of the leg. A mechanism information calculation unit that
   The two-wheel moving body according to claim 1, wherein the intermediate mode is executed based on a rotation speed of the wheel and information indicating the positional relationship.
3. The controller is
   A weight calculating unit that calculates the weighting element E ₂ based on (Formula 11),
   The ratio of the first weight value and the second weight value is 1: E ₂ ,
   By multiplying the feedback gain of the parallel two-wheel mode by the first weight value, the feedback gain of the first control is calculated,
   The two-wheeled vehicle according to claim 2, wherein the feedback gain of the second control is calculated by multiplying the feedback gain of the bicycle mode by the second weighting value.
   Here, m ₁ is the mass of the two-wheel moving body excluding the wheel, r is the radius of the wheel, h is the distance between the straight line connecting the centers of the wheels and the center of gravity of the two-wheel moving body, and ζ is the wheel A rotation angle, L is a distance between the centers of the wheels, ξ is an angle formed by a straight line connecting the centers of the wheels and the front direction of the upper body, and δ is an average value of the steering angles of the wheels.
   

   
4. The weight calculation unit
   A weighting element E ₁ is calculated based on (Equation 10),
   The two-wheeled vehicle according to claim 3, wherein the first weight value is (1 / E ₁ + E ₂ ) and the second weight value is (E ₂ / E ₁ + E ₂ ).
   Here, K is a constant, ξ is an angle formed by a straight line connecting the centers of the wheels and the front direction of the upper body, and δ is an average value of the steering angles of the wheels.
   

   
5. When the controller executes the intermediate mode,
   Steering angle for following to move the actual path along the target trajectory,
   Calculate the steer angle for posture stabilization in order to stabilize the posture by generating centrifugal force,
   The two-wheeled vehicle according to claim 3, wherein the steering angle for stabilizing the posture is calculated using a new feedback gain obtained by multiplying the feedback gain in the bicycle mode by the second weighting value. .
6. An imaging means for imaging the outside world,
   The controller is
   Based on image information input from the imaging means, it has an image recognition unit that recognizes at least unevenness of the floor surface,
   The controller is
   When the image recognition unit recognizes that there is unevenness on the travel path, the weight of the feedback gain used for the first control is reduced and used for the second control as compared with the case of traveling on a flat floor surface. The two-wheeled vehicle according to any one of claims 1 to 5, wherein weighting of the feedback gain is increased.
7. A method for controlling a two-wheeled mobile body in which wheels installed on a pair of legs rotatable around one end are rotated by a wheel drive motor and the steering angle of the wheel is adjusted by a steering drive motor, respectively.
   When shifting from one to the other among the parallel two-wheel mode in which each of the wheels is substantially coaxial and the bicycle mode in which the axis of the wheel is shifted and the surface direction is substantially matched,
   The first control for stabilizing the posture by accelerating / decelerating the wheel drive motor and the second control for stabilizing the posture by adjusting the rotation angle of the steer drive motor are performed together, and the rotation of the wheel is performed. A control method for a two-wheeled vehicle, comprising: executing an intermediate mode in which a weighting of a feedback gain used for the first control and the second control is smoothly changed according to speed.
8. Information indicating the positional relationship between the wheel and the upper body on which the one end of the leg is installed based on the connection relationship between the plurality of devices including the leg and the wheel and the target leg opening angle of the leg. The method for controlling a two-wheeled vehicle according to claim 7, wherein the intermediate mode is executed based on the calculated rotation speed of the wheel and information indicating the positional relationship.
9. Based on (Formula 11), the weighting element E ₂ is calculated,
   The ratio of the first weight value and the second weight value is 1: E ₂ ,
   By multiplying the feedback gain of the parallel two-wheel mode by the first weight value, the feedback gain of the first control is calculated,
   The method for controlling a two-wheeled vehicle according to claim 8, wherein the feedback gain of the second control is calculated by multiplying the feedback gain of the bicycle mode by the second weighting value.
   Here, m ₁ is the mass of the two-wheel moving body excluding the wheel, r is the radius of the wheel, h is the distance between the straight line connecting the centers of the wheels and the center of gravity of the two-wheel moving body, and ζ is the wheel A rotation angle, L is a distance between the centers of the wheels, ξ is an angle formed by a straight line connecting the centers of the wheels and the front direction of the upper body, and δ is an average value of the steering angles of the wheels.
   

   
10. A weighting element E ₁ is calculated based on (Equation 10),
    The method for controlling a two-wheeled vehicle according to claim 9, wherein the first weighting value is (1 / E ₁ + E ₂ ) and the second weighting value is (E ₂ / E ₁ + E ₂ ). .
    Here, K is a constant, ξ is an angle formed by a straight line connecting the centers of the wheels and the front direction of the upper body, and δ is an average value of the steering angles of the wheels.
    

    
11. When executing the intermediate mode,
    Steering angle for following to move the actual path along the target trajectory,
    Calculate the steer angle for posture stabilization in order to stabilize the posture by generating centrifugal force,
    The two-wheeled vehicle according to claim 9, wherein the steering angle for stabilizing the posture is calculated using a new feedback gain obtained by multiplying the feedback gain in the bicycle mode by the second weighting value. Control method.
12. Based on the image information input from the imaging means, when it is recognized that there is unevenness on the travel path, the weight of the feedback gain used for the first control is reduced as compared with the case of traveling on a flat floor surface, The method for controlling a two-wheeled vehicle according to any one of claims 8 to 11, wherein the weighting of the feedback gain used for the second control is increased.

Images