JP2009064190A - Method for determining control parameter of drive of multi-axis moving element and its apparatus - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a method for determining control parameters of drives of a moving element, which enables stable travel of a loaded moving element that takes movement of its center of gravity into consideration, in view of the fact that it has been conventionally impossible to provide proper instructions to each driving wheel of a loaded moving element during travel since movement of its center of travel has not been taken into consideration. <P>SOLUTION: A load W of unknown weight is placed on a multi-axis moving element 1 and away from its center of gravity. Until a certain speed is reached, the torque of each of the drives 3a to 3c which occurs as the moving element 1 is driven linearly is detected. The torque ratio of the drives 3a to 3c is calculated from the torques detected. The distance from the center of gravity is estimated from the torque ratio. With the estimated distance as a control parameter, computations are performed. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a traveling control method for a moving body such as a transport vehicle, and more particularly to a control parameter determination method and apparatus for each driving device of a multi-axis moving body.

When traveling with a load on a transport vehicle or moving carriage that uses an electric motor as a drive wheel, if the weight of the load is not known, the optimal control gain for speed control of the drive wheel cannot be set and stable. Cannot be controlled.
As a conventional method of measuring the weight of a load by a moving body, the weight is measured by placing the moving body with the load mounted on a pressure sensor or a weight measuring instrument, and the weight is calculated as a control parameter for the driving wheel. The optimum gain was sought.
On the other hand, there existed the example of patent documents 1-4 as a weight measuring method which does not use a sensor and a measuring device. In these cases, the load is obtained by comparing the travel distance under the change in the outer circumference due to the deformation of the front wheel provided on the mounting table when the mounted object is mounted on the mounting table with the traveling distance in the case of no load. As another weight estimation method for the load, the moving object is accelerated to a constant speed, and the weight of the load on the moving object is estimated from the torque and acceleration obtained at this time. By calculating this weight as a control parameter and calculating a gain control signal of the speed control system, stable traveling control has been realized.
JP 2004-091079 A Japanese Patent Laid-Open No. 60-169912 (page 4, FIG. 1) JP-A-60-58313 (page 6, FIG. 3) JP-A-62-659109 (5th page, FIG. 3)

The moving body having a plurality of driving devices of the conventional example estimates the weight of the load from the torque detected when accelerating to a constant speed, but does not consider the deviation of the center of gravity position, so when moving the moving body An appropriate command could not be given to each drive unit, and stable running could not be performed.
The present invention detects or estimates the displacement of the weight and the center of gravity position of the mounted object even if an unknown weight mounted object is placed on the moving object, and the weight and the gravity center position deviation are controlled by the control parameter. It is an object of the present invention to provide a control parameter determination method for a multi-axis moving body that can realize stable traveling by calculating the gain of each driving device automatically.
Further, in the conventional example, since the dynamic friction coefficient of the road surface is not taken into consideration, the control parameter according to the road surface condition cannot be determined, and the driving wheel cannot be controlled stably. The present invention calculates the dynamic friction coefficient μ detected by each driving device during constant speed motion as a control parameter, and automatically adjusts the gain of each driving device to control the multi-axis moving body that can realize stable traveling The purpose is to provide a decision method.

In order to solve the above problems, the present invention is configured as follows.
The control parameter determining method for each driving device of the multi-axis moving body according to claim 1 is a control parameter determining method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on a side surface of a housing. In each of the driving devices generated when the load of unknown weight is placed on the multi-axis moving body in a state where the position of the center of gravity is shifted and the multi-axis moving body travels in a straight line until reaching a constant speed. The multi-axis movement is performed by detecting a torque, obtaining a torque ratio of each of the driving devices from each detected torque, estimating a distance in which the center of gravity is shifted from the torque ratio, and calculating the distance as the control parameter. The body was controlled to run stably.
The control parameter determining method for each driving device of the multi-axis moving body according to claim 2 is a control parameter determining method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on a side surface of a housing. In this case, the load of unknown weight is placed on the multi-axis moving body in a state where the position of the center of gravity is shifted, and the multi-axis moving body is rotated around the center of gravity of the multi-axis moving body until a certain angular velocity is reached. Thus, the torque ratio between the drive devices is obtained, and the torque ratio is calculated as a control parameter, so that the multi-axis moving body is stably controlled.
The control parameter determining method for each driving device of the multi-axis moving body according to claim 3 is a control parameter determining method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on a side surface of a housing. The load of unknown weight is placed on the multi-axis moving body in a state where the position of the center of gravity is shifted, and the torque of each driving device generated when the multi-axis moving body is moved at a constant speed is detected, The dynamic friction coefficient of the road surface is estimated from the torque increase detected by the driving device, and the dynamic friction coefficient is calculated as a control parameter, so that the multi-axis moving body is stably controlled.

