JP2022144549A - Control system and control method for automated guided vehicle - Google Patents

Abstract

To provide a control system and control method for an automated guided vehicle making it possible to estimate the posture of a tow cart to be towed without using an additional sensor.SOLUTION: A control system 1 for an automated guided vehicle that tows a tow cart includes a self position/posture estimation unit 9 that estimates the position and posture of the automated guided vehicle, and a tow cart posture estimation unit 11 that estimates two posture angles of the tow cart on the assumption that initial values of posture angles which are angles of the tow cart with respect to the automated guided vehicle are predesignated left and right maximum posture angles.SELECTED DRAWING: Figure 1

Description

The present invention relates to an automatic guided vehicle control system and control method.

2. Description of the Related Art Conventionally, an unmanned guided vehicle is known that detects a target, an obstacle, etc. while searching the surrounding environment, determines a moving route, moves according to the determined moving route, and transports an article. An unmanned guided vehicle may be connected to a tractor to transport articles. In this case, obstacle avoidance may become difficult due to the influence of the inner ring difference of the tow truck. At that time, it is necessary to calculate the trajectory calculation of the automatic guided vehicle and the tow truck, and the interference with obstacles, and to implement the route planning. However, in order to calculate the trajectory of the tractor for the movement of the AGV, it is necessary to know the posture of the tractor relative to the AGV. In order to detect the attitude of the towing vehicle, a distance sensor is installed at the rear of the automatic guided vehicle and the angle is calculated from the sensing results, or a potentiometer is installed at the connection point of the towing vehicle and the angle is calculated from the output value. It is necessary to install some kind of sensor, such as a method.

Patent Literature 1 discloses an independent mobile robot in which a connecting portion between a towing cart and a robot towing the towing cart is fixed and the position of the center of gravity of the whole is used as a reference. Patent Literature 2 discloses an automatic guided vehicle in which a range sensor is installed at the rear of the automatic guided vehicle to detect the position and orientation of the towing vehicle for transportation. In Patent Document 3, the main sensor used for self-position estimation etc. senses the tow truck when making a large turn, detects the presence or absence of the tow truck, and travels accordingly to prevent contact with obstacles. An autonomous mobile robot system is disclosed.

JP 2019-117431 A JP 2014-186680 A International Publication No. WO2019/053798

In the self-supporting mobile robot described in Patent Document 1, control is performed based on the position of the center of gravity of the entire transportation system, but there is a possibility that the transportation may not be flexible because the connecting portion is fixed. The automatic guided vehicle described in Patent Document 2 requires an additional sensor for detecting the towing vehicle in addition to the sensor used for normal running, which may increase the cost for implementation. In the autonomous mobile robot system described in Patent Document 3, it is not possible to detect the tow truck during small turns that cannot be sensed by existing sensors, or depending on the mounting position and height of the sensor. may not be able to.

SUMMARY OF THE INVENTION The present invention has been made in view of the above-described circumstances, and provides a control system and control method for an unmanned guided vehicle that can estimate the posture of a towed vehicle to be towed without using additional sensors or the like. The challenge is to

In order to solve the above problems, the present invention employs the following means.
That is, the control system for an automatic guided vehicle that tows a towing vehicle according to the present invention includes a self-position/orientation estimating unit that estimates the position and attitude of the automatic guided vehicle, and an attitude angle that is the angle of the tow vehicle with respect to the automatic guided vehicle. and a towing vehicle attitude estimation unit that estimates two attitude angles of the towing vehicle based on the estimated position and attitude of the automated guided vehicle, using predetermined left and right maximum attitude angles as initial values. Prepare.
According to the present invention, the two attitude angles of the towing vehicle are estimated using predetermined left and right maximum attitude angles as initial values, so the attitude of the towing vehicle can be estimated without using additional sensors. .

