JP4269170B2 - Trajectory tracking control method and apparatus - Google Patents

Description

  The present invention obtains control command values necessary to move the moving body by following the target trajectory as the traveling speed, the traversing speed, and the turning speed, respectively, and moves the moving body to the target trajectory with high accuracy. The present invention relates to a trajectory tracking control method and apparatus that can be used.

  A technique for moving a moving body following a preset target trajectory is used for running control of an automated guided vehicle in a factory or the like. The track following control of this kind of automatic guided vehicle exclusively detects a travel line previously laid on the road surface (floor surface) using a sensor mounted on the automatic guided vehicle, and the amount of deviation from the travel line of the automatic guided vehicle This is realized by steering control of the automatic guided vehicle so that the amount of deviation is zero (0).

In addition, as a method for smoothly performing such track tracking control, a non-holonomic method is adopted, and its traveling speed and steering angle can be controlled according to the deviation angle of the position and orientation of the unmanned traveling vehicle with respect to the target track. Has been proposed (see, for example, Patent Document 1). In a motion control system under non-holonomic constraints, it has been proposed to perform trajectory tracking control by correcting the orientation of the moving body while correcting the lateral displacement of the moving body with respect to the target trajectory by turning motion. (For example, see Non-Patent Document 1).
JP-A-11-327641 Journal of the Robotics Society of Japan Vol.11, No.6, 837-844, 1993

  However, in the above-described control method, if the deviation angle of the moving body changes, even if the actuator that controls the speed and the steering angle is driven by the same amount according to the deviation amount, the method of returning to the target trajectory There is a defect that is different. That is, even if the actuator is driven in proportion to the deviation amount, it is difficult to obtain an appropriate tracking performance in a wide operation range. In particular, even when steering is performed at a low speed, there is almost no traversing / turning motion, so that the deviation (deviation amount) is not reduced by steering. Therefore, there is a problem that a large load is applied to the feedback control system for steering.

  The present invention has been made in view of such circumstances, and its purpose is to make the moving body follow the target trajectory stably according to the amount of deviation from the target trajectory, regardless of the moving speed of the moving body. It is an object of the present invention to provide a trajectory following control method and a control device capable of appropriately obtaining a control command necessary for the above and improving the trajectory following performance.

Trajectory tracking control method according to the present invention in order to achieve the above objects, and steering control of the mobile, the mobile body to follow the target trajectory in the two-dimensional plane with a predetermined progression target speed u _d swivel target speed When moving with γ _d ,
<a> the lateral longitudinal displacement amount x _e with the mobile of the moving body with respect to the control target position of the moving object in the target orbit and an amount of deviation y _e with finding respectively, with the control target position It determined the orientation deviation angle phi _e of the moving body with respect to the control target posture of the movable body, given as a tangential direction of the target track,
<b> The shift amount _{x e,} and _{y e} and deviation angle phi _e, from said traveling target speed _{u d} and the turning target speed gamma _d, the shift amount _{x e,} while correcting the _{y e} and deviation angle phi _e A longitudinal speed command value u _r , a lateral speed command value ν _r , and a turning speed command value γ _r for the moving body for causing the moving body to follow the target trajectory are respectively calculated in accordance with preset arithmetic expressions. ,
<c> The traveling speed and the steering angle of the moving body are determined in accordance with these speed command values u _r , ν _r , and γ _r to control the traveling speed, the traversing speed, and the turning speed of the moving body, respectively. It is a feature.

Specifically, the control target posture on the target trajectory is obtained as a tangential direction of the target trajectory at the control target position, and the moving body with respect to the control target position and the control target posture. The respective deviation amounts of are calculated from the distance to the target trajectory and the distance to the control target position in the width direction of the moving body respectively measured by sensors mounted before and after the moving body, or stored in advance. The absolute coordinates (x _d , y _d ) of the control target position, the control target posture φ _d , the absolute position (x, y) of the mobile body determined by the sensor mounted on the mobile body, and the direction of the mobile body It is obtained by calculation from the posture φ shown.

