JP5712643B2 - Moving body - Google Patents

Description

  The present invention relates to a moving body.

  In recent years, mobile bodies that autonomously move on a defined trajectory have been developed. When the moving body wants to follow a predetermined trajectory in space, it is necessary to align the position of the moving body on the trajectory and to align the direction of the moving body with the direction of the trajectory.

  Here, when the moving body follows the track by feedforward control, a deviation from the track occurs due to bumping of the carriage due to road surface unevenness, wheel slipping, or the like. Therefore, it is necessary for the moving body to measure the position and direction of the moving body with the sensor, and to correct the speed in the traveling direction of the moving body and the rotational speed in the left-right direction so as to return to the orbit.

  Furthermore, when the mobile body passes through a narrow place, it is required to quickly converge on the trajectory without overshooting the trajectory.

  Patent Document 1 describes a moving body that sets a target displacement that counteracts changes in running resistance and lateral sliding force received from a floor surface. According to this, the moving body performs feedback control so as to coincide with the azimuth displacement of the moving body with respect to the turning direction, and performs feedforward control with respect to the translation direction.

  Patent Document 2 describes a moving body that performs feedback control with respect to a turning direction and sets a feedback operation amount when exceeding a stability limit speed. According to this, the moving body sets the feedforward amount while leaving the cost of the stabilization feedback operation that can be safely fed back with respect to the limit amount of the set feedback operation amount.

Japanese Patent Laid-Open No. 06-119038 JP 2008-074184 A

However, when the moving body approaches the track, it cannot get on the track well, and overshoot or vibration may occur. Further, when the moving body changes its speed in the traveling direction by feedback control, the speed change in the traveling direction becomes not smooth. Furthermore, the moving body may be off track if one of the wheels is limited by the rotational speed limit when an excessive speed is applied to the wheel.
The moving body according to the present invention is made to solve such problems, and provides a moving body that appropriately travels on a set track.

The moving body according to the present invention is a moving body having driving wheels, and includes position information acquisition means for acquiring a current position and direction, position information acquired by the position information acquisition means, and information on the trajectory of the moving body. Storage means for storing the control means, and the control means for calculating the distance deviation and the angle deviation, and calculating the relationship between the rotational speed of the drive wheel and the time variation of the distance deviation and the angle deviation And a setting means for giving a feedforward value as the translation speed of the moving body, and the time variation of the distance deviation using the same gain as the distance deviation, the angle deviation, and the gain of the distance deviation, Perform feedback control using.
As a result, the moving body can run smoothly without overshooting the track due to the influence of the gain and speed.

  The moving body travels appropriately on the set track.

1 is a block diagram of a moving body according to a first exemplary embodiment. FIG. 6 is a diagram illustrating a processing operation in the moving object according to the first embodiment. FIG. 3 is a diagram of a coordinate system according to the first embodiment. It is a figure when the moving body concerning Embodiment 1 changes minutely. It is a figure which shows the alignment of the moving body concerning Embodiment 1, and a driving wheel. It is a figure which shows the input to the controller concerning Embodiment 1, and the relationship of the mobile body 10 and a track | orbit. It is a figure which shows increase / decrease in the distance deviation of the moving body concerning Embodiment 1. FIG. It is a figure of the state which the mobile body concerning Embodiment 1 drive | works in parallel with a track | orbit. It is a figure which shows the relationship between the angle deviation of the moving body concerning Embodiment 1, and a rotation direction. It is a figure which shows the turning direction at the time of performing feedback control of the mobile body concerning Embodiment 1. FIG. It is a figure which shows the relationship between the distance deviation of a moving body concerning Embodiment 1, and an angle deviation. It is a figure which shows the relationship between the distance deviation of a moving body concerning Embodiment 1, and an angle deviation. It is a figure which shows the relationship between the distance deviation of a moving body concerning Embodiment 1, and an angle deviation. It is a figure of restriction | limiting of the left-right wheel speed of the moving body concerning Embodiment 1. FIG.

Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram of a moving body according to the present invention.

  The moving body 10 includes position information acquisition means 110, storage means 120, drive wheels 130, and control means 140.

The position information acquisition unit 110 is a sensor provided in the moving body 10. The position information acquisition unit 110 acquires the position information of the moving body 10 and information on the direction in which the moving body 10 is facing.
The position information acquisition unit 110 outputs the acquired position information and direction information of the moving body 10 to the storage unit 120.