In one aspect of the present invention, there is provided a route planning unit that plans a travel route based on the estimation results of the self-position/posture estimating unit and the tow truck posture estimating unit, which include the two posture angles. .
According to this aspect, since the travel route is planned based on the estimation result including the two attitude angles, it is possible to select the travel route that reliably avoids obstacles.

In one aspect of the present invention, the towing vehicle attitude estimation unit estimates the two attitude angles based on vehicle information including information on sizes and shapes of the automatic guided vehicle and the towing vehicle, and information on their connection. do.
According to this aspect, since two attitude angles are estimated based on the vehicle information of the automatic guided vehicle and the tow truck, it is possible to estimate the attitude accuracy with high accuracy.

A control method for an automated guided vehicle that tows a towing vehicle according to the present invention includes estimating the position and attitude of the automated guided vehicle, and adjusting the attitude angle, which is the angle of the tow vehicle with respect to the automated guided vehicle, to a predetermined left and right angle. and estimating two attitude angles of the towing vehicle based on the estimated position and attitude of the automated guided vehicle, with the maximum attitude angle of .
According to the present invention, the two attitude angles of the towing vehicle are estimated using predetermined left and right maximum attitude angles as initial values, so the attitude of the towing vehicle can be estimated without using additional sensors. .

An aspect of the present invention includes planning a travel route based on estimation results including the two posture angles and the position and posture of the automatic guided vehicle.
According to this aspect, since the travel route is planned based on the estimation result including the two attitude angles, it is possible to select the travel route that reliably avoids obstacles.

In one aspect of the present invention, estimating the two attitude angles is performed based on vehicle information including information on sizes and shapes of the automatic guided vehicle and the towing vehicle and connection between them.
According to this aspect, since two attitude angles are estimated based on the vehicle information of the automatic guided vehicle and the tow truck, it is possible to estimate the attitude accuracy with high accuracy.

ADVANTAGE OF THE INVENTION According to this invention, the control system and control method of an automatic guided vehicle which can estimate the attitude|position of the towing vehicle pulled without using an additional sensor etc. can be provided.

1 is a block diagram showing the configuration of an automatic guided vehicle control system according to an embodiment of the present invention; FIG. It is a top view which shows the positional relationship of the automatic guided vehicle which concerns on embodiment of this invention, and a tow truck. It is a top view which shows the positional relationship of the automatic guided vehicle which concerns on embodiment of this invention, and a tow truck. It is a graph which shows the change of the attitude|position angle of the automatic guided vehicle which concerns on embodiment of this invention, and a tow truck. It is a graph which shows the change of the attitude|position angle width of the automatic guided vehicle and tow truck which concern on embodiment of this invention. FIG. 3 is a diagram showing a grid-like cost map in route planning according to an embodiment of the present invention; 4 is a graph showing the evaluation (weight) of obstacles on the cost map in route planning according to the embodiment of the present invention; FIG. 4 is a diagram showing setting of a cost map within a local area in route planning according to the embodiment of the present invention; FIG. 4 is a diagram showing generation of route candidates on a cost map in route planning according to the embodiment of the present invention; FIG. 4 is a diagram illustrating calculation of cumulative costs of route candidates in route planning according to the embodiment of the present invention; FIG. 4 is a diagram illustrating calculation of cumulative costs according to the position and orientation range of the towing vehicle in the route planning according to the embodiment of the present invention;

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the present invention, the "position" of an automatic guided vehicle or towing vehicle means the x, y coordinate position on the map where these vehicles move. When referring to the "attitude" of an automatic guided vehicle or towing vehicle, it refers to the angle on the map of the traveling direction of these vehicles.

FIG. 1 shows a block diagram showing the configuration of an automatic guided vehicle control system according to an embodiment of the present invention. FIG. 2 is a plan view showing the positional relationship between the automatic guided vehicle 3 and the tow truck 5 according to the embodiment of the present invention. The automatic guided vehicle control system 1 according to the present embodiment includes an external sensor 7, a self-position/orientation estimator 9, a tow truck orientation estimator 11, an obstacle detector 13, a route planner 15, and a travel controller 17. ing. The automatic guided vehicle control system 1 according to this embodiment further includes map information 19 and vehicle information 21 for data reference.

