JP7181800B2 - moving body - Google Patents

Description

The technology disclosed in this specification relates to a moving object that recognizes the position and orientation of an object.

Patent Literature 1 discloses a technique for calculating the position and size of a marker by attaching a marker to a cargo handling object such as a pallet or a loading platform and performing pattern matching processing on the marker image captured by a camera. ing. This detects the relative position and orientation of the object to be handled with respect to the forklift.

Japanese Patent No. 3921968

The technique of Patent Literature 1 cannot calculate the three-dimensional posture of the cargo handling object in the depth direction. An example of the three-dimensional posture in the depth direction is the rotational posture of the cargo handling object in the yaw direction (rotational direction about the vertical axis) and the pitch direction (rotational direction about the horizontal axis). This specification discloses a technique capable of estimating the three-dimensional position and orientation of an object to be handled with high accuracy.

One embodiment of the moving body disclosed in this specification is a moving body that recognizes the position and orientation of an object that includes a marker group in which a plurality of markers are arranged at a predetermined first predetermined distance. be. The moving body has an optical sensor for capturing an image of the marker group. The moving object detects the positions and orientations of the marker group at each of the one or more moving points based on the image of the object captured by the optical sensor at each of the one or more moving points. and a marker group position/orientation estimating unit for estimating . The moving body includes a graph structure data generator that generates graph structure data including a plurality of sensor nodes, a marker node, and a plurality of arcs that define relative positional relationships between the plurality of sensor nodes and the marker nodes. . The moving body includes a correction unit that corrects the estimated values of the position and orientation of the marker group by optimizing the generated graph structure data so that the sum of error functions calculated from a plurality of arcs is minimized. . A plurality of sensor nodes are nodes indicating the position and orientation of the optical sensor at each of one or more movement points. The marker node is a node indicating the position and orientation of the marker group estimated by the marker group position/orientation estimator.

In the moving object described above, a plurality of sensor nodes indicating the position and orientation of the optical sensor at each of one or more movement points, and a marker node indicating the position and orientation of the marker group estimated by the marker group position and orientation estimator. and a plurality of arcs defining relative positional relationships between the plurality of sensor nodes and the marker nodes. By optimizing the graph structure data, the estimated values of the positions and orientations of the marker group can be corrected. Global optimization can be performed using multiple estimation results of the position and orientation of the marker group estimated at each of one or more movement points. Therefore, it is possible to more accurately estimate the position and orientation of the marker group compared to the case of using the estimation result of the position and orientation of the marker group estimated at one movement point.

A moving body position/orientation calculation unit may be further provided for calculating the position and orientation of the moving body at the moving point at time t _{i+1 when} the moving body moves from the moving point at time t i to the moving point at time t _i+1. . The marker group position/orientation estimation unit may estimate the position and orientation of the marker group at the movement point at time t _i+1 . The graph structure data generation unit generates a sensor node and a marker node at the movement point at time t _i+1 and an arc that defines the relative positional relationship between the sensor node and the marker node at the movement point at time t _i+1 , It may be added to the graph structure data. Details of the effect will be described in Examples.

The mobile body may comprise a support structure capable of holding the object. The moving body may further include an access position estimation unit that estimates an access position for accessing the support structure based on the positions and orientations of the marker group corrected by the correction unit. Details of the effect will be described in Examples.

The moving object may further include a trajectory generating unit that generates a trajectory and speed pattern from the current position of the moving object to the target based on the positions and orientations of the marker group corrected by the correcting unit. The moving body may further include a movement control section that moves the moving body to the object based on the generated trajectory and speed pattern. The mobile body may further comprise an access controller for causing the support structure to access the access position based on the access position estimated by the access position estimator. Details of the effect will be described in Examples.

The support structure may be a fork. The object may be a pallet. The optical sensor may operate integrally with the movement of the forks. Details of the effect will be described in Examples.

The group of markers may comprise two or more markers horizontally separated by a first predetermined distance. Details of the effect will be described in Examples.

The marker group may further include one or more markers above or below the two or more markers horizontally separated by the first predetermined distance. Details of the effect will be described in Examples.

The marker group position/orientation estimating unit may estimate the position and orientation of each of the plurality of marker groups when there are a plurality of marker groups in the image captured by the optical sensor. The graph structure data generation unit may generate a plurality of marker nodes indicating respective positions and orientations of a plurality of marker groups. Details of the effect will be described in Examples.