The storage unit 120 stores position information and direction information supplied from the position information acquisition unit 110 and information on a trajectory along which the moving body 10 moves. Typically, the storage unit 120 is a memory or an HDD, but the storage medium is not limited to these. The storage unit 120 stores the curvature of the trajectory along with the path of the trajectory as trajectory information.
The storage unit 120 outputs the stored position information and direction information and trajectory information to the control unit 140.

The driving wheel 130 is a moving means provided in the moving body 10. The driving wheel 130 is driven to rotate when the driving unit 131 whose driving is controlled by the control unit 140 is operated. The moving body 10 moves in the front-rear direction, rotates, and turns when the driving wheel 130 is rotationally driven.
It is assumed that the drive wheels 130 are coaxially arranged on the left and right of the moving body 10. Below, the drive wheel 130 may be called a wheel.

  The driving unit 131 is driven by receiving a control signal from the control unit 140. The drive means 131 is typically a motor. The driving means 131 gives a driving force to the driving wheels 130 based on the control signal.

The control unit 140 includes a calculation unit 141 and a setting unit 142. The control unit 140 controls the traveling state of the moving body 10. More specifically, the control unit 140 performs feedback control on the turning speed of the moving body 10 based on the distance deviation, the angle deviation, and the time variation of the distance deviation. At this time, the control means 140 makes the gain of the time variation of the distance deviation the same as the gain of the distance deviation. This will be described in detail later.
The control means 140 is supplied with the rotation speed (wheel speed) of the drive wheels 130 input by the user and the information stored in the storage means 120.

The arithmetic means 141 typically has a CPU (Central Processing Unit) and performs arithmetic processing for the moving body 10 to travel on the track. The computing means 141 may include a ROM (Read Only Memory), a RAM (Random Access Memory), etc. for temporarily storing information.
The calculation unit 141 receives the position information, the direction information, and the trajectory information of the moving body 10 from the storage unit 120, and calculates the shortest distance (distance deviation) from the moving body 10 to the trajectory and the direction of the moving body 10 and the trajectory. Find the difference (angle deviation). In addition, the calculation unit 141 calculates the relationship between the wheel speed and the time variation of the distance deviation and the angle deviation.
Further, the computing means 141 performs computation for feedback control related to the turning speed of the moving body 10. The control unit 140 outputs a control signal to the drive unit 131 based on the value calculated by the calculation unit 141. Thereby, the drive wheel 130 operates so that the moving body 10 turns.

  The setting means 142 is a setting means for the user to set an arbitrary value (feed forward value) as the speed in the translation direction with respect to the trajectory of the moving body 10. The control unit 140 outputs a control signal to the drive unit 131 based on the feedforward value set by the setting unit 142. Thereby, the drive wheel 130 operates so that the moving body 10 travels in the translational direction of the track. Hereinafter, the setting unit 142 may be described as a controller.

Next, the operation of the moving body 10 will be described. Hereinafter, the moving body 10 and the track may be handled as an integrated system.
FIG. 2 is a diagram illustrating a state in which the moving body 10 travels by feedforward control and feedback control.
The user inputs the wheel speed to the moving body 10. The moving body 10 outputs the shortest distance to the track and the angle formed by the direction of the moving body 10 and the track. Moreover, the mobile body 10 expresses the relationship between the wheel speed and the time differentiation of the angle formed with the shortest distance to the track in Jacobian. The controller of the moving body 10 converts the input instruction into a reverse Jacobian format.
Translational velocity of the moving body 10 is set on the basis of the feedforward control. The turning speed of the moving body 10 is set by performing feedback control so that the distance deviation and the angle deviation converge according to the angle formed with the shortest distance to the track.

Furthermore, although the wheel speed of the moving body 10 has an upper limit in hardware and software, either one of the driving wheels 130 provided on the left and right of the moving body 10 is on the upper limit, and the track does not rotate according to the command value. Since the followability deteriorates, the wheel speed is corrected so as to follow the track without exceeding the upper limit.
Details will be described below.

  First, calculation of the relationship between the wheel speed, the translation speed, and the differential value of the angle deviation by the calculation means 141 will be described. The computing means 141 obtains the relationship between the translation speed, the rotational speed and the distance deviation, and the differential value of the angle deviation of the moving body 10, and then obtains the relation between the wheel speed, the translation speed, and the rotational speed.