According to a fourth aspect of the present invention, in the method for determining a control parameter of each driving device for a multi-axis moving body according to any one of the first to third aspects, the mounting is performed using a time derivative of speed from each detected torque. By estimating the weight of the object and calculating the weight as a control parameter, the multi-axis moving body is stably controlled.
The invention according to claim 5 is the control parameter determination method for each drive device of the multi-axis moving body according to claim 4, and is optimal for each drive device based on the control parameter of each drive device determined by the parameter determination method. Thus, the multi-axis moving body is stably controlled by adjusting the gain appropriately.
According to a sixth aspect of the present invention, in the control parameter determining method for each drive device of the multi-axis moving body according to the fourth aspect, the multi-axis moving body can be stabilized by estimating the weight of the load from the equation (1). The driving was controlled.
(T _3a1 + T _3b1 ) ・ cosθ = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (1)
Here, T _3a1 + T _3b1 : Detected torque of the driving device, θ is an angle between the multi-axis moving body and the driving wheel, and changes depending on the number of driving wheels attached to the multi-axis moving body. When θ = 0, Three wheel θ = 30 °, M ₁ : Weight of multi-axis moving body 1, M ₂ : Weight of load W,
M _{3 is} the weight of the driving wheel 3, and R is the radius of the driving wheel 3.
According to a seventh aspect of the present invention, in the control parameter determining method for each drive device of the multi-axis moving body according to the fifth aspect, the torque ratio of the drive wheel is obtained from the equation (2), and from the equations (3) to (5) The multi-axis moving body is stably controlled by correcting the gain of each drive wheel.
T _3a : T _3b : T _3c = (T _3a1 * T _3a2 ) :( T _3b1 * T _3a2 ) :( T _3c2 * T _3a1 ) ・ ・ ・ ・ Formula (2)
Here, T _3a , T _3b , T _3c : Torque of driving wheel,
T _3a1 , T _3b1 : Torque of the drive wheels 3a, 3b when linearly moving in one direction,
T _3a2 , T _3c2 : Torques of the drive wheels 3a, 3c when linearly moving in other directions,
G _3a = G • T _3a / (T _3a + T _3b + T _3c ) ・ ・ ・ ・ ・ ・ Equation (3)
G _3b = G · T _3b / (T _3a + T _3b + T _3c ) ··· Equation (4)
G _3c = G · T _3c / (T _3a + T _3b + T _3c ) ········· Equation (5)
Where G _3a , G _3b , G _3c : the corrected gain of each drive wheel,
G: Gain of the drive wheel when moving the estimated weight of the load on the center of gravity

The control parameter determining device for each driving device of the multi-axis moving body according to claim 8 is the control parameter determining device for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing. The torque detecting means for detecting the torque of each of the driving devices generated when the multi-axis moving body is accelerated and rotated or rotated, and the torque ratio of each of the driving devices is obtained from the detected torques. Torque ratio calculation means, center of gravity position estimation means for estimating the distance of the center of gravity position from the torque ratio calculated by the torque ratio calculation means, and control parameter calculation for calculating the distance estimated by the center of gravity position estimation means as control parameters The multi-axis moving body is stably controlled.
The invention of the control parameter determining device for each driving device of the multi-axis moving body according to claim 9 is the control parameter determining device for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing. A torque detecting means for detecting a torque of each of the driving devices generated when the multi-axis moving body is linearly moved at a constant speed, and a road surface for estimating a dynamic friction coefficient of the road surface from the detected increase in torque. Dynamic friction coefficient estimating means;
Control parameter calculation means for calculating the dynamic friction coefficient estimated by the road surface dynamic friction coefficient estimation means as a control parameter is provided, so that the multi-axis moving body is stably controlled.
A tenth aspect of the present invention is the control parameter determining device for each driving device of the multi-axis moving body according to the eighth or ninth aspect, wherein the control parameter calculating means uses a time derivative of the speed from each detected torque. The weight of the mounted object is estimated, and the estimated weight is calculated as a control parameter, so that the multi-axis moving body is stably controlled.
The invention as set forth in claim 11 is the control parameter determination device for each drive device of the multi-axis moving body according to claim 10, wherein the gain adjustment means performs optimum gain adjustment for each drive device based on the calculated control parameter. Thus, the multi-axis moving body is stably controlled for traveling.