1 is a schematic perspective view of a forklift 1; FIG. 1 is a block diagram of a forklift 1; FIG. 4 is a diagram showing an example of a palette 50; FIG. 4 is a diagram showing the positional relationship between the vehicle body and the pallet in Embodiment 1. FIG. 5 is a diagram showing graph structure data generated from the positional relationship of FIG. 4; FIG. 9 is a flowchart showing the contents of position/orientation estimation processing; It is a flowchart which shows the content of cargo-handling processing. FIG. 10 is a diagram for explaining the reason why the position and orientation of a marker node can be corrected; FIG. 10 is a diagram showing an example of results of estimating the position and orientation of a marker; FIG. 10 is a diagram showing the positional relationship between a vehicle body and a pallet in Embodiment 2; FIG. 11 is a diagram showing graph structure data generated from the positional relationship of FIG. 10; FIG. 10 is a diagram showing the positional relationship among the vehicle body, pallet, and pillars in Example 3; FIG. 13 is a diagram showing graph structure data generated from the positional relationship of FIG. 12;

(Configuration of forklift 1)
A forklift 1 of this embodiment will be described below with reference to FIGS. 1 and 2. FIG. FIG. 1 is a schematic perspective view of a forklift 1, and FIG. 2 is a block diagram. A forklift 1 is an unmanned forklift. As shown in FIG. 1, the forklift 1 includes a vehicle body 2, a control section 10 (shown in FIG. 2), a mast 20, a fork 22, a backrest 23, a camera fixing section 24, and a camera 30.

The vehicle body 2 has front wheels 28 and rear wheels 29 on both side surfaces thereof. A drive wheel motor (not shown) is connected to the front wheel 28 via a drive mechanism, and is rotationally driven by the movement control unit 17 (shown in FIG. 2). The rear wheels 29 are connected to a steering device (not shown), and the direction of the wheels is adjusted by the movement control section 17 . The movement control unit 17 controls the driving wheel motors and the steering device, so that the vehicle body 2 can travel on the road surface and change the traveling direction of the vehicle body 2 .

The mast 20 is a post attached to the front surface of the vehicle body 2, and its axis extends in the vertical direction. The fork 22 is attached to the mast 20 so as to be vertically movable. The fork 22 has a pair of claws 22a and 22b. The claws 22a and 22b are arranged at positions separated from each other in the lateral direction of the vehicle body 2 and extend forward of the vehicle body 2 from the mast 20 side. The fork 22 is raised and lowered by the cargo handling device control section 18 (illustrated in FIG. 2). The vertical position of the fork 22 can be identified by the amount of drive of the cargo handling device control section 18 .

The backrest 23 is a member that prevents the load placed on the fork 22 from falling behind the mast 20. - 特許庁The camera fixing portion 24 is a member that fixes the camera 30 so as to be integrated with the backrest 23 . Accordingly, when the fork 22 is moved up and down by the mast 20 or tilted back and forth by a tilt mechanism (not shown), the camera 30 also moves together. Therefore, a marker image with the fork 22 as a reference can be acquired.

The block diagram of FIG. 2 will be described. The forklift 1 includes a control unit 10 , a graph structure storage unit 19 , a fork 22 , a front wheel 28 , a rear wheel 29 , a camera 30 and an encoder 40 . The control unit 10 is a device that controls the forklift 1 . The control unit 10 can be configured by, for example, a computer including a CPU, ROM, RAM, and the like. By the computer executing the program, the control unit 10 functions as the marker group position/orientation estimation unit 11 to the cargo handling device control unit 18 shown in FIG. The control unit 10 includes a position/orientation estimation module MOD1 and a vehicle control module MOD2. The position/orientation estimation module MOD1 executes position/orientation estimation processing (FIG. 6), which will be described later. The vehicle control module MOD2 executes cargo handling processing (FIG. 7), which will be described later. The position and orientation estimation module MOD1 and the vehicle control module MOD2 are independent of each other and can operate in parallel. The position/posture estimation module MOD1 includes a marker group position/posture estimation unit 11, a graph structure data generation unit 13, a graph optimization unit 14, and an access position estimation unit 15. FIG. The vehicle control module MOD2 includes a vehicle body position/orientation calculator 12, a trajectory generator 16, a movement controller 17, and a cargo handling device controller . Details of the operations of each of the marker group position/orientation estimation unit 11 to the cargo handling device control unit 18 will be described later. The graph structure storage unit 19 is a part that stores graph structure data, which will be described later. The encoder 40 is a part provided with a sensor that detects the rotation angle of the wheel. Other parts have already been described with reference to FIG.

(Pallet composition)
FIG. 3 shows an example of the pallet 50 according to this embodiment. Pallet 50 is a basket pallet. A space SP for inserting the fork 22 is arranged in the front lower portion of the pallet 50 . Pallet reference coordinates PC are set in advance in the space SP. The origin position of the pallet reference coordinate PC in the y direction (horizontal direction) is the front center of the pallet 50 . The origin position of the pallet reference coordinate PC in the z direction (vertical direction) is the height at which the fork 22 is inserted. The origin position of the pallet reference coordinates PC is a position determined by the designer and is a virtual position in space.