First, the coordinate system of the moving body 10 is defined. Here, the positional relationship between the moving body 10 and the track is determined based on the moving body 10. The direction of rotation is determined to be positive in the counterclockwise direction. FIG. 3 is a diagram illustrating the positional relationship between the moving body 10 and the trajectory.
As shown in FIG. 3, the shortest distance from the moving body 10 to the orbit is defined as a distance deviation. The shortest distance is the length of a line segment connecting the position of the moving body 10 and the nearest point on the trajectory, and is assumed to be perpendicular to the tangent of the trajectory at the nearest point. Codes, vector p _r up trajectory as viewed from the mobile 10 _and the tangent vector of the direction in which the trajectory progresses and p _s, the sign of the z component of the cross product p _r × p _s.
The angle deviation is an angle of the trajectory viewed from the moving body 10. In the case of FIG. 3, the angle deviation is positive and the distance deviation is also positive. The computing means 141 performs computation based on this coordinate system.

である。角度偏差θの変化ｄθは、

である。また、以下の関係が成り立っている。

式（１．２）、（１．３）からｄθ_ｓを消去すると、

である。式（１．１）、（１．４）の微小時間ｄｔにおける変化は、

である。ここで、ｄｓ／ｄｔは移動体１０の並進速度、ｄθ_ｒ／ｄｔは移動体１０の回転速度であるから、これらをそれぞれｖ_ｒ、ω_ｒとおくと、

である。式（１．７）と式（１．８）を行列表現すると、

と表現できる。ここで、表記の簡単化のため、

として、

と表記する。 The computing means 141 obtains the relationship between the translational speed, the rotational speed, the distance deviation, and the differential value of the angle deviation of the moving body 10.
FIG. 4 is a diagram when the moving body 10 is slightly changed. 4A is a diagram related to a minute change in the distance deviation of the moving body 10, and FIG. 4B is a diagram related to a minute change in the angle deviation of the moving body 10.
If the minute variation along the moving direction of the moving body 10 is d _s , the minute rotation angle of the moving body 10 is dθ _r , the radius of curvature of the orbit is ρ _s , and the minute rotation dθ _{s of the} orbit is geometrically expressed by the following relationship. Holds.
The change dl of the distance deviation l is

It is. The change dθ in the angle deviation θ is

It is. In addition, the following relationship holds.

If dθ _s is eliminated from equations (1.2) and (1.3),

It is. The change in the minute time dt in the equations (1.1) and (1.4) is

It is. Here, since ds / dt is the translation speed of the moving body 10 and dθ _r / dt is the rotation speed of the moving body 10, these are set as v _r and ω _r , respectively.

It is. When Expression (1.7) and Expression (1.8) are expressed as a matrix,

Can be expressed as Here, for simplicity of notation,

As

Is written.

ここで、

は、移動体１０の右側に備えられた駆動輪１３０の回転速度であり、

は、移動体１０の左側に備えられた駆動輪１３０の回転速度である。
表記の簡単化のため、

として、

と表記する。
式（１．１０）と、式（１．１２）から、車輪速度と距離偏差、角度偏差の微分値の関係は、式（１．１３）となる。

Next, the calculation means 141 calculates | requires the relationship between the wheel speed of the mobile body 10, the translation speed of the mobile body 10, and a rotational speed. FIG. 5 is a diagram showing the alignment of the moving body 10 and the drive wheel 130. FIG. 5A is a plan view of the moving body 10, and FIG. 5B is a front view of the moving body 10.
If the distance W between wheels of the moving body 10 and the wheel radius r are kinematically, the relationship of equation (1.11) is established kinematically.

here,

Is the rotational speed of the drive wheel 130 provided on the right side of the moving body 10,

Is the rotational speed of the drive wheel 130 provided on the left side of the moving body 10.
To simplify the notation,

As

Is written.
From Expression (1.10) and Expression (1.12), the relationship between the wheel speed, the distance deviation, and the differential value of the angle deviation is Expression (1.13).

  Next, a case where the user inputs to the controller that commands the speed to the moving body 10 will be described. Here, the operation control of the moving body 10 by the operation of the controller is the same as the control of the normal Jacobian inverse matrix. The calculation means 141 performs the following calculation.

とする。ここで、

である。Ｊ_ｒの逆行列は、

であり、Ｊ_ｅの逆行列は、

である。コントローラは、式（１．１３）の行列の逆行列なので、ｕ＝[ｕ_ｌ，ｕ_θ]^Ｔで移動体１０を動かしたとき、距離偏差、角度偏差の微分値は以下のようにｕと等価である。

The input to the controller by the user is u = [u _l, u _θ ] ^T. Here, the wheel speed given to the moving body 10 is an inverse matrix of the equation (1.13). That is,

