JP7227849B2 - Trajectory generator - Google Patents

Description

The technology disclosed in this specification relates to a trajectory generation device.

Mobile objects (eg, unmanned towing vehicles) that self-propelled to transport cargo have been developed. In order to generate a trajectory to the loading/unloading position of the cargo, it is necessary to estimate the loading/unloading position of the cargo on the basis of the moving body. For example, Patent Literature 1 discloses a technique of recognizing a sign placed at a destination using a camera.

JP 2003-285288 A

The recognition accuracy of a camera is low compared to the case of using a laser or the like. Therefore, it is difficult to estimate the position/orientation of the destination with a high degree of accuracy through sign recognition using a camera.

One embodiment of the trajectory generation device disclosed in this specification is a trajectory generation device that generates a trajectory for movement of a moving object from a starting position to a transport device. The conveying device includes a conveying path section extending in a first direction, and a load loading/unloading section arranged at an end of the conveying path section. The trajectory generation device includes a determination unit that determines whether the starting point position of the moving body is positioned in a positive direction or a negative direction of a second direction that is perpendicular to the first direction and extends in the horizontal direction. The trajectory generation device includes an acquisition unit that acquires measurement data indicating the position and orientation of the side surface of the transport route based on the measurement data of the environment sensor provided in the mobile body. The trajectory generating device calculates the position and orientation of the transport device with respect to the moving object by using an outer shape model represented by outer shape data indicating the outer shape of the transport device, which is prepared in advance, and measurement data. An estimator for estimating is provided. The trajectory generating device is a target trajectory for moving from the starting position to the front surface of the baggage loading/unloading section, and the orientation of the moving body, which is the direction in which the front surface of the moving body faces in the front surface of the loading/unloading section, matches the second direction. A trajectory generator for generating a target trajectory such as

The conveying device includes a conveying path portion extending in the first direction. Also, the starting position of the moving body is located in the positive direction or the negative direction of the second direction orthogonal to the first direction. Accordingly, the acquisition section can measure the position and orientation of the side surface of the transport path section using the environment sensor included in the moving body. Then, using the acquired measurement data and the external model, the position and attitude of the transport device can be estimated. Compared with the case of using a camera, it is possible to estimate the position and orientation of the conveying device with high accuracy.

The estimation unit may estimate the position and orientation of the transport device by superimposing the outer shape model on the measurement data. Details of the effect will be described in Examples.

The measurement data may be data indicating a predetermined angle of the side surface of the transport path portion with respect to the direction of the moving object and a reference position of the transport path portion. The estimating unit rotates the external model so that the angle that the side surface of the external model has with respect to the orientation of the moving body matches a predetermined angle, and rotates the external model so that the reference position of the external model matches the reference position of the measurement data. may be superimposed on the measurement data by moving . Details of the effect will be described in Examples.

The environmental sensor may be a laser sensor. The measurement data may be point cloud data measured by a laser sensor.

The conveying device includes a first marker arranged on a first side surface on the positive direction side in the second direction, and a second marker arranged on a second side surface on the negative direction side in the second direction. may be The determination unit may determine that the start point position is positioned in the positive direction of the second direction when the marker detected in the image captured by the camera of the moving object is the first marker. . The determination unit may determine that the starting point position is located in the negative direction of the second direction when the marker detected in the image is the second marker. Details of the effect will be described in Examples.

The determining unit may determine to which of the first template image and the second template image the image of the conveying device captured by the camera of the moving object matches to a higher degree. The first template image may be an image of the conveying device captured in advance from the positive side of the second direction. The second template image may be an image of the conveying device captured in advance from the negative direction side of the second direction. The determination unit may determine that the starting point position is located in the positive direction of the second direction when determining that the image of the conveying device matches the first template image to a high degree. The determining unit may determine that the starting point position is located in the negative direction of the second direction when determining that the image of the conveying device matches the second template image to a high degree. Details of the effect will be described in Examples.

