JP5316336B2 - Inverted type moving body, its control method and control program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an inverted moving body stably traveling; and to provide method and program for controlling the same. <P>SOLUTION: The inverted moving body 100 includes a moving body 140 having a load body 201 and a wheel 202 rotatably connected to the load body 201, and performs inversion control of the load body 201 by controlling the drive of the wheel 202. The inverted moving body 100 also includes: a detection means for detecting a state amount of the moving body 140; a command means for creating a command value to bring the moving body 140 into a desired traveling state; an estimation means for estimating a load wheel inter-gravity-center distance L that is the distance between the center of gravity of the load body 201 and the center of gravity of the wheel 202, based on the state amount detected by the detection means; and a control means for performing inversion control by creating a torque command value for driving the wheel 202 based on the state amount detected by the detection means, the command value created by the command means, and the load wheel inter-gravity-center distance L estimated by the estimation means. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to an inverted moving body that travels while maintaining an inverted state, a control method thereof, and a control program.

  2. Description of the Related Art An inverted moving body that travels while performing an inverted control that maintains an inverted state is known. For example, an inverted two-wheeled robot that travels autonomously while performing inverted control to maintain the inverted state is disclosed (see Patent Document 1). In this inverted two-wheeled robot, the operation of the inverted robot is controlled based on a linearized model obtained by linearizing an inverted robot to be controlled in the vicinity of a desired posture.

JP 2006-123014 A

  However, in the above-described inverted two-wheeled robot, the inverted robot vibrates when the position of the center of gravity changes greatly, for example, when an occupant boarding the inverted robot squats down or when the load to be loaded collapses. Problems such as erratic movements and instability and falling.

  The present invention has been made in view of such problems, and a main object of the present invention is to provide an inverted moving body that can travel more stably, a control method thereof, and a control program.

  One aspect of the present invention for achieving the above object includes a mobile body having a load body and a wheel rotatably connected to the load body, and controls driving of the wheel to control the load body. An inverted moving body that performs an inverted control, comprising: a detecting means for detecting a state quantity of the moving body main body; a command means for generating a command value for causing the moving body main body to enter a desired running state; and the detecting means. Based on the detected state quantity, an estimation means for estimating a distance between the load wheel center of gravity, which is a distance between the center of gravity of the load body and the center of gravity of the wheel, a state quantity detected by the detection means, and the command means Control means for generating a torque command value for driving the wheel based on the command value generated by the estimation means and the distance between the load wheel centers of gravity estimated by the estimation means, and performing the inversion control. Defeat characterized by providing It is the type mobile.

  In this aspect, the estimating means calculates a wheel horizontal speed that is a moving speed in the horizontal direction of the wheel based on a state quantity of the moving body detected by the detecting means; Based on the state quantity of the moving body detected by the detection means, a vertical acceleration calculation unit that calculates a wheel vertical acceleration that is an acceleration in the vertical direction of the wheel, and a wheel horizontal calculated by the horizontal speed calculation unit. Based on the speed and the wheel vertical acceleration calculated by the vertical acceleration calculation unit, the equation of motion of the moving body is represented as a polynomial in the distance between the centers of gravity of the loaded wheels, and a polynomial coefficient calculation for calculating the coefficient of the polynomial A center-of-gravity distance calculation unit that calculates the distance between the load wheel centers of gravity based on the coefficients of the polynomial calculated by the polynomial coefficient calculation unit. There.

In this aspect, the polynomial coefficient calculation unit may calculate the coefficients a ₂ , a ₁ , and a ₀ of the polynomial according to the following equation (14). Further, in this aspect, the center-of-gravity distance calculation unit may switch the calculation method of the distance between the load wheel centers of gravity according to the inclination angular acceleration of the load body detected by the detection unit. Furthermore, in this one aspect, the center-of-gravity distance estimation unit may calculate the distance l between the load wheel centers of gravity by the following equation (15). In this aspect, the center-of-gravity distance calculation unit may apply a low-pass filter to the calculated load wheel center-of-gravity distance.