And here,

It is. Inverse matrix of J _r is,

And the inverse matrix of J _e is

It is. Since the controller is an inverse matrix of the matrix of equation (1.13), when the moving body 10 is moved with u = [u _l, u _θ ] ^T , the differential values of the distance deviation and the angle deviation are u and Is equivalent.

移動体１０への指令速度ｖ^＊は、入力[−ｖ_ｒｅｆｓｉｎθ，ｕ_θ]^Ｔを、式（２．４）に与えることで得られる。すなわち、

となる。
ここで、距離偏差、角度偏差が０のときｕ_θ＝０となるようにｕ_θを決めれば、ω^＊＝ｖ_ｒｅｆ／ρ_ｓとなり、軌道の曲率に追従するｖ^＊とω^＊になる。
式（２．７）のｖ^＊とω^＊を、移動体１０に与えると、式（１．９）より

となる。ｄｌ／ｄｔはθの値によって正にも負にもなるので、必ずしもｌが０に近づくとは限らない。図７は、距離偏差の増減を示した図である。図７（ａ）に示すように、ｌ＞０かつθ＞０の場合には、ｄｌ／ｄｔ＜０であり、ｌは減少する。図７（ｂ）に示すように、ｌ＞０かつθ＜０の場合には、ｄｌ／ｄｔ＞０であり、ｌは増加する。図７（ｃ）に示すように、ｌ＜０かつθ＜０の場合には、ｄｌ／ｄｔ＞０であり、ｌは減少する。図７（ｄ）に示すように、ｌ＜０かつθ＞０の場合には、ｄｌ／ｄｔ＜０であり、ｌは増加する。 FIG. 6 is a diagram illustrating the relationship between the input u ₁ to the controller, the moving body 10 and the trajectory. As shown in FIG. 6, the differential value of the distance deviation is dl / dt = −v _ref sin θ in a state where the moving body 10 is moving with respect to the trajectory at an angle θ and a command speed v _ref . _{U l} to generate such dl / dt is because it is _u l = dl / dt from the formula (2.5) is as follows a _{u l.}

The command speed v ^* to the moving body 10 is obtained by giving the input [−v _ref sin θ, u _θ ] ^T to the equation (2.4). That is,

It becomes.
Here, if u _θ is determined so that u _θ = 0 when the distance deviation and the angle deviation are 0, ω ^* = v _ref / ρ _{s is obtained} , and v ^* and ω ^* follow the curvature of the trajectory.
When v ^* and ω ^* in the equation (2.7) are given to the moving body 10, from the equation (1.9)

It becomes. Since dl / dt can be positive or negative depending on the value of θ, l does not always approach 0. FIG. 7 is a diagram showing increase and decrease of the distance deviation. As shown in FIG. 7A, when l> 0 and θ> 0, dl / dt <0 and l decreases. As shown in FIG. 7B, when l> 0 and θ <0, dl / dt> 0 and l increases. As shown in FIG. 7C, when l <0 and θ <0, dl / dt> 0 and l decreases. As shown in FIG. 7D, when l <0 and θ> 0, dl / dt <0 and l increases.

である。しかしながら、ｕ_θを角度偏差だけの関数にすると、軌道と移動体１０の方向が同じ場合に、移動体１０は回転せず、距離偏差が減少しない。図８に、移動体１０が軌道と平行に走行している状態を示す。図８のように、移動体１０は、角度偏差が無くとも距離偏差があるときには回転させる必要がある。
図９は、移動体１０の角度偏差と回転方向の関係を示した図である。
Next, the turning speed input _uθ is determined. Here, u _θ is a function of l and θ. That is,

It is. However, when the function of the u _theta angle deviation, when the track and the direction of the moving body 10 are the same, the moving body 10 does not rotate, distance deviation does not decrease. FIG. 8 shows a state in which the moving body 10 is traveling in parallel with the track. As shown in FIG. 8, the moving body 10 needs to be rotated when there is a distance deviation even if there is no angle deviation.
FIG. 9 is a diagram illustrating the relationship between the angular deviation of the moving body 10 and the rotation direction.

である。ここでαとβはゲインであり、正の値とする。 Therefore, for f (l, θ), when f (0, 0) = 0 and the moving body 10 approaches the orbit as shown in FIG. 9A, dθ / dt <0, and FIG. When the moving body 10 moves away from the orbit as shown in b), dθ / dt> 0 is set.
One form that satisfies these conditions is

