JP5261081B2 - Inverted wheel type moving body and control method thereof - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an inverted wheel type mobile body having a controller with excellent controllability and traveling property, and a method for controlling the same. <P>SOLUTION: A control unit 80 of the inverted wheel type mobile body 100 calculates the posture deviation between the acquired angle of inclination and the acquired inclined angular velocity and the target angle of inclination and the target inclined angular velocity, the wheel deviation between the driving amount acquired by a first measurement unit and the target driving amount, the slider deviation between the driving amount acquired by a second measurement unit and the target driving amount, and the arm deviation between the driving amount acquired by a third measurement unit and the target driving amount. and further calculates the wheel torque command for driving the wheels, the slider force command for driving the slider mechanism, and the arm torque command for driving the arm mechanism by multiplying the four calculated deviations by the predetermined gain. <P>COPYRIGHT: (C)2010,JPO&amp;INPIT

Description

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

An inverted wheel type moving body such as an inverted two-wheeled vehicle is normally controlled to move while correcting the center of gravity so as to maintain the stable state by driving the left and right drive wheels. Furthermore, in order to stabilize an inverted state, the structure which drives the inertial body provided above the wheel is disclosed (patent document 1). In this inverted wheel type moving body, the inertial body is slid and moved during traveling. Thereby, since a gravity center position moves rapidly on the vertical line of an axle, inversion can be stabilized.
JP 2006-205839 A

  However, in the prior art, the controller design of the inverted wheel type mobile body having such a multi-degree of freedom is designed by focusing on the operation of each reason and combining the designed control controllers. It was done by doing. For this reason, the conventional control controller has a problem that the operations of the respective degrees of freedom interfere with each other, and good operation control cannot be realized unless the operation plan is appropriately adjusted. In addition, there is a problem that the control performance of each control controller cannot be sufficiently exhibited by being too conscious of the interference of each degree of freedom.

  The inverted wheel type moving body described in Patent Document 1 includes a control controller that controls a wheel in the cart translation direction and a control controller that controls a control arm that can slide in the cart translation direction. For example, when the inverted wheel type moving body described in Patent Document 1 performs acceleration / deceleration, it is necessary to appropriately control the operation of the control arm in accordance with the torque command value of the wheel for performing acceleration / deceleration. However, in the inverted wheel type moving body described in Patent Document 1, since each control controller is independent of each other, there is still another arithmetic unit (CPU or the like) for performing the control of the wheel and the control arm in a coordinated manner. Necessary.

  The present invention designs a control controller capable of realizing good motion control for a controlled object having multiple degrees of freedom, and has an inverted wheel type moving body having the control controller excellent in controllability and traveling performance, and control thereof It aims to provide a method.

  The inverted wheel type moving body according to the present invention is rotatably supported with respect to the chassis via a chassis that rotatably supports the wheel, a first drive unit that rotationally drives the wheel, and a support member. A vehicle body portion, a second drive portion that drives the vehicle body portion, a third drive portion that changes the height of the vehicle body portion, the first drive portion, the second drive portion, A control unit that controls the three driving units, a posture inclination measuring unit that acquires an inclination angle and an inclination angular velocity with respect to a vertical direction of the vehicle body unit, and a first measuring unit that acquires a driving amount of the first driving unit; An inverted wheel type moving body comprising: a second measuring unit that obtains the driving amount of the second driving unit; and a third measuring unit that obtains the driving amount of the third driving unit, The control unit obtains the tilt angle, the tilt angular velocity, the target tilt angle, Attitude deviation from target inclination angular velocity, first deviation between drive amount and target drive amount acquired by first measurement unit, second difference between drive amount and target drive amount acquired by second measurement unit A deviation and a third deviation between the drive amount acquired by the third measurement unit and the target drive amount are calculated, and the calculated four deviations are multiplied by a predetermined gain to drive the first drive unit. A first control amount, a second control amount for driving the second drive unit, and a third control amount for driving the third drive unit.

  As described above, the control relating to the first drive unit, the second drive unit, and the third drive unit is realized by a single controller, which is favorable for an inverted wheel type moving body having multiple degrees of freedom. Operation control can be realized.

  Further, by including an integral calculation value in the third deviation, gravity compensation for the third drive unit can be performed. Furthermore, by including an integral calculation value in the posture deviation, it is possible to eliminate a steady deviation related to the posture deviation. In addition, the control unit calculates a feedforward amount that compensates for a nonlinear load due to acceleration including gravity, and the calculated feedforward amount is used as the first control amount, the second control amount, and the third control amount. By adding to the control amount, nonlinear terms including gravity can be compensated.

  Furthermore, a plurality of gain patterns of the gain may be set in advance, and the gain pattern may be switched according to the driving amount of the third driving unit acquired by the third measuring unit. Thus, even with a single control controller, it is possible to suppress a decrease in control performance by switching the gain pattern of the gain according to the driving amount having a large influence.

  In addition, the third drive unit includes an arm mechanism that extends and contracts in a vertical direction while supporting the vehicle body, and the third measurement unit determines the amount of change in the arm mechanism as the third drive unit. It is preferable to acquire as the driving amount.

  Furthermore, the second drive unit has a slide mechanism that slides the vehicle body part back and forth, and the third drive unit has an arm mechanism that extends and contracts in the vertical direction while supporting the vehicle body unit, The control unit outputs a wheel torque command as the calculated first control amount to the wheel, outputs a slide force command as the calculated second control amount to the slide mechanism, and calculates the calculated third It is preferable to output an arm torque command as a control amount to the arm mechanism.

  The control method for an inverted wheel type moving body according to the present invention is a vehicle body that rotatably supports a wheel, a first drive unit that rotationally drives the wheel, and a rotatable member with respect to the vehicle body via a support member. And a third drive unit for changing the height of the vehicle body part, and a method for controlling an inverted wheel type moving body comprising: a vehicle body part supported by the vehicle body; a second drive part that drives the vehicle body part; and a third drive part that changes a height of the vehicle body part. , Obtaining an inclination angle and an inclination angular velocity with respect to a vertical direction of the vehicle body, and obtaining a driving amount of the first driving unit, a driving amount of the second driving unit, and a driving amount of the third driving unit. , Attitude deviation between the acquired inclination angle and inclination angular velocity and target inclination angle and target inclination angular velocity, a first deviation between the acquired driving amount and target driving amount, and the acquired driving amount and target driving amount. A second deviation and a third of the acquired drive amount and target drive amount Calculating a deviation, multiplying the calculated four deviations by a predetermined gain, a first control amount for driving the first drive unit, a second control amount for driving the second drive unit, and A third control amount for driving the third drive unit is calculated.

  As described above, the control relating to the first drive unit, the second drive unit, and the third drive unit is realized by a single controller, which is favorable for an inverted wheel type moving body having multiple degrees of freedom. Operation control can be realized.

  ADVANTAGE OF THE INVENTION According to this invention, the inverted wheel type mobile body which can implement | achieve favorable operation control with respect to the control object which has many degrees of freedom, and its control method can be provided.