Each functional block of the self-position/orientation estimating unit 9, the tow truck orientation estimating unit 11, the obstacle detecting unit 13, the route planning unit 15, and the traveling control unit 17 of the control system 1 may be configured by hardware, Each function may be implemented by software. These functional blocks are implemented, for example, on an information processing device such as a personal computer. Each functional block may be software or a program executed by the CPU or GPU in the information processing apparatus. Alternatively, these functional blocks may be software or programs provided on cloud services. The map information 19 and the vehicle information 21 are databases composed of a semiconductor memory or the like, and transmit information according to requests from each functional block of the control system 1 .

In each of the above components, the external sensor 7 is an LRF (Laser Range Finder), a LiDAR (Light Detection and Ranging), a monocular or stereo camera, etc. A sensor that measures the distance and angle to surrounding structures and obstacles. is. The obstacle detection unit 13 detects surrounding structures/obstacles from the distance measurement data acquired by the external sensor 7 . The self-position/orientation estimating unit 9 uses map information created in advance, distance measurement data acquired by the external sensor 7, and odometry of the automatic guided vehicle 3 from the travel control unit 17 to estimate the automatic guided vehicle 3 on the map. Estimate the position and pose of

The tow truck orientation estimation unit 11 receives the position and orientation estimation result of the automatic guided vehicle 3 by the self-position and orientation estimation unit 9, the odometry of the automatic guided vehicle 3 from the travel control unit 17, the vehicle information 21, and the past tow truck orientation estimation result. , the current posture of the tow truck 5 is estimated. The route planning unit 15 uses the map information 19, the obstacle detection result by the obstacle detection unit 13, the position/orientation estimation result of the automatic guided vehicle 3, the vehicle information 21, and the orientation estimation result of the tow truck 5 to determine the traveling route. to plan

The travel control unit 17 follows the travel route planned by the route planning unit 15 and sends speed, angular velocity, and the like to the unmanned guided vehicle 3 to issue travel instructions. It also receives the odometry of the automatic guided vehicle 3 and sends the information to the self-position/attitude estimator 9 and the tractor trolley attitude estimator 11 . The map information 19 is a map of an area including the planned travel range, and is used by the self-position/orientation estimating section 9 and the route planning section 15 . The vehicle information 21 includes information about the size, shape, and connection of the automatic guided vehicle 3 and the tow truck 5 .

<Position estimation of tow truck>
Next, the attitude estimation of the tow truck in the above configuration will be described. As shown in FIG. 2, the attitude angle ζ that the tow truck 5 can take with respect to the automatic guided vehicle 3 is limited to a range in which the tow truck 5 does not come into contact with the automatic guided vehicle 3. Therefore, the attitude angle ζ The range is limited according to the shape of the tractor 5 and the like. In FIG. 2, the towing vehicle R at the position of the right maximum posture angle and the towing vehicle L at the position of the left maximum posture angle are indicated by two-dot chain lines. The range of these two posture angles is the posture angle range t that can be assumed as the posture angle. In this embodiment, the left and right maximum posture angles are used as initial values for estimating the posture angle ζ.

FIG. 3 is a plan view showing in further detail each parameter of the positional relationship between the automatic guided vehicle 3 and the towing vehicle 5 according to the present embodiment, and shows the state of the automatic guided vehicle 3 and the towing vehicle 5 at time t in FIG. and inputs to the automatic guided vehicle 3 are shown as follows.