According to a twelfth aspect of the present invention, in the control parameter determining device for each drive device of the multi-axis moving body according to the tenth aspect, the multi-axis moving body can be stabilized by estimating the weight of the load from Equation (1). The driving was controlled.
(T _3a1 + T _3b1 ) ・ cosθ = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (1)
Here, T _3a1 + T _3b1 : Detected torque of the driving device, θ is an angle between the multi-axis moving body and the driving wheel, and changes depending on the number of driving wheels attached to the multi-axis moving body. When θ = 0, Three wheel θ = 30 °, M ₁ : Weight of multi-axis moving body 1, M ₂ : Weight of load W,
M _{3 is} the weight of the driving wheel 3, and R is the radius of the driving wheel 3.
According to a thirteenth aspect of the present invention, in the control parameter determination device for each driving device of the multi-axis moving body according to the eleventh aspect, the gain adjusting means obtains the torque ratio of the driving wheel from the equation (2), and the equation (3 From (5) to (5), the gain of each drive wheel is corrected to stably control the multi-axis moving body.
T _3a : T _3b : T _3c = (T _3a1 * T _3a2 ) :( T _3b1 * T _3a2 ) :( T _3c2 * T _3a1 ) ・ ・ ・ ・ Formula (2)
Here, T _3a , T _3b , T _3c : Torque of driving wheel,
T _3a1 , T _3b1 : Torque of the drive wheels 3a, 3b when linearly moving in one direction,
T _3a2 , T _3c2 : Torques of the drive wheels 3a, 3c when linearly moving in other directions,
G _3a = G • T _3a / (T _3a + T _3b + T _3c ) ・ ・ ・ ・ ・ ・ Equation (3)
G _3b = G · T _3b / (T _3a + T _3b + T _3c ) ··· Equation (4)
G _3c = G · T _3c / (T _3a + T _3b + T _3c ) ········· Equation (5)
Where G _3a , G _3b , G _3c : the corrected gain of each drive wheel,
G: Gain of the drive wheel when moving the estimated weight of the load on the center of gravity

According to the first aspect of the present invention, the deviation of the center of gravity is estimated from the torque ratio between the driving devices generated when the multi-axis moving body travels linearly until reaching a constant speed, and is calculated as a control parameter. In addition, by performing optimum gain adjustment for each driving device, it is possible to realize stable traveling control around the center of gravity of the multi-axis moving body.
According to the second aspect of the present invention, in a multi-axis moving body, until a constant angular velocity is reached in a state where the center of gravity position of the multi-axis moving body is shifted by placing a load of unknown weight on the multi-axis moving body. By rotating the multi-axis moving body around the center of gravity, the center of gravity position is estimated from the torque ratio between the drive units, and by calculating the control parameters, the center of gravity of the multi-axis moving body is stable. Traveling control is possible.
According to the third and ninth aspects of the invention, when the multi-axis moving body is moved at a constant speed, the dynamic friction coefficient μ of the road surface is estimated from the torque increase generated in the drive device, and is calculated as a control parameter. Can be stably controlled.

According to the invention described in claims 4 and 5, the weight of the load is estimated from the torque of each driving device using the time derivative of the speed, is calculated as a control parameter, and the optimum gain adjustment is performed for each driving device. Thus, stable traveling control centered on the position of the center of gravity of the multi-axis moving body can be realized.
According to the invention described in claims 6 and 12, the weight of the load can be estimated easily and accurately by estimating the weight of the load from the equation (1).
According to the seventh and thirteenth aspects of the present invention, the gain of each drive wheel can be easily and accurately corrected from the equations (2) to (5).

According to the invention described in claim 8, the center-of-gravity position deviation is estimated from the torque ratio between the driving devices generated when the multi-axis moving body is linearly moved or rotated until reaching a constant speed, and is used as a control parameter. By calculating and performing optimum gain adjustment for each drive device, stable traveling control centered on the position of the center of gravity of the multi-axis moving body can be realized.
According to the invention described in claims 10 and 11, the weight of the load is estimated from the torque of each driving device using the time derivative of the speed, is calculated as a control parameter, and the optimum gain adjustment is performed for each driving device. Thus, stable traveling control centered on the position of the center of gravity of the multi-axis moving body can be realized.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

<Method of estimating the center of gravity>
FIG. 1 is an example of a multi-axis moving body 1 according to the present invention, in which (a) is a top view and (b) is a front view.
Three drive wheels 3 (3a to 3c) are attached to the side surface of the housing 2 at intervals of 120 degrees. A large number of wheels 5 that rotate in the axial directions 3as to 3cs of the drive wheels 3a to 3c are arranged on the outer circumferences of the drive wheels 3a to 3c, respectively, so that the drive wheels 3a to 3c are in addition to the rotation directions 3ar to 3cr. It can also move in the axial direction 3as-3cs. The multi-axis moving body 1 travels and rotates around the center of gravity position 4 with the load W placed on the center of the upper surface thereof.

FIG. 2 is a top view when a load is placed on the three-wheeled multi-axis moving body of the present invention.
In the drawing, a state in which a load W of unknown weight is placed on the upper surface of the multiaxial moving body 1 while being shifted from the center of gravity position 4 of the multiaxial moving body 1 is shown. By arranging in this way, the center of gravity of the multi-axis moving body 1 including the load W moves from the center of gravity position 4 to the center of gravity position 7.

FIG. 3 is a flowchart 8 of a method for estimating the weight and the center of gravity position of the load W when the load W is placed on the multi-axis moving body 1 at a position off the center as shown in FIG.
First, it is determined whether or not there is a change in the weight of the load W on the multi-axis moving body 1 (9a), and if there is no change as a result of the determination, the process jumps to step 15a and continues running.
Further, if there is a change in the determination result, first, the routine moves to a linear motion routine in one direction, and linear motion in one direction is performed in step 10a, and each torque is detected in each of the drive wheels 3a to 3c (step 11a). Torque ratio detection between the drive wheels 3a to 3c is performed (step 12a).
Next, the process moves to a linear motion routine in another direction, and linear motion is performed in a direction different from the linear motion in the previous step 10a (step 10b). Each torque detection in each driving wheel 3a-3c is performed (step 11b), and the torque ratio detection between the driving wheels 3a-3c is performed (step 12b).
Thereafter, in step 13a, the weight and the center of gravity position of the load W are estimated, calculated as control parameters, and after gain correction for each drive wheel 3 (step 14a), the multi-axis moving body 1 is run ( Step 15a).

The method of the flowchart 8 will be specifically described with reference to FIGS.
First, the first routine of the flowchart 8 of FIG. 3 is executed as shown in FIG. FIG. 4 is a top view when the three-wheeled multi-axis moving body of the present invention is linearly traveled. As shown in FIG. 4, the multi-axis moving body 1 is linearly moved in a direction 19c parallel to the axis of the drive wheel 3c passing through the center of gravity position 4 with the load W mounted thereon until a constant speed is reached (step of FIG. 3). 10a). In this case, a speed command is given to the drive wheels 3a and 3b, but a speed command is not given to the drive wheel 3c, and the wheels 5 arranged on the outer periphery of the drive wheel 3c are idled.
As a result of the eccentricity of the center of gravity position 7 from the center of gravity position 4, the torque T _3a1 20 of the driving wheel 3a and the torque T _3b1 21 of the driving wheel 3b detected as reaction force from the driving wheels 3a and 3b that are linearly moving are detected. (Step 11a in FIG. 3) and a difference occur between the two. Therefore, the axis 22 connecting the mounting portions of the drive wheels 3a and 3b is distributed at this torque ratio (T _3a1 : T _3b1 ). In this case, the line segment 23 has a length due to the torque T _3b1 21 of the drive wheel 3b, and the line segment 24 has a length 20 due to the torque T _3a1 of the drive wheel 3a, and the distribution point 25 is obtained by this ratio.