A marker group MG comprising two markers M1 and M2 is arranged in front of the pallet 50 . In this embodiment, the markers M1 and M2 are AR (Augmented Reality) markers. That is, the markers M1 and M2 are markers that serve as indicators for designating positions where additional information is displayed in the augmented reality system. In FIG. 3, marker reference coordinates MC1 and MC2 indicate the positions and orientations (orientations) of markers M1 and M2, respectively. The markers M1 and M2 are horizontally arranged apart from each other by a predetermined first predetermined distance D1. Markers M1 and M2 are arranged in the same plane. As a result, the relative positions of the origin of the pallet reference coordinates PC and the origins of the marker reference coordinates MC1 and MC2 are uniquely determined. Specifically, first, the placement positions of the markers M1 and M2 and the first predetermined distance D1 are determined. Then, the relative positions of the markers M1 and M2 and the origin of the pallet reference coordinates PC are registered in the data file by measuring the distance from the pick-up position or calculating the distance from the design information. Therefore, the relationship between the positions of the markers M1 and M2 and the pallet reference coordinates PC can be defined (constrained). In other words, the markers M1 and M2 must be placed at locations where the positional relationship with the pallet reference coordinates PC can be determined.

(Automatic loading and unloading of pallets)
Specific processing of automatic handling of pallets will be described with reference to FIGS. 4 to 9. FIG. In this explanation example, as shown in FIG. 4, the contents of processing when the vehicle body 2 travels along the track TR1 from the position P1 at the time t _i to the position P2 at the time t _i+1 will be explained. In FIG. 4, vehicle body reference coordinates VC1 and VC2 indicate the position and attitude (orientation) of vehicle body 2 at times t _i and t _i+1 , respectively. The origin of the vehicle body reference coordinates VC1 and VC2 indicates the origin of the vehicle body 2 . Camera reference coordinates CC1 and CC2 indicate the position and orientation (orientation) of camera 30 at times t _i and t _i+1 , respectively.

(Details of position and orientation estimation processing)
The position/orientation estimation process in FIG. 6 starts when a new cargo handling process starts. Specifically, it is started when it is determined in S210 of the cargo handling process to be described later that the cargo handling process is to be started. In S10, image data reception processing is performed. In this illustrative example, the camera 30 captures an image of the pallet 50 at the position P1 at time t _i in FIG. The control unit 10 then receives image data from the camera 30 .

In S20, the marker group position/orientation estimation unit 11 estimates the positions and orientations of the markers M1 and M2 based on the received image data. Since this estimation process can use a software library that is open to the public, the description is omitted. FIG. 9 shows an example of estimation results of the positions and orientations of the markers. The estimation result based on the measurement result by the camera 30 includes an error in the position and orientation of the marker. For example, in the estimation result of FIG. 9, the orientations of the markers M1 and M2 indicated by the marker reference coordinates MC1 and MC2 contain an error and are not perpendicular to the front of the pallet 50. In FIG. That is, in the estimation results of FIG. 9, the yaw angle attitudes of the markers M1 and M2 cannot be accurately estimated.

In S<b>30 , the vehicle body position/orientation calculation unit 12 calculates the position and orientation of the vehicle body 2 . Specifically, based on the rotation angle of the front wheels 28 detected by the encoder 40, the movement direction and movement amount of the vehicle body 2 are obtained.

In S35, the graph optimization unit 14 determines whether or not to execute graph optimization processing. This determination may be made, for example, based on whether or not the vehicle body 2 has advanced by 0.5 m or more from the time when the graph optimization process was last executed. As a result, it is possible to prevent the graph structure from becoming unnecessarily long. If the determination is negative (S35: NO), the process returns to S10, and if the determination is positive (S35: YES), the process proceeds to S40.

In S40 to S70, graph structure data generation processing is performed. The graph structure data is data generated as intermediate information by the control unit 10 . The graph structure data is data used to suppress errors in estimating the positions and orientations of markers M1 and M2.

The graph structure data shown in FIG. 5 will be described. The graph structure data comprises vehicle body nodes VN1 and VN2, sensor nodes SN1 and SN2, and marker nodes MN1 and MN2. The vehicle body nodes VN1 and VN2 are information indicating the position and orientation of the vehicle body 2 at each of the movement points at times t _i and t _i+1 . Sensor nodes SN1 and SN2 are information indicating the position and orientation of camera 30 at each of the movement points at times t _i and t _i+1 . The marker nodes MN1 and MN2 are information indicating the positions and orientations of the markers M1 and M2 estimated at each of the movement points at times t _i and t _i+1 . In FIG. 5, the marker nodes MN1 and MN2 are indicated by circles for convenience of explanation. In practice, however, the marker nodes MN1 and MN2 also have pose information such as yaw angle pose, as described in FIG.