Here, x _t and y _t are the position of the reference point M of the automatic guided vehicle 3 on the map, θ _t is the orientation angle of the automatic guided vehicle 3 on the map, and ψ _t is the orientation of the tow truck on the map. represents the angle of Each parameter is shown in FIG. 3 as a symbol that does not include the time component t, and the reference point of the tow truck is represented by N(x _T , y _T ). v _t and ω _t represent the velocity and angular velocity of the automatic guided vehicle 3 . The automatic guided vehicle 3 in FIG. 3 is shown moving at a turning center P and a turning radius r. At this time, the turning radius r _t at time t is represented by r _t =v _t /ω _t . However, turning left is + and turning right is -. When the turning radius at the connection point Q between the automatic guided vehicle 3 and the tow truck 5 is rc _,t , the speed vc _,t at the connection point Q and each speed ωc _,t are expressed by the following equations.

Therefore, the angle φ _t at time t of the connection point Q with respect to the automatic guided vehicle 3 with respect to the turning center P is expressed as follows.

In the above formula, d represents the distance between the reference point M and the connection point Q of the automatic guided vehicle. Therefore, the amount of change in the position and orientation of the automatic guided vehicle 3 is as follows.

The amount of change in the angle ψ _t of the towing truck 5 is given by the following equation.

In the above formula, l represents the distance between the reference point N of the tow truck 5 and the connection point Q. Therefore, finally, the model formula of the state change of the automatic guided vehicle 3 and the towing vehicle 5 is as follows.

Based on this, the attitude angle ζ _t of the tow truck 5 with respect to the automatic guided vehicle 3 is expressed as ζ _t = ψ _t - θ _t . Attitude angle ζ _t+1 after Δt seconds can be obtained.

When estimating the attitude angle of the tow truck 5, two initial values, ie, the maximum and minimum angles of the attitude angle range, are used to perform two estimations. It is assumed that there is a tow truck between the two states. FIG. 4 is a graph showing the results of calculation of the posture angle ζ when the maximum left-right angle is set to 90 degrees as an initial value and l is 2 m. In FIG. 4, the horizontal axis represents the running distance, and the vertical axis represents the posture angle. As can be seen from FIG. 4, as the running distance increases, the attitude angles with the maximum attitude angle of 90 degrees and the minimum attitude angle of −90 degrees as the initial values converge toward 0 degrees. In this case, the calculation is for the automatic guided vehicle 3 traveling straight ahead (ω _{t =} 0). is the initial value, the difference between the estimated attitude angles decreases. FIG. 5 is a graph showing the estimated range width, which is the difference between the maximum value and the minimum value, against the traveled distance on a logarithmic axis. As shown in FIG. 5, the difference between the maximum value and the minimum value decreases as the running distance increases. From FIG. 5, it can be seen that the width of 180 degrees at the initial value decreases to about 10 degrees at 6.5 m and to about 1 degree at 11 m and converges.

<Route planning using two attitude angles of the tow truck 5>
By the above estimation, two types of position/attitude data of the position/attitude of the automatic guided vehicle 3 and two types of position/attitude ranges (position/attitude within the maximum/minimum angle range) of the towing vehicle 5 can be obtained. The route planning unit 15 plans a travel route based on these two types of data. That is, these two types of data are input to the route planning algorithm, and a route that does not cause contact with the position of the obstacle measured by the external sensor 7 is selected for all of the two types of data.

An example of using Dynamic Window Approach (DWA) as a local route planning algorithm is shown below. In contrast to global route planning that searches for routes that avoid previously found obstacles on the map, local route planning dynamically searches for routes to avoid obstacles found by the automated guided vehicle 3 after the fact. It is. In the case of DWA, some route candidates are randomly generated within the dynamic window, that is, the control range that the automatic guided vehicle can take at the present time, for example, the range of combinations of speed and turning speed, and a predetermined evaluation function is used from among them. choose the route that maximizes The evaluation function can be the distance to the global route, the distance traveled, the distance to the obstacle, or a combination of these. is used.