Embodiment 1 of the Invention
The moving body according to the present embodiment is an inverted wheel type moving body that moves by the inverted pendulum control. The moving body moves to a predetermined position by driving a wheel grounded on the ground. Furthermore, the inverted state can be maintained by driving the wheel according to the output from the gyro sensor or the like. Further, the moving body moves according to the operation amount operated by the operator while maintaining the inverted state.

  The configuration of the moving body 100 according to the present embodiment will be described with reference to FIGS. 1 and 2. FIG. 1 is a side view schematically showing the configuration of the moving body 100, and FIG. 2 is a front view schematically showing the configuration of the moving body 100.

  As shown in FIG. 2, the moving body 100 is an inverted wheel type moving body (running body), and includes a right driving wheel 18, a left driving wheel 20, a right swing arm 17, a left swing arm 19, A vehicle body 12. The vehicle body 12 is a part of the upper body portion of the moving body 100 disposed above the right drive wheel 18 and the left drive wheel 20. Here, the traveling direction of the moving body 100 (perpendicular to the paper surface of FIG. 2) is defined as the front-rear direction, and the direction perpendicular to the front-rear direction on the horizontal plane is defined as the left-right direction (lateral direction). Therefore, FIG. 2 is a view of the moving body 100 viewed from the front side in the traveling direction, and FIG. 1 is a view of the moving body 100 viewed from the left side.

  During traveling, the right swing arm 17 and the left swing arm 19 adjust the vehicle height. Furthermore, one or both swing arms are driven to adjust the left and right inclination angles of the vehicle body 12 with respect to the ground. For example, it is assumed that only the right drive wheel 18 rides on a step while traveling on a horizontal ground, or the ground changes to an upwardly inclined surface. In this case, the right drive wheel 18 is higher than the left drive wheel 20. For this reason, the joint of the right swing arm 17 is driven so that the right driving wheel 18 is brought closer to the direction of the vehicle body 12. As a result, the height of the right drive wheel 18 can be absorbed, and the vehicle body 12 can be leveled in the lateral direction (left-right direction). That is, the right swing arm 17 and the left swing arm 19 correspond to a third drive unit.

  A right mount 26 is fixed to the right swing arm 17 side end side, and the right drive wheel 18 is rotatably supported via an axle 30. The right drive wheel 18 is fixed to the rotation shaft C <b> 1 of the right wheel drive motor 34 via the axle 30. The right wheel drive motor 34 is fixed in the right mount 26 and functions as a wheel drive unit (actuator).

  A left mount 28 is fixed to the side end side of the left swing arm 19 and supports the left driving wheel 20 via an axle 32 so as to be rotatable. The left drive wheel 20 is fixed to the rotation shaft C <b> 2 of the left wheel drive motor 36 via the axle 32. The left wheel drive motor 36 is fixed in the left mount 28 and functions as a wheel drive unit (actuator). The right driving wheel 18 and the left driving wheel 20 are a pair of wheels that are in contact with the ground and rotate substantially coaxially.

  As the right driving wheel 18 and the left driving wheel 20 rotate, the moving body 100 moves. Further, the right wheel drive motor 34 and the left wheel drive motor 36 are drive wheel motors for driving the wheels. That is, the right wheel drive motor 34 and the left wheel drive motor 36 correspond to the first drive unit. The right mount 26 and the left mount 28 serve as a chassis that rotatably supports the left and right drive wheels.

  The right wheel drive motor 34 and the left wheel drive motor 36 (hereinafter also referred to as motors 34 and 36) are, for example, servo motors. The wheel actuator is not limited to an electric motor, and may be an actuator using pneumatic pressure or hydraulic pressure. In the following description, the right driving wheel 18 and the left driving wheel 20 may be collectively referred to as driving wheels.

  The right mount 26 includes a right wheel encoder 52. The right wheel encoder 52 detects a rotation angle as a rotation amount of the right drive wheel 18. The left mount 28 includes a left wheel encoder 54. The left wheel encoder 54 detects a rotation angle as a rotation amount of the left drive wheel 20.

  The right swing arm 17 includes an upper right link 21, a right swing shaft 62, and a right swing arm drive motor 60. The left swing arm 19 includes an upper left link 22, a left swing shaft 66, and a left swing arm drive motor 64. An upper right link 21 and an upper left link 22 are fixed to the lower portion of the vehicle body 12. A right swing arm drive motor 60 is fixed to the upper right link 21 and drives the right swing arm 17 around the rotation axis C4 via the right swing shaft 62. A left swing arm drive motor 64 is fixed to the left swing shaft 66, and drives the left swing arm 19 around the rotation axis C5 via the left swing shaft 66. Thus, the right swing arm 17 is provided with a rotary joint that rotates about the rotation axis C4, and the left swing arm 19 is provided with a rotary joint that rotates about the rotation axis C5. The joints provided on the right swing arm 17 and the left swing arm 19 (hereinafter also referred to as swing arms 17 and 19) are referred to as swing arm joints.

  A passenger seat drive motor 70, a rack and pinion 72, a gyro sensor 48, and a passenger seat 74 are attached to the vehicle body 12. Further, an upper right link 21 and an upper left link 22 are attached to the vehicle body 12 so as to face each other.

  A rack and pinion 72 is provided near the center of the vehicle body 12. The rack of the rack and pinion is provided along the front-rear direction. The boarding seat 74 is supported by the rack and pinion 72. That is, the boarding seat 74 is attached to the vehicle body 12 via the rack and pinion 72. The passenger seat 74 has a shape of a chair on which a passenger can sit. Instead of the rack and pinion 72, it may be slid using a ball screw or the like.

  A passenger seat drive motor 70 is fixed to the upper portion of the vehicle body 12. The passenger seat 74 and the passenger seat drive motor 70 are connected by a rack and pinion 72. The passenger seat drive motor 70 rotates about the rotation axis C3. Thereby, a rotational force is applied to the pinion of the rack and pinion 72. The rotational motion of the passenger seat drive motor 70 is converted into a linear motion by the rack and pinion 72. That is, when the passenger seat drive motor 70 is driven, the position of the passenger seat 74 with respect to the vehicle body 12 slides back and forth. That is, the passenger seat drive motor 70 corresponds to the second drive unit. At this time, the position of the center of gravity of the passenger seat 74 and the occupant or the vehicle changes forward and backward with respect to the vehicle body 12. As a means for changing the position of the center of gravity of the passenger seat 74 and the occupant or the vehicle with respect to the vehicle body 12, in addition to the slide mechanism, a rotating shaft mechanism, a turning mechanism, or the like can be realized. . Further, the power of the passenger seat drive motor 70 may be transmitted to the passenger seat 74 via a gear, a belt, a pulley, or the like. Here, the entire structure that moves back and forth by the passenger seat drive motor 70 is referred to as a vehicle body portion 77. The vehicle body portion 77 includes a boarding seat 74, an operation module 46, and the like. Of course, when an actuator for driving the vehicle body 12 is provided, the vehicle body 12 is also included in the vehicle body portion 77. The passenger seat drive motor 70 is provided with an encoder (not shown) for measuring the slide position.

  The rotation axis C3 is parallel to the rotation axes C1 and C2, and is located above the rotation axes C1 and C2. A right swing arm 17 is provided between the rotation axis C3 and the rotation axis C1, and a left swing arm 19 is provided between the rotation axis C3 and the rotation axis C2. The right swing arm drive motor 60 rotates the right swing arm 17 around the rotation axis C4, and the left swing arm drive motor 64 rotates the left swing arm 19 around the rotation axis C5. During normal travel, the rotation axis C1 to the rotation axis C5 are horizontal.

  Furthermore, the moving body 100 is provided with two auxiliary wheels 51 in order to prevent the mobile body 100 from falling. The auxiliary wheel 51 is rotatably supported with respect to the auxiliary wheel support block 55. The auxiliary wheel support block 55 is attached to the vehicle body 12. Here, one auxiliary wheel 51 is disposed on the front side of the driving wheel, and the other auxiliary wheel 51 is disposed on the rear side of the driving wheel. The auxiliary wheel 51 is a driven wheel and rotates as the moving body 100 moves.

  The rotation axis of one auxiliary wheel 51 is located on the front side above the rotation axes C1 and C2, and the rotation axis of the other auxiliary wheel 51 is located on the rear side above the rotation axes C1 and C2. That is, one of the auxiliary wheels 51 is disposed in front of the axle of the driving wheel, and the other is disposed in the rear of the axle of the driving wheel. Thereby, it can prevent that the mobile body 100 falls forward and backward. In addition, you may prevent a fall by the fall prevention member other than an auxiliary wheel. For example, the fall can be prevented by a stopper or the like protruding in the front-rear direction.

  A battery module 44 and a sensor 58 are housed in the vehicle body 12. The sensor 58 is an optical obstacle detection sensor, for example, and outputs a detection signal when an obstacle is detected in front of the moving body 100. The sensor 58 may be a sensor other than the obstacle sensor. For example, an acceleration sensor can be used as the sensor 58. Of course, two or more sensors may be used as the sensor 58. The sensor 58 detects a change amount that changes in accordance with the state of the moving body 100. The battery module 44 has a sensor 58, a gyro sensor 48, a right wheel drive motor 34, a left wheel drive motor 36, a right swing arm drive motor 60, a left swing arm drive motor 64, a passenger seat drive motor 70, a control unit 80, and the like. Supply power.

  A gyro sensor 48 is provided on the vehicle body 12. The gyro sensor 48 detects an angular velocity with respect to the inclination angle of the vehicle body 12. Here, the inclination angle of the vehicle body 12 is a degree of inclination from the axis at which the center of gravity of the moving body 100 extends vertically above the axles 30 and 32. For example, the vehicle body 12 is inclined forward in the traveling direction of the moving body 100. The case is represented as “positive”, and the case where the vehicle body 12 is inclined rearward in the traveling direction of the moving body 100 is represented as “negative”. Therefore, when the vehicle body 12 is horizontal, the tilt angle is 0 °. During normal traveling, the control target value of the tilt angle is 0 °. Feedback control is performed so as to follow the control target value. Further, the inclination angle in the front-rear direction is set as the inclination angle of the posture of the moving body 100.

  In addition to the front-rear direction of the traveling direction, the tilt angular velocity in the left-right direction is measured using a three-axis gyro sensor 48 of roll, pitch, and yaw. Thus, the gyro sensor 48 measures the change in the tilt angle of the vehicle body 12 as the tilt angular velocity of the vehicle body 12. Of course, the gyro sensor 48 may be attached to another location. The tilt angular velocity measured by the gyro sensor 48 changes according to the change in the posture of the moving body 100. That is, the inclination angular velocity is a change amount that changes according to the position of the center of gravity of the vehicle body 12 with respect to the position of the axle. Therefore, when the inclination angle of the posture changes suddenly due to disturbance or the like, the value of the inclination angular velocity increases.

  An operation module 46 is provided on the side surface of the boarding seat 74. The operation module 46 is provided with an operation lever (not shown) and a brake lever (not shown). The operating lever is an operating member for the passenger to adjust the traveling speed and traveling direction of the moving body 100. The passenger adjusts the moving speed of the moving body 100 by adjusting the operation amount of the operating lever. Can do. Moreover, the passenger can specify the moving direction of the moving body 100 by adjusting the operating direction of the operating lever. The moving body 100 can make forward, stop, reverse, left turn, right turn, left turn, and right turn according to the operation applied to the operation lever. The moving body 100 can be braked when the passenger tilts the brake lever. The traveling direction of the moving body 100 is a direction perpendicular to the axles 30 and 32 in the horizontal plane. The operation module 46 is provided with a switch for switching the control mode.

  Further, a control unit 80 is mounted on the backrest portion of the passenger seat 74. The control unit 80 controls the right wheel drive motor 34 and the left wheel drive motor 36 in accordance with the operation performed by the occupant on the operation module 46, and controls the travel (movement) of the moving body 100. The control unit 80 controls the right wheel drive motor 34 and the left wheel drive motor 36 in accordance with an operation on the operation module. As a result, the right wheel drive motor 34 and the left wheel drive motor 36 are driven with the acceleration and speed command values according to the operation of the operation module 46.

  The control unit 80 includes a central processing unit (CPU), a read only memory (ROM), a random access memory (RAM), a communication interface, and the like, and controls various operations of the mobile unit 100. And this control part 80 performs various control according to the control program stored, for example in ROM. The control unit 80 performs robust control, state feedback control, PID control, or the like so as to achieve a desired acceleration and target speed according to an operation in the operation module 46, and so that the moving body 100 is maintained upside down. The right wheel drive motor 34 and the left wheel drive motor 36 are controlled by the known feedback control. As a result, the moving body 100 travels while accelerating / decelerating in accordance with the operation of the operation module 46.

  That is, the operation module 46 acquires the operation amount given by the passenger's operation, and outputs this operation amount to the control unit 80 as an operation signal. Then, the control unit 80 calculates a target acceleration and a target speed of the moving body 100 based on the operation signal, and feedback-controls the moving body 100 so as to follow the target acceleration and target speed. Thereby, the moving body 100 can be moved while being inverted.

  Further, the control unit 80 controls the right wheel drive motor 34, the left wheel drive motor 36, the right swing arm drive motor 60, the left swing arm drive motor 64, and the passenger seat drive motor 70. Here, the control unit 80 performs control so that the passenger seat drive motor 70 operates in cooperation with the right wheel drive motor 34 and the left wheel drive motor 36. That is, the driving wheel is rotated and the boarding seat 74 is slid so as to stabilize the inversion. Thereby, the inclination angle of the vehicle body 12 becomes small, and the inversion can be stabilized. In this manner, the passenger seat drive motor 70 operates in cooperation with the right swing arm drive motor 60, the left swing arm drive motor 64, and the passenger seat drive motor 70.

  Hereinafter, a method for calculating the control target value by the control unit 80 will be specifically described. FIG. 3 is a diagram illustrating definitions of variables used in the following description. FIG. 3A is a diagram conceptually illustrating the configuration of the moving object according to the present embodiment, and is a diagram illustrating the definition of variables related to length. FIG. 3B is a diagram conceptually showing the configuration of the moving object according to the present embodiment, and is a diagram showing the definition of variables related to angles.