The determining unit may determine that the start point position is located in the positive direction of the second direction when the work plan is to unload the load from the conveying device to the moving body. The determination unit may determine that the start position is located in the negative direction of the second direction when the work plan is to load the cargo from the moving body to the conveying device. Details of the effect will be described in Examples.

A moving body equipped with the above-described trajectory generation device is also novel and useful.

FIG. 2 is a block diagram showing the configuration of the towing vehicle; The figure which shows the specific example of track|orbit generation. The figure which shows the specific example of track|orbit generation. The figure which shows an example of external model data. 4 is a flowchart showing a method of generating a target trajectory; FIG. 4 is a top view showing how the position and orientation of equipment to be loaded and unloaded are measured. An example of measurement data for on-board equipment. The figure which shows the example which superimposed the external model data and the measurement data by an environmental sensor.

(Configuration of towing vehicle 10)
FIG. 1 shows a block diagram of a towing vehicle 10 according to this embodiment. The towing vehicle 10 is an unmanned towing vehicle. The towing vehicle 10 includes a vehicle body 20 , an arithmetic device 30 , an environment sensor 40 and a camera 41 . The vehicle body 20 has a steering control section 21 and a drive control section 22 . The steering control section 21 and the drive control section 22 are configured by a microprocessor including a CPU and the like. The steering control unit 21 is communicably connected to a steering device (not shown) and controls the traveling direction of the towing vehicle 10 . The drive control unit 22 is communicably connected to a driving wheel motor (not shown) and controls the running speed of the towing vehicle 10 .

The environment sensor 40 is installed on the vehicle body 20 . In this embodiment, the environment sensor 40 is a 2D-LiDAR. 2D-LiDAR is a range sensor that scans laser light in the horizontal direction. The environment sensor 40 measures the distance between the vehicle body 20 and an object in a space set in front of the vehicle body 20 . Thereby, the distance data in front of the towing vehicle 10 is acquired. The environment sensor 40 is installed at a height that allows measurement of the side surface of the loading/unloading equipment 15 . The distance data acquired by the environment sensor 40 is input to the measurement data acquisition unit 32 and the position/orientation estimation unit 35 of the computing device 30 . The camera 41 images the front of the vehicle body 20 . Image data captured by the camera 41 is input to the determination section 31 .

The computing device 30 is configured by a microprocessor including a CPU and the like. Arithmetic device 30 includes determination unit 31 , measurement data acquisition unit 32 , external model storage unit 33 , coordinate conversion unit 34 , position/orientation estimation unit 35 , trajectory generation unit 36 , speed pattern generation unit 37 , and control command generation unit 38 . Prepare. The contents of the determination unit 31 to the control command generation unit 38 will be described later.

(Outline description)
The technology described in this specification relates to trajectory generation of a guideless unmanned towing vehicle for loading and unloading containers (cargo) on the aircraft 1 . Specific examples are shown in FIGS. 2 and 3. FIG. FIG. 2 is a diagram showing a case where the tractor 10 approaches the aircraft 1 from the front (that is, a case where cargo is loaded onto the aircraft 1).

A tow vehicle 10 tows dolly (towed vehicle) 11-14. Containers C1-C4 are placed on each of the dollys 11-14. A loading/unloading device (high lift loader) 15 is arranged near the cargo compartment door of the aircraft 1 . The loading/unloading equipment 15 is equipment for loading/unloading the container onto/from the aircraft 1 .

The loading/unloading equipment 15 includes a baggage loading/unloading section 15a, a first side surface 15b, a second side surface 15c, and a transport path section 15d. The transport path portion 15d extends in the direction y0. The transport path portion 15d is a path along which the container is slid and moved. At the end of the transport path 15d in the positive direction of the direction y0, the baggage loading/unloading section 15a is arranged. The loading/unloading section 15a is a section for delivering containers to and from the dolly.