The graph structure data also comprises arcs AC1 and AC2, arcs AVC1 and AVC2, arc AVN12, and arc AM. Arcs are relative position constraints. Arc AC1 is information defining the relative positional relationship between sensor node SN1 and marker node MN1 and the relative positional relationship between sensor node SN1 and marker node MN2. Similarly, arc AC2 is information defining the relative positional relationship between sensor node SN2 and marker node MN1 and the relative positional relationship between sensor node SN2 and marker node MN2. Arcs AVC1 and AVC2 are information defining the relative positional relationship between the vehicle node and the sensor node. Arc AVN12 is information that defines the relative positional relationship between vehicle nodes VN1 and VN2. Arc AM is information that defines the relative positional relationship between markers M1 and M2.

Each arc has prior knowledge about "initial values of relative positions and orientations between nodes" and "probability". "Probability" can be represented by an information matrix. The information matrix is the inverse of the variance-covariance matrix for the relative position and pose errors between nodes. The relative positions and attitudes between the vehicle node and the sensor node defined by the arcs AVC1 and AVC2 are a matter of design of the forklift 1 and are known. Similarly, the relative position and orientation between markers M1 and M2 defined by arc AM are known as described in FIG. Since the arcs AVC and AM are known, they can be stored in advance in the graph structure storage unit 19 and read out for use when generating the graph structure.

A specific example of graph structure data generation processing will be described. In S<b>40 , the graph structure data generator 13 sets a vehicle body node representing the position and orientation of the vehicle body 2 obtained by the vehicle body position/orientation calculator 12 . In this illustrative example, the vehicle body position/orientation calculator 12 obtains the vehicle body reference coordinates VC2 (FIG. 4) at time t i ₊₁ . Then, a vehicle node VN2 (FIG. 5) corresponding to the vehicle reference coordinate VC2 is added.

In S<b>50 , the graph structure data generator 13 sets sensor nodes representing the position and orientation of the camera 30 . The sensor node can be set based on the known positional relationship between the camera 30 and the axle of the rear wheel 29 of the vehicle body 2 . In this illustrative example, a sensor node SN2 (FIG. 5) is added.
In S60, the graph structure data generator 13 sets marker nodes MN1 and MN2 (FIG. 5) representing the positions and orientations of the markers M1 and M2 estimated in S20.

In S70, the graph structure data generator 13 adds arcs between the sensor node and the marker node, between the vehicle node and the sensor node, between the vehicle node, and between the markers M1 and M2. In this illustrative example, arc AC2, arc AVC2, arc AVN12 and arc AM are added. In S<b>80 , the graph structure data generation unit 13 stores the graph structure data to which the nodes and arcs are added in the graph structure storage unit 19 .

In S<b>100 , the graph optimization unit 14 optimizes graph structure data stored in the graph structure storage unit 19 . For optimization, for example, Graph-Based SLAM (Simultaneous Localization and Mapping) can be used. Graph-Based SLAM is optimized to minimize the total error of each constraint for a graph with camera positions and marker positions as nodes (vertices) and relative position constraints between them as arcs (edges). It is a method of performing conversion calculations. Since the overall optimization is performed using all the data associated with the movement of the vehicle body 2 from the past to the present, it has the feature that the marker positions can be corrected retroactively.

ここで、Ｃは制約条件の集合である。ｅ_ｉｊ（ｘ_ｉ,ｘ_ｊ）はノードｘ_ｉとノードｘ_ｊの間の相対位置制約からの誤差ベクトル関数である。Σ_ｉｊはその制約の共分散行列である。制約の共分散によるマハラノビス距離の二乗和が最小となるように上式を最小化することで、各ノードの位置、姿勢関係を最適化する。これにより、マーカノードＭＮ１およびＭＮ２の位置および姿勢を補正することができる。 Graph-Based SLAM is explained concretely. A nonlinear least-squares problem is solved using the sum of errors from the relative position constraints of each node as the evaluation function. This evaluation function is given by the following equation.

where C is a set of constraints. e _ij (x _i , x _j ) is the error vector function from the relative position constraint between node x _i and node x _j . Σ _ij is the constraint covariance matrix. By minimizing the above equation so that the sum of squares of the Mahalanobis distances due to the covariance of the constraints is minimized, the position and orientation relationship of each node is optimized. Thereby, the positions and orientations of the marker nodes MN1 and MN2 can be corrected.