As shown in FIG. 3A, with respect to the passenger seat 74 to slide the passenger seat drive motor 70, the center of gravity of the passenger seat 74 and m _p, the position of the center of gravity m _p from the origin O which is provided on the vehicle body 12, the x-axis direction slide position location _{P gx,} y-axis direction of the slide position location _{P gy,} shown as slide position location _{P gz} z-axis direction. Further, it provided the right swing arm 17 and the left swing arm 19 at a distance l _w from the origin O, the center of gravity m _A, shall be located at a distance l _a from the end portion of the swing arm. Further, the center of gravity of the vehicle body 12 and _{m B,} indicating the position of its center of gravity _{m B} from the origin O x axis direction of the distance _{B gx,} y-axis direction of the distance _{B gy,} as the distance _{B gz} in the z-axis direction. Moreover, the radius of the right driving wheel 18 and the left driving wheel 20 and _{R T,} the center of gravity _{m t,} provided from the center of gravity _{m A} of the swing arm at a distance _{l A.}

As shown in FIG. 3B, the inclination angle of the vehicle body 12 with respect to the vertical direction is θ _by , the rotation angles of the right swing arm 17 and the left swing arm 19 with respect to the horizontal direction are θ _ar and θ _al , the right driving wheel 18 and the left driving wheel, respectively. The rotation angles of 20 are indicated as θ _tr and θ _tl respectively.