The loading and unloading equipment 15 has a loading and unloading equipment coordinate system CS0. The loading/unloading equipment coordinate system CS0 represents the posture of the loading/unloading equipment 15 with reference to the reference position P0. The reference position P0 may be any position. In this embodiment, the right corner of the loading/unloading section 15a is the reference position P0. The posture of the loading/unloading equipment 15 is determined by the direction y0, which is the direction in which the conveying path portion 15d extends, and the direction x0 perpendicular to the direction y0.

The first side surface 15b is the side surface of the loading and unloading equipment 15 on the positive direction side of the direction x0. A first marker M1 is arranged on the first side surface 15b. The second side 15c is the side of the loading and unloading equipment 15 on the negative direction side of the direction x0. A second marker M2 is arranged on the second side surface 15c. The first marker M1 and the second marker M2 are visual markers (for example, AR markers) that can detect unique IDs. The 1st marker M1 and the 2nd marker M2 are affixed by the magnet sheet.

The towing vehicle 10 moves from the start position P1 to the stop position P3. The starting point position coordinate system CS1 represents the attitude of the towing vehicle 10 at the starting point position P1. The attitude of the towing vehicle 10 is defined by a direction of travel x1 and a vertical direction y1 perpendicular to the direction of travel x1. The stop position P3 is a position where the dolly 11 is placed alongside the load loading/unloading section 15a of the loading/unloading equipment 15. As shown in FIG. In order to stop the tractor 10 at the stop position P3 with the dolly aligned in this manner, a target trajectory TT (dotted line in FIG. 2) from the start position P1 to the stop position P3 must be set. However, the aircraft 1 does not necessarily stop at a fixed position and attitude. Therefore, it is necessary to set the target trajectory TT each time the cargo is loaded or unloaded. Therefore, it is necessary to accurately estimate the position and orientation of the loading/unloading equipment 15 from the starting position P1.

FIG. 3 is a diagram showing a case where the tug 10 approaches from behind the aircraft 1 (that is, when cargo is unloaded from the aircraft 1). 3 and 2 is only in the direction of approach. Therefore, since the contents of each part in FIG. 3 are the same as those in FIG. 2, description thereof will be omitted.

(Creation of external model data of loading/unloading equipment 15)
As a preliminary preparation, it is necessary to prepare external model data representing the external shape of the loading/unloading equipment 15 and store it in the external model storage unit 33 . As a first step, a three-dimensional point cloud model of the loading/unloading equipment 15 is obtained. Specifically, the shape of the loading/unloading equipment 15 is measured using a three-dimensional surveying instrument.

As a second step, the 3D point cloud model is down-sampled. This is because the three-dimensional point cloud obtained in the first step has a very large number of points (for example, approximately 430,000 points). An example of downsampling will be described. First, down-sampling is performed at predetermined intervals. In this example, downsampling was performed at intervals of 4 cm, thereby reducing the number of points to approximately 20,000 points. Next, portions necessary for matching processing (step S70), which will be described later, are left, and unnecessary portions are removed. In the present embodiment, the driver's cab and cargo bed (the surface on which the container slides and moves) are removed. As a result, it was possible to reduce the number to about 1000 points.

As a third step, the 3D point cloud is compressed to 2D. Specifically, a three-dimensional point cloud is extracted in a cross-slice shape on a horizontal plane scanned with a laser beam by the environment sensor 40 (2D-LiDAR). As a result, external model data MD shown in FIG. 4 is obtained. FIG. 4 shows the external model data MD when the loading/unloading equipment 15 is viewed from above. The external model data MD has position coordinate values in an arbitrary coordinate system. The upper point group in FIG. 4 corresponds to the first side surface 15b, and the lower point group corresponds to the second side surface 15c. The left side of FIG. 4 corresponds to the loading/unloading section 15a. Since the data size of the external model data MD is compressed, it is possible to shorten the calculation time in the later-described matching process (step S70).

(Generation method of target trajectory)
A method of generating the target trajectory TT will be described with reference to the flowchart of FIG. The flow of FIG. 5 is started when an instruction is input to move the tractor 10 from the start position P1 to the stop position P3.