The reason why the position and orientation of the marker node can be corrected will be described with reference to FIG. FIG. 8 shows an example in which the positions and orientations of markers M1 and M2 are estimated (S20) at position P1 at time t _i , position P2 at time t _i+1 , and position P3 at time t _i+2 . That is, this is an example of observation from multiple viewpoints. In estimating a position and orientation using a marker, a distance error occurs on a line connecting the camera 30 and the marker (in the depth direction). For example, the standard deviation is 20 mm when the distance between the camera 30 and the marker is 4 m. On the other hand, almost no error occurs in the horizontal direction. Then, error ellipses EE11 to EE13 indicating error anisotropy exist in the estimation results of the position and orientation of the marker M1 at each of the positions P1 to P3. Therefore, by creating a graph structure based on observation results from a plurality of different directions and optimizing it, the intersection point IS1 of the error ellipses EE11 to EE13 can be obtained as a solution. Similarly, for the marker M2, the intersection point IS2 of the error ellipses EE21 to EE23 can be obtained as a solution. Therefore, errors in the position and orientation of the marker can be corrected. As will be described later, since there is a strong constraint (high probability) regarding the position and orientation between the markers M1 and M2, the position in the depth direction and the yaw angle orientation of the marker reference coordinates MC1 and MC2 can be accurately determined. It is possible to estimate

In S110, the access position estimation unit 15 estimates the access position where the fork 22 is inserted from the corrected position and orientation of the marker node. Specifically, the origin and coordinate orientation of the pallet reference coordinates PC are estimated. As described with reference to FIG. 3, since the relative positional relationship between the markers M1 and M2 and the pallet reference coordinates PC is known, the origin and coordinate orientation of the pallet reference coordinates PC can be easily calculated. The access position estimation unit 15 then outputs the estimated access position to the trajectory generation unit 16 and the cargo handling device control unit 18 .

In S120, it is determined whether or not to end the position/orientation estimation process. Specifically, it is determined in S120 whether or not to end the position/orientation estimation process depending on whether or not the process of scooping up the pallet 50 in S280 of the cargo handling process described later is completed. If the determination is negative (S120: NO), the process returns to S10. When it is determined again that the graph optimization process should be executed (S35: YES), the vehicle node VN3 and the sensor node SN3 at time t i ₊₂ are newly added to the graph structure data. Also, the arc AC3 between the sensor node SN3 and the marker nodes MN1 and MN2, the arc AVC3 between the vehicle node VN3 and the sensor node SN3, and the arc AVN23 between the vehicle nodes VN2 and VN3 are newly added to the graph structure data. added to. Then, the graph structure data is optimized again in S100, and the positions and orientations of the marker nodes are corrected again in S110. On the other hand, if the determination is affirmative (S120: YES), the position/orientation estimation process ends.

(Details of cargo handling)
The cargo handling process will be described with reference to FIG. In S210, the vehicle control module MOD2 determines whether or not to start the cargo handling process. For example, the determination may be made depending on whether or not the user has input a cargo handling start operation. If the determination is negative (S210: NO), the process returns to S210 and waits, and if the determination is positive (S210: YES), the process proceeds to S220.

In S220, the trajectory generator 16 acquires the target position. Acquisition of the target position can be executed in response to the first estimation of the access position of the fork 22 in S110 of the above-described position/orientation estimation processing (FIG. 6). The target position may be set about 30 cm in front of the fork 22 access position. At S230, the trajectory generator 16 generates a trajectory and a speed pattern from the current position of the vehicle body 2 to the object based on the origin and coordinate orientation of the pallet reference coordinates PC calculated at S110. In S240, the movement control unit 17 moves the vehicle body 2 toward the target position based on the generated trajectory and speed pattern.

In S250, the movement control unit 17 determines whether or not the target position has been reached. If the determination is negative (S250: NO), the process returns to S240 to further move the vehicle body 2, and if the determination is positive (S250: YES), the process proceeds to S260. Here, in the position/orientation estimation processing (FIG. 6) that operates in parallel with the cargo handling processing (FIG. 7), the graph structure is added (S40 to S70) and the graph structure is optimized (S100) as the vehicle body 2 moves. ) and access position estimation (S110) are repeatedly executed. That is, the visual reference position is updated from the fork 22 while the vehicle body 2 is moving. Then, the vehicle body 2 can be moved toward the target position updated as needed. Therefore, it becomes possible to accurately perform the approaching operation to the pallet 50 .

In S260, the cargo handling device control unit 18 adjusts the position of the fork 22 via a cargo handling actuator (not shown). Specifically, the lift mechanism adjusts the height of the fork 22 and the side shift mechanism adjusts the lateral position of the fork 22 . In S270, the cargo handling device control unit 18 inserts the fork 22 into the access position based on the calculated origin and coordinate orientation of the pallet reference coordinates PC. Specifically, considering the depth of the pallet 50, the vehicle body 2 is moved straight. In S<b>280 , the cargo handling device control unit 18 scoops up the pallet 50 . Specifically, the fork 22 is tilted by the tilt mechanism and the fork 22 is raised by the mast 20 .