尚、数１におけるｆ_ｉｊ及びｇ_ｉは、以下の数２により示される関数ｆ及びｇを用いて計算される。関数ｆは、状態量に基づいて、係数ｆ_ｉｊを計算する。関数ｇは、状態量に基づいて、重力を含めた各非線形項ｇ_ｉを計算する。

The moving body shown in FIG. 3 includes a total of two axes of the right drive wheel 18 and the left drive wheel 20, two axes of the right swing arm 17 and the left swing arm 19, and one axis of the slide mechanism of the passenger seat drive motor 70. This is a three-dimensional five-axis model having five axes of freedom. For the model shown in FIG. 3, the following five balanced equations shown in Equation 1 can be obtained. Equation 1 shows the inverse dynamics for the model of FIG. Τ _tr is the torque of the right drive wheel 18, τ _tl is the torque of the left drive wheel 20, τ _ar is the torque of the right swing arm 17, τ _al is the torque of the left swing arm 19, and F _p is the passenger seat drive motor 70. It shows the slide force acting on the slide mechanism. A dot (•) on the variable indicates a first-order derivative with respect to time, and a double dot (••) indicates a second-order derivative with respect to time. That is, θ _by ^(•) is the tilt angular velocity of the vehicle body 12, θ _ar ^(•) is the rotational angular velocity of the right swing arm 17, θ _al ^(•) is the rotational angular velocity of the left swing arm 19, and θ _tr ^(•) is the right drive. rotational angular velocity of the wheel 18, θ _tl ^(·) is the rotational angular velocity of the left driving wheel _{20, P} ^{gx (·)} indicates the moving speed of the slide. θ _by ^(··) is the inclination angular acceleration of the vehicle body 12, θ _ar ^(··) is the rotational angular acceleration of the right swing arm 17, θ _al ^(··) is the rotational angular acceleration of the left swing arm 19, and θ _tr ^{(· -)} rotation angular acceleration of the right driving wheel 18, θ _tl ^(··) is rotational angular acceleration of the left driving wheel _{20, P} ^{gx (··)} shows the movement acceleration of the slide.