Furthermore, in this one aspect, the control means is based on the state quantity detected by the detection means, the command value generated by the command means, and the distance between the load wheel centers of gravity estimated by the estimation means. And adjusting the control of the torque command calculation unit based on the torque command calculation unit for calculating the torque command value for driving the wheels of the mobile body and the distance between the load wheel centers of gravity estimated by the estimation means And a control adjustment unit that performs. Furthermore, in this one aspect, the torque command calculation unit may switch the calculation method of the torque command value according to the inclination angular acceleration of the load body detected by the detection means. Furthermore, in this one aspect, the torque command calculation unit may calculate the torque command value T _ref by the following equation (18) or (20).

  In this aspect, the torque command calculation unit may set the torque command value as a constant value while the estimation unit estimates the distance between the load wheel centers of gravity. Further, in this aspect, the command means includes a horizontal speed command generation unit that generates a horizontal speed command value that is a moving speed of the wheel in the horizontal direction, and the wheel horizontal speed follows the horizontal speed command value. A load angle command calculation unit that calculates a load angle command value that is an inclination angle of the load body.

  On the other hand, another aspect of the present invention for achieving the above object includes a mobile body having a load body and a wheel rotatably connected to the load body, and controls driving of the wheel, An inverted moving body control method for performing inversion control of a load body, the step of detecting a state quantity of the moving body main body, and the step of generating a command value for the moving body main body to be in a desired running state; Estimating a distance between the load wheel center of gravity, which is a distance between the center of gravity of the load body and the center of gravity of the wheel, based on the detected state quantity, the detected state quantity, and the generated command value And a step of generating a torque command value for driving the wheel based on the estimated distance between the load wheel centers of gravity, and a control method for an inverted moving body, comprising: Also good.

  Further, another aspect of the present invention for achieving the above object includes a moving body main body having a load body and a wheel rotatably connected to the load body, and controlling the driving of the wheel, An inverted moving body control program for performing inversion control of a load body, wherein the load body is based on a process for generating a command value for the mobile body body to enter a desired running state, and a state quantity of the mobile body body. A process of estimating the distance between the load wheel centers of gravity, which is the distance between the center of gravity of the body and the center of gravity of the wheels, the state quantity of the mobile body, the generated command value, and the estimated distance between the center of gravity of the load wheels And a program for generating a torque command value for driving the wheel based on the control program for causing the computer to execute the process.

  ADVANTAGE OF THE INVENTION According to this invention, the inverted type mobile body which can drive | work more stably, its control method, and a control program can be provided.

1 is a block diagram showing a schematic system configuration of an inverted moving body according to an embodiment of the present invention. It is the model figure which modeled the inverted type mobile body main body which concerns on one Embodiment of this invention. It is a figure which shows an example of the simulation result regarding the estimated value of the distance between load wheel centroids estimated with an estimator. It is the figure which compared the time change of inclination-angle (theta) ₁ of a load body in the inverted type mobile body which concerns on one Embodiment of this invention, and a prior art. It is a figure which shows roughly the coaxial two-wheeled vehicle which is one specific example of a mobile body main body. It is a figure which shows roughly the inverted autonomous traveling robot which is a specific example of a mobile body main body. It is a schematic diagram which shows the other specific example of a moving body main body. It is a flowchart which shows an example of the control processing flow of the inverted type mobile body which concerns on one Embodiment of this invention.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram showing a schematic system configuration of an inverted moving body according to an embodiment of the present invention. The inverted moving body 100 according to the present embodiment includes a moving body main body 140 as a control target, a detector 150 that detects a state quantity of the moving body main body 140, and a command value at which the moving body main body 140 is in a desired running state. , An estimator 120 for estimating the distance l between the load wheel centers of gravity described later in the moving body main body 140, and a controller for executing control of the moving body main body 140 based on the distance l between the load wheel centers of gravity. 130.

  The mobile body 140 is a mobile body that can move to an arbitrary position by driving one or a plurality of (for example, two) wheels, and travels while maintaining an inverted state. As the mobile body 140, for example, as shown in FIG. 5, a coaxial two-wheeled vehicle that performs a traveling operation by moving the center of gravity of the occupant, or as shown in FIG. 6, the body is inclined while maintaining an inverted state. Inverted autonomous traveling robots and the like are applicable. However, the present invention is not limited to this, and includes a traveling means using wheels and a load body connected to the traveling means, and performs an inverted control for maintaining the inverted state of the load body. It can be applied to any vehicle.

  For example, as shown in FIG. 7, the moving body main body 140 may have a configuration in which a link mechanism 702 is swingably connected to a four-wheel traveling means 701. Further, a cage member 703 may be attached to the upper portion of the link mechanism 702, and articles or the like may be placed on the cage member 703 and transported.

  FIG. 2 is a model diagram modeling the inverted mobile body 140 shown in FIG. In FIG. 2, the moving body main body 140 travels while maintaining an inverted state on a road surface 203, and includes a wheel 202 and a load body 201 connected to the wheel 202.

The load body 201 includes, for example, a robot body, a person who rides on the moving body main body 140, or a load to be loaded. Further, the wheel 202 can move using the frictional force with the road surface 203 by rotating with the load body 201 placed thereon. Furthermore, the mobile body 140, for example, the load 201 before傾又is configured to in response to the inclination angle theta ₁ of the load body 201 when the inclined rearwards to forward or reverse.