Next, the second routine of the flowchart 8 of FIG. 3 is executed as shown in FIG. FIG. 5 is a top view when the three-wheeled multi-axis moving body of the present invention is linearly moved in the other direction. FIG. 5 shows a linear motion in a linear direction different from the linear direction of FIG. 4. In the figure, the multiaxial moving body 1 is moved at a constant speed in a direction 19 b parallel to the axial direction of the drive wheel 3 b passing through the center of gravity position 4. 10b linear motion until reached.
In this case, the speed command is given to the drive wheels 3a and 3c, but the speed command is not given to the drive wheel 3b, and the wheels 5 arranged on the outer periphery of the drive wheel 3b are idled.
As a result, since the center of gravity position 7 is eccentric from the center of gravity position 4, a difference is generated between the torque T _3a2 26 of the driving wheel 3a detected as a reaction force from the driving wheels 3a and 3c and the torque T _3c2 27 of the driving wheel 3c. The axis 28 connecting the respective attachment portions of the drive wheels 3a and 3c is distributed at this torque ratio (T _3a2 : T _3c2 ). In this case, the line segment 29 has a length due to the torque T _3a2 26 of the drive wheel 3a, and the line segment 30 has a length due to the torque T _3c2 27 of the drive wheel 3c, and the distribution point 31 is obtained.

  FIG. 6 is a top view for explaining how to obtain the position of the center of gravity when a load is placed on the three-wheeled multi-axis moving body of the present invention. As can be seen from FIG. 6, from the intersection of the straight line 32 parallel to the axis of the drive wheel 3c from the distribution point 25 obtained in FIG. 4 and the straight line 33 parallel to the axis of the drive wheel 3b from the distribution point 31 obtained in FIG. The barycentric position 7 can be estimated.

  In order to obtain the position of the center of gravity position 7 with higher accuracy, the multi-axis moving body 1 is linearly moved also in a direction 19a (FIG. 6) parallel to the direction of the axis 3as (see FIG. 1a) of the drive wheel 3a passing through the center of gravity position 4. Also good. In this case, a speed command is given to the drive wheels 3b and 3c, but a speed command is not given to the drive wheel 3a, and the wheel 5 provided on the outer periphery of the drive wheel 3a is idled so that the torque of the drive wheel 3b and the drive wheel The torque of 3c is measured. Since a difference in torque occurs because the center of gravity moves, a distribution point can be obtained by distributing the axis line connecting the mounting portions of the drive wheels 3b and 3c at this torque ratio. Therefore, when a straight line parallel to the axis of the drive wheel 3a is drawn from this distribution point, it can be confirmed that the estimated center of gravity is accurate by passing through the center of gravity position obtained in FIGS. If the center of gravity is shifted from the previous center of gravity, correction such as taking the center of gravity of the triangle drawn by the three estimated centers of gravity may be performed.