Note that f _ij and g _i in _Equation 1 are calculated using functions f and g shown by Equation 2 below. The function f calculates a coefficient f _ij based on the state quantity. Function g, on the basis of the state quantity, calculates the respective nonlinear terms g _i, including gravity.

ここで、ｇ´を関数ｇに線形化する。以下の数４から、θ_ｂｙとＰ_ｘ０の比例項を含めた関数とする（θ_ｂｙ０とＰ_ｘ０は、それぞれ線形化原点を示す）。また、擬似逆行列Ｈ_１及び行列Ｈ_２は以下の数５により示され、行列中の各要素ｈ_ｉｊは、数１を変形することにより計算することができる。

Inclination angle theta _By the vehicle body 12 shown in Expression _1, and a slide position location P _gx linearized, by using the pseudo-inverse matrix H ₁ and matrix H _2, represented by the matrix format showing the number 1 in the following equation (3) can do.

Here, g ′ is linearized into a function g. From the following equation 4, a function including a proportional term of θ _by and P _x0 is assumed (θ _by0 and P _x0 indicate the linearization origin, respectively). Further, the pseudo inverse matrix H ₁ and the matrix H ₂ are expressed by the following formula 5, and each element h _ij in the matrix can be calculated by _modifying the formula 1.

ここで、状態変数ｘ、制御入力ｕ、行列Ａ、及び行列Ｂを、以下の数７に示す。尚、Ｉ_θａｌは左スイングアーム１９の回転角度θ_ａｌの時間積分値、Ｉ_θａｒは右スイングアーム１７の回転角度θ_ａｒの時間積分値、Ｉ_θｂｙは車体１２の傾斜角θ_ｂｙの時間積分値をそれぞれ示す。また、行列Ａ及び行列Ｂの要素のうち、値を表示していない要素の値は０である。

本実施の形態では、１３個の状態量（Ｉ_θａｌ、Ｉ_θａｒ、Ｉ_θｂｙ、θ_ａｌ、θ_ａｒ、θ_ｂｙ、Ｐ_ｇｘ、θ_ｔｌ ^（・）、θ_ｔｒ ^（・）、θ_ａｌ ^（・）、θ_ａｒ ^（・）、θ_ｂｙ ^（・）、Ｐ_ｇｘ ^（・））を用いて、多自由度を有する移動体の制御系であって、その拡大系を生成する。ここで、状態量に積分値を追加することで、以下のような効果を得ることができる。即ち、スイングアームの回転角度に関する時間積分値Ｉ_θａｌ及びＩ_θａｒを追加することで、スイングアームの重力補償が可能になる。また、車体１２の傾斜角に関する時間積分値Ｉ_θｂｙを追加することで、傾斜角について生じる定常偏差を解消することができる。尚、ここでは、傾斜角の時間積分値Ｉ_θｂｙを０に収束させやすくするために、スライド位置に関する時間積分値を設けていない。 The model of the moving body shown in FIG. 3 is expressed by the following equation (6).

Here, the state variable x, the control input u, the matrix A, and the matrix B are shown in Equation 7 below. _{Incidentally,} I θal the rotation angle theta time integral of _al of the left swing arm _19, I θar the time integration value of the rotational angle theta _ar right swing arm _17, I θby the inclination angle theta _By the time integral value of the vehicle body 12 Respectively. In addition, among the elements of the matrix A and the matrix B, the value of the element not displaying the value is 0.

In the present embodiment, 13 state quantities (I _θal , I _θar , I _θby , θ _al , θ _ar , θ _by , P _gx , θ _tl ^(•) , θ _tr ^(•) , θ _al ^(•) , Θ _ar ^(•) , θ _by ^(•) , P _gx ^(•) ), a control system for a moving body having multiple degrees of freedom, and an expanded system thereof is generated. Here, by adding an integral value to the state quantity, the following effects can be obtained. That is, by adding the time integral values I _θal and I _θar related to the rotation angle of the swing arm, the gravity compensation of the swing arm can be performed. _Further, by adding the time integral value I _θby related to the tilt angle of the vehicle body 12, it is possible to eliminate the steady deviation that occurs with respect to the tilt angle. Here, in order to the time integral value I _Shitaby tilt angle tends to converge to 0, it is not provided time integration values for slide position.

Next, the control system is designed based on the state equation of the system shown in Equations 6 and 7. Here, as shown in the following equation 8, using the function lqr, designing the feedback gain K _L of the linear feedback controller. The feedback gain K _L is a 5 × 13 matrix, and the control input u is calculated by multiplying the 13 state quantities described above by the gain matrix K _L. The control input u is configured a torque of the right driving wheel 18, the torque of left driving wheel 20, the torque of the right swing arm 17, the torque of the left swing arm 19, the slide force acting on the sliding mechanism of the passenger seat drive motor 70 The The function lqr is a function to design the feedback gain K _L by optimal regulator. That is, the optimum regulator is a method for determining a state feedback gain matrix K that minimizes a quadratic evaluation function. A and B are matrices shown in Equation 7. Q and R are positive definite matrices used for the evaluation function and are determined by simulation. Moreover, the design method of the state feedback gain K _L is not limited to the optimal regulator may alternatively be designed by known control theory, such as pole placement method.