In step S<b>10 , the determination unit 31 images the loading/unloading equipment 15 with the camera 41 . Then, it is determined whether or not the first ID information indicating the first marker M1 or the second ID information indicating the second marker M2 is detected from within the captured image. If it is not detected (S10: NO), the process returns to S10, and the imaging of the equipment 15 to be loaded and unloaded is restarted. If detected (S10: YES), proceed to S20.

In step S<b>20 , the determination unit 31 determines the position of the start position P<b>1 of the towing vehicle 10 . If the second ID information is detected in step S10, as shown in FIG. ). On the other hand, when the first ID information is detected, as shown in FIG. 3, the starting position P1 is in the positive direction of the direction x0 with respect to the loading/unloading equipment 15 (the rear side of the aircraft 1 indicated by the arrow Y1). It is determined that it is located at

In step S25, the measurement data acquisition unit 32 measures the position and orientation of the loading/unloading equipment 15 using the environment sensor 40 (2D-LiDAR). FIG. 6 shows a top view showing how the measurement is performed. FIG. 7 shows an example of measurement data. FIG. 7 shows the measurement results of 2D-LiDAR as a top view. For clarity, the tractor 10 is shown in dashed lines in FIG. As shown in FIG. 7, the measurement data of the environment sensor 40 is point cloud data. The point cloud data has position coordinate values in a starting point position coordinate system CS1, which is a coordinate system with the towing vehicle 10 as a reference.

In step S30, the measurement data acquisition unit 32 executes segmentation processing. The segmentation process is a process of clustering measurement points existing within a certain distance as the same object in the measurement data acquired in step S25. In the measurement data example of FIG. 7, point clouds within the regions R1 and R2 are clustered. The point cloud within the region R1 is the measurement data of the second side surface 15c. The point group within the region R2 is the measurement data of the loading/unloading section 15a.

In step S40, the measurement data acquisition unit 32 determines whether or not there is a specific cluster indicating the position and orientation of the side surface of the transport path portion 15d among the obtained clusters. A specific cluster is a cluster whose length is greater than or equal to a predetermined size. A specific cluster is a cluster in which the angle formed by the direction in which the specific cluster extends and the travel direction x1 of the towing vehicle 10 is equal to or greater than a predetermined angle. The predetermined size may be determined based on the length LE (see FIG. 6) of the transport path portion 15d. For example, the predetermined size may be 70% of the length LE. Also, the predetermined angle may be, for example, 30 degrees or more. If the specific cluster does not exist (S40: NO), the process returns to S10 and each process is redone. On the other hand, if the specific cluster exists (S40: YES), the process proceeds to S50. In the measurement data example of FIG. 7, the clusters within the region R1 are determined as the specific clusters.

In step S50, the measurement data acquisition unit 32 obtains the tilt angle and reference position of the specific cluster. First, a straight line SL that fits a point group included in a specific cluster is obtained. An example of a method for obtaining the straight line SL is the RANSAC (Random Sample Consensus) method. A straight line SL represents the linear shape of the side surface of the loading/unloading equipment 15 . The inclination angle A1 (see FIG. 7) that the straight line SL has with respect to the traveling direction x1 is the inclination angle of the cluster. Next, the reference position PE of the specific cluster is determined. The reference position PE is the end point of the cluster in the area R1 on the tractor 10 side (left side in FIG. 7).

In step S60, the coordinate transformation unit 34 performs coordinate transformation on the external model data MD (FIG. 4) stored in the external model storage unit 33 so as to overlap the data measured by the environment sensor 40 (FIG. 7). That is, in step S60, as a pre-process for performing the matching process in step S70, a process of roughly superimposing both data is performed. The dimensions of the loading/unloading equipment 15 represented by the external model data MD and the dimensions of the loading/unloading equipment 15 represented by the measurement data of the environment sensor 40 are both measured values and are therefore equal. Therefore, both data can be superimposed by rotating and translating the external model data MD.