<Method for estimating the weight of the load>
Next, a method for estimating the weight of the load W will be described.
Torque T _3a1 17 detected as a reaction force from the drive wheels 3a and 3b obtained when the multi-axis moving body 1 is linearly moved in the linear motion direction 19c shown in FIG. , T _3b1 18 and each speed ω of the driving wheel 3 at that time are calculated by the following equations.
(T _3a1 + T _3b1 ) ・ cosθ = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (1)
Here, M _1: weight of the multi-axis moving body 1, M _2: weight of the mounted object W, M _3: the weight of the driving wheel 3
R: Radius of the drive wheel 3 The weight M _{2 of the} load W can be estimated from the equation (1). θ is an angle between the multi-axis moving body 1 and the driving wheel 3 and changes depending on the number of driving wheels 3 attached to the multi-axis moving body 1. When there are two drive wheels, θ = 0, and when there are three wheels, θ = 30 °.

Similarly, when the multiaxial moving body 1 is linearly moved in the direction 19b until reaching a constant speed, the torques T _3a2 23 and T _3c2 24 detected from the drive wheels 3a and 3c are used to obtain the equation (1). The weight of the load W can be estimated.

<Calculation method of drive wheel gain>
Next, the gain G of the driving wheel 3 when the estimated weight of the load W is moved on the center of gravity position 4 of the multi-axis moving body 1 is calculated in advance.
The torque ratio (T _3a1 : T _3b1 ) of the drive wheels 3a and 3b obtained when the multi-axis moving body 1 is linearly moved in the linear movement direction direction 19c shown in FIG. 4 until reaching a constant speed. From the torque ratio (T _3a2 : T _3c2 ) of the drive wheels 3a and 3c obtained when the multi-axis moving body 1 is linearly moved in the linear movement direction 19b shown in FIG. 5 until reaching a constant speed. The torque ratio (T _3a : T _3b : T _3c ) of the three drive wheels 3a, 3b, and 3c is obtained by the equation (2).

T _3a : T _3b : T _3c = (T _3a1 * T _3a2 ) :( T _3b1 * T _3a2 ) :( T _3c2 * T _3a1 ) ・ ・ ・ ・ Formula (2)

Using the torque ratio (T _3a : T _3b : T _3c ), the gains of the drive wheels 3a, 3b, and 3c are corrected by the following equations (3), (4), and (5).
G _3a = G • T _3a / (T _3a + T _3b + T _3c ) ・ ・ ・ ・ ・ ・ Equation (3)
G _3b = G · T _3b / (T _3a + T _3b + T _3c ) ··· Equation (4)
G _3c = G · T _3c / (T _3a + T _3b + T _3c ) ········· Equation (5)
Where G _3a , G _3b , G _3c : the corrected gain of each drive wheel,
G: The gain of the driving wheel 3 when the estimated weight of the load W is moved on the center of gravity position 4.
The corrected gain calculated in this way is used for speed control and position control of the drive wheels 3.

<Another method for calculating drive wheel gain>
Next, another correction gain calculation method for each drive wheel 3 will be described.
FIG. 7 is a flowchart of another gain correction method for the three-wheeled multi-axis moving body of the present invention. In the flowchart 40 in FIG. 7, first, it is determined whether or not the weight of the load W on the multi-axis moving body 1 has changed (9b). If the result of the determination is that there is no change, the process jumps to step 15b and continues running. If there is a change as a result of the determination, the process moves to step 41 to perform a rotational motion, detect each torque in each drive wheel 3a-3c (step 11c), and detect a torque ratio between the drive wheels 3a-3c (step 11c). Step 12c).
Next, the weight of the load W is estimated (step 42), and this is calculated as a control parameter. After performing gain correction for each drive wheel 3 (step 14b), the multi-axis moving body 1 is caused to travel (step 15b). ).