Furthermore, in the control system according to the present embodiment, a nonlinear load due to acceleration including gravity is compensated by adding a nonlinear compensation term g ′ to the control input u as shown in the following equation (9). That is, by giving g in a feed forward manner, the operation of the moving body can be made smooth. Note that x _ref represents the target value of the state variable x, and x _now represents the measured value of the state variable x. e _x represents the deviation of the state quantity.

  Further, in the control system according to the present embodiment, it is possible to suppress a decrease in control performance by setting a plurality of gain patterns of feedback gain in advance and switching the feedback gain according to the driving amount having a large influence. Thereby, even if it is a single control controller, the fall of control performance can be suppressed by switching the gain pattern of a gain according to the drive amount with a large influence degree.

  In the above-described three-dimensional model, the effect of vertical movement (change in the height of the vehicle body by the swing arm) and the effect of forward / backward movement (sliding change of the passenger seat due to movement of the slide mechanism) are determined from the posture acceleration graph. . Since the moving body according to the present embodiment has a particularly large influence due to vertical movement, a configuration in which the feedback gain of the control controller is switched according to the arm angle of the swing arm will be described below.

  FIG. 4 is a conceptual diagram showing the influence of a change in the rotation angle of the swing arm on the tilt angle acceleration of the vehicle body when a constant wheel torque is applied to the drive wheels. As shown in FIG. 4, when the arm angle of the swing arm is small, the position of the center of gravity of the vehicle body is lowered, so that the moment of inertia around the center of gravity of the moving body is small. For this reason, if it is going to control the movement by the same wheel torque, responsiveness will become high. That is, the wheel torque required to maintain the posture of the moving body in a predetermined state is reduced as compared with the case where the arm angle of the swing arm is large. On the other hand, when the arm angle of the swing arm is large, the position of the center of gravity of the vehicle body becomes high, so that the moment of inertia around the center of gravity of the moving body is large. For this reason, responsiveness becomes low with respect to the same wheel torque. Therefore, the position of the center of gravity of the vehicle body changes with an increase in the rotation angle of the swing arm, so that the influence of wheel torque related to posture control cannot be ignored. For this reason, posture control is changed according to the rotation angle of the swing arm.

In the moving body according to the present embodiment, in particular, the difference in the rotation angle between the left and right swing arms (shift in the posture in the roll direction) affects the inversion control. For this reason, the control part 80 suppresses the fall of the control performance of inversion control by switching a feedback gain according to the rotation angle of each swing arm. FIG. 5 is a gain map showing the relationship between the rotation angle of each swing arm and the feedback gain of the controller. When the movable range of the rotation angle of each swing arm is, for example, 0 deg to 70 deg, the rotation angle of each swing arm is divided into a plurality of regions in a predetermined unit. Then, for each phase of the rotation angle of the swing arm divided, and calculates the feedback gain K _L by the method described above. In FIG divides the rotation angle of the swing arm at 10deg units, and calculates the feedback gain K _L with respect to the rotation angle of the swing arm 8 stages. Furthermore, the feedback gain K _L between each stage is calculated by linear interpolation from the values of the feedback gain K _L for each phase calculated. That is, the gain map is composed of a plurality of gain patterns of feedback gains calculated in this way. The control unit 80 switches the feedback gain according to the combination of the detected rotation angles of the swing arms, and calculates the control amount using the switched feedback gain.

  Note that the embodiment for switching the feedback gain with reference to the gain map is not limited to the case of switching according to the rotation angle of the swing arm. FIG. 6 is a diagram showing an embodiment in which the feedback gain is switched according to the drive amount other than the rotation angle of the swing arm. 6A and 6B are conceptual diagrams showing the moving body from the front in the traveling direction, and FIG. 6C is a conceptual diagram showing the moving body from the side in the traveling direction. For example, as shown in FIG. 6A, the feedback gain may be switched according to a change in the width of the drive wheel. Further, as shown in FIG. 6B, for example, the feedback gain may be switched in accordance with a change in a vertical movement mechanism that changes the boarding seat up and down. Further, as shown in FIG. 6C, the feedback gain may be switched in accordance with a change in the forward / backward movement mechanism (slide mechanism).

  Next, the configuration of the control system of the moving body that performs the above control will be described with reference to FIG. FIG. 7 is a block diagram illustrating a configuration of a control system including the control unit 80. The control unit 80 includes a cooperative control unit 91, a command value generation unit 92, and a nonlinear compensation unit 93 as a control controller designed as described above. The moving body 100 is provided with an amplifier that drives and controls each motor. Here, the amplifiers provided in the right wheel drive motor 34, the left wheel drive motor 36, the right swing arm drive motor 60, the left swing arm drive motor 64, and the passenger seat drive motor 70 are an amplifier 34a, an amplifier 36a, and an amplifier 60a, respectively. , Amplifier 64a and amplifier 70a. Each amplifier operates based on a control signal from the cooperative control unit 91.

  The sensors 83 indicate various sensors provided in the moving body 100, and include, for example, a gyro sensor 48, a right wheel encoder 52, a left wheel encoder 54, a sensor 58, and the like. That is, the sensors 83 include a posture inclination measuring unit, a first measuring unit, a second measuring unit, and a third measuring unit.