First, the reference position PD of the external model data MD is determined. The reference position PD is a position corresponding to the reference position PE (FIG. 7) of the specific cluster. The reference position PD is the end point of the second side surface 15c in the external model data MD on the towing vehicle 10 side (left side in FIG. 4). Next, the outline model data MD is rotated so that the inclination of the second side surface 15c of the outline model data MD with respect to the traveling direction x1 becomes the same as the inclination angle A1 (FIG. 7) of the straight line SL. Finally, the external model data MD is translated so that the reference position PD of the external model data MD coincides with the reference position PE (FIG. 7) of the specific cluster. As a result, the external model data MD can be superimposed on the measurement data obtained by the environment sensor 40, as shown in FIG.

In step S<b>70 , the position/orientation estimation unit 35 performs matching processing between the external model data MD and the point group measured by the environment sensor 40 . For the matching process, for example, ICP (Iterative Closest Points) can be used, but other techniques may be used. The matching process finely adjusts the position and orientation of the external model data MD shown in FIG. As a result, the data measured by the environment sensor 40 and the external model data MD can be superimposed so as to completely match. In other words, the coordinate system of the external model data MD can be converted from the arbitrary coordinate system to the starting position coordinate system CS1. The position and orientation of the external model data MD on the starting point position coordinate system CS1 after the matching process is completed are the estimation results of the position and orientation of the loading equipment 15 .

In step S80, the trajectory generator 36 generates the target trajectory TT. As shown in FIGS. 2 and 3, the target trajectory TT is a trajectory that allows the vehicle to start from the start position P1 in the attitude indicated by the start position coordinate system CS1, and to stop at the stop position P3 in the stop attitude indicated by the stop position coordinate system CS3. be. As shown in FIGS. 2 and 3, the parking posture is a posture in which the traveling direction x3 of the tractor 10 at the stopping position P3 coincides with the direction x0 parallel to the loading/unloading section 15a.

In step S90, the speed pattern generator 37 designs a speed pattern based on the target trajectory TT. In step S100, the control command generator 38 calculates a control command value for moving the tractor 10 to the stop position P3 based on the target trajectory TT and the speed pattern. The generated control command value is input to the steering control section 21 and the drive control section 22 to move the towing vehicle 10 toward the stop position P3.

It is also possible to execute the flow of FIG. 5 while the towing vehicle 10 is running. For example, each time the vehicle travels a certain distance toward the stop position P3, a new starting position P1 may be set and the flow of FIG. 5 may be executed. As a result, the estimated values of the position and orientation of the loading/unloading equipment 15 can be updated while the towing vehicle 10 is running.

In this embodiment, as shown in FIG. 2, the target trajectory TT generation flow (FIG. 5) in the case where the starting point P1 is positioned in front of the aircraft 1 has been described. However, as shown in FIG. 3, even when the starting position P1 is positioned behind the aircraft 1, the flow of generating the target trajectory TT can be similarly executed.

(effect)
A tractor 10 for loading and unloading a container on an aircraft 1 needs to self-propell to a loading and unloading equipment 15 located far away, such as ten and several meters away. Therefore, it is necessary to estimate the position and orientation of the loading/unloading equipment 15 with high accuracy. For example, in the method of recognizing an AR marker placed on the loading/unloading equipment 15 using a camera, the farther the loading/unloading equipment 15 is positioned, the smaller the captured image of the marker becomes. It is difficult to estimate the position and orientation of the robot with high accuracy. According to the technology described in this specification, the measurement data obtained by the laser sensor (2D-LiDAR) can be matched with the external model data MD (step S70). By using a laser sensor, the distance to the loading/unloading equipment 15 can be measured more accurately than when using a camera. The external model data MD is data generated based on three-dimensional surveying, and is data with extremely high accuracy. Therefore, by using the external model data MD, it is possible to estimate the position and orientation of the loading/unloading equipment 15 with higher accuracy than when using a camera.