At S290, the unloading operation and the return operation to the standby position are performed. These operations may be performed automatically or may be operated by an operator. Since these operations can be realized by the conventional technology, description thereof is omitted.

(effect)
In technology for recognizing the position and orientation of an object by photographing a marker attached to an object such as a pallet or loading platform from a distance with a facing camera, generally, the position of the object in the depth direction, Since it is difficult to accurately determine the rotational postures in the yaw and pitch directions estimated from the normal vectors of the markers, unintended collisions may occur when inserting the fork into the pallet. The technique herein generates graph structure data comprising a plurality of sensor nodes, marker nodes, and a plurality of arcs, and optimizes the generated graph structure data. A plurality of sensor nodes is information indicating the position and orientation of camera 30 at each of a plurality of movement points at different times. A marker node is information representing the positions and orientations of markers M1 and M2 estimated at each of a plurality of movement points at different times. Therefore, using a plurality of estimation results of the positions and orientations of the markers M1 and M2 estimated at each of a plurality of movement points at different times, the estimated values of the positions and orientations of the markers M1 and M2 can be corrected. . In other words, a graph structure is used for the information in the depth direction of the object estimated using the markers and the information in the depth direction of the position (odometry) of the vehicle body 2 obtained by accumulating the movement amounts of a plurality of moving points. can be associated with Insufficient accuracy of the information in the depth direction of the object can be compensated by using the information in the depth direction of the odometry. As described above, the positions and orientations of the markers M1 and M2 can be estimated more accurately than when using the estimation results of the positions and orientations of the markers M1 and M2 estimated at one movement point.

In the technology of this specification, the relative positional relationship among the camera, vehicle body, and marker is expressed using graph structure data. The relative positional relationship between the markers M1 and M2 is known and positioned with high accuracy. In other words, the "probability" of the arc AM between markers is high. Therefore, arcs that need to be corrected by optimization can be narrowed down to arcs AC between sensor nodes and markers. The positions and orientations of the markers M1 and M2 can be estimated more accurately than when the "likelihood" of all arcs is low.

When the vehicle body 2 moves from the movement point at time t _i to the movement point at time t _i+1 , the graph structure data generator 13 generates sensor nodes and marker nodes at the movement point at time t _i+1 and the movement point at time t _i+1. and arcs that define the relative positional relationship between the sensor nodes and the marker nodes in (S40 to S70). Then, the generated sensor node, marker node, and arc at the movement point at time t _i+1 can be newly added to the graph structure data generated for the movement points up to time t _i (S80). That is, the forklift 1 according to the present embodiment can update the graph structure data during travel.

As shown in FIG. 3, the markers M1 and M2 are horizontally arranged apart from each other by a predetermined first predetermined distance D1. That is, the markers M1 and M2 are arranged in a direction orthogonal to the rotation axis in the yaw direction (the z-axis in FIG. 3). As a result, the accuracy of estimating the rotational posture in the yaw direction, which is the three-dimensional posture of the pallet 50 in the depth direction, can be increased. In other words, the yaw measurement accuracy of each of the two markers M1 and M2 is low. For example, the measurement accuracy when observed from the front of the marker may have independent angular variations of several degrees. On the other hand, the measurement accuracy of the position (particularly in the horizontal direction and the vertical direction) is higher than the measurement accuracy in the yaw direction. And, as explained in FIG. 3, the first predetermined distance D1 between the two markers is precisely defined. Also, the normal direction of the marker reference coordinates MC1 and MC2 (x-axis negative direction in FIG. 3) is accurately positioned so as to be orthogonal to the direction connecting the two markers (y-axis direction in FIG. 3). there is By optimizing the graph structure based on this prior knowledge, it is possible to improve the accuracy of estimating the yaw angle posture of the marker.

A second embodiment is an example of automatic cargo handling when there are a plurality of pallets with marker groups. The processing contents are the same as those of the first embodiment except that there are a plurality of pallets. Only points different from the first embodiment will be described below. As an example, as shown in FIG. 10, a case where a pallet 50a is arranged in addition to the pallet 50 will be described. Pallet 50a includes markers M1a and M2a. The contents and positional relationship of the markers M1a and M2a are the same as those of the markers M1 and M2 of the palette 50. FIG. Also, a case where the graph structure data in FIG. 11 is generated based on the positional relationship in FIG. 10 will be described.

Pallets 50 and 50a are imaged by camera 30 at position P1 at time t _i in FIG. 10 (S10 in FIG. 6). The marker group position/orientation estimation unit 11 estimates the positions and orientations of the markers M1, M2, M1a, and M2a (S20). The graph structure data generator 13 sets marker nodes MN1a and MN2a in addition to the marker nodes MN1 and MN2, as shown in FIG. 11 (S60). In addition to arc AC1, arc AVC1, arc AVN12, and arc AM, arc AC1a is set (S70). Subsequent processing is the same as that of the first embodiment, so description thereof is omitted.