As shown in FIG. 1, a detector 150, which is a specific example of detection means, is provided in the moving body main body 140, and can detect the state quantity of the moving body main body 140. Detector 150, for example, the attitude sensor for detecting an inclination angle theta ₁ of the load body 201, the rotation sensor for detecting a rotational angle theta ₂ of the wheel 202, any sensor capable of detecting a state quantity of the moving body 140 such as Contains. The detector 150 outputs the detected inclination angle θ ₁ of the load body 201, rotation angle θ ₂ of the wheel 202, and the like as state quantities of the moving body main body 140 to the estimator 120 and the controller 130.

  The command device 110, which is a specific example of the command means, is configured so that the mobile body 140 is in a desired travel state (forward / reverse travel, acceleration, etc.) in response to a travel operation (handle operation, switch operation, center of gravity movement operation, etc.) by the passenger. A command value for decelerating, turning left and right, stopping, etc.) is generated. In addition, the command device 110 includes a horizontal speed command generation unit 111 and a load angle command calculation unit 112.

  For example, the horizontal speed command generation unit 111 generates a horizontal speed command value that is a command value of the moving speed of the wheels 202 in the horizontal direction in accordance with a traveling operation by the passenger. The horizontal speed command generation unit 111 outputs the generated horizontal speed command value to the load angle command calculation unit 112.

For example, when the road surface 203 on which the mobile body 140 travels is horizontal, the load angle command calculation unit 112 generates the wheel horizontal speed calculated by the horizontal speed calculation unit 121 described later by the horizontal speed command generation unit 111. The load angle command value that is the inclination angle θ ₁ of the load body 201 that follows the horizontal speed command value is calculated. The load angle command calculation unit 112 outputs the calculated load angle command value to the controller 130.

  The estimator 120, which is a specific example of the estimation unit, estimates the distance l between the load wheel centers of gravity of the mobile body 140 based on the state quantity of the mobile body 140 from the detector 150. Here, the load wheel center-of-gravity distance l indicates the distance between the center of gravity of the load body 201 and the center of gravity of the wheel 202. The estimator 120 includes a horizontal velocity calculation unit 121, a vertical acceleration calculation unit 122, a polynomial coefficient calculation unit 123, and a center-of-gravity distance calculation unit 124.

For example, the horizontal speed calculation unit 121 calculates a wheel horizontal speed that is a moving speed of the wheel 202 in the horizontal direction based on the rotation angle θ ₂ of the wheel 202 from the detector 150. The horizontal speed calculation unit 121 outputs the calculated wheel horizontal speed to the polynomial coefficient calculation unit 123.

  Based on the state quantity of the moving body main body 140 from the detector 150, the vertical acceleration calculation unit 122 calculates wheel vertical acceleration, which is acceleration in the vertical direction of the wheel 202, using the following equation (12). Thus, calculating the wheel vertical acceleration without using an acceleration sensor leads to cost reduction. Note that the vertical acceleration calculation unit 122 may obtain the wheel vertical acceleration using the acceleration of the wheel 202 detected by the acceleration sensor. The vertical acceleration calculation unit 122 outputs the calculated wheel vertical acceleration to the polynomial coefficient calculation unit 123.

  The polynomial coefficient calculation unit 123 calculates the equation of motion of the mobile body 140 based on the wheel horizontal speed from the horizontal speed calculation unit 121 and the wheel vertical acceleration from the vertical acceleration calculation unit 122 as Expressed as a polynomial, a distance coefficient between the centers of gravity, which is a coefficient of the polynomial, is calculated. The polynomial coefficient calculation unit 123 outputs the calculated center-of-gravity distance coefficient to the center-of-gravity distance calculation unit 124.

  The center-of-gravity distance calculation unit 124 solves the equation of motion of the moving body 140 with respect to the load wheel center-of-gravity distance l using the distance coefficient between the center of gravity from the polynomial coefficient calculation unit 123, and calculates the load wheel center-of-gravity distance l. The center-of-gravity distance calculation unit 124 outputs the calculated load wheel center-of-gravity distance l to the controller 130.

  Based on the state quantity from the detector 150, the load angle command value from the command device 110, and the load wheel center-of-gravity distance l from the estimator 120, the controller 130, which is a specific example of the control means, The driving of the mobile body 140 is controlled. The controller 130 includes a control adjustment unit 131 and a torque command calculation unit 132.

  The control adjustment unit 131 adjusts control parameters and control laws of the torque command calculation unit 132 based on the load wheel center-of-gravity distance l from the center-of-gravity distance calculation unit 124 of the estimator 120.