The loading/unloading equipment 15 has a transport path portion 15d extending in the direction y0. Also, there is a feature that the starting point position P1 of the tractor 10 is positioned in front or behind the aircraft 1 (see FIGS. 2 and 3). As a result, the environment sensor 40 provided in the towing vehicle 10 can measure either the first side surface 15b or the second side surface 15c of the transport path portion 15d. The measurement data of the first side surface 15b or the second side surface 15c is of a substantially linear simple shape, as shown in the example of FIG. Therefore, since the matching process (step S70) can be executed using a simple shape, the calculation load for matching can be reduced. The matching process can be performed while the towing vehicle 10 is running.

If the matching process (step S70) is performed on the external model data MD (FIG. 4) and the measurement data (FIG. 7) measured by the environment sensor 40 in a state where the positions and orientations of the data are significantly different, it may result in only partial overlap. Mismatches may occur. According to the technique described in this specification, after executing the process of roughly superimposing the external model data MD and the measurement data from the environment sensor 40 (step S60), the matching process (step S70) is executed on both data. Since matching processing can be executed in a state in which the positions and orientations of both data are close to each other, it is possible to suppress erroneous matching.

When the tug 10 approaches the aircraft 1, there are a pattern of approaching from the front (FIG. 2) and a pattern of approaching from the rear (FIG. 3). With the technique described in this specification, the second marker M2 is arranged on the second side surface 15c visible when approaching from the front, and the first marker M1 is arranged on the first side surface 15b visible when approaching from the rear. , and from which direction it is approaching (step S20). It is possible to accurately estimate the position and orientation of the loading/unloading equipment 15 .

In a second embodiment, a case will be described in which the determination unit 31 determines the position of the starting position P1 without using the first marker M1 and the second marker M2. Only parts different from the contents of the first embodiment will be described.

The determination unit 31 stores the first template image and the second template image in advance. The first template image is an image of the loading/unloading equipment 15 captured in advance from the positive side of the direction x0 (the rear side of the aircraft 1 indicated by the arrow Y1 in FIG. 3). In the first template image, the aircraft 1 is shown on the left side and the loading/unloading equipment 15 is shown on the right side. Also, the first side surface 15b is shown. The second template image is an image of the transport device captured in advance from the negative direction side of the direction x0 (the front side of the aircraft 1 indicated by the arrow Y2 in FIG. 2). In the second template image, the aircraft 1 is shown on the right side, and the loading/unloading equipment 15 is shown on the left side. Also, the second side surface 15c is shown.

In step S<b>10 , the determination unit 31 images the loading/unloading equipment 15 with the camera 41 . In step S20, the determination unit 31 determines to which of the first template image and the second template image the captured image of the loading/unloading equipment 15 matches to a higher degree. For example, image recognition technology may be used to determine which of the first side surface 15b and the second side surface 15c is included in the captured image of the loading/unloading equipment 15 . Further, for example, the lateral positional relationship between the loading/unloading equipment 15 and the aircraft 1 may be determined. If it is determined that the degree of matching with the first template image is high, it is determined that the starting point position P1 is located in the positive direction of the direction x0 (the direction of the arrow Y1 in FIG. 3). On the other hand, when it is determined that the degree of matching with the second template image is high, it is determined that the starting point position P1 is positioned in the negative direction of the direction x0 (the direction of the arrow Y2 in FIG. 2). The processing after step S30 is the same as in the first embodiment.

Note that the image matching determination in step S20 may use a machine learning model (eg, neural network). In this case, it suffices to prepare a large number of teaching image data obtained by photographing the on-board equipment 15 in various situations. Then, a machine learning model may be learned in advance so that accurate image matching determination can be performed.

(effect)
It is not necessary to place the first marker M1 and the second marker M2 on the loading/unloading equipment 15 . It is possible to accurately estimate the position and orientation of the loading/unloading equipment 15 with a simpler configuration.