From the above, as shown in FIG. 11, graph structure data can be generated for a plurality of palettes. Then, the graph structure data for multiple palettes can be optimized (S100). It is possible to collectively correct the positions and orientations of the markers placed on each of the plurality of pallets at the same time.

Example 3 is an example of automatic cargo handling processing in which markers are placed on known fixed objects such as pillars and walls in addition to pallets. The contents of processing are the same as those of the first embodiment, except that markers are also placed on fixed objects. Only points different from the first embodiment will be described below. As an example, as shown in FIG. 12, a case where pillars PL1 to PL4 are arranged in addition to the pallet 50 will be described. The pillars PL1-PL4 are provided with markers MP1-MP4. Also, the processing contents when the vehicle body 2 travels from the position P11 at the time t _i to the position P14 at the time t _i+3 will be described. Also, a case where the graph structure data in FIG. 13 is generated based on the positional relationship in FIG. 12 will be described. In FIG. 13, pillar nodes PN1 to PN4 are arranged corresponding to the pillars PL1 to PL4, respectively.

The graph structure data generator 13 sets arcs AP1 to AP5 between the pillar nodes PN1 to PN4, as shown in FIG. Arc AC11 is set between sensor node SN11 and pillar nodes PN1 and PN2. An arc AC12 is set between the sensor node SN12 and the pillar node PN3. An arc AC13 is set between the sensor node SN13 and the pillar node PN4. Arc AC14 is set between sensor node SN14 and marker nodes MN1 and MN2.

From the above, as shown in FIG. 13, graph structure data can also be generated for the pillars PL1 to PL4. Then, the generated graph structure data can be optimized (S100). Since the relative positions and postures of the pillars PL1 to PL4 are known, the arcs AP1 to AP5 set between the pillar nodes PN1 to PN4 have a high "probability". Since the arcs AP1 to AP5 with high "probability" can be included in the graph structure data, the total error included in the graph structure data can be reduced compared to the case where the arcs AP1 to AP5 are not included. It is possible to correct the positions and orientations of the marker nodes MN1 and MN2 more accurately.

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)
In the present embodiment, a forklift was described as a moving object that recognizes the position and orientation of the marker, but the present invention is not limited to this form. The technology of the present specification can be applied to any moving object that can move using the position/orientation information of the marker. Other examples of mobile objects include mobile robots, mobile manipulators, and drones.

Graph structure data that does not include a vehicle body (moving body) node may be used. In this case, for example, in the graph structure data example of FIG. 5, the vehicle body nodes VN1 and VN2, the arcs AVC1 and AVC2, and the arc AVN12 can be omitted. For example, even if there is no information about the position and orientation of the camera, such as a hand-held camera, the marker position can be estimated with high accuracy by applying Graph-Based SLAM to the observation results from multiple different directions and the error model. can do. This principle has already been explained in FIG. As a result, it is possible to obtain the effect of improving the accuracy of the trajectory at the step of generating the trajectory and speed pattern for guiding the pallet before the vehicle body 2 moves (S230). Note that the optimization of the graph structure does not necessarily require the initial value of the camera position at the time of observation. The position of the camera node can be obtained as a result of optimizing the graph structure.

In this embodiment, the case of generating graph structure data including a plurality of sensor nodes at a plurality of movement points at different times has been described, but the present invention is not limited to this form. For example, as shown in FIG. 9, it is possible to estimate the positions and orientations of markers M1 and M2 using a graph structure with one sensor node at one moving point at one time.

In FIG. 3, the case where a plurality of markers are arranged in the same plane in the horizontal direction has been described, but the present invention is not limited to this form. As long as the relative position between the origin of the pallet reference coordinates PC and the origin of the marker reference coordinates is uniquely determined, the plurality of markers may be arranged in different planes, or the plurality of markers may be arranged in the horizontal direction. It doesn't have to be.

In FIG. 3, in addition to the markers M1 and M2 horizontally arranged at a distance of a first predetermined distance D1, ) may further comprise one or more markers. This allows additional markers to be placed in a direction orthogonal to the pitch direction axis of rotation (y-axis in FIG. 3). It is possible to increase the accuracy of estimating the rotation posture in the pitch direction, which is the three-dimensional posture of the pallet 50 in the depth direction.

Although the case where the number of markers attached to the pallet is two has been described, the present invention is not limited to this form. A plurality of markers is sufficient as long as arcs can be arranged between the markers.

Although FIG. 3 illustrates a mode in which markers are arranged only on the front surface of the pallet 50, the present invention is not limited to this mode. A marker may also be placed on the side surface of the pallet 50 in preparation for the case where the camera 30 takes an image of the pallet 50 from a direction oblique to the front of the pallet 50 .