The cost map is a grid-like map on a two-dimensional space in which the cost is determined according to the distance from the obstacle S (Fig. 6). In the cost map of FIG. 6, the area of the obstacle S is shaded. Along arrow A, the cost (weight) of obstacle S is evaluated as shown graphically in FIG. First, when an obstacle S is found, a cost map is set within the local area (Fig. 8), then route candidates are randomly generated within the Dynamic Window (Fig. 9), and furthermore, unmanned transport is performed for each route candidate during route movement. The cumulative cost of the area occupied successively by the car 3 and the towing truck 5 is calculated (Fig. 10), and finally the route with the lowest cost is selected. When obtaining this occupied area, the posture position is used for the above-described automatic guided vehicle 3, but the position and posture range is used for the tow truck 5 (FIG. 11).
In addition to DWA, Trajectory Rollout algorithm, Rapidly exploring Random Tree star (RRT*) algorithm, and the like can also be used as the route planning algorithm.

As described above, according to the present embodiment, based on the vehicle information of the automatic guided vehicle 3 and the towing vehicle 5, the predetermined left and right maximum attitude angles are set as initial values, and the two attitude angles of the towing vehicle 5 are set as initial values. Estimate ζ. Therefore, the attitude angle of the tow truck can be estimated with high accuracy without using an additional sensor. Since the travel route is planned based on the estimation results including the posture angle range estimated with the maximum and minimum posture angles as initial values, it is possible to select a travel route that can reliably avoid obstacles.

In the above-described embodiment, two types of position and orientation data of the towing vehicle 5 are used to plan the travel route. Attitude estimation of the cart 7 may be performed. For example, when the estimated range width becomes smaller than 1 degree, the next position/orientation estimation is performed by estimating only one of the position/orientation with the maximum or minimum as the initial value, and control of the automatic guided vehicle 3 is continued. good. In addition to the above, for example, if the estimated range width is between 5 degrees and 1 degree, the average value of the estimated attitude angles with the maximum and minimum initial values is used to plan the driving route. can be planned.

1 Automatic guided vehicle control system 3 Automated guided vehicle 5 Tow truck 9 Self-position/attitude estimation unit 11 Tow truck attitude estimation unit 15 Route planning unit 21 Vehicle information ζ Attitude angle

Claims (6)

A control system for an automated guided vehicle that tows a tow truck,
a self-position/orientation estimator that estimates the position and orientation of the automatic guided vehicle;
A posture angle, which is the angle of the towing vehicle with respect to the automatic guided vehicle, is set to a predetermined left and right maximum posture angle as an initial value, and based on the estimated position and posture of the automated guided vehicle, the two positions of the towing vehicle are calculated. A control system for an unmanned guided vehicle, comprising: a traction vehicle posture estimation unit that estimates two posture angles. The unmanned carrier according to claim 1, further comprising a route planning unit that plans a travel route based on the estimation results of the self-position/attitude estimating unit and the tow vehicle attitude estimating unit, which include the two attitude angles. car control system. 3. The towing trolley posture estimating unit estimates the two posture angles based on vehicle information including information on sizes and shapes of the automatic guided vehicle and the towing trolley and connection between them. A control system for an automated guided vehicle as described. A control method for an automated guided vehicle that tows a towing vehicle,
estimating the position and orientation of the automated guided vehicle;
A posture angle, which is the angle of the towing vehicle with respect to the automatic guided vehicle, is set to a predetermined left and right maximum posture angle as an initial value, and based on the estimated position and posture of the automated guided vehicle, the two positions of the towing vehicle are calculated. and estimating one attitude angle. 5. The method of controlling an automatic guided vehicle according to claim 4, comprising planning a travel route based on an estimation result including the two attitude angles and the position and attitude of the automatic guided vehicle. 6. The unmanned vehicle according to claim 4 or 5, wherein estimating the two posture angles is performed based on vehicle information including information on sizes and shapes of the automated guided vehicle and the towing vehicle, and connections between them. How to control the vehicle.

Images