The trajectory tracking control system according to the present invention is mounted on a mobile body by the steering control of the movable body, to follow the target trajectory set in advance in a two-dimensional plane moving body to a predetermined progression target speed u _d And turn at a turning target speed γ _d ,
<A> before and after the moving body with respect to the control target position of the movable body in the target orbit direction of the shift amount x _e and a transverse direction and a shift amount y _e with determining each of the target track in the control target position Sensing means for obtaining a deviation angle φ _e of the moving body relative to a control target posture given as a tangential direction ;
<B> the amount of deviation _{x e,} and _{y e} and deviation angle phi _e, from said traveling target speed _{u d} and the turning target speed gamma _d, the shift amount _{x e,} while correcting the _{y e} and deviation angle phi _e A microcomputer that calculates the longitudinal speed command value u _r , the lateral speed command value ν _r , and the turning speed command value γ _r for causing the moving body to follow the target trajectory according to a preset arithmetic expression, etc. Computing means;
<C> According to these speed command values u _r , ν _r , and γ _{r, the} moving speed of the moving body and the steering angle are determined to drive a steering system such as an actuator that controls the behavior of the moving body to drive the moving It is characterized by comprising movement control means for controlling the traveling speed, traversing speed, and turning speed of the body.

Specifically, as described in claim 4 , the calculating means sets the moving target speed set to the calculating means as u _d , the traversing target speed as ν _d , and the turning target speed as γ _d. , A longitudinal speed command value u _r , a lateral speed command value ν _r , and a turning speed command value γ _r for the moving body according to the deviation amounts x _e , y _e and the deviation angle φ _e detected by the sensing means. u _r = u _d cos φ _e + K _x x _{e
ν r = ν d + K y} u d y e
_{γ r = γ d + K p} u d sinφ e
However, K _x , K _y , and K _p are each configured to be obtained as a positive control gain.

According to the trajectory tracking control method having the above-described configuration, the front and rear of the moving body are determined from the target moving speeds u _d and γ _{d of the} moving body and the deviation amounts x _e and y _e with respect to the target trajectory according to the direction deviation angle φ _e. The direction speed command value u _r , the lateral direction speed command value ν _r , and the turning speed command value γ _r are respectively obtained, and the traveling speed, the traversing speed, and the turning speed of the moving body are independently controlled. precisely follow are not the target track by the advanced or turning operation in response to the angular phi _e becomes possible to move the moving body.