Various methods of arranging the markers on the palette may be used as long as the relative positions between the markers are uniquely determined. Applied with a tape, adhered, screwed, magnetized, or pinned may be used. Markers may be printed directly onto the pallet by laser printing, hot stamping, screen printing, or the like. A mode is also possible in which the marker mounting part, in which the marker is inserted in the transparent pocket, is fixed to the pallet.

In this embodiment, a mode using an AR marker as a marker has been described, but the present invention is not limited to this mode. Any marker can be used as long as the position and orientation of the marker can be determined. For example, invisible markers may be used.

The camera 30 is not limited to a monocular camera, and may be a stereo camera or an invisible marker camera.

The camera 30 can be attached freely as long as it is integrated with the fork 22 . For example, in FIG. 1, it may be arranged between the claws 22a and 22b and on the inner side (the side of the vehicle body 2) from the backrest surface of the fork 22. As shown in FIG.

Although the case where the pallet 50 is a basket pallet has been described, it may be a pallet of other shapes such as a flat pallet.

In this embodiment, the vehicle body position/orientation calculator 12 obtains the movement amount of the vehicle body 2 with the encoder 40 in S30, but the present invention is not limited to this form. Laser odometry using a two-dimensional laser sensor or the like may also be used.

The control unit 10 is not limited to being mounted on the forklift 1 . For example, it may be a server located on a network such as the Internet.

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.

Camera 30 is an example of an optical sensor. The graph optimization unit 14 is an example of a correction unit. Fork 22 is an example of a support structure.

1: Forklift 11: Marker group position/posture estimation unit 12: Car body position/posture calculation unit 13: Graph structure data generation unit 14: Graph optimization unit 15: Access position estimation unit 16: Trajectory generation unit 17: Movement control unit 18: Cargo handling Device control unit 19: Graph structure storage unit 22: Fork 30: Camera

Claims (8)

A moving body for recognizing the position and orientation of an object including a group of markers in which a first marker and a second marker are arranged apart from each other by a predetermined first predetermined distance,
an optical sensor for imaging the marker group;
At each of the one or more movement points, the first marker and the a marker group position/orientation estimation unit that estimates the position and orientation of the second marker ;
a plurality of sensor nodes; a first marker node ; a second marker node ; a graph structure data generation unit that generates graph structure data including one arc and a second arc that defines a constraint on the relative positional relationship between the first marker and the second marker;
a correction unit that corrects estimated values of the positions and orientations of the first marker and the second marker by optimizing the generated graph structure data;
with
the plurality of sensor nodes are nodes indicating the position and orientation of the optical sensor at each of the one or more movement points;
The first marker node and the second marker node are nodes indicating the positions and orientations of the first marker and the second marker estimated by the marker group position/orientation estimation unit,
the constraint on the second arc is stronger than the constraint on the first arc;
wherein the optimization modifies the first arc more than the second arc;
Mobile. a moving body position/orientation calculating unit that calculates the position and orientation of the moving body at the moving point at time t _{i+1 when} the moving body moves from the moving point at time t i to the moving point at time t _i+1; ,
The marker group position/orientation estimation unit estimates the position and orientation of the marker group at the movement point at the time t _i+1 ,
The graph structure data generator generates a sensor node and a marker node at the movement point at time t _i+1 and an arc that defines a relative positional relationship between the sensor node and the marker node at the movement point at time t _i+1 . and added to the graph structure data. The moving body includes a support structure capable of holding the object,
2. The movement according to claim 1, wherein the moving body further comprises an access position estimating unit that estimates an access position for accessing the support structure based on the positions and orientations of the marker group corrected by the correcting unit. body. a trajectory generation unit that generates a trajectory and a speed pattern from the current position of the moving body to the object based on the positions and orientations of the group of markers corrected by the correction unit;
a movement control unit that moves the moving object to the object based on the generated trajectory and speed pattern;
an access control unit that causes the support structure to access the access position based on the access position estimated by the access position estimation unit;
The moving object according to claim 3, further comprising: the support structure is a fork;
the object is a pallet,
5. The moving body according to claim 3, wherein said optical sensor operates in unison with the movement of said fork. 6. The moving body according to any one of claims 1 to 5, wherein said marker group comprises two or more markers horizontally separated by said first predetermined distance. 7. The moving body according to claim 6, wherein said marker group further comprises one or more markers above or below said two or more markers horizontally separated by said first predetermined distance. The marker group position and orientation estimation unit estimates the position and orientation of each of the plurality of marker groups when there are a plurality of the marker groups in the image captured by the optical sensor,
8. The graph structure data generator according to any one of claims 1 to 7, wherein said graph structure data generator generates a plurality of said first marker nodes and said second marker nodes indicating respective positions and orientations of said plurality of marker groups. mobile.

Images