Next, the specific method of the method of the flowchart 40 of FIG. 7 is demonstrated using FIG. FIG. 8 is a top view of the three-wheeled multi-axis moving body of the present invention, and shows a circumference 42 (dotted line) centered on the center of gravity 4 where the position of the drive wheel 3a of the three-wheeled multi-axis moving body 1 is zero. It is a thing. When a speed command is given to rotate the multi-axis moving body 1 around the center of gravity position 4 until a constant angular velocity is reached in one rotation (step 41 in FIG. 7), torque T _{3a of} the drive wheels 3a, 3b, 3c, T _3b and T _3c are obtained as shown in FIG. 9 depending on the position of the center of gravity position 7 on the circumference 42 indicated by dotted lines. FIG. 9 is a diagram showing the correlation between the position of the center of gravity and the torque of each drive wheel in the three-wheel multi-axis moving body of the present invention. In the figure, (a) shows the torque change of the drive wheel 3a, (b) shows the torque change of the drive wheel 3b, and (c) shows the torque change of the drive wheel 3c.
As shown in the figure, the torques T _3a , T _3b , T _3c of the drive wheels 3a, 3b, 3c draw sine waves whose phases are shifted by 2 / 3π depending on the position of the center of gravity position 7 on the circumference 43, The amplitude of each sine wave increases or decreases in proportion to the weight of the load W.
For example, when the center-of-gravity position 7 is placed on the circumference indicated by the dotted line on the multi-axis moving body 1 as shown in FIG. 8, the torque of the driving wheel 3a, the torque of the driving wheel 3b, and the torque of the driving wheel 3c. The respective sizes are the sizes of arrows 44, 45 and 46 in the figure.
Using the lengths of the torque magnitudes 44, 45, and 46 of the drive wheels as the torque ratio (T _3a : T _3b : T _3c ), the above formulas (3), (4), and (5) Gain can be corrected.

<Method of estimating the weight and center of gravity of the load in the case of a two-wheeled multi-axis moving body>
Next, a method for estimating the weight of the load and the center of gravity position when there are two drive wheels 3 of the multi-axis moving body 1 as shown in FIG. 10 will be described.
<Method of estimating the center of gravity for a two-wheeled multi-axis moving body>
First, a method for estimating the center of gravity in the case of a two-wheeled multi-axis moving body will be described.
FIG. 10 is an example of the two-wheeled multi-axis moving body of the present invention, (a) is a top view, (b) is a side view, and FIG. 11 is a mounted object on the two-wheeled multi-axis moving body of the present invention. It is a top view at the time.
In FIG. 10, drive wheels 3 a and 3 b are provided on opposite surfaces of the housing 2, and the center of gravity position when there is no mounted object is 4. In addition, an auxiliary wheel 47 is attached to the housing 2 to prevent it from falling.
As shown in FIG. 11, by placing the mounted object W of unknown weight on the upper surface of the multi-axis moving body 1, the center of gravity including the multi-axis moving body 1 and the mounted object W moves from the center of gravity position 4 to the center of gravity position 7.

FIG. 12 is a flowchart of the center-of-gravity and weight estimation method and gain correction method in the two-wheeled multi-axis moving body of the present invention. In the flowchart 48 shown in FIG. 12, first, it is determined whether or not the weight of the load W on the multi-axis moving body 1 has changed (step 9c), and if there is no change, the process jumps to step 15c and continues running. .
If there is a change as a result of the determination, a linear motion is performed at step 10c, each torque is detected at each drive wheel 3a, 3b (step 11d), and a torque ratio between the drive wheels 3a, 3b is detected (step). 12d). Thereafter, in step 13c, the weight and the center of gravity of the load W are estimated, calculated as control parameters, and after gain correction of each drive wheel 3a, 3b (step 14c), the multi-axis moving body 1 is traveled. (Step 15c).

A specific method of the method of the flowchart 48 will be described.
FIG. 13 is a top view for explaining how to obtain the position of the center of gravity when a load is placed on the two-wheeled multi-axis moving body of the present invention. When the multi-axis moving body 1 is linearly moved in the direction 49 until reaching a constant speed V as shown in FIG. 13 (10c in FIG. 12), the center of gravity position 7 is decentered from the center of gravity position 4, thereby driving wheels 3a, each torque detected becomes T _3a3 50, T _3b3 51 in 3b.
Driving wheels 3a, the axis 52 connecting the mounting shaft of 3b, resulting torque ratio in linear motion 10c: the locations are split (T _3a3 T _3b3) can be estimated that the center of gravity position 7 is present. In this case, the center-of-gravity position 7 of the length 53 due to the torque T _3b3 51, the top of the distribution lines 55 divided by the torque T _3a3 50 by a length 54.

<Method of estimating the weight of the load in the case of a two-wheeled multi-axis moving body>
Next, a method for estimating the weight of a load in the case of a two-wheeled multi-axis moving body will be described.
The weight of the load W can be estimated as M ₂ from Equation (6).
T _3a + T _3b = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (6)
Here, M _1: weight of the multi-axis moving body 1, M _2: weight of the mounted object W, M _3: the weight of the driving wheel 3
R: radius of drive wheel 3