It is. Here, α and β are gains, which are positive values.

FIG. 10 is a diagram qualitatively showing the case where the moving body 10 travels by the feedback control of Expression (2.10).
If θ> 0 and θ> βl, the moving body 10 moves so as to reduce the angular deviation while approaching the trajectory set as shown in FIG. This is state a.
If θ> 0 and θ = βl, the moving body 10 moves so as to linearly approach the trajectory set as shown in FIG. This is state b.
If θ> 0 and θ <βl, the moving body 10 moves so that the angular deviation becomes larger with respect to the trajectory as shown in FIG. This is state c.
If θ = 0, the moving body 10 moves so as to approach the track from the state of traveling in parallel with the track as shown in FIG. Let this be state d.
If θ <0, the moving body 10 changes its direction from the state of traveling away from the track as shown in FIG. 10E, and moves so as to travel in the direction approaching the track. This is state e.

FIG. 11 is a diagram showing loci of l and θ when l ≧ 0. The states a to e described in FIG. 11 correspond to the states a to e in FIG. In the case of l ≦ 0, the locus is a point object with respect to the origin.
In the process from state b to state a, when dθ / dt is smaller than dl / dt, the trajectory of pattern 1 in FIG. 11 is followed and a cycle of l ≦ 0 is entered, so that the moving body 10 vibrates. It will be. Further, when the absolute value of the final value theta _e than the absolute value of the initial value theta _s is large, the amplitude is increased while going cycled, diverging vibration. On the contrary, when dθ / dt is larger than dl / dt, it converges to l = 0 and θ = 0 along θ = βl.
When β is large, the angle θ at which the state b is reached becomes large, and the moving body 10 enters the track with an angle, so that the locus of the pattern 1 is likely to occur. Conversely, if β is reduced, the angle of state b is reduced, so convergence until l = 0 is slow.
When α is large, dθ / dt is large, so that the locus of pattern 2 is obtained.

Next, feedback of the differential value will be described.
This is because the vibration generated when the moving body 10 approaches the track becomes a track of the pattern 1 in FIG. 11 because the change in the distance deviation is relatively large with respect to the angle deviation.
Therefore, the calculation unit 141 performs a calculation so as to change the change of the angle deviation according to the change of the distance deviation.

Since the speed change of the distance deviation is −v _ref sin θ, the gain β used as the gain of the distance deviation is used and u _θ is set as follows.

FIG. 12 is a diagram illustrating the action of −βv _ref sin θ. As shown in FIG. 12, at the intersection of the l-θ trajectory and the straight line β1, dl / dt = v _ref sin θ, so the l-θ trajectory changes in a direction along the straight line. Therefore, the trajectory of the moving body 10 does not exceed the straight line θ = βl.

In addition, the straight line in which the expression (2.11) is 0 is θ = β · l · α / (α + βv _ref ) within a range where sin θ can be regarded as linear. FIG. 13 is a diagram illustrating the action of θ = β · l · α / (α + βv _ref ). In FIG. 13, θ = β · l · α / (α + βv _ref ) is always lower than βl. Furthermore, since dl / dt <0 and dθ / dt = 0 on this straight line, once the l-θ trajectory of the moving body 10 crosses this straight line and enters region 1 in FIG. Never enter 2.

Therefore, when feedback control of the expression (2.11) is performed in the moving body 10, l and θ change in a region sandwiched between the straight line θ = βl and θ = β · l · α / (α + βv _ref ). Will do. As a result, l and θ do not vibrate and converge to 0 at the same time.
Therefore, the calculation unit 141 performs a calculation related to the turning speed of the moving body 10 based on the formula (2.11), and the control unit 140 appropriately performs feedback control based on the calculation result.

Moreover, although the speed | rate given to the mobile body 10 was calculated | required by operation of a controller in Formula (2.7), the method to convert this speed into wheel speed is demonstrated.
At this time, the calculation means 141 obtains a wheel speed that does not cause the wheel speed to be excessive. For example, when the vehicle speed is set so that the moving body 10 cannot physically travel, it becomes impossible to follow the trajectory.
Below, the method of suppressing wheel speed is explained in full detail.

とすると、

となるが、

になるように制限する。ここで、

は、例えば、移動体１０がハードウェアとして走行可能な最高速度とする。具体的には以下のように設定する。

ここで、式（３．２）の最高速度は、

に制限される。 The wheel speed may be obtained by multiplying the inverse matrix J _r ⁻¹ of Equation (2.3) by v ^* obtained by Equation (2.7). That is, the wheel speed