Incidentally, conventionally, for example while correcting the longitudinal displacement of the moving body in search of the front-rear direction speed command value u _r as introduced in Non-Patent Document 1 described above, the mobile seeking rotation speed command value gamma _r In this case, the right and left direction deviation and the direction angle deviation correction are made to work in a trade-off manner. It is difficult to determine. In this regard, since in the present invention obtains a lateral velocity command value [nu _r and the rotation speed command value gamma _r independently in addition to the front-rear direction speed command value u _r, and shift correction in the horizontal direction of displacement correction and orientation angle Can be controlled individually. Accordingly, it is possible to accurately correct the deviation with respect to the target trajectory and increase the tracking accuracy with respect to the target trajectory.

In particular, the longitudinal speed command value u _r , the lateral speed command value ν _r , and the turning speed command value γ _r are _expressed as u _r = u _d cos φ _e + K _x x _{e
ν r = ν d + K y} u d y e
_{γ r = γ d + K p} u d sinφ e
Therefore, non-linear control is performed according to the direction deviation angle φ _e , so that, for example, even if the direction deviation angle φ _e is large, movement in a direction away from the target trajectory can be eliminated. In other words, when the speed is increased even though the direction deviation angle φ _e is large, the moving body may move away from the target trajectory. However, according to the control described above, the traverse speed is taken into account by taking the deviation angle φ _e into consideration. Therefore, it is possible to optimize the behavior and suppress the deviation from the target trajectory. Further, since each of the command values u _r , ν _r , and γ _r described above is obtained by adding a correction amount determined according to the target traveling speed u _d , for example, unintentional transverse motion or turning motion during low-speed traveling Can be prevented. Therefore, effects such as the ability to improve followability with respect to the target trajectory are exhibited. That is, it is possible to reduce the useless steering control, reduce the control burden, and further reduce the influence of disturbance.

A trajectory tracking control method and control apparatus according to an embodiment of the present invention will be described below with reference to the drawings.
FIG. 1 shows the basic control principle of the trajectory tracking control method according to this embodiment. L is a preset target trajectory, and M is a moving body that is controlled to follow the target trajectory L. . The target track L is composed of a travel line defined by, for example, laying a plurality of magnetic elements along a predetermined route in a port site, and the moving body M travels along the travel line L. Consisting of automated guided vehicles. The travel (movement) of the automatic guided vehicle (moving body) M along the traveling line (target track) L is basically the current position of the automatic guided vehicle (moving body) M and the traveling line (target track) L. This is performed by controlling the traveling speed of the automatic guided vehicle (moving body) M according to the deviation from the target position and controlling the steering angle.

That is, the trajectory tracking control method according to the present invention is a point to be controlled in the moving body M, specifically, a representative point expressing the behavior of the moving body M, for example, the current position (x, y) of the center of gravity G in the moving body M. ) And its direction (posture) φ and the difference (deviation amount) between the target position (x _d , y _d ) and its direction φ _d on the target trajectory L at a certain control point, for example, This is done by controlling the steering angle. The direction φ _d of the target position is defined as the tangential direction of the target trajectory L at the target position (x _d , y _d ).

That is, the follow-up control of the moving body M with respect to the target trajectory L starts with the current position / posture (x, y, φ) of the moving body M, as shown in FIG. ), The amount of displacement x _e (= x _d −x) in the front-rear direction of the moving body M with respect to the target position / posture (x _d , y _d , φ _d ) on the target trajectory L at the present time, and the moving body M The right and left displacement amount y _e (= y _d −y) and the displacement angle φ _e (= φ _d −φ) of the moving body M are respectively obtained [processing A]. Next, as will be described later, according to these shift amounts (x _e , y _e , φ _e ), the moving body M moves in the front-rear direction (traveling speed) u _r and moves in the left-right direction (transverse speed) ν. _r and turning speed γ _r are obtained as control command values [Process B], and the movement of the moving body M is controlled according to these control command values u _r , ν _r and γ _r [Process C]. Is called. Specifically, the movement control of the moving body M is performed by controlling the moving speed (traveling speed) of the moving body M and controlling the steering angle thereof.

Specifically, the detection of the amount of deviation (x _e , y _e , φ _e ) is performed using, for example, various sensors (for example, a magnetic sensor, an ultrasonic sensor, a transponder, a distance meter, etc.) mounted on the moving body M. It is obtained by directly measuring the distance to the track L. Alternatively, the absolute coordinates and orientation (x _d , y _d , φ _d ) of the target position are obtained from the data series that specifies the target trajectory L stored in advance, and the absolute position / posture (x, y, φ) of the moving body M is obtained. derived from GPS information or the like, the shift amount in accordance with the information _{(x e, y e, φ} e) , and it suffices to determine, based on the following calculation equation.

Incidentally, a technique for correcting the deviation based on such deviation amounts (x _e , y _e , φ _e ) and thereby performing the follow-up control with respect to the target trajectory L will be introduced in detail in Non-Patent Document 1 described above. Street. More specifically, _{u r = u d cosφ e +} K r x e
_{γ r = γ d + K y} u d y e + K p u d sinφ e
However, K _x, as K _y, K _p is a positive control gain, respectively, movement of the movable body M in search of said progress command speed of the moving object M u _r and the turning command speed gamma _r for correcting the deviation Can be controlled to follow the target trajectory L. That is, it is only necessary to correct the deviation in the direction while correcting the deviation in the left-right direction by the turning motion of the moving body M at a predetermined traveling speed. However, in this case, there is a problem of how to correct the lateral displacement y _e and the orientation displacement φ _e, and it is generally difficult to optimally set the control gains K _y and K _p .