<Method for determining control parameters by constant velocity motion>
Next, a method for estimating the dynamic friction coefficient when the multi-axis moving body 1 is moved at a constant speed, calculating as a control parameter, and stably controlling the multi-axis moving body will be described.
When the multi-axis moving body 1 is moved at a constant speed, the torque Tm detected by the drive wheel 3 is as shown in Expression (7). Here, the coefficient of static friction is ignored.
Tm = μ · (M ₁ + M ₂ ) · g · r / S Equation (7)
Where μ: dynamic friction coefficient, M ₁ : weight of the multi-axis moving body 1, M ₂ : weight of the load W,
g: gravitational acceleration, r: radius of the driving wheel 3, and S: the number of the driving wheel 3.
The dynamic friction coefficient μ is estimated from the equation (7). It is calculated as a control parameter and used for gain adjustment of the drive wheel 3.
Where G _3a '= G _3a · (T + Tm) / T ·········································
G _3b '= G _3b (T + Tm) / T (8)
G _3c '= G _3c・ (T + Tm) / T ・ ・ ・ ・ ・ ・ Equation (9)
Where G _3a ', G _3b ', G _3c ': Gain after correction of each drive wheel
G _3a , G _3b , G _3c : _Load gain, gain of each driving wheel corrected after estimating the center of gravity
T: The weight of the load, and the torque value in the speed command after estimating the position of the center of gravity.

  The multi-axis moving body of the present invention estimates the weight and the center of gravity position of the load by moving the load on the multi-axis moving body at a constant speed or each constant speed, and automatically obtains control parameters. Therefore, it is possible to save the trouble of setting the control even when a load of an arbitrary weight is placed, and it can be expected to improve the convenience of operation of the multi-axis moving body.

It is an example of the three-wheeled multi-axis moving body of this invention, (a) is a top view, (b) is a front view. It is a top view when a load is placed on the three-wheeled multi-axis moving body of the present invention. 3 is a flowchart of a center of gravity and weight estimation method and gain correction method for a three-wheeled multi-axis moving body of the present invention. It is a top view when the three-wheeled multi-axis moving body of the present invention is linearly traveled. It is a top view when the three-wheeled multi-axis moving body of the present invention is linearly traveled in another direction. It is a top view explaining how to obtain the position of the center of gravity when a load is placed on the three-wheeled multi-axis moving body of the present invention. It is a gain correction method flowchart of the three-wheeled multi-axis moving body of the present invention. It is a top view of the three-wheeled multi-axis moving body of the present invention. It is a diagram which shows the correlation of the gravity center position in the three-wheeled multi-axis moving body of this invention, and the torque of each driving wheel. It is an example of the two-wheeled multi-axis moving body of this invention, (a) is a top view, (b) is a side view. It is a top view when a load is placed on the two-wheeled multi-axis moving body of the present invention. It is a flowchart of the gravity center and weight estimation method and gain correction method in the two-wheeled multi-axis moving body of the present invention. It is a top view explaining how to obtain the position of the center of gravity when a load is placed on the two-wheeled multi-axis moving body of the present invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Multi-axis moving body 2 Housing | casing 3a, 3b, 3c Drive wheel 4 Center-of-gravity position when there is no load 7 Position of center of gravity when there is a load 5 Wheel which exists in the outer periphery of a drive wheel and can rotate to the axial direction of a drive wheel 47 Auxiliary wheel W

Claims (13)