In a third embodiment, a case will be described in which the determining unit 31 determines the position of the starting point P1 based on information about the work plan. Only parts different from the contents of the first embodiment will be described.

In step S20, the determination unit 31 determines whether the work plan is a work plan for loading cargo from the loading/unloading equipment 15 onto the towing vehicle 10 or a work plan for unloading cargo from the towing vehicle 10 onto the loading/unloading equipment 15. to decide. This determination may be made based on the work plan information input to the determination unit 31, for example. Further, for example, if the total weight of the dolly towed by the towing vehicle 10 is heavy, it may be determined that the work plan is to load the load, and if it is light, the work plan is to unload the load.

If the work plan is to unload a load, it is determined that the starting position P1 is located in the positive direction of direction x0 (the direction of arrow Y1 in FIG. 3). This is because the tractor 10 is approached from the rear of the aircraft 1 when unloading the cargo. On the other hand, if the work plan is to load cargo, it is determined that the starting position P1 is positioned in the negative direction of the direction x0 (the direction of the arrow Y2 in FIG. 2). This is because the tug 10 is approached from the front of the aircraft 1 when loading cargo. The processing after step S30 is the same as in the first embodiment.

(effect)
The determination unit 31 can determine the position of the starting point P1 without imaging the loading/unloading equipment 15 with the camera 41 . It becomes possible to omit the process of step S10. It is possible to accurately estimate the position and orientation of the loading/unloading equipment 15 with a simpler configuration.

Although the embodiments of the technology disclosed in this specification have been described in detail above, these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.

(Modification)
Various methods may be used by the determination unit 31 to determine the position of the start position P1. For example, when the towing vehicle 10 starts from the cargo loading/unloading area of the terminal and moves to the starting point position P1 in FIG. 2 or the starting point position P1 in FIG. 3, the route branches on the way. The starting point position P1 may be determined depending on whether the branch direction is toward the front side or the rear side of the aircraft 1 . Also, the towing vehicle 10 may determine the starting position P1 by recognizing its own position on the map.

In step S60, the order of the process of rotating and the process of translating the external model data MD does not matter.

The method of generating the external model data MD is not limited to the method using the three-dimensional surveying instrument. Any method may be used as long as it provides sufficient accuracy. For example, a technique called "Structure from Motion method" may be used to restore the three-dimensional shape of the object and the relative position of the camera from a plurality of images from different viewpoints.

The reference position PE (FIG. 7) of the specific cluster and the reference position PD (FIG. 4) of the external model data MD are not limited to the ends. Any position may be used as long as the position is clear.

The method of arranging the first marker M1 and the second marker M2 on the loading/unloading equipment 15 is not limited to using magnets, and various modes may be employed. A marker may be directly printed on the loading/unloading equipment 15 . Also, the number and positions of the markers to be arranged may be any number and positions as long as they can be reliably visually recognized from the towing vehicle 10 .

Although the 2D-LiDAR is used as the environment sensor 40 in this embodiment, various sensors may be used. For example, it may be a 3D-LiDAR or a TOF (Time of Flight) camera.

The external model data MD is not limited to two-dimensionally compressed data, and three-dimensional data can also be used.

In this embodiment, the case where the tractor 10 is used in a cargo transportation system at an airport has been described, but the invention is not limited to this form. The technology described herein can be applied to any system that tows cargo and loads and unloads cargo to a predetermined location.

The technical elements described in this specification or in the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims as of the filing. In addition, the techniques exemplified in this specification or drawings achieve multiple purposes at the same time, and achieving one of them has technical utility in itself.

The towing vehicle 10 is an example of a mobile object. The loading/unloading equipment 15 is an example of a transport device. Arithmetic device 30 is an example of a trajectory generation device. Direction y0 is an example of a first direction. The direction x0 is an example of a second direction. The coordinate transformation unit 34 and the position/orientation estimation unit 35 are examples of an estimation unit. The direction of travel x1 is an example of the orientation of the moving object. The inclination angle A1 is an example of a predetermined angle. The reference position PE is an example of the reference position of the conveying route. The reference position PD is an example of the reference position of the external model.