Therefore, in the trajectory tracking control method according to the present invention, a control command value for independently correcting the aforementioned deviation amounts (x _e , y _e , φ _e ) is used as the target moving speed of the moving body M at that time. According to u _d , γ _d and the direction angle deviation φ _e , u _r = u _d cos φ _e + K _x x _e (1)
ν _r = ν _d + K _y u _d y _e (2)
γ _r = γ _d + K _p u _d sinφ _e (3)
[Processing B].

That is, the traveling speed command value u _r for the longitudinal direction of the moving body M, a target moving velocity u _d and the speed determined by the shift amount phi _e orientation angle at that time, the deviation amount in the longitudinal direction of the target position x _e It is determined by adding a correction amount determined according to the above. The rampant speed command value [nu _r for the right and left direction of the moving body M, by adding a correction amount determined by the target moving velocity u _d and the left-right direction of the deviation amount y _e of the movable body in the transverse velocity [nu _d at that time determined To do. Furthermore its turning speed command value gamma _r, is determined by adding a correction amount determined by the shift amount phi _e target moving velocity u _d and orientation angle of the movable body in turning velocity gamma _d at that time. The traversing speed ν _d may be obtained from the aforementioned target moving speed u _d and the direction angle φ of the moving body M at that time, and the turning speed γ _d is determined by the moving body M at that time. What is necessary is just to obtain | require according to the turning radius r etc. which are calculated | required from a steering angle.

The control command value for the moving body M is determined as the traveling speed command value u _r , the traverse speed command value ν _r , and the turning speed command value γ according to the deviation amount (x _e , y _e , φ _e ) of the moving body M with respect to the target position. after obtaining each as _r, these speed command value u _r, [nu _r, controls the operation of the actuator that controls the behavior of the moving object M in accordance with gamma _r. Specifically, the number of revolutions of the motor or engine that determines the traveling speed of the moving body M is controlled, and the operation amount of the steering and the rudder that determines the steering angle is controlled. The speed and the turning speed are respectively controlled [Process C].

If the traveling speed, the traversing speed, and the turning speed of the moving body M are individually controlled in this way, the above-described deviation amounts (x _e , y _e , φ _e ) can be independently corrected. Therefore, the moving body M can be moved along the target trajectory L with high accuracy. In addition, the correction to each of the speed command values u _r , v _r , γ _r can be performed independently based on the aforementioned deviation amounts (x _e , y _e , φ _e ), so that the control gains K _x , K _y , K It is possible to optimally set each _p independently.

In particular, according to the follow-up control described above, it is possible to control the traveling operation and the turning operation of the moving body M in accordance with the direction deviation angle φ _e , so that the direction deviation angle φ _e is qualitatively determined. Even if it is large, it is possible to eliminate an unintended behavior in which the moving body M moves away from the target trajectory L. That is, the traveling speed and the turning speed are respectively controlled according to the command value including the value determined nonlinearly according to the direction deviation angle φ _e as shown in the above-described equations (1) and (3). Therefore, even if the direction deviation angle φ _e is large, it is possible to eliminate the unintended behavior in which the moving body M gradually moves away from the target trajectory L.

Further, as shown in the aforementioned equations (2) and (3), the traversing speed ν _d and the turning speed γ _d at that time are multiplied by the target moving speed u _d and the deviation amount (y _e , φ _e ). since the determined corrected according to the correction amount determining that the traverse speed command value [nu _r a turning speed command value gamma _r respectively by, the traverse speed command value [nu _r and the rotation speed command when the target movement velocity u _d is small The value γ _r can be kept small to make it difficult for traversing motion and turning motion to occur.

Incidentally, when the moving body M target moving velocity u _d is minute of, if so be rampant and turning is set larger the steering angle is never little occurs, when the target moving velocity u _d is less than a predetermined threshold value It is desirable to add a restriction that suppresses the above-mentioned traverse speed and turning speed to zero. Displacement amount mentioned above when specifically a small target movement velocity _{u d (x e, y e} , φ e) based on the traverse speed command value [nu _r and the rotation speed command value gamma _r is calculated on the basis of the transverse-turning operation It is preferable to avoid the occurrence of unnecessary steering control. In other words, even if steering is performed during low-speed movement, if this does not cause traversing / turning motion, the above-described control is applied to limit the transverse speed and the turning speed to zero, thereby reducing unnecessary steering itself. It becomes possible.

And since traveling speed u _r of the moving object M under the transverse-pivoting movement described above is controlled, following the movement is to be effectively guaranteed to the target trajectory L of the moving object M. In particular, since the traveling speed, the traversing speed, and the turning speed of the moving body M are independently controlled according to the direction deviation angle φ _e , the moving body M is moved along the target trajectory L with good followability ( It is possible to achieve a great effect such as being able to travel).

The above-described control mode is an example of follow-up control on a two-dimensional plane in which the automatic guided vehicle (moving body) M is controlled to travel along the target track L in the premises of a harbor, The present invention can be similarly applied to a case where the target track is controlled to follow in a three-dimensional space like a submarine or an aircraft. In this case, a displacement amount x in the front-rear direction from the coordinate position (x, y, z) of the moving body M in the three-dimensional space and the target coordinates (x _d , y _d , z _d ) on the target trajectory L. _e (= x _d −x) is obtained, and the lateral displacement amount y _e is determined by the lateral displacement amount y _e (= y _d −y) of the movable body M with respect to the target trajectory L and the vertical displacement. the amount z _e (= z _d -z) as it suffices to determine, respectively. Further, the displacement angle φ _e of the moving body M is obtained as the yaw angle φ _ye , the pitch angle φ _pe, and the roll angle φ _{re in} the longitudinal direction of the moving body M with respect to the tangential direction of the target track L. Just do it.

Then, the control command value is _converted into a traveling speed command value x _{r for} correcting the deviation amounts x _e , y _e , z _e , φ _ye , φ _pe , and φ _re , respectively, and two kinds of transverse speed command values y _r , z _r and three types of turning speed command values φ _yr , φ _pr , and φ _rr may be obtained, and the traveling speed, traverse speed, and turning speed may be controlled independently. Even in this case, it is needless to say that it is preferable not to perform the steering when the target traveling speed is small and the transverse movement and the turning movement hardly occur.

As described above, according to the present invention, the longitudinal speed command value u _r for controlling the behavior (movement) of the moving body M according to the deviation angle φ _e of the direction of the moving body M with respect to the target trajectory L, the lateral speed. Since the command value ν _r and the turning speed command value γ _r are respectively determined, the traveling speed, the traversing speed, and the turning speed of the moving body M are controlled so that the moving body M follows the target trajectory L with high accuracy. Can be moved. Controlling the advancing speed (travel speed) according to the longitudinal direction the speed command value u _r in particular, also controls the steering angle in accordance with a lateral velocity command value [nu _r and the rotation speed command value gamma _r Thus, the moving body M can be moved following the target trajectory L. The followability can be sufficiently enhanced.

Particularly when the moving object M is such as automatic guided vehicle four-wheel steering type, in the steering angle of the rear wheels and the front wheel steering angle only uniquely determined respectively in accordance with the deviation angle phi _e orientation, the AGV It is possible to drive the vehicle (moving body) along the target track L, and since the control is simple, the practical advantage is great. Moreover, even if the four wheels (front wheels and rear wheels) are respectively steered, it is possible to prevent problems such as the unmanned transport vehicle (moving body) unintentionally leaving the target track (travel line).

  The present invention is not limited to the embodiment described above. For example, a deviation amount detecting means and a sensor used for deviation amount detection may be determined according to the type of the moving body and its specifications. Regarding the calculation means for the control command value, a dedicated hardware circuit may be constructed and the calculation process may be executed at a high speed, but may be executed by a software process using a microcomputer or the like. . In addition, the present invention can be variously modified and implemented without departing from the scope of the invention.

The figure which shows the basic control principle of the track following control method which concerns on this invention. The figure which shows the rough process concept of the track | orbit tracking control apparatus which concerns on this invention, and its process sequence.

Explanation of symbols

A deviation detection processing means B command value calculation means C movement control means

Claims (4)

Mobile steering control to, when moving the movable body so as to follow the target track at a predetermined traveling target speed u _d and the turning target speed gamma _d in a two-dimensional plane,
Together determine the lateral deviation amount y _e in the longitudinal direction of the deviation amount x _e with the mobile of the moving body with respect to the control target position of the movable body in the target orbit, respectively, said target at said control target position It obtains a deviation angle phi _e direction of the moving body with respect to the control target posture of the movable body, given as a tangential trajectory,
The shift amount x _e, and y _e and deviation angle phi _e, wherein the the traveling target speed u _d and the turning target speed gamma _d, the mobile the shift amount x _e, while correcting the y _e and deviation angle phi _e To calculate the longitudinal speed command value u _r , the lateral speed command value ν _r , and the turning speed command value γ _r for following the target trajectory according to a preset arithmetic expression ,
The traveling speed, the traversing speed, and the turning speed of the moving body are controlled by determining the traveling speed and the steering angle of the moving body according to the speed command values u _r , ν _r , and γ _r , respectively. Trajectory tracking control method. The control target attitude on the target trajectory is obtained as a tangential direction of the target trajectory at the control target position,
The respective displacement amounts of the moving body with respect to the control target position and the control target attitude are respectively measured by sensors mounted before and after the moving body , the distance to the target trajectory in the width direction of the moving body, and the control target position Or the absolute coordinates (x _d , y _d ) of the control target position calculated in advance or the absolute position of the moving body (x _d , y _d ) and the control target posture φ _d obtained by the sensor mounted on the moving body ( 2. The trajectory tracking control method according to claim 1, wherein the trajectory tracking control method is calculated from x, y) and a posture φ indicating the direction of the moving body. Is mounted on a mobile steering control movable body, the trajectory tracking of moving in advance the movable body to follow the target trajectory set in the predetermined traveling target speed u _d and the turning target speed gamma _d in a two-dimensional plane A control device,
The longitudinal displacement amount x _e and lateral deviation amount y _e of the moving body with respect to the control target position of the movable body in the target orbit with respectively obtained, as the tangential direction of the target track in the control target position Sensing means for obtaining an angle of deviation φ _e of the moving body relative to a given control target posture;
The shift amount x _e, and y _e and deviation angle phi _e, wherein the the traveling target speed u _d and the turning target speed gamma _d, the mobile the shift amount x _e, while correcting the y _e and deviation angle phi _e Calculating means for calculating a longitudinal speed command value u _r , a lateral speed command value ν _r , and a turning speed command value γ _r for following the target trajectory according to a preset arithmetic expression ,
The traveling speed and steering angle of the moving body are determined according to each of these speed command values u _r , ν _r , γ _r and the steering system of the moving body is driven to drive the traveling speed, traversing speed, and turning of the moving body. A trajectory follow-up control device comprising movement control means for controlling each speed. The computing means sets the deviation amount x detected by the sensing means, where u _d is the traveling target speed of the moving body set in the computing means, ν _d is the traversing target speed, and γ _d is the turning target speed. _{According to e 1} , y _e and the deviation angle φ _{e, the} longitudinal speed command value u _r , the lateral speed command value ν _r , and the turning speed command value γ _r for the moving body are changed to u _r = u _d cos φ _e + K _x x _{e
ν r = ν d + K y} u d y e
_{γ r = γ d + K p} u d sinφ e
However, the trajectory tracking control device according to claim 3, wherein K _x , K _y , and K _p are each obtained as a positive control gain.

Images