Then,

But

To be limited. here,

Is, for example, the maximum speed at which the moving body 10 can travel as hardware. Specifically, it is set as follows.

Here, the maximum speed of the equation (3.2) is

Limited to

  FIG. 14 is a diagram illustrating the limitation of the left and right wheel speeds. When the formula (3.2) is applied, the wheel speed on the circumference of FIG. 14 is obtained, and when the formula (3.3) is applied, the wheel speed within the circle is taken.

であるため、演算手段１４１は、まず、

を計算し、左右の車輪速度を求める。次に、

を計算し、許容する車輪速度

と比較する。すなわち、

とする。
これにより、移動体１０に入力されたフィードフォワード量が過大となっても、左右の車輪の比は保存され、軌道に追従することができる。 Implementation is as follows.

Therefore, the calculation means 141 first has

To calculate the left and right wheel speeds. next,

Calculate and allow wheel speed

Compare with That is,

And
Thereby, even if the feedforward amount input to the mobile body 10 becomes excessive, the ratio of the left and right wheels is preserved and can follow the track.

さらに、移動体１０の左右の車輪速度を抑制することができる。

Therefore, when the speed of the moving body 10 is commanded, the speed of the translation direction with respect to the trajectory is set by feedforward and the turning speed is set by feedback control by using the equation (4.1). Can do.

Furthermore, the left and right wheel speeds of the moving body 10 can be suppressed.

The moving body 10 is based on the premise that the shortest distance direction and the tangential direction of the track are at right angles. For example, in the relationship between the tangent line at the starting point of the track and the current moving body 10, the angular deviation greatly deviates from the right angle. In some cases, the assumption may not be true.
In this case, the trajectory is regenerated with the current position of the moving body 10 as the starting point, the processing is started from a position where the distance deviation is perpendicular to the trajectory, or the position that becomes the starting point of the trajectory by another method. It is desirable to take measures such as moving to
In addition, when the moving body 10 reaches the end point of the track while traveling, it may not stop at the end point, and the subsequent behavior may become unstable. In this case, it is desirable that the moving body 10 is controlled to decelerate near the end point and stop at the end point.

Thereby, the moving body 10 can travel while preventing overshooting of the track and occurrence of vibration when adjusting the position so as to get on the track. Moreover, since the moving body 10 has set the speed by feedforward about the translation direction with respect to an orbit, it can prevent that the vibration of a speed | rate generate | occur | produces.
Furthermore, in the moving body 10, when the feedforward value is excessive, the speed of the driving wheel 130 is suppressed, and the operation ratio of the left and right wheels is maintained, so that the driving wheel 130 is limited by hardware or software. It can be prevented that the robot does not operate according to the operation instruction and goes off the track.
Therefore, the moving body 10 can travel appropriately with respect to the set track.

  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. For example, the calculation means for calculating the distance deviation and the angle deviation and the calculation means for calculating the relationship between the rotational speed of the driving wheel and the time variation of the distance deviation and the angle deviation may be different calculation means.

DESCRIPTION OF SYMBOLS 10 Mobile body 110 Position information acquisition means 120 Storage means 130 Drive wheel 140 Control means 141 Calculation means 142 Setting means

Claims (2)

A moving body having drive wheels,
Position information acquisition means for acquiring the current position and direction in space;
Storage means for storing information on the current position and the running track ;
Control means, and
The control means obtains a distance deviation which is the shortest distance between the current position and the trajectory, and an angular deviation which is a difference between the direction and the direction of the trajectory at the position where the distance deviation is obtained;
A setting means for setting an arbitrary feedforward value by a user input as the translation speed of the moving body,
The control means calculates a turning speed such that the distance deviation and the angle deviation converge using the distance deviation, the angle deviation, and an arbitrary feedback value,
Without using the value calculated by feedback, the translation speed is calculated using the feedforward value, and the rotational speed of the drive wheel is determined based on the translation speed and the turning speed.
Moving body. It said arithmetic means performs arithmetic operation for converting the rotation speed and the translational speed, the rotational speed of the driving wheel,
Wherein, the mean square of the rotational speed of the converted driving wheels, if it exceeds the maximum speed set in advance, the rotational speed of the drive wheel in accordance with the ratio between the mean square and the maximum wheel speed the driven by Genho state, if not exceeding the maximum speed set in advance, it controls to drive at a rotational speed of said converted the driving wheel,
The moving body according to claim 1.

Images