The command value generation unit 92 performs an inversion control calculation and calculates a control target value for driving each driving unit. Then, a deviation between the control target value and the current value is obtained by the subtracter. The current value can be calculated based on the output from the sensors 83, for example. The cooperative control unit 92, by multiplying the feedback gain K _L as described above to this deviation, feedback control. In the inversion control calculation, for example, the tilt angle and the tilt angular velocity of the vehicle body 12 are calculated as control target values. The inclination angular velocity of the vehicle body 12 is measured by the gyro sensor 48. Then, the inclination angle of the vehicle body 12 is obtained by integrating the inclination angular velocity. For example, during inverted traveling, feedback control is performed so that the target inclination angle of the posture becomes 0 °. When stopping on the spot, feedback control is performed so that the target inclination angular velocity becomes zero.

  More specifically, the command value generation unit 92 generates a target inclination angle and a target inclination angular speed, a target wheel rotation angle and a target wheel rotation angular speed, a target slide position and a target slide speed, a target swing arm angle and a target swing arm speed. To do. Using the subtractor, the attitude deviation between the inclination angle and inclination angular velocity obtained from the detection value of the sensors 83 and the target inclination angle and target inclination angular velocity, the wheel rotation angle and wheel rotation angular velocity obtained from the detection value of the sensors 83, The first deviation between the target wheel rotation angle and the target wheel rotation angular speed, the slide position and the second deviation between the slide speed and the target slide position and the target slide speed acquired from the detection values of the sensors 83, the detection of the sensors 83 A third deviation between the swing arm angle and swing arm speed acquired from the value and the target swing arm angle and target swing arm speed is calculated.

  The deviation for calculating the control amount related to the swing arm may include an integral calculation value. That is, an integrated value calculated by integrating the deviation between the swing arm angle acquired from the detection value of the sensor 83 and the target swing arm angle may be further input to the cooperative control unit 92. Thereby, the gravity compensation regarding a swing arm can be performed. Further, the posture deviation may include an integral calculation value. That is, an integrated value calculated by integrating the deviation between the inclination angle acquired from the detection value of the sensor 83 and the target inclination angle may be further input to the cooperative control unit 92. Thereby, the gravity compensation regarding a swing arm can be performed. Thereby, the steady-state deviation regarding the attitude deviation can be eliminated.

  The cooperative control unit 92 multiplies each deviation calculated as described above by a feedback gain, and slide force, right swing arm torque, left swing arm torque, right wheel torque, left wheel as control amounts for driving each drive unit. Calculate wheel torque. The cooperative control unit 92 outputs the calculated control amounts as control signals, and the right wheel drive motor 34, the left wheel drive motor 36, the right swing arm drive motor 60, the left swing arm drive motor 64, and the passenger seat drive motor 70. Are coordinated and controlled.

  More specifically, the cooperative control unit 92 outputs a control signal to drive the right wheel drive motor 34 and the left wheel drive motor 36 so as to travel stably while maintaining an inverted state. The cooperative control unit 92 outputs the wheel torque for the right wheel drive motor 34 and the left wheel drive motor 36 as a command value. That is, a control signal corresponding to the wheel torque is output to the amplifiers 34a and 36a of the motors 34 and 36. A control signal from the control unit 80 is input to the right wheel drive motor 34 and the left wheel drive motor 36 via the amplifiers 34a and 36a, and the right wheel drive motor 34 and the left wheel drive motor 36 are driven.

  Further, the cooperative control unit 92 outputs a control signal, and drives the swing arm joint 67 so that the swing arm extends and contracts in the vertical direction while supporting the vehicle body. The cooperative control unit 92 outputs the right swing arm torque and the left swing arm torque for the right wheel drive motor 34 and the left wheel drive motor 36 as command values. Thereby, it is possible to switch between a grounded state where the auxiliary wheel 51 is grounded and a grounded state where the auxiliary wheel 51 is grounded. When traveling on an inclined surface, a control signal is output based on the output of the gyro sensor 48 or the like. Thereby, the angle of the inclined surface is absorbed and the vehicle body 12 becomes horizontal. A control signal from the control unit 80 is input to the right swing arm drive motor 60 and the left swing arm drive motor 64 via the amplifiers 60a and 64a, and the right swing arm drive motor 60 and the left swing arm drive motor 64 are driven. .

  Further, the cooperative control unit 92 outputs a control signal to drive the passenger seat drive motor 70 so that the slide slides the vehicle body part back and forth. The cooperative control unit 92 outputs the force of the passenger seat drive motor 70 to the amplifier 70a as a command value. A control signal from the control unit 80 is input to the passenger seat drive motor 70 via the amplifier 70a, and the passenger seat drive motor 70 is driven.

The rotational speed of the drive wheel 78 can be obtained from the outputs of the right wheel encoder 52 and the left wheel encoder 54. The position of the swing arm and the speed of the swing arm can be obtained from the output of an encoder provided at each joint of the swing arm. The position of the slide of the passenger seat 74, the speed of the slide can be determined by the output of the encoder provided in the passenger seat drive motor 70. Further, the sliding force of the sliding mechanism 68 can be obtained from the rotational torque of the passenger seat drive motor 70.

  The cooperative control unit 92 switches the gain pattern according to the swing arm angle acquired from the detection values of the sensors 83 from a plurality of feedback gain gain patterns set in advance. Thereby, even if it is a single control controller, the fall of control performance can be suppressed by switching the gain pattern of a gain according to the drive amount with a large influence degree.

  The nonlinear compensator 93 calculates a feedforward amount that compensates for a nonlinear load due to acceleration including gravity. The calculated feedforward amount is added to the control amount calculated by the cooperative control unit 92 by an adder. This makes it possible to compensate for nonlinear terms including friction.

  As described above, by realizing the control related to the wheels, the swing arm, and the slide by the cooperative control unit 92, it is possible to realize good operation control even for the inverted wheel type moving body having multiple degrees of freedom.

  It should be noted that the present invention is not limited to the above-described embodiment, and various modifications can be made without departing from the gist of the present invention already described.

It is a side view which shows the structure of the moving body concerning embodiment of this invention. It is a front view which shows the structure of the mobile body concerning embodiment of this invention. It is a figure for demonstrating the calculation method of the control target value by the moving body concerning embodiment of this invention. It is a figure which shows the model of the mobile body concerning embodiment of this invention. It is a figure which shows an example of the gain map concerning embodiment of this invention. It is a figure which shows the Example which switches a feedback gain according to drive amounts other than the rotation angle of a swing arm. It is a block diagram which shows the structure of the control system of the moving body concerning embodiment of this invention.

Explanation of symbols

12 body, 17 right swing arm, 19 left swing arm,
18 right drive wheel, 20 left drive wheel, 21 upper right link, 22 upper left link,
26 Right mount, 28 Left mount,
30 axles, 32 axles, 34 right wheel drive motor, 36 left wheel drive motor,
41 main body, 42 operation lever, 43 operation angle sensor, 44 battery module,
46 operation module, 48 gyro sensor, 51 auxiliary wheel,
52 right wheel encoder, 54 left wheel encoder, 55 auxiliary wheel support block,
58 sensor, 60 right swing arm drive motor, 62 right swing shaft 64 left swing arm drive motor, 66 left swing shaft 67 swing arm joint, 68 slide mechanism,
70 Boarding seat drive motor, 72 Rack and pinion, 74 Boarding seat,
76 upper body part, 77 car body part, 78 drive wheel,
80 control units, 83 sensors,
91 cooperative control unit, 92 command value generation unit, 93 nonlinear compensation unit,
100 mobile,

Claims (8)