10: Towing vehicle 15: Loading/unloading equipment 15a: Loading/unloading unit 15b: First side surface 15c: Second side surface 15d: Conveyance path unit 31: Determination unit 32: Measurement data acquisition unit 33: External model storage unit 34: Coordinate transformation Unit 35: Position and orientation estimation unit 36: Trajectory generation unit 37: Speed pattern generation unit 38: Control command generation unit 40: Environment sensor 41: Camera M1: First marker M2: Second marker P1: Start position P3: Stop position

Claims (8)

A trajectory generating device for generating a trajectory for movement of a moving body from a starting position to a conveying device,
The conveying device includes a conveying path portion extending in a first direction, and a cargo loading/unloading portion disposed at an end of the conveying path portion,
a determination unit that determines whether the starting point position of the moving body is positioned in a positive direction or a negative direction of a second direction that is orthogonal to the first direction and extends in the horizontal direction;
an acquisition unit that acquires measurement data indicating the position and orientation of the side surface of the transport path unit based on the measurement data of an environment sensor included in the mobile object;
The position and orientation of the transport device with respect to the moving body are calculated using the profile model represented by the profile data indicating the profile of the transport device and prepared in advance and the measurement data. an estimating unit for estimating;
A target trajectory for moving from the starting position to the front surface of the baggage loading/unloading section, wherein the orientation of the moving body, which is the direction in which the front surface of the moving body faces in front of the baggage loading/unloading section, coincides with the second direction. a trajectory generator that generates the target trajectory such that
A trajectory generator. The trajectory generation device according to claim 1, wherein the estimation unit estimates the position and orientation of the transport device by superimposing the outer model on the measurement data. The measurement data is data indicating a predetermined angle of the side surface of the transport path section with respect to the orientation of the moving body and a reference position of the transport path section,
The estimation unit
Rotating the external model so that the angle formed by the side surface of the external model with respect to the direction of the moving object matches the predetermined angle, and
superimposing the external model on the measurement data by moving the external model so that the reference position of the external model matches the reference position of the measurement data;
The trajectory generation device according to claim 2. the environment sensor is a laser sensor;
The trajectory generation device according to any one of claims 1 to 3, wherein the measurement data is point cloud data measured by the laser sensor. The conveying device is
a first marker disposed on the first side surface on the positive direction side in the second direction;
a second marker arranged on a second side surface on the negative direction side in the second direction;
and
The determination unit is
determining that the start point position is positioned in the positive direction of the second direction when the marker detected in the image captured by the camera provided to the moving body is the first marker;
5. The method according to any one of claims 1 to 4, wherein when the marker detected in the image is the second marker, it is determined that the starting point position is located in the negative direction of the second direction. A trajectory generator as described. The determining unit determines to which of the first template image and the second template image the image of the conveying device imaged by the camera of the moving object has a higher degree of matching,
the first template image is an image of the conveying device captured in advance from the positive direction side of the second direction;
the second template image is an image of the conveying device captured in advance from the negative direction side of the second direction;
The determination unit is
determining that the starting point position is positioned in the positive direction of the second direction when it is determined that the degree of matching of the image of the conveying device to the first template image is high;
determining that the starting point position is located in the negative direction of the second direction when it is determined that the image of the conveying device matches the second template image to a high degree;
A trajectory generation device according to any one of claims 1 to 4. The determination unit is
determining that the start point position is positioned in the positive direction of the second direction when the work plan is to unload the cargo from the conveying device to the moving body;
Determining that the start point position is located in the negative direction of the second direction when the work plan is to load the cargo from the moving body to the conveying device;
A trajectory generation device according to any one of claims 1 to 4. A moving object comprising the trajectory generation device according to any one of claims 1 to 7.

Images