In the control parameter determination method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing,
Place the load of unknown weight on the multi-axis moving body with the center of gravity shifted,
Detecting the torque of each of the driving devices generated when the multi-axis moving body travels in a straight line until reaching a constant speed,
The torque ratio of each driving device is obtained from each detected torque,
Estimate the distance of the center of gravity shift from the torque ratio,
A control parameter determination method for each driving device of a multi-axis moving body, wherein the distance is calculated as the control parameter. In the control parameter determination method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing,
Place the load of unknown weight on the multi-axis moving body with the center of gravity shifted,
Obtaining the torque ratio between the respective driving devices by rotating the multi-axis moving body around the center of gravity of the multi-axis moving body until reaching a certain angular velocity;
A control parameter determination method for each driving device of a multi-axis moving body, wherein the torque ratio is calculated as a control parameter. In the control parameter determination method for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing,
Place the load of unknown weight on the multi-axis moving body with the center of gravity shifted,
Detecting the torque of each driving device generated when the multi-axis moving body is moved at a constant speed,
Estimating the dynamic friction coefficient of the road surface from the increase in torque detected by the drive device,
A control parameter determination method for each driving device of a multi-axis moving body, wherein the dynamic friction coefficient is calculated as a control parameter. Estimating the weight of the load using a time derivative of speed from each detected torque,
4. The method for determining a control parameter for each driving device of a multi-axis moving body according to claim 1, wherein the weight is calculated as a control parameter.   5. The control parameter determining method for each driving device of a multi-axis moving body according to claim 4, wherein an optimum gain adjustment is made for each driving device based on a control parameter for each driving device determined by the parameter determining method. . 5. The method for determining control parameters for each drive unit of a multi-axis moving body according to claim 4, wherein the weight of the load is estimated from the equation (1).
(T _3a1 + T _3b1 ) ・ cosθ = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (1)
Here, T _3a1 + T _3b1 : Detected torque of the driving device, θ is an angle between the multi-axis moving body and the driving wheel, and changes depending on the number of driving wheels attached to the multi-axis moving body. When θ = 0, Three wheel θ = 30 °, M ₁ : Weight of multi-axis moving body 1, M ₂ : Weight of load W,
M _{3 is} the weight of the driving wheel 3, and R is the radius of the driving wheel 3. 6. The control of each driving device of a multi-axis moving body according to claim 5, wherein the torque ratio of the driving wheel is obtained from the equation (2), and the gain of each driving wheel is corrected from the equations (3) to (5). Parameter determination method.
T _3a : T _3b : T _3c = (T _3a1 * T _3a2 ) :( T _3b1 * T _3a2 ) :( T _3c2 * T _3a1 ) ・ ・ ・ ・ Formula (2)
Here, T _3a , T _3b , T _3c : Torque of driving wheel,
T _3a1 , T _3b1 : Torque of the drive wheels 3a, 3b when linearly moving in one direction,
T _3a2 , T _3c2 : Torques of the drive wheels 3a, 3c when linearly moving in other directions,
G _3a = G • T _3a / (T _3a + T _3b + T _3c ) ・ ・ ・ ・ ・ ・ Equation (3)
G _3b = G · T _3b / (T _3a + T _3b + T _3c ) ··· Equation (4)
G _3c = G · T _3c / (T _3a + T _3b + T _3c ) ········· Equation (5)
Where G _3a , G _3b , G _3c : the corrected gain of each drive wheel,
G: Gain of the drive wheel when moving the estimated weight of the load on the center of gravity In the control parameter determination device for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing,
Torque detecting means for detecting the torque of each of the driving devices generated when the multi-axis moving body is accelerated to travel linearly or rotate;
Torque ratio calculating means for obtaining a torque ratio of each of the drive devices from each detected torque;
Centroid position estimating means for estimating a distance shifted from the centroid position from the torque ratio calculated by the torque ratio calculating means;
And a control parameter calculation unit that calculates the distance estimated by the center-of-gravity position estimation unit as a control parameter. In the control parameter determination device for each driving device of the multi-axis moving body in which two or more driving devices are arranged on the side surface of the housing,
Torque detecting means for detecting the torque of each of the driving devices generated when the multi-axis moving body travels linearly with constant speed motion;
Road surface dynamic friction coefficient estimating means for estimating the dynamic friction coefficient of the road surface from the detected increase in torque;
A control parameter determining device for each driving device of a multi-axis moving body, comprising: control parameter calculating means for calculating the dynamic friction coefficient estimated by the road surface dynamic friction coefficient estimating means as a control parameter.   The said control parameter calculating means estimates the weight of the said mounting thing using the time derivative of speed from each said detected torque, and calculates this estimated weight as a control parameter. A control parameter determination device for each driving device of a multi-axis moving body. (Device claim of claim 5)
11. The control parameter determination device for each drive device of a multi-axis moving body according to claim 10, further comprising gain adjustment means for performing an optimum gain adjustment for each drive device based on the calculated control parameter. 11. The method for determining control parameters for each drive unit of a multi-axis moving body according to claim 10, wherein the weight of the load is estimated from the equation (1).
(T _3a1 + T _3b1 ) ・ cosθ = (M ₁ + M ₂ ) ・ R ²・ dω / dt + M ₃・ R ²・ dω / dt (1)
Here, T _3a1 + T _3b1 : Detected torque of the driving device, θ is an angle between the multi-axis moving body and the driving wheel, and changes depending on the number of driving wheels attached to the multi-axis moving body. When θ = 0, Three wheel θ = 30 °, M ₁ : Weight of multi-axis moving body 1, M ₂ : Weight of load W,
M _{3 is} the weight of the driving wheel 3, and R is the radius of the driving wheel 3. The multi-axis moving body according to claim 11, wherein the gain adjusting means obtains a torque ratio of the driving wheels from the equation (2) and corrects the gain of each driving wheel from the equations (3) to (5). Of determining control parameters for each of the drive units.
T _3a : T _3b : T _3c = (T _3a1 * T _3a2 ) :( T _3b1 * T _3a2 ) :( T _3c2 * T _3a1 ) ・ ・ ・ ・ Formula (2)
Here, T _3a , T _3b , T _3c : Torque of driving wheel,
T _3a1 , T _3b1 : Torque of the drive wheels 3a, 3b when linearly moving in one direction,
T _3a2 , T _3c2 : Torques of the drive wheels 3a, 3c when linearly moving in other directions,
G _3a = G • T _3a / (T _3a + T _3b + T _3c ) ・ ・ ・ ・ ・ ・ Equation (3)
G _3b = G · T _3b / (T _3a + T _3b + T _3c ) ··· Equation (4)
G _3c = G · T _3c / (T _3a + T _3b + T _3c ) ········· Equation (5)
Where G _3a , G _3b , G _3c : the corrected gain of each drive wheel,
G: Gain of the drive wheel when moving the estimated weight of the load on the center of gravity

Images