Based on the state quantity of the moving body 140 from the detector 150, the load angle command value from the commander 110, and the load wheel center-of-gravity distance l from the estimator 120, the torque command calculation unit 132, for example, When the road surface 203 on which the mobile body 140 travels is horizontal, a torque command value T _ref is calculated so that the wheel horizontal speed follows the horizontal speed command value. The controller 130 controls driving of the wheels 202 of the mobile body 140 based on the torque command value T _ref calculated by the torque command calculation unit 132.

  The command unit 110, the estimator 120, and the controller 130 include, for example, a CPU (Central Processing Unit) that performs control processing, arithmetic processing, and the like, and a ROM ( It is possible to configure a hardware configuration centering on a microcomputer including a read only memory (RAM) and a RAM (random access memory) that temporarily stores processing data.

Next, specific inversion control of the inverted mobile body 100 according to the present embodiment will be described using the model of the inverted mobile body 140 shown in FIG. 2, the mass of the load body 201 is m ₁ , the inertia moment of the load body 201 is J ₁ , the mass of the wheel 202 is m ₂ , the inertia moment of the wheel 202 is J ₂ , and the distance between the load wheel centers of gravity is l, The radius of the wheel 202 is r, the inclination angle of the load body 201 is θ ₁ , the rotation angle of the wheel 202 is θ ₂ , and the torque command value output by the torque command calculation unit 132 is T _ref .

First, the kinetic energy T and the potential energy V of the moving body 140 can be expressed by the following equations (3) and (4) using the following equations (1) and (2). Here, in the following (1) and (2), _{x 2} horizontal position of the wheel 202, _{y 2} the vertical position of the wheel 202, _{x 1} the horizontal position of the load body 201, the vertical position of the load body 201 y _Set to ₁ .

Further, using the above equations (3) and (4) and the Euler-Lagrange equation, the equation of motion of the moving body 140 can be obtained as the following equations (5) to (8). However, in the following formula (8), g is a gravitational acceleration.

Further, considering the viscous friction between the wheel 202 and the road surface 203, the above equations (6) and (7) can be rewritten as the following equations (9) and (10). In the following equations (9) and (10), D is a viscous friction coefficient.

Moreover, the following (11) Formula can be calculated | required using said (9) and (10) Formula.

Further, the following equation (12) can be obtained by subtracting the above equation (11) from the above equation (5).

In the above equation (12), N _x is a nonlinear term using the horizontal position x ₂ of the wheel 202 as a parameter, and N _y is a nonlinear term using the vertical position y ₂ of the wheel 202 as a parameter. Further, the moment of inertia J ₁ of the load body 201, so can be represented as a partial sum of the others in proportion to the square of the load wheel distance between centers of gravity l, using the moment of inertia constants c _J of the load 201, It can be expressed as the following equation (13).

Furthermore, the following equation (14) can be obtained by substituting the above equation (13) into the above equation (12). Here, it is assumed that the torque command value T _ref is not a function of the load wheel center-of-gravity distance l.

The polynomial coefficient calculation unit 123 calculates the horizontal position x ₂ and the vertical position y ₂ of the wheel 202 based on the inclination angle θ ₁ of the load body 201 from the detector 150 and the rotation angle θ _{2 of} the wheel 202. . Then, the polynomial coefficient calculator 123, the rotation angle theta ₂ of the inclination angle theta ₁ and the wheel 202 of the load 201 from the detector 150, the horizontal position _{x 2} and vertical position _{y 2} of the wheel 202, which is calculated, based on the Then, the center-of-gravity distance coefficients a ₀ , a ₁ , and a ₂ are calculated using the above equation (14). The polynomial coefficient calculation unit 123 outputs the calculated center-of-gravity distance coefficients a ₀ , a ₁ , and a ₂ to the center-of-gravity distance calculation unit 124.

The center-of-gravity distance calculation unit 124 uses the center-of-gravity distance coefficients a ₀ , a ₁ , a ₂ from the polynomial coefficient calculation unit 123 to express the above equation (14), which is the equation of motion of the moving body 140, between the load wheel center of gravity. Solving for the distance l, the following equation (15) is obtained, and the distance l between the load wheel centers of gravity is calculated based on the equation (15).

In the above equation (15), δ ₁ is a threshold value of the inclination angular acceleration (two-time differential of the inclination angle θ ₁ ) of the load body 201 for switching the calculation method (estimation expression) of the load wheel center-of-gravity distance l. Is a load body acceleration switching value. As described above, the center-of-gravity distance calculation unit 124 calculates the load wheel center-of-gravity distance l in the equation (15) according to the inclination angular acceleration of the load body 201 (hereinafter referred to as load body acceleration). Switch. Thereby, even when the load body acceleration is small, the distance l between the load wheel centers of gravity can be accurately calculated.

Further, as shown in the following equation (16), the center-of-gravity distance calculation unit 124 applies the low-pass filter LPF () to the calculated load wheel center-of-gravity distance ₁₀ to obtain the load wheel center-of-gravity distance l. It may be calculated. Thereby, when the load body acceleration passes through the load body acceleration switching value δ ₁ , the load wheel center-of-gravity distance l changes smoothly, and as a result, the control parameter is smoothly changed by the control adjustment unit 131. Therefore, for example, even when a passenger who rides on the mobile body 140 squats down or the loaded luggage collapses, the mobile body 140 can continue to travel smoothly without vibration. That is, the mobile body 140 can travel more stably.

In the above equation (16), l ₀ is a value before adjustment of the load wheel center-of-gravity distance l, and LPF () is a low-pass filter function. Thus, the distance between the centers of gravity calculation unit 124, the load by applying a low-pass filter to the wheel center of gravity distance l _0, Equation (16) smoothly varying load wheel distance between the centers of gravity l even when switching Therefore, the operation of the moving body main body 140 can be continued more smoothly.

  The center-of-gravity distance calculation unit 124 outputs the load wheel center-of-gravity distance l obtained as described above to the controller 130.

The controller 130 controls the driving of the moving body 140 using the load wheel center-of-gravity distance l from the center-of-gravity distance calculation unit 124. Here, the non-linear torque N _x + N _y can be expressed by the following equation (17) using the above equation (12). In the following equations (17) and (18), d ₁ and d ₀ are functions of the inclination angle θ ₁ of the load body 201 and the rotation angle θ ₂ of the wheel 202. Further, d ₁ is a coefficient of a term proportional to the inclination angular velocity of the load body 201 among the nonlinear terms in the equation of motion of the moving body main body 140, and d ₀ is among the nonlinear terms in the equation of motion of the moving body main body 140. The coefficient of the term not proportional to the inclination angular velocity of the load body 201.

The torque command calculation unit 132 calculates a torque command value T _ref for driving the wheels 202 of the mobile body 140 using the following equation (18).

In the equation (18), δ _a is a threshold value of the load body acceleration when the calculation equation (calculation method) is switched, and d _m0 is the inclination angle θ ₁ of the load body 201 and the rotation angle of the wheel 202. it is a function of θ _2. Also, σ is a degree of convergence representing the speed at which the inclination angle θ ₁ of the load body 201 converges to the load angle command value calculated by the load angle command calculation unit 112, and u is the equation of motion of the mobile body 140. Is a linearized torque command value for stabilizing the following expression (19) with the convergence degree σ.

The control adjustment unit 131 determines δ _a in the above equation (18) and instructs switching of the above calculation equation for _obtaining the torque command value T _ref . Further, the control adjustment unit 131 uses the load wheel center-of-gravity distance l from the center-of-gravity distance calculation unit 124 to set a control gain (function of the load wheel center-of-gravity distance l) in the proportional control of the torque command calculation unit 132 or the like. I do.

Thus, the torque command calculation unit 132 switches the calculation formula of the torque command value T _ref according to the magnitude of the load body acceleration using the formula (18). Thereby, for example, when the mobile body 140 is not accelerating / decelerating, the calculation of the torque command value T _ref can be simplified, the load on the CPU can be reduced, and the amount of heat generated by the electrical hardware can also be reduced.

The torque command calculation unit 132 may calculate the torque command value T _ref using the following equation (20) instead of the above equation (18).

The controller 130 controls driving of the wheels 202 by controlling driving means such as a motor provided in the movable body main body 140 based on the torque command value T _ref calculated by the torque command calculation unit 132.

As described above, the controller 130 calculates the torque command value T _ref using the load wheel center-of-gravity distance l as shown in the equation (18) or (20), for example, the boarding of the mobile body 140. Even when a person squats down or stands up, changes his posture, or a load loaded on the movable body 140 collapses and its center of gravity changes greatly, the inclination angle θ ₁ of the load body 201 always converges. It converges to the load angle command value at degree σ. Therefore, the movable body main body 140 can travel stably.

In addition, while the estimator 120 estimates the load wheel center-of-gravity distance l, the torque command calculation unit 132 of the controller 130 may calculate the torque command value T _ref as a constant value. Thereby, it can reduce that the distance l between load wheel centroids is influenced by the noise contained in torque command value _Tref , and can improve the estimation precision of the distance l between load wheel centroids. For example, the torque command calculation unit 132 may set the torque command value T _ref when the estimator 120 starts estimating the load wheel center-of-gravity distance l as the above-mentioned constant value, or even if substantially 0 is set as the constant value. Good.

  Next, the simulation performed using the model of the inverted movable body main body 140 shown in FIG. 2 will be described.

In this simulation, m ₁ = 70 [kg], r ₁ = 0.6 [m], c _J = 1/12 * m ₁ * 4 [kg], c ₀ = 1/12 * m ₁ * 3 * R _l ² [kg], l ₀ = 0.9 [m], J ₁ = (m ₁ + c _J ) * l ² + c ₀ , r = 0.1 [m], m ₂ = 15 [kg], ^{_{J 2 = m 2 * r 2}} /2 [kg * m 2], g = 9.8 [m / s 2], T = 1 × 10 -3 [s], K p = 10 [s -1], Let K _v = 10 * 2 * π [s ⁻¹ ] and K _vj = K _v * J ₁ .

M ₁ is the mass of the load body 201, r _l is half of the shoulder width of the occupant, c _J is proportional to the square of the distance l between the load wheel centers of gravity in the equation (13) when the occupant is approximated to a cylinder C ₀ is a term that does not depend on the load wheel center-of-gravity distance l in the above equation (13) when the passenger is approximated to a cylinder, and l ₀ is the load wheel center-of-gravity distance l in the initial state, J ₁ moment of inertia of the load 201, the mass _{m 2} of the wheel 202, r is the radius of the wheel 202, the moment of inertia of _{J 2} are wheels 202, g is the gravitational acceleration, T is the control cycle, _{K p} is a position proportional control gain, _Kv is a normalized speed proportional control gain, and _Kvj is a speed proportional control gain.

Further, in this simulation, the torque command computation unit 132, the position proportional / speed proportional control in which the position proportional control gain K _p and velocity proportional control gain K _vj as parameters, the equation (19) linearized torque command value u Is calculated and the wheel 202 of the mobile body 140 is driven and controlled. Hereinafter, a comparison between the case where the inverted control according to the present invention is applied and the case where the inverted control according to the present invention is not applied in the case where the distance l between the load wheel centers of gravity is changed as in the following equation (21) will be shown.

  FIG. 3 is a diagram illustrating an example of a simulation result regarding the estimated value of the distance between the centers of gravity of the loaded wheels estimated by the estimator. In FIG. 3, the solid line indicates the true value of the load wheel center-of-gravity distance l, and the broken line indicates the estimated value of the load wheel center-of-gravity distance l estimated by the estimator 120. As shown in FIG. 3, the estimated value of the load wheel center-of-gravity distance l follows the true value of the load wheel center-of-gravity distance l, and the estimated value and the true value overlap with each other, and a good result is obtained. You can see that

Figure 4 is the inverted vehicle 100 according to this embodiment and the prior art and is a graph comparing the time variation of the inclination angle theta ₁ of the load body 201. In FIG. 4, a solid line (1) shows the time variation of the inclination angle theta ₁ of the load body 201 according to this embodiment, one-dot chain line (2) the time change of the inclination angle theta ₁ of the load body according to the prior art The dotted line (3) indicates the load angle command value of the command device 110. In this prior art, it is assumed that only position proportional / velocity proportional control is executed using the same control gain.

As shown in FIG. 4, it can be seen that in the conventional technique (dashed line (2)), the inclination angle θ ₁ of the load body diverges after about 3 [s], and the mobile body is overturned. Meanwhile, in the present embodiment (solid line (1)), load body 201 inclined angle theta ₁ is suppressed to have ,, mobile body 140 in the following 0.1rad of, the load angle command value from the command unit 110 Accordingly, it can be seen that the operation is always stable. As described above, in the inverted mobile body 100 according to the present embodiment, even if the position of the center of gravity of the passenger of the mobile body 140, the loaded baggage, or the like changes significantly, the mobile body 140 does not vibrate and is not defective. It is stable and can continue to run or stop safely without falling.

  Next, a method for controlling the inverted moving body 100 according to the present embodiment will be described in detail. FIG. 8 is a flowchart illustrating an example of a control processing flow of the inverted moving body according to the present embodiment.

The detector 150 detects state quantities of the moving body main body 140 such as the inclination angle θ ₁ of the load body 201 and the rotation angle θ ₂ of the wheel 202 (step S101), and outputs them to the estimator 120 and the controller 130. .

  Further, the horizontal speed command generation unit 111 of the command device 110 generates a horizontal speed command value that is a moving speed of the wheel 202 in the horizontal direction in accordance with the traveling operation by the passenger (step S102), and the generated horizontal speed. The command value is output to the load angle command calculation unit 112.

  Next, the load angle command calculation unit 112 calculates a load angle command value based on the horizontal speed command value from the horizontal speed command generation unit 111 (step S103), and sends the calculated load angle command value to the controller 130. Output.

On the other hand, the horizontal speed calculation unit 121 of the estimator 120 calculates a wheel horizontal speed based on, for example, the rotation angle θ ₂ of the wheel 202 from the detector 150 (step S104), and calculates the calculated wheel horizontal speed using a polynomial coefficient. Output to the unit 123.

  Further, the vertical acceleration calculation unit 122 calculates the wheel vertical acceleration using the equation (12) (step S105), and outputs the calculated wheel vertical acceleration to the polynomial coefficient calculation unit 123.

  Thereafter, the polynomial coefficient calculation unit 123 calculates the center-of-gravity distance coefficient based on the wheel horizontal speed from the horizontal speed calculation unit 121 and the wheel vertical acceleration from the vertical acceleration calculation unit 122 (step S106). The center-of-gravity center distance coefficient is output to the center-of-gravity distance calculation unit 124.

  The center-of-gravity distance calculation unit 124 calculates the load wheel center-of-gravity distance l based on the center-of-gravity distance coefficient from the polynomial coefficient calculation unit 123 (step S107), and calculates the calculated load wheel center-of-gravity distance l as a controller. To 130.

  Next, the control adjustment unit 131 of the controller 130 adjusts the control parameter and the control law of the torque command calculation unit 132 based on the load wheel center-of-gravity distance l from the center-of-gravity distance calculation unit 124 (step S108).

The torque command calculation unit 132 is based on the state quantity of the moving body main body 140 from the detector 150, the load angle command value from the commander 110, and the load wheel center-of-gravity distance l from the center-of-gravity distance calculation unit 124. Then, the torque command value T _ref is calculated using the above formula (18) or (20) (step S109).

The controller 130 controls the driving of the wheels 202 of the moving body main body 140 based on the torque command value T _ref calculated by the torque command calculation unit 132 (step S110).

As described above, in the inverted movable body 100 according to the present embodiment, the estimator 120 estimates the load wheel center-of-gravity distance l, and the controller 130 calculates the torque command value T _ref based on the load wheel center-of-gravity distance l. Invert control. As a result, for example, even when the position of the center of gravity greatly changes, such as when the passenger of the mobile body 140 changes the posture or the loaded luggage collapses, it is not unstable and unstable. Thus, it is possible to continue running or stopping safely without falling. That is, the inverted moving body 100 can travel more stably.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. In the above-described embodiment, the present invention has been described as a hardware configuration, but the present invention is not limited to this. The present invention can also realize arbitrary processing by causing a CPU (Central Processing Unit) to execute a computer program. In this case, the computer program can be provided by being recorded on a recording medium, or can be provided by being transmitted via the Internet or another communication medium. The storage medium includes, for example, a flexible disk, hard disk, magnetic disk, magneto-optical disk, CD-ROM, DVD, ROM cartridge, RAM memory cartridge with battery backup, flash memory cartridge, and nonvolatile RAM cartridge. The communication medium includes a wired communication medium such as a telephone line, a wireless communication medium such as a microwave line, and the like.

  The present invention can be widely applied to, for example, a robot that runs with two wheels inverted, an electric wheelchair, an automatic transfer device, a robot that works in a narrow place such as lifesaving in a disaster, and an assembly device for electronic components that are vulnerable to vibration.

DESCRIPTION OF SYMBOLS 100 Inverted type mobile body 110 Command device 111 Horizontal speed command generation part 112 Load angle command calculation part 120 Estimator 121 Horizontal speed calculation part 122 Vertical acceleration calculation part 123 Polynomial coefficient calculation part 124 Center of gravity distance calculation part 130 Controller 131 Control adjustment Unit 132 torque command calculation unit 140 moving body main body 150 detector 201 load body 202 wheel 203 road surface

Claims (12)

A moving body having a load body and a wheel rotatably connected to the load body, an inverted moving body for controlling the driving of the wheel and performing an inversion control of the load body,
Detecting means for detecting a state quantity of the mobile body;
Command means for generating a command value at which the mobile body is in a desired running state;
Estimating means for estimating a distance between the load wheel center of gravity, which is a distance between the center of gravity of the load body and the center of gravity of the wheel, based on the state quantity detected by the detection unit;
Torque command value for driving the wheel based on the state quantity detected by the detection means, the command value generated by the command means, and the distance between the load wheel centers of gravity estimated by the estimation means And control means for performing the inversion control,
Equipped with a,
The estimation means includes
A horizontal speed calculation unit that calculates a wheel horizontal speed, which is a moving speed of the wheel in the horizontal direction, based on a state quantity of the movable body detected by the detection unit;
A vertical acceleration calculation unit that calculates a wheel vertical acceleration that is an acceleration in the vertical direction of the wheel, based on a state quantity of the moving body detected by the detection unit;
Based on the wheel horizontal speed calculated by the horizontal speed calculation unit and the wheel vertical acceleration calculated by the vertical acceleration calculation unit, the equation of motion of the mobile body is expressed as a polynomial in the distance between the load wheel centers of gravity. A polynomial coefficient calculation unit for calculating coefficients of the polynomial;
Based on the coefficient of the polynomial calculated by the polynomial coefficient calculation unit, the center-of-gravity distance calculation unit that calculates the load wheel center-of-gravity distance,
An inverted mobile body characterized by comprising: The inverted mobile body according to claim 1 ,
The polynomial coefficient calculator calculates the coefficients a ₂ , a ₁ , a ₀ of the polynomial according to the following equation, and is an inverted mobile body.

Where m ₁ represents the mass of the load body, c _J represents the moment of inertia constant of the load body, m ₂ represents the mass of the wheel, J ₂ represents the moment of inertia of the wheel, and x ₂ represents the horizontal position of the wheel, y ₂ represents the vertical position of the wheel, r is represents the radius of the wheel, theta ₁ represents the inclination angle of the load body, theta ₂ represents the rotation angle of the wheel , T _ref represents the torque command value. The inverted mobile body according to claim 1 or 2 ,
The center-of-gravity distance calculation unit switches the calculation method of the distance between the load wheel centers of gravity according to the inclination angular acceleration of the load body detected by the detection means. An inverted mobile object according to claim 3 ,
The center-of-gravity distance estimation unit calculates the distance l between load wheel centers of gravity according to the following equation.

_{However, a 2, a 1, a} 0 represents the coefficient of the polynomial, [delta] ₁ represents the threshold of the tilt angular acceleration of the load member for switching the calculation method of the load wheel distance between center of gravity l. The inverted mobile body according to any one of claims 1 to 4 ,
The center-of-gravity distance calculation unit applies a low-pass filter to the calculated load wheel center-of-gravity distance. The inverted mobile body according to any one of claims 1 to 5 ,
The control means includes
Based on the state quantity detected by the detection means, the command value generated by the command means, and the load wheel center-of-gravity distance estimated by the estimation means, to drive the wheels of the mobile body. A torque command calculation unit for calculating the torque command value of
A control adjustment unit for adjusting the control of the torque command calculation unit based on the distance between the load wheel centers of gravity estimated by the estimation unit;
An inverted mobile body characterized by comprising: An inverted mobile object according to claim 6 ,
The inverted moving body characterized in that the torque command calculation unit switches a calculation method of a torque command value according to the inclination angular acceleration of the load body detected by the detecting means. The inverted moving body according to claim 7 ,
The torque command calculation unit calculates the torque command value T _{ref according} to the following formula, and is an inverted moving body.

However, d ₁ among the nonlinear terms in the equation of motion of said mobile body, a coefficient term proportional to the inclination angular speed of the load body, d ₀ is out of the non-linear terms in the equation of motion of said mobile body, wherein Is _a coefficient of a term that is not proportional to the inclination angle velocity of the load body, δ _a is a threshold value of the inclination angle acceleration of the load body when switching the calculation method, and d _m0 is the inclination angle θ _{1 of the} load body and the wheel of the rotation angle theta is the _second function, u is the linearized torque command value to stabilize the linear part of the equation of motion of said mobile body, sigma is the inclination angle theta ₁ is the command value of the load body Is the degree of convergence representing the speed of convergence. The inverted mobile body according to any one of claims 6 to 8 ,
While the estimating means estimates the distance between the load wheel centers of gravity, the torque command calculation unit sets the torque command value to a constant value. The inverted mobile body according to any one of claims 1 to 9 ,
The command means includes
A horizontal speed command generating unit that generates a horizontal speed command value that is a moving speed of the wheel in the horizontal direction;
A load angle command calculation unit that calculates a load angle command value that is an inclination angle of the load body such that the wheel horizontal speed follows the horizontal speed command value;
An inverted mobile body characterized by comprising: An inverted moving body control method comprising a moving body main body having a load body and a wheel rotatably connected to the load body, wherein the driving of the wheel is controlled to perform an inversion control of the load body. ,
Detecting a state quantity of the mobile body;
Generating a command value at which the mobile body is in a desired running state;
Calculating a wheel horizontal speed, which is a moving speed of the wheel in the horizontal direction, based on the detected state quantity of the moving body,
Calculating wheel vertical acceleration, which is acceleration in the vertical direction of the wheel, based on the detected state quantity of the mobile body;
Based on the calculated wheel horizontal speed and the calculated wheel vertical acceleration, expressing the equation of motion of the mobile body as a polynomial in the distance between the load wheel centers of gravity, and calculating the coefficient of the polynomial; ,
Calculating the load wheel center-of-gravity distance based on the calculated coefficient of the polynomial;
Generating a torque command value for driving the wheel based on the detected state quantity, the generated command value, and the estimated load wheel center-of-gravity distance;
A method for controlling an inverted moving body comprising: A control program for an inverted mobile body comprising a mobile body having a load body and a wheel rotatably connected to the load body, wherein the drive of the wheel is controlled and the load body is inverted. ,
Processing for generating a command value for the mobile body to be in a desired running state;
Based on the detected state quantity of the moving body, a process of calculating a wheel horizontal speed that is a moving speed of the wheel in the horizontal direction;
Based on the detected state quantity of the moving body, a process of calculating a wheel vertical acceleration that is an acceleration in the vertical direction of the wheel;
Based on the calculated wheel horizontal speed and the calculated wheel vertical acceleration, the equation of motion of the mobile body is represented as a polynomial in the distance between the load wheel centers of gravity, and the coefficient of the polynomial is calculated. ,
Based on the calculated coefficient of the polynomial, a process of calculating the distance between the load wheel center of gravity,
A process of generating a torque command value for driving the wheel based on the state quantity of the mobile body, the generated command value, and the estimated distance between the load wheel centers of gravity.
A program for controlling an inverted moving body that causes a computer to execute.

Images