A chassis that rotatably supports the left and right wheels respectively provided on the left and right,
A first drive unit corresponding to left and right drive motors that respectively rotate and drive the left and right wheels;
A vehicle body part rotatably supported with respect to the chassis via a support member;
A second drive unit corresponding to a passenger drive motor that slides the position of the passenger seat included in the vehicle body part back and forth;
An arm mechanism that extends and contracts in a vertical direction while supporting the vehicle body portion, and is provided on each of the left and right swing arms, provided on each of the left and right sides of the vehicle body, and provided with a rotation joint that rotates the arm around a rotation axis. A corresponding third drive,
A control unit for controlling the first drive unit, the second drive unit, and the third drive unit;
A posture inclination measuring unit for acquiring an inclination angle and an inclination angular velocity with respect to a vertical direction of the vehicle body part;
A first measuring unit for obtaining a driving amount of the first driving unit;
A second measuring unit for obtaining a driving amount of the second driving unit;
A third measuring unit that obtains a driving amount of the third driving unit, and an inverted wheel type moving body comprising:
The control unit is
A model of the inverted wheel type moving body is obtained by integrating time integrated values I _θal and I _θar with respect to the rotation angles θ _al and θ _ar of the left and right swing arms , and a time integrated value I _θby with respect to the inclination angle θ _by of the vehicle body , The rotational angles θ _al and θ _{ar of the} left and right swing arms , the inclination angle θ _by of the vehicle body, and the gravity center position of the passenger seat from the origin provided on the vehicle body, and the slide position P _gx in the x-axis direction Rotation angular velocities θ _tl ^(•) and θ _tr ^(•) of the left and right wheels, rotational angular velocities θ _al ^(•) and θ _ar ^{(•) of the} left and right swing arms ^, and the tilt angular velocity of the vehicle body, respectively. represents theta _by the ^_(-), and the slide position is first-order differential with respect to time of the P _gx sliding movement speed P _{gx ^(·),} the equation of state was used as the state quantity Is designed as a linear feedback controller based on the state equation,
Attitude deviation between the inclination angle and inclination angular velocity and the target inclination angle and target inclination angular velocity acquired by the attitude inclination measurement unit, a first deviation between the drive amount and target drive amount acquired by the first measurement unit, Calculating a second deviation between the driving amount and the target driving amount acquired by the second measuring unit, and a third deviation between the driving amount and the target driving amount acquired by the third measuring unit;
The calculated four deviations are multiplied by the feedback gain of the linear feedback controller, the first control amount for driving the first drive unit, the second control amount for driving the second drive unit, and the A third control amount for driving the third drive unit is calculated, a plurality of gain patterns of the feedback gain are set in advance, and the drive amount of the third drive unit acquired by the third measurement unit Depending on the, the gain pattern is switched,
Inverted wheel type moving body. The inverted wheel type moving body according to claim 1, wherein the third deviation includes an integral calculation value. The inverted wheel type moving body according to claim 1 or 2, wherein the first posture deviation includes an integral calculation value. The controller is
Calculating a feedforward amount that compensates for a non-linear load due to acceleration including gravity, and adding the calculated feedforward amount to the first control amount, the second control amount, and the third control amount. The inverted wheel type moving body according to any one of claims 1 to 3. A chassis that rotatably supports the left and right wheels respectively provided on the left and right,
A first drive unit corresponding to left and right drive motors that respectively rotate and drive the left and right wheels;
A vehicle body part rotatably supported with respect to the chassis via a support member;
A second drive unit corresponding to a passenger drive motor that slides the position of the passenger seat included in the vehicle body part back and forth;
An arm mechanism that extends and contracts in a vertical direction while supporting the vehicle body portion, and is provided on each of the left and right swing arms, provided on each of the left and right sides of the vehicle body, and provided with a rotation joint that rotates the arm around a rotation axis. A corresponding third drive,
A control unit for controlling the first drive unit, the second drive unit, and the third drive unit,
The control unit is
A model of the inverted wheel type moving body is obtained by integrating time integrated values I _θal and I _θar with respect to the rotation angles θ _al and θ _ar of the left and right swing arms , and a time integrated value I _θby with respect to the inclination angle θ _by of the vehicle body , The rotational angles θ _al and θ _{ar of the} left and right swing arms , the inclination angle θ _by of the vehicle body, and the gravity center position of the passenger seat from the origin provided on the vehicle body, and the slide position P _gx in the x-axis direction Rotation angular velocities θ _tl ^(•) and θ _tr ^(•) of the left and right wheels, rotational angular velocities θ _al ^(•) and θ _ar ^{(•) of the} left and right swing arms ^, and the tilt angular velocity of the vehicle body, respectively. represents theta _by the ^_(-), and the slide position is first-order differential with respect to time of the P _gx sliding movement speed P _{gx ^(·),} the equation of state was used as the state quantity A control method of the inverted wheel type moving body which is designed as a linear feedback controller based on the state equation,
Obtain the tilt angle and tilt angular velocity with respect to the vertical direction of the vehicle body,
Obtaining a driving amount of the first driving unit, a driving amount of the second driving unit, and a driving amount of the third driving unit;
Attitude deviation between the acquired inclination angle and inclination angular velocity and target inclination angle and target inclination angular velocity, the first deviation between the acquired driving amount of the first driving unit and the target driving amount, the acquired first Calculating a second deviation between the driving amount of the second driving unit and the target driving amount, and a third deviation between the acquired driving amount of the third driving unit and the target driving amount;
The calculated four deviations are multiplied by the feedback gain of the linear feedback controller, the first control amount for driving the first drive unit, the second control amount for driving the second drive unit, and the A third control amount for driving the third drive unit is calculated, a plurality of gain patterns of the feedback gain are set in advance, and the drive amount of the third drive unit acquired by the third measurement unit Depending on the, the gain pattern is switched,
Control method of an inverted wheel type moving body. The method of controlling an inverted wheel type moving body according to claim 5, wherein the third deviation includes an integral calculation value. The method for controlling an inverted wheel type moving body according to claim 5 or 6, wherein the first posture deviation includes an integral calculation value. Calculating a feedforward amount that compensates for a non-linear load due to acceleration including gravity, and adding the calculated feedforward amount to the first control amount, the second control amount, and the third control amount. The method for controlling an inverted wheel type moving body according to any one of claims 5 to 7.

Images