WO2019234814A1 - カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット - Google Patents

カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット Download PDF

Info

Publication number
WO2019234814A1
WO2019234814A1 PCT/JP2018/021497 JP2018021497W WO2019234814A1 WO 2019234814 A1 WO2019234814 A1 WO 2019234814A1 JP 2018021497 W JP2018021497 W JP 2018021497W WO 2019234814 A1 WO2019234814 A1 WO 2019234814A1
Authority
WO
WIPO (PCT)
Prior art keywords
calibration
orientation
task
robot
time
Prior art date
Application number
PCT/JP2018/021497
Other languages
English (en)
French (fr)
Inventor
秀行 粂
亮祐 三木
聡 笹谷
誠也 伊藤
Original Assignee
株式会社日立製作所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社日立製作所 filed Critical 株式会社日立製作所
Priority to JP2020523873A priority Critical patent/JP7064586B2/ja
Priority to US17/055,821 priority patent/US11865730B2/en
Priority to PCT/JP2018/021497 priority patent/WO2019234814A1/ja
Publication of WO2019234814A1 publication Critical patent/WO2019234814A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/021—Optical sensing devices
    • B25J19/023—Optical sensing devices including video camera means
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B25J13/089—Determining the position of the robot with reference to its environment
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39016—Simultaneous calibration of manipulator and camera
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39024—Calibration of manipulator

Definitions

  • the present invention relates to a camera position / orientation calibration apparatus, a camera position / orientation calibration method, and a robot in a camera used by being mounted on a robot, for example.
  • Japanese Patent Application Laid-Open No. H10-228867 discloses that the robot arm visual sensor correction method according to the present invention recognizes the position of the workpiece by recognizing the position of the workpiece by a visual sensor attached to the tip of the robot arm.
  • a movement amount specifying processing step for specifying a movement amount of the tip of the robot arm by movement of the robot arm with reference coordinates;
  • An image processing step for obtaining a processed image before and after the movement by performing image processing on the image of the workpiece acquired by the visual sensor before and after the movement, and attachment of the visual sensor to the processed image before and after the movement Applying a temporary parameter as an estimate of the state value before and after the movement
  • a position change amount extraction processing step for obtaining a position change amount of the workpiece before and after the movement by converting the processed image into reference coordinates to correspond to the temporary parameter, and a difference between the movement amount and the position change amount
  • a state difference calculation processing step for calculating as a difference and a positional change amount extraction processing step by changing the temporary parameter and repeating the plurality of times to obtain a plurality of the state differences, and the smallest state difference among the plurality of the state differences
  • the image used for calibration is acquired by causing the robot to perform a calibration-specific operation, and the task set for the robot is interrupted while the calibration-specific operation is being performed. Also, it takes time to execute the calibration-specific operation.
  • the two-stage operation for performing the calibration operation is sequentially executed in time series.
  • the present invention provides a camera position / orientation calibration apparatus, a camera position / orientation calibration method, and a robot that can calibrate the relative position / orientation of the camera and the robot without interrupting the task set in the robot.
  • the purpose is to do.
  • a task operation planning unit that plans an operation for executing a task set in a robot from an image taken by a camera mounted on the robot and the position and orientation of the robot, And the robot's position and orientation, the calibration operation planning unit that plans the operations necessary to calibrate the relative position and orientation of the camera and robot, the task operation plan planned by the task operation planning unit, and the calibration planned by the calibration operation planning unit
  • the camera position / orientation calibration apparatus includes an integrated motion planning unit that plans motion by integrating motion plans and operates a robot.
  • the present invention is “a robot operated by a camera position and orientation calibration device”.
  • the present invention provides a task operation plan in which an operation for executing a task set in a robot is planned based on an image taken by a camera mounted on the robot and the position and orientation of the robot, and the image and the position and orientation of the robot. From the calibration operation plan that planned the operation necessary for the calibration of the relative position and orientation of the camera and robot, the integrated operation plan that integrated the task operation plan and the calibration operation plan, and the task status and goal, the task status and A camera position / orientation calibration method that calculates a priority of a calibration operation plan for a task operation plan based on a target, selects an operation plan based on the priority, and operates a robot according to the selected operation plan ".
  • a camera position and orientation calibration apparatus that can calibrate the relative position and orientation of a camera and a robot without interrupting a task set in the robot.
  • FIG. 3 is a diagram illustrating a block configuration example of a camera position and orientation calibration apparatus 100.
  • movement data D1 memorize
  • movement data data D2 memorize
  • 5 is a flowchart showing processing executed in a calibration operation planning unit 102.
  • the figure which shows an example of task status target table TB2. 5 is a flowchart showing processing executed in a calibration priority calculation unit 104.
  • 6 is a flowchart showing processing executed in an integrated operation planning unit 103.
  • the flowchart which shows the process performed in process step S710.
  • 5 is a flowchart showing processing executed in a calibration priority calculation unit 104.
  • FIG. 3 An example of a typical robot to which the camera position / orientation calibration apparatus of the present invention can be applied will be described with reference to FIG.
  • a picking robot will be described as an example of the robot 200.
  • the picking robot 200 is a robot that picks up the workpieces 221 stacked on the work table 220 one by one and arranges them in a predetermined place.
  • the picking robot 200 includes a plurality of joints 212, links 214, and an end effector 215, and a camera 201 as a camera device is mounted at an appropriate position of the robot 200 by a camera mounting jig 213.
  • Each joint 212 of the robot 200 is appropriately provided with a position and orientation sensor 202 that is a sensor for measuring the angle of each joint 212.
  • the picking robot 200 recognizes the work 221 on the work table 220 by the camera 201 and controls the angle of the joint 212 to move the end effector 215 to a place where the work 221 can be lifted.
  • the end effector 215 is, for example, a suction pad or a robot hand, and lifts the work 221 by suction or gripping.
  • the number of joints 212 and links 214 of the picking robot 200 is arbitrary.
  • an RGB-D camera that can acquire a distance in addition to an image may be used as the camera 201.
  • the picking robot 200 needs to confirm the position and orientation of the end effector 215. For example, the picking robot 200 lifts the workpiece 221 using the position and orientation sensor 202 that measures the angle of each joint 212. The position and orientation of the end effector 215 that are important at the time are confirmed. By measuring the angle of each joint 212, the position and orientation of the end effector 215 can be obtained by solving the forward kinematics by a known method based on the geometric model of the picking robot 200.
  • the robot 200 using the recognition result by the camera 201, the robot 200 is moved to a place where a task (in the example of the picking robot 200 in FIG. 3, the work 221 is taken out and arranged in a predetermined place) can be executed.
  • a task in the example of the picking robot 200 in FIG. 3, the work 221 is taken out and arranged in a predetermined place
  • the relative position and orientation of the camera 201 and the robot 200 are required. This is, for example, the relative position and orientation between the installation point N of the camera 201 in FIG. 3 and the tip position M of the end effector 215 on the robot 200 side, for example.
  • the relative position and orientation of the camera 201 and the robot 200 are changed by vibrations during the operation of the robot 200 or when the camera 201 and the robot 200 collide with an object existing in the surrounding environment during the operation, and are initially set. An error may occur for the value. Since the error of the relative position and orientation of the camera 201 and the robot 200 becomes an error of the position and orientation of the robot 200, if the error is large, the task will fail.
  • the control device of the robot 200 includes a camera position and orientation calibration device that is a function of calibrating the relative position and orientation of the camera 201 and the robot 200.
  • the camera position / orientation calibration apparatus calculates an operation plan necessary for executing the task and an operation plan necessary for calibrating the camera position / orientation before the error of the relative position / orientation between the camera 201 and the robot 200 becomes so large that the task fails.
  • the operation of the robot 200 that can calibrate the position and orientation of the camera without interrupting the task is planned.
  • FIG. 2 is a diagram illustrating a configuration example of a robot system including the robot 200 and the robot control device 10.
  • the robot 200 includes at least a camera 201 and a position / orientation sensor 202, and gives image information and robot position / orientation information to the robot control apparatus 10, respectively.
  • the robot control apparatus 10 includes a camera position / orientation calibration apparatus 100, a task information / target acquisition unit 310, and a camera position / orientation estimation unit 300.
  • the robot 200 includes a camera 201 and a position / orientation sensor 202.
  • the camera 201 captures an image of the surrounding environment of the robot 200.
  • the position / orientation sensor 202 outputs the position / orientation of the robot 200.
  • the position / orientation sensor 202 may be a sensor that directly measures the position and orientation of the robot 200 or a sensor that measures the state of the robot 200. When a sensor that measures the state of the robot 200 is used, the robot 200 estimates the position and orientation of the robot 200 from the state of the robot 200 and outputs it.
  • the camera position / orientation estimation unit 300 in the robot control apparatus 10 shown in FIG. 2 includes an image captured by the camera 201 mounted on the robot 200 and the position / orientation sensor mounted on the robot 200 during the operation of the robot 200.
  • the relative position and orientation of the camera 201 and the robot 200 are estimated from the position and orientation of the robot 200 acquired by 202.
  • the problem of estimating the relative position and orientation of the camera 201 and the robot 200 from the image and the position and orientation of the robot 200 is called Hand-Eye calibration.
  • the camera position / orientation estimation unit 300 may use a known Hand-Eye calibration method.
  • a method that does not use a marker, such as 3497-3503 (2011), is preferable.
  • the robot 200 executes a task using the camera position and orientation estimated by the camera position and orientation estimation unit 300.
  • the task status / target acquisition unit 310 acquires the task status and target set in the robot 200.
  • the task is to take out the work 221 and arrange it at a predetermined place, as described in detail later.
  • the task status refers to, for example, the type of work 221, For example, the shape and the number of workpieces 221 that have been arranged.
  • the task goal is, for example, the number of works 221 to be arranged, the time to finish arranging the works 221, and the like.
  • the camera position / orientation calibration apparatus 100 includes an image photographed by a camera 201 mounted on the robot 200, a robot position / orientation acquired by a position / orientation sensor 202 mounted on the robot 200, and a task status / target acquisition unit 310.
  • the operation of the robot 200 is planned based on the task status / target acquired by the above.
  • the robot 200 executes the operation planned by the camera position / orientation calibration apparatus 100. For this reason, the robot includes an actuator (not shown) that drives a joint and the like.
  • the camera position / orientation calibration apparatus 100 is necessary for the calibration of the operation plan and camera position / orientation necessary for executing the task before the error of the relative position / orientation between the camera 201 and the robot 200 becomes so large that the task fails.
  • the operation of the robot 200 that can calibrate the position and orientation of the camera without interrupting the task is planned.
  • FIG. 1 is a diagram illustrating a block configuration example of the camera position / orientation calibration apparatus 100.
  • the camera position / orientation calibration apparatus 100 includes a task operation planning unit 101, a calibration operation planning unit 102, an integrated operation planning unit 103, and a calibration priority calculation unit 104.
  • the task operation plan unit 101 creates a task operation plan L1 that is an operation for executing a task set in the robot, from the image and the position and orientation data of the robot.
  • the calibration operation planning unit 102 creates a calibration operation plan L2 that is an operation necessary for calibration of the camera position and orientation from the image and the position and orientation of the robot.
  • the integrated operation plan unit 103 integrates the task operation plan L1 planned by the task operation plan unit 101 and the calibration operation plan L2 planned by the calibration operation plan unit 102 based on the task status and goals. Create L.
  • the integrated operation planning unit 103 includes a calibration priority calculation unit 104.
  • the calibration priority calculation unit 104 calculates the priority of the calibration operation plan L2 with respect to the task operation plan L1.
  • the task operation plan unit 101 creates a task operation plan L1 for executing a task set in the robot 200 from the image and the position and orientation data of the robot 200.
  • the task operation planning unit 101 is an operation for lifting the work 221 and arranging it at a predetermined place from the image of the work 221 taken by the camera 201 and the position and orientation data of the picking robot 200.
  • An operation plan L1 is created.
  • the task motion planning unit 101 calculates the position and orientation of the picking robot 200 that can lift the work 221 by estimating the position and orientation of the work 221 from the image using a known pattern matching method or machine learning method. .
  • the position and orientation of the picking robot 200 that can lift the workpiece 221 corresponding to each workpiece 221 is calculated.
  • L1 an example of an operation locus at the time of task operation planning of the position and orientation planned by the task operation planning unit 101 is shown as L1 in FIG.
  • a task operation plan for the workpiece 221A among the plurality of workpieces 221 is shown as L1
  • task operation plans for other workpieces are separately obtained.
  • the calibration operation planning unit 102 plans an operation (calibration operation plan L2) necessary for calibration of the camera position and orientation from the image and the position and orientation of the robot 200.
  • FIG. 3 shows an example of operations necessary for camera position / orientation calibration.
  • the camera position / orientation calibration requires an image at point X and information on the position / orientation of the robot. From the current position toward point X, A plan for newly acquiring an image at point X and information on the position and orientation of the robot is a calibration operation plan L2. Details of the calibration operation planning process will be described later with reference to FIGS.
  • the integrated operation planning unit 103 operates by integrating the task operation plan L1 planned by the task operation planning unit 101 and the calibration operation plan L2 planned by the calibration operation planning unit 102 based on the task status and the target ( Plan an action plan L). Details of the integrated operation plan processing will be described later with reference to FIGS.
  • connection between the robot 200 and the camera position / orientation calibration apparatus 100 may be a wired connection such as USB or Ethernet (registered trademark), or a wireless connection via a wireless network.
  • the camera position / orientation calibration apparatus 100 may be provided in the robot 200, or may be provided in a PC or server connected to the robot 200.
  • the calibration operation planning unit 102 plans a calibration operation plan L2 that is an operation necessary for calibration of the camera position and orientation from the image and the position and orientation of the robot 200.
  • the calibration operation planning unit 102 includes an acquired operation database DB1 that stores acquired operation data D1, and a calibration operation database DB2 that stores calibration operation data D2.
  • the calibration operation planning unit 102 outputs an operation necessary for calibration (calibration operation plan L2) as a calibration operation plan table TB1.
  • the acquired motion database DB1 acquires images at various positions and postures at appropriate points in time while the robot is operating, and is supplemented with stored contents.
  • FIG. 4 is a diagram showing an example of the acquired operation data D1 stored in the acquired operation database DB1.
  • the acquired motion data D1 shown in each row of the acquired motion database DB1 is obtained by storing image data D11 captured by the camera 201 and position / attitude data D12 of the robot 200 at the time of image capture as a pair.
  • the identification code ID is individually assigned to the pair information.
  • the position / orientation of the robot 200 is expressed by a total of six parameters by the three-parameter position and the three-parameter attitude.
  • the identifier of each row of the acquired operation database DB1 is i.
  • the posture pi is ( ⁇ 10, 10, 30, 10, 0, 0).
  • the position and orientation data when each image is acquired is sequentially accumulated and stored as a pair.
  • the acquired motion data D1 in FIG. 4 is the position and orientation data when the image is acquired at the same location as the past. Although it should show the same value as the data, this relationship is lost when the relative position and orientation of the camera 201 and the robot 200 change. That is, even if images are acquired from the same place as in the past, the position and orientation are different.
  • the acquired motion data D1 stored in the acquired motion database DB1 erases all images, positions and orientations when the relative position and orientation of the camera 201 and the robot 200 are considered to have changed, and is initialized. Then, images are acquired and formed again at various positions and orientations of the robot 200. For example, when the task fails or when the camera 201 and the robot 200 come into contact with an object in the surrounding environment, it is determined that a change has occurred in the relative position and orientation of the camera 201 and the robot 200.
  • the failure of the task is a failure in lifting the workpiece 221, and, for example, when a suction pad is used as the end effector 215, the success or failure of the lifting is determined by the pressure in the suction pad.
  • the contact with the object in the surrounding environment of the camera 201 and the robot 200 is, for example, when a change other than the change assumed by the task of the robot 200 occurs in the recognition result of the surrounding environment using the image. Is determined to have occurred.
  • FIG. 5 shows an example of the calibration operation data D2 stored in the calibration operation database DB2.
  • the calibration operation data D2 stored in each row of the calibration operation database DB2 includes a plurality of position / orientation data D21 of the robot 200 used in the previous calibration performed once, and each work target W (work of FIG. 3) based on the calibration result. 221) of task success / failure data D22.
  • Position and orientation information group D21 of position and orientation pj (10, 10, 10, 10, 10, 45) form a set of patterns, and the camera of FIG.
  • the position / orientation estimation unit 300 estimates the position / orientation, calibrates the camera, and executes the task, all the tasks for the work objects Wa, Wb, and Wj are successful.
  • the camera position / orientation estimation unit 300 in FIG. 2 estimates the position / orientation using the data of the series of position / orientation information group D21 described here, calibrates the camera, and performs the task. Is executed, the task for the work target Wa has succeeded, the task for the work target Wb has failed, and the task for the work target Wj has not yet been executed.
  • the camera position / orientation estimation unit 300 in FIG. 2 estimates the position / orientation using the data of the series of position / orientation information group D21 described here, calibrates the camera, and executes the task. As a result, the task for the work target Wj has succeeded, the task for the work target Wb has failed, and the task for the work target Wa has not yet been executed.
  • the position / orientation data p shown in FIG. 5 has not yet been acquired as the acquired motion data D1 stored in the acquired motion database DB1 of FIG. 4 and the image / robot 200 position / orientation pair information. Some data is not stored in the acquired operation data D1 in FIG. In this case, the pair information of the image and position / orientation data to be acquired is determined and planned, which is described in the calibration operation plan table TB1, and the operation for acquiring the pair information of the image and position / orientation data (FIG. 3). Is the calibration operation plan L2.
  • the calibration operation database DB2 executes the task using the estimated relative position and orientation of the camera 201 and the robot 200 and the operation plan by the task operation planning unit 101, determines whether the task is successful, If the task is successful, the position and orientation used for calibration and the success or failure of the task are added to the calibration operation database DB2.
  • the calibration operation database DB2 at first, calibration is performed using a sufficient number of image / position / posture pairs (acquired operation data D1) stored in the acquired operation database DB1. It is preferable to register it in the calibration operation database DB2 by the above-described method. Specifically, for example, the 6-degree-of-freedom space of the work environment is divided by equally spaced grids, and data is acquired at positions and orientations corresponding to the respective grids. Next, several pairs are extracted at random from the image / position / orientation pair (acquired motion data D1) used for the calibration, and calibration is performed using only the extracted pair.
  • the calibration operation database DB2 is updated when calibration by the camera position / orientation estimation unit 300 is performed. Specifically, the calibration operation is performed for the success or failure of the task using the position and orientation included in the acquired operation database DB1 used for calibration by the camera position and orientation estimation unit 300 and the camera position and orientation estimated by the camera position and orientation estimation unit 300. It adds as one line of database DB2.
  • FIG. 6 is a diagram showing an example of the calibration operation plan table TB1.
  • Each row of the calibration operation plan table TB1 corresponds to each row of the calibration operation database DB2, and among the plurality of positions and orientations included in each row of the calibration operation database DB2, positions and orientations that do not exist in the acquired operation database DB1 and each work The success or failure of the task for the target is included.
  • the success or failure of the task for each work target is the same in the calibration operation database DB2 and the calibration operation plan table TB1.
  • “ ⁇ ” indicates that the position and orientation are not included. That is, in the example of FIG. 6, only one position and orientation is included in the second row of the calibration operation plan table TB1.
  • FIG. 7 is a flowchart showing processing executed in the calibration operation planning unit 102.
  • the calibration operation plan table TB1 is initialized. Specifically, the calibration operation plan table TB1 is created so that no information on the position and orientation p is left blank, and the success or failure of the task for each work target W is the same as in the calibration operation database DB2.
  • the calibration operation plan table TB1 created at this time has the contents shown in FIG.
  • processing step S510a loop processing for each row j of the calibration operation database DB2 is started.
  • the loop process is repeatedly executed with the condition changed between process step S510a and process step S510b.
  • process step S520a a loop process is started for each position and orientation p j included in the j row of the calibration operation database DB2.
  • the loop process is repeatedly executed with the conditions changed between process step S520a and process step S520b.
  • process step S530a a loop process for each row i of the acquired operation database DB1 is started.
  • the loop process is repeatedly executed with the condition changed between process step S530a and process step S530b. Accordingly, the pair information (image information D11 and position / orientation information D12) in the acquired motion database DB1 in FIG. 4 is sequentially called.
  • processing step S540 the position and orientation p j included in the j row of the calibration operation database DB2 is compared with the position and orientation included in the i row of the acquired operation database DB1. If the position and orientation do not match, the loop processing is continued. If the position and orientation match, the loop processing for each row i in the acquired motion database DB1 is skipped and included in the next calibration motion database DB2. It proceeds to loop processing for each position and orientation p j.
  • the position / posture coincidence determination is performed when, for example, two position / postures match when the difference between the two positions is within a preset threshold value and the difference between the two postures is within a preset threshold value. judge.
  • the position / posture pj is added to the jth row of the calibration operation plan table TB1.
  • the position / orientation information p extracted in the calibration operation plan table TB1 in FIG. 6 is information that does not exist in the acquired operation database DB1 in FIG. For this reason, the acquired motion data D1, which is a pair of an image and a position / posture, is newly acquired and supplemented and stored in the acquired motion database DB1. Then, the pattern including the acquired position / orientation information p in FIG. 5 is estimated by the camera position / orientation estimation unit 300 in FIG. 2, and the task is executed using the calibration data, thereby improving the probability of task success. it can.
  • the calibration operation data D2 stored in the calibration operation database DB2 is created when the camera position / orientation calibration apparatus 100 is used for the first time, and then updated every time the camera position / orientation is calibrated.
  • the integrated operation planning unit 103 calculates the task operation plan L1 planned by the task operation planning unit 101 and the calibration operation plan L2 planned by the calibration operation planning unit 102 by the priority calculation unit 104 based on the task status and the target.
  • the operation plan L is created by integration based on the calibration priorities.
  • the calibration priority calculation unit 104 calculates the priority of the calibration operation plan L2 with respect to the task operation plan L1 based on the task status and the target.
  • FIG. 9 is a diagram illustrating an example of the task status target table TB2.
  • Each row of the task status target table TB2 is a status and a target of tasks processed by the robot 200, and is arranged in the order of processing by the robot 200.
  • the task target is a management target management number D31 and an end target time D32
  • the task status is an achievement level D33, an estimated end time D34, and a task failure count D35.
  • the task status / target table TB2 is updated by the task status / target acquisition unit 310.
  • the work target management number D31 and the end target time D32, which are target tasks, are input by an operator or a production management system.
  • the achievement level D33 and the failure count D35 which are the status of the task, are updated based on the result of the picking operation by the picking robot 200.
  • the scheduled end time D34 is calculated from, for example, the past operation time and the achievement level.
  • the achievement level D33 is calculated from the total number of workpieces and the remaining number. Further, as the scheduled end time D34, the time required for the operation may be calculated from the task operation plan L1 output by the task operation planning unit 101.
  • the calibration priority calculation unit 104 calculates, for example, the output operation type and the calibration available time as the priority of the calibration operation plan L2 with respect to the task operation plan L1.
  • the output operation type is the type of operation plan output by the integrated operation plan unit 103, and is the calibration operation plan L1 or the integrated operation plan L.
  • the integrated operation plan L is a plan for executing the task operation plan L1 and the calibration operation plan L2.
  • the calibration usable time is a time during which the calibration operation can be performed.
  • FIG. 10 is a flowchart showing processing executed in the calibration priority calculation unit 104.
  • a work allowance time that is an allowance time for a task currently set in the robot 200 is calculated.
  • the result of subtracting the scheduled end time D35 from the end target time D32 of the task status target table TB2 corresponding to the current task is set as a work surplus time.
  • ID 1 in FIG. 9
  • 5 minutes which is the result of subtracting the scheduled end time D35 (9:55) from the end target time D32 (10:00) in the task status target table TB2
  • the work margin time is set to 0.
  • process step S610 if the failure count D35 for the task currently set in the robot 200 is greater than or equal to a preset threshold value, the process proceeds to process step S620; otherwise, the process proceeds to process step S630.
  • the output operation type is set to the calibration operation plan L1, and the calibration usable time is set to the work margin time.
  • the calibration operation is set to be performed using the work margin time. In the previous example, the calibration operation is set to be performed in 5 minutes.
  • process step S630 if the work margin time is greater than 0, the process proceeds to process step S640, and if not, the process proceeds to process step S650.
  • processing step S640 the output operation type is set to the integrated operation plan L, and the calibration usable time is set to the work surplus time. Thereby, it sets so that integrated operation may be implemented using work surplus time, continuing a task. In the previous example, setting is performed so that the integrated operation by the calibration operation and the task operation is performed so that the task is completed by 10:00.
  • an estimated failure time which is a time estimated to be an operation that fails the task. For example, the number of times the task will fail in the future is calculated from the achievement level and the number of failures for the task currently set in the robot 200. Finally, the estimated failure time is calculated from the average time taken for one operation calculated from past results or the average operation time when the task fails and the number of failures.
  • process step S660 the output operation type is set to the integrated operation plan L, and the calibration available time is set to the estimated failure time.
  • the integrated operation that can be executed in a shorter time than the time lost due to the task failure is set.
  • FIG. 11 is a flowchart showing processing executed in the integrated operation planning unit 103.
  • the process proceeds to processing step S710, and otherwise, the process proceeds to processing step S730. move on.
  • process step S710 for each calibration operation plan included in the calibration operation plan table TB1, the integrated operation plan L and the calibration operation time that is an extra time required by the integrated operation plan L than the task operation plan L1 are calculated. . Details of the processing will be described later.
  • an integrated operation output by the integrated operation planning unit 103 is selected from the calibration operation plan table TB1 and the integrated operation and calibration operation time corresponding to each calibration operation included in the calibration operation plan table TB1.
  • an integrated operation having a calibration operation time shorter than the calibration available time is selected as a selection candidate.
  • an integrated operation that can succeed in the task is output to the work target W that is the target of the earliest task among the selection candidates.
  • an integrated operation with the shortest calibration operation time is output. If there is no selection candidate, the task operation output by the task operation planning unit 101 is output instead of the integrated operation.
  • Processing step S730 calculates a calibration operation time required for the calibration operation for each calibration operation plan included in the calibration operation plan table TB1. Details of the processing will be described later.
  • Processing step S740 selects the calibration operation output by the integrated operation planning unit 103 from the calibration operation plan table TB1 and the calibration operation time corresponding to each calibration operation included in the calibration operation plan table TB1.
  • a calibration operation having a calibration operation time shorter than the calibration usable time is selected as a selection candidate.
  • a calibration operation that can succeed in the task is output for the work target that is the target of the earliest task among the selection candidates.
  • the calibration operation with the shortest calibration operation time is output. If there is no selection candidate, the calibration operation with the shortest calibration operation time is output among the calibration operations that can succeed in the current task.
  • FIG. 12 is a flowchart showing the process executed in process step S710.
  • processing step S711 a loop process is executed for each row k of the calibration operation plan table TB1 between S711a and S711b.
  • processing step S712 it starts a loop processing for allocation pattern to the task operation plan for each position and orientation p k contained in k rows of the calibration operation plan table TB1 between S712a and S712b.
  • the k rows of the calibration operation plan table TB1 is includes a plurality of position and orientation p k.
  • the task operation plan unit 101 outputs a plurality of task operation plans. That is, when a plurality of positions and orientations pk are integrated with a plurality of task action plans, there are a plurality of combinations of positions and orientations pk and task action plans. Further, for a single task operation plan, when integrating a plurality of position and orientation p k, a plurality of patterns are present in order to integrate the position and orientation p k. Processing step S712 starts loop processing for all these patterns.
  • processing step S713 to plan the integration operation that integrates the position and orientation p k and task operation plan included in the calibration operation plan table TB1.
  • each position and orientation p k is assigned tasks operation plan included in the calibration operation plan table TB1, by a known operation planning methods, starting from the reference position and orientation of the robot 200, via the position and orientation p k included in the calibration operation plan table TB1, to plan the operation to reach the task output as the operation plan by the task operation plan unit 101 to viable position and orientation.
  • process step S714 the time required for the calibration operation is calculated. More specifically, for each position and orientation p k is assigned tasks operation plan included in the calibration operation plan table TB1, a planned integrated operation in the process step S713 the time required for the robot 200 is performed, the robot The calculation is based on 200 specifications.
  • the operation time of the robot 200 without going through the calibration operation that is, the operation of starting the reference position of the robot 200 and reaching the place where the task can be executed is planned, and the time required for the operation is calculated.
  • the difference between the time required when the robot 200 performs the integration operation and the operation time of the robot 200 when not passing through the calibration operation is defined as the time required for the calibration operation.
  • Processing step S715 is started in the processing step S712, the calibration operation plan table TB1 all calibration operation time calculated by the loop processing for allocation pattern to the task operation plan for each position and orientation p k contained in k rows Select the shortest calibration operation time and the corresponding integrated operation.
  • processing step S730 the calibration operation plan and the calibration operation time are calculated by the same processing as in processing step S710. However, unlike the processing step S710, the calibration operation plan and the task operation plan are not integrated, and the calibration operation plan and the calibration operation time including only the calibration operation are calculated.
  • an operation plan for reaching the position and orientation from the reference position of the robot 200 with respect to all patterns in the order of a plurality of positions and orientations included in the row is known operation. Plan by the planning method, and make a calibration operation plan. Further, the time required for executing the calibration operation plan from the specification of the robot 200 is defined as the calibration operation time. Finally, the calibration operation corresponding to the shortest calibration operation time is selected for each row of the calibration operation plan table TB1.
  • the camera position / orientation calibration apparatus 100 includes a task operation planning unit 101, a calibration operation planning unit 102, and an integrated operation planning unit 103.
  • the task operation planning unit 101 plans an operation for executing a task set in the robot 200 from the image and the position and orientation data of the robot 200.
  • the calibration operation planning unit 102 plans an operation necessary for calibration of the camera position and orientation from the image and the position and orientation of the robot 200.
  • the integrated operation planning unit 103 plans the operation by integrating the task operation plan planned by the task operation planning unit 101 and the calibration operation plan planned by the calibration operation planning unit 102 (FIG. 1). Therefore, the task operation plan and the calibration operation plan are integrated, and the relative position and orientation of the camera 201 and the robot 200 can be calibrated without interrupting the task set in the robot 200. Further, the time required for calibration of the relative position and orientation of the camera 201 and the robot 200 can be shortened.
  • the calibration operation planning unit 102 includes an acquired operation database DB1 including acquired images and the position and orientation of the robot 200, and a calibration operation database including success and failure of tasks based on the position and orientation of the robot 200 used for calibration and the calibration result. Operations necessary for calibration are planned based on DB2 and output as a calibration operation plan table TB1 (FIGS. 4 to 8). Therefore, by considering the acquired operation, the time required for calibration of the relative position and orientation of the camera 201 and the robot 200 can be shortened.
  • the integrated operation planning unit 103 includes a calibration priority calculation unit 104.
  • the calibration priority calculation unit 104 calculates the priority of the calibration operation plan with respect to the task operation plan based on the task operation target table TB2 including the task status and the target. Based on the priority of the calibration operation plan calculated by the calibration priority calculation unit 104, the integrated operation planning unit 103 and the task operation plan planned by the task operation planning unit 101 and the calibration planned by the calibration operation planning unit 102.
  • the operation is planned by integrating the operation plans (FIGS. 9 to 12). Therefore, the robot 200 can perform the given task as intended by calibrating the relative position and orientation of the camera 201 and the robot 200 while considering the task status and the target.
  • the calibration priority calculation unit 104 can calculate a work surplus time that is a surplus time for the task from the target end time and the scheduled end time of the task, and can apply the calibration operation based on the work surplus time.
  • the calibration available time which is time, is calculated as the priority of the calibration operation plan (FIGS. 9 and 10). Therefore, the robot 200 can perform the given task as intended by calibrating the relative position and orientation of the camera 201 and the robot 200 while considering the work allowance time.
  • the calibration priority calculation unit 104 determines the output operation type and the calibration available time, which are types of operation plans output by the integrated operation plan unit 103, based on the number of failures for the task set in the robot 200. It is calculated as the priority of the calibration operation plan (FIGS. 9 and 10). Therefore, considering the number of failures for a task, a calibration operation is performed when there are many failures, and the robot 200 performs the given task as intended by calibrating the relative position and orientation of the camera 201 and the robot 200. be able to.
  • the calibration priority calculation unit 104 is estimated to take an operation of failing the task when the task is continued without performing the calibration work from the achievement level and the number of failures for the task set in the robot 200.
  • An estimated failure time that is a time required for the calibration operation is calculated based on the estimated failure time, and a calibration available time that is a time that can be used for the calibration operation is calculated as a priority of the calibration operation plan (FIGS. 9 and 10). ). Therefore, considering the estimated failure time, if the camera position and orientation can be calibrated in a time shorter than the estimated failure time, the task is completed in a shorter time than when no calibration is performed by calibrating the camera position and orientation. be able to.
  • the integrated operation plan unit 103 integrates and integrates each task operation plan planned by the task operation plan unit 101 and each calibration operation plan planned by the calibration operation plan unit 102 in a plurality of combination patterns.
  • the integrated operation with the shortest calibration operation time which is the operation time added to the task operation plan, is selected as the integrated operation for each calibration operation plan (FIGS. 11 and 12). Therefore, the time required for calibration of the relative position and orientation of the camera 201 and the robot 200 can be shortened.
  • the integrated operation planning unit 103 performs integration based on the success or failure of the task based on the calibration result included in the calibration operation database TB1, and the work target schedule given as a task to the robot 200 included in the task status target table TB2.
  • the operation is selected (FIGS. 9 and 11). Therefore, the task can be completed in a short time in the long run by considering the schedule of the work target.
  • the calibration operation database DB2 acquires images with various positions and orientations of the robot 200, and tasks based on calibration results using the acquired images and the positions and orientations of the robot 200 Created by carrying out.
  • the method for creating the calibration operation database DB2 is not limited to this.
  • the integrated operation planning unit integrates and integrates each task operation plan planned by the task operation planning unit and each calibration operation plan planned by the calibration operation planning unit in a plurality of combination patterns.
  • the integrated operation having the shortest calibration operation time, which is the operation time added to the above, may be selected as the integrated operation for each calibration operation plan.
  • the integrated operation planning unit may select an integrated operation using the calibration operation database and the targets of a plurality of scheduled tasks.
  • a calibration operation database may be created by simulation.
  • the calibration operation database DB2 may be created by simulation without operating the robot 200.
  • a simulation environment is created from the configuration and arrangement of the robot 200, the shape of the work target, etc., a plurality of pairs of images and positions and orientations of the robot 200 are virtually acquired, and calibration is performed in the same manner as when using actual data.
  • the operation database DB2 may be created.
  • an image of the actual work environment is acquired, and a simulation environment that simulates the real environment is created by using a three-dimensional restoration method and object recognition method using the image, and a calibration operation database You may use for preparation of DB2.
  • the camera position / orientation calibration apparatus 100 creates the calibration operation database DB2 by simulation. Therefore, the time required for creating the calibration operation database DB2 is shortened, so that the camera position / orientation calibration apparatus can be used at an early stage.
  • Embodiment 2 of the present invention will be described with reference to FIGS.
  • the same components as those in the first embodiment are denoted by the same reference numerals, and different points will be mainly described. Points that are not particularly described are the same as those in the first embodiment.
  • the case where the robot 200 is an autonomous driving vehicle is targeted.
  • the configuration of the camera position / orientation calibration apparatus according to the second embodiment is basically the same as that in FIG. 1, and is different only in that the processing of the priority calculation unit 104 is different.
  • the robot 200 is an autonomous driving vehicle.
  • FIG. 13 is a diagram illustrating a situation where the autonomous driving vehicle 230 is parked in the parking space.
  • the automatic driving vehicle 230 automatically travels to the target position / posture 241 that is a parking space in the parking lot.
  • the automatic driving vehicle 230 is equipped with a camera 231.
  • the camera 231 recognizes the surrounding environment and controls the automatic driving vehicle 230 to move the automatic driving vehicle 230 to the target position / posture 241.
  • the autonomous driving vehicle 230 recognizes a parking space in which another vehicle 242 is not parked, thereby setting the target position / posture 241 and realizing automatic parking by running automatically.
  • the configuration of the automatic driving vehicle 230 is arbitrary.
  • the autonomous driving vehicle 230 may be equipped with a plurality of cameras 231 or other sensors such as LiDAR and radar. Further, the autonomous driving vehicle 230 may communicate with the monitoring center by a communication device.
  • the position / orientation sensor 202 in the robot of FIG. 2 is a sensor that measures, for example, the amount of rotation of a tire and a steering wheel.
  • the position and orientation of the autonomous driving vehicle 230 the position and orientation of the center of the rear wheel axle that is important for vehicle control is used.
  • wheel odometry which is a known method, based on the geometric model of the autonomous driving vehicle 230.
  • the set task is to travel to the target position / posture 241.
  • the task status includes, for example, the type and shape of the target position / posture 241 and the distance to the target position / posture 241.
  • the task target is, for example, the time to reach the target position / posture 241.
  • the task operation planning unit 101 in FIG. 1 moves the autonomous driving vehicle 230 to the target position / posture 241 based on the surrounding environment image captured by the camera 231 and the position / posture data of the autonomous driving vehicle 230. Plan actions to make it happen.
  • the task motion planning unit 101 calculates the position and orientation at which parking is possible by estimating the position and orientation of the parking space using a known pattern matching method or machine learning method, and sets the target position and orientation 241 as the target position and orientation 241.
  • the position / posture that can be parked corresponding to each parking space is set as the target position / posture 241.
  • the target position / posture 241 is not limited to the parking space, and may be any position / posture requested by the user.
  • the target position and orientation 241 may be calculated using other sensors and map information.
  • FIG. 14 is a diagram showing an example of the task status target table TB2 in the autonomous driving vehicle 230. As shown in FIG. The number of passengers D30 is added to the items of the task status target table TB2 of the first embodiment shown in FIG.
  • the work target D31 is the target position / posture 241.
  • the task failure D35 is a failure in traveling to the target position / posture 241, for example, when the driver detects the danger of contact with an object in the surrounding environment and steps on the brake, it is determined to be a failure. In addition, it is also determined as a failure when a risk of contact with an object in the surrounding environment is detected by an ultrasonic sensor or the like.
  • the task 14 is updated by the task status / target acquisition unit 310.
  • the task status target table TB2 shown in FIG. For example, in the automatic driving vehicle 230, the management number of the work target, which is the task target, and the end target time D32 are input by the user or set by the automatic driving system.
  • the achievement level D33 and the number of failures D35, which are the status of the task, are updated based on the traveling result of the autonomous driving vehicle 230.
  • the scheduled end time D34 is calculated from, for example, the past operation time and the achievement level.
  • the achievement level D33 is calculated from the distance to the target position / posture 241 and the travel distance. Further, as the scheduled end time D34, a time required for the operation may be calculated from the operation plan output by the task operation planning unit 101.
  • FIG. 15 is a flowchart showing processing executed in the calibration priority calculation unit 104.
  • processing step S970 and processing step S980 are added to the flowchart of FIG. 10 executed by the calibration priority calculation unit 104 of the first embodiment shown in FIG.
  • process step S970 if the number of passengers is 0, the process proceeds to process step S980. Otherwise, the process proceeds to process step S650.
  • the output operation type is set to the integrated operation plan L and the calibration available time is set to a preset value.
  • the calibration work is prioritized over the task at the end target time by setting the calibration available time to a preset value.
  • the calibration priority calculation unit 104 calculates the calibration available time, which is the time that can be used for the calibration operation based on the number of passengers of the autonomous driving vehicle 230, as the priority of the calibration operation plan. Therefore, by carrying out calibration when the number of passengers is 0, the calibration operation time when there are passengers can be shortened, so that the burden on the passengers due to performing calibration can be reduced. .
  • this invention is not limited to the above-mentioned Example, Various modifications are included.
  • the above-described embodiments have been described in detail for easy understanding of the present invention, and are not necessarily limited to those having all the configurations described.
  • Other embodiments conceivable within the scope of the technical idea of the present invention are also included in the scope of the present invention.
  • a part of the configuration of a certain embodiment can be replaced with the configuration of another embodiment, and the configuration of another embodiment can be added to the configuration of a certain embodiment.
  • Each of the above-described configurations, functions, processing units, processing means, and the like may be realized by hardware by designing a part or all of them with, for example, an integrated circuit.
  • Each of the above-described configurations, functions, and the like may be realized by software by interpreting and executing a program that realizes each function by the processor.
  • Information such as programs, tables, and files that realize each function can be stored in a memory, a hard disk, a recording device such as an SSD (Solid State Drive), or a recording medium such as an IC card, an SD card, or a DVD.
  • DESCRIPTION OF SYMBOLS 100 Camera position and orientation calibration apparatus, 101: Task operation plan part, 102: Calibration operation plan part, 103: Integrated operation plan part, 104: Calibration priority calculation part, 200: Robot (picking robot), 201: Camera, 202 : Position / orientation sensor, 220: work table, 230: autonomous vehicle, 300: camera position / orientation estimation unit, 310: task status / target acquisition unit, DB1: acquired operation database, DB2: calibration operation database, TB1: calibration operation Plan table, TB2: Task status target table

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Multimedia (AREA)
  • Manipulator (AREA)

Abstract

ロボットに設定されているタスクを中断することなく、カメラとロボットの相対位置姿勢を校正することができるカメラ位置姿勢校正装置を提供することを目的とする。ロボットに搭載されたカメラにより撮影された画像とロボットの位置姿勢から、ロボットに設定されたタスクを実行するための動作を計画するタスク動作計画部と、画像とロボットの位置姿勢から、カメラとロボットの相対位置姿勢の校正に必要な動作を計画する校正動作計画部と、タスク動作計画部によって計画されたタスク動作計画と校正動作計画部によって計画された校正動作計画を統合することで動作を計画する統合動作計画部とを備え、ロボットを操作することを特徴とするカメラ位置姿勢校正装置。

Description

カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット
　本発明は、例えばロボットに搭載されて使用されるカメラにおけるカメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボットに関する。
　カメラを搭載したロボットを用いたシステムにおいては、カメラとロボットの相対位置姿勢の校正が必要である。
　係るカメラ位置姿勢校正手法について、特許文献１には「本発明に係るロボットアームの視覚センサ補正方法は、ロボットアームの先端に取り付けられた視覚センサによってワークの位置を認識することにより前記ロボットアームの先端での前記視覚センサの取り付け状態値を補正するロボットアームの視覚センサ補正方法において、前記ロボットアームの移動による前記ロボットアームの先端の移動量を基準座標で特定する移動量特定処理ステップと、前記移動の前後それぞれで前記視覚センサにより取得したワークの画像に対して画像処理を施すことにより移動前後での処理画像を求める画像処理ステップと、前記移動前後の処理画像に対して前記視覚センサの取り付け状態値の推定値としての仮パラメータを適用して前記移動前後での処理画像を基準座標に座標変換することにより前記移動前後に対するワークの位置変化量を前記仮パラメータに対応させて求める位置変化量抽出処理ステップと、前記移動量と前記位置変化量との差を状態差として算出する状態差算出処理ステップと、前記仮パラメータを変更して位置変化量抽出処理ステップを複数回繰り返すことにより複数の前記状態差を求め、複数の前記状態差の中で最小の状態差に対応する前記仮パラメータを視覚センサの取り付け状態値として抽出する状態値抽出処理ステップとを備えることを特徴とする。」という記載がある。
特開２００７－６１９７９号公報
　特許文献１に記載されている発明では、ロボットに搭載されたカメラにより撮影されたロボットの周辺環境の画像を用いることで、マーカの設置などの人手の作業を必要とせずにカメラとロボットの相対位置姿勢を校正することができる。
　しかし、校正に用いる画像はロボットに校正専用の動作を実行させることで取得しており、校正専用の動作を実施している間はロボットに設定されているタスクが中断される。また、校正専用の動作の実行に時間が必要である。この従来手法によれば、タスク動作の実行に先立って、校正動作を行う２段階動作を時系列的に順次実行することになる。
　以上のことから本発明においては、ロボットに設定されているタスクを中断することなく、カメラとロボットの相対位置姿勢を校正することができるカメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボットを提供することを目的とする。
　以上のことから本発明においては、「ロボットに搭載されたカメラにより撮影された画像とロボットの位置姿勢から、ロボットに設定されたタスクを実行するための動作を計画するタスク動作計画部と、画像とロボットの位置姿勢から、カメラとロボットの相対位置姿勢の校正に必要な動作を計画する校正動作計画部と、タスク動作計画部によって計画されたタスク動作計画と校正動作計画部によって計画された校正動作計画を統合することで動作を計画する統合動作計画部とを備え、ロボットを操作することを特徴とするカメラ位置姿勢校正装置」としたものである。
　また本発明は、「カメラ位置姿勢校正装置により操作されるロボット」としたものである。
　また本発明は、「ロボットに搭載されたカメラにより撮影された画像とロボットの位置姿勢から、ロボットに設定されたタスクを実行するための動作を計画したタスク動作計画と、画像とロボットの位置姿勢から、カメラとロボットの相対位置姿勢の校正に必要な動作を計画した校正動作計画と、タスク動作計画と校正動作計画を統合した統合動作計画と、タスクの状況および目標を得、タスクの状況および目標に基づいて、タスク動作計画に対する校正動作計画の優先度を算出し、優先度に基づいて、動作計画を選択し、選択した動作計画によりロボットを操作することを特徴とするカメラ位置姿勢校正方法」としたものである。
　本発明によれば、ロボットに設定されているタスクを中断することなく、カメラとロボットの相対位置姿勢を校正することができるカメラ位置姿勢校正装置を実現することが可能である。
カメラ位置姿勢校正装置１００のブロック構成例を示す図。 ロボットとロボット制御装置で構成されるロボットシステムの構成例を示す図。 本発明のカメラ位置姿勢校正装置を適用可能な典型的なロボットの一例としてピッキングロボットを示す図。 取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１の一例を示す図。 校正動作データベースＤＢ２に記憶された校正動作データデータＤ２の一例を示す図。 校正動作計画表ＴＢ１の一例を示す図。 校正動作計画部１０２において実行される処理を示すフローチャート。 校正動作計画表ＴＢ１の初期状態の一例を示す図。 タスク状況目標表ＴＢ２の一例を示す図。 校正優先度算出部１０４において実行される処理を示すフローチャート。 統合動作計画部１０３において実行される処理を示すフローチャート。 処理ステップＳ７１０において実行される処理を示すフローチャート。 自動運転車２３０を駐車スペースに駐車する状況を示す図。 自動運転車２３０におけるタスク状況目標表ＴＢ２の一例を示す図。 校正優先度算出部１０４において実行される処理を示すフローチャート。
　以下、本発明の実施例について図面を参照して説明する。
　以下、図１～図１２を参照して、本発明の実施例１に係るカメラ位置姿勢校正装置を説明する。
　最初に、本発明のカメラ位置姿勢校正装置を適用可能な典型的なロボットの一例について図３を用いて説明する。図３では、ロボット２００としてピッキングロボットを例に説明する。
　ピッキングロボット２００は、作業台２２０にバラ積みされたワーク２２１を１つずつ取り出し、所定の場所に並べるロボットである。ピッキングロボット２００は、複数の関節２１２やリンク２１４、並びにエンドエフェクタ２１５から構成されており、さらにカメラ装置としてカメラ２０１をカメラ取り付け治具２１３によりロボット２００の適宜位置に搭載している。またロボット２００の各関節２１２には、各関節２１２の角度を計測するセンサである位置姿勢センサ２０２が適宜備えられている。
　ピッキングロボット２００は、カメラ２０１により作業台２２０上のワーク２２１を認識し、関節２１２の角度を制御することで、ワーク２２１を持ち上げることが可能な場所にエンドエフェクタ２１５を移動させる。エンドエフェクタ２１５はたとえば吸着パッドやロボットハンドであり、ワーク２２１を吸着や把持することで持ち上げる。なお、ピッキングロボット２００の関節２１２やリンク２１４の数といった構成は任意である。また、カメラ２０１として画像に加えて距離も取得可能なＲＧＢ－Ｄカメラを用いても良い。
　ピッキングロボット２００は、エンドエフェクタ２１５をワーク２２１に近接させるに際し、その位置姿勢を確認する必要があり、各関節２１２の角度を計測するセンサである位置姿勢センサ２０２を用いて、たとえばワーク２２１を持ち上げる際に重要となるエンドエフェクタ２１５の位置姿勢を確認する。なお各関節２１２の角度を計測することで、ピッキングロボット２００の幾何モデルに基づき、公知の手法により順運動学を解くことでエンドエフェクタ２１５の位置姿勢を得ることができる。
　ロボット２００において、カメラ２０１による認識結果を用いて、タスク（図３のピッキングロボット２００の例では、ワーク２２１を取り出して所定の場所に並べること）を実行することが可能な場所にロボット２００を移動させるためには、カメラ２０１とロボット２００の相対位置姿勢が必要となる。これは例えば図３においてカメラ２０１の設置点Ｎと、ロボット２００側として例えばエンドエフェクタ２１５の先端位置Ｍの間の相対位置姿勢である。
　しかし、カメラ２０１とロボット２００の相対位置姿勢は、ロボット２００の動作時の振動や、動作時にカメラ２０１およびロボット２００が周辺環境に存在する物体などに衝突することにより変化し、初期に設定された値に対して誤差が生じることがある。カメラ２０１とロボット２００の相対位置姿勢の誤差は、ロボット２００の位置姿勢の誤差となるため、誤差が大きい場合には、タスクの失敗につながる。
　このため、ロボット２００の制御装置は、カメラ２０１とロボット２００の相対位置姿勢を校正する機能であるカメラ位置姿勢校正装置を備えている。カメラ位置姿勢校正装置は、カメラ２０１とロボット２００の相対位置姿勢の誤差が、タスクに失敗するほど大きくなる前に、タスクの実行に必要な動作計画とカメラ位置姿勢の校正に必要な動作計画を統合した動作を計画することで、タスクを中断することなくカメラの位置姿勢を校正可能なロボット２００の動作を計画する。
　図２は、ロボット２００とロボット制御装置１０で構成されるロボットシステムの構成例を示す図である。
　図２のロボットシステムの内、ロボット２００は少なくともカメラ２０１と位置姿勢センサ２０２を備えており、夫々画像情報とロボット位置姿勢情報をロボット制御装置１０に与える。
　図２のロボットシステムの内、ロボット制御装置１０はカメラ位置姿勢校正装置１００、タスク情報・目標取得部３１０、カメラ位置姿勢推定部３００を含んで構成されている。
　図２のロボットシステムについて、以下に詳細に説明する。まず、ロボット２００はカメラ２０１と位置姿勢センサ２０２を搭載している。カメラ２０１はロボット２００の周辺環境の画像を撮影する。位置姿勢センサ２０２はロボット２００の位置姿勢を出力する。
　なお、位置姿勢センサ２０２は、直接ロボット２００の位置姿勢を計測するセンサでも良いし、ロボット２００の状態を計測するセンサでも良い。ロボット２００の状態を計測するセンサを用いる場合には、ロボット２００は、ロボット２００の状態からロボット２００の位置姿勢を推定し、出力する。
　次に図２のロボット制御装置１０内のカメラ位置姿勢推定部３００は、ロボット２００の動作中に、ロボット２００に搭載されたカメラ２０１によって撮影された画像と、ロボット２００に搭載された位置姿勢センサ２０２によって取得されたロボット２００の位置姿勢と、からカメラ２０１とロボット２００の相対位置姿勢を推定する。
　画像とロボット２００の位置姿勢から、カメラ２０１とロボット２００の相対位置姿勢を推定する問題は、Ｈａｎｄ－Ｅｙｅキャリブレーションと呼ばれる。本発明の実施例１に係るカメラ位置姿勢推定部３００は、公知のＨａｎｄ－Ｅｙｅキャリブレーション手法を用いるのがよい。ただし、本発明においては、Ｊ．Ｈｅｌｌｅｒ、Ｍ．Ｈａｖｌｅｎａ,Ａ．Ｓｕｇｉｍｏｔｏ,Ｔ．Ｐａｊｄｌａ,Ｓｔｒｕｃｔｕｒｅ－ｆｒｏｍ－ｍｏｔｉｏｎ　ｂａｓｅｄ　ｈａｎｄ－ｅｙｅ　ｃａｌｉｂｒａｔｉｏｎ　ｕｓｉｎｇ　Ｌ∞　ｍｉｎｉｍｉｚａｔｉｏｎ,　ＩＥＥＥ　Ｃｏｎｆ．ｏｎ　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ,ｐｐ．３４９７－３５０３（２０１１）のように、マーカを用いない手法が好適である。ロボット２００はカメラ位置姿勢推定部３００によって推定されたカメラ位置姿勢を用いてタスクを実行する。
　タスク状況・目標取得部３１０はロボット２００に設定されたタスクの状況および目標を取得する。ここでタスクとは、詳細を後述するように、図３のピッキングロボット２００の例では、ワーク２２１を取り出して所定の場所に並べることであり、タスクの状況とは、たとえば、ワーク２２１の種類や形状、並べ終えたワーク２２１の数などである。またタスクの目標とは、たとえば、並べるワーク２２１の数や、ワーク２２１を並べ終える時間などである。
　カメラ位置姿勢校正装置１００は、ロボット２００に搭載されたカメラ２０１によって撮影された画像と、ロボット２００に搭載された位置姿勢センサ２０２によって取得されたロボットの位置姿勢と、タスク状況・目標取得部３１０によって取得されたタスク状況・目標と、からロボット２００の動作を計画する。ロボット２００はカメラ位置姿勢校正装置１００によって計画された動作を実行する。このためロボットは、関節などを駆動する図示せぬアクチュエータを備えている。
　これによりカメラ位置姿勢校正装置１００は、カメラ２０１とロボット２００の相対位置姿勢の誤差が、タスクに失敗するほど大きくなる前に、タスクの実行に必要な動作計画とカメラ位置姿勢の校正に必要な動作計画を統合した動作を計画することで、タスクを中断することなくカメラの位置姿勢を校正可能なロボット２００の動作を計画する。
　図１は、カメラ位置姿勢校正装置１００のブロック構成例を示す図である。カメラ位置姿勢校正装置１００は、タスク動作計画部１０１と、校正動作計画部１０２と、統合動作計画部１０３と、校正優先度算出部１０４と、を備える。
　タスク動作計画部１０１は、画像とロボットの位置姿勢のデータから、ロボットに設定されたタスクを実行するための動作であるタスク動作計画Ｌ１を作成する。校正動作計画部１０２は、画像とロボットの位置姿勢から、カメラ位置姿勢の校正に必要な動作である校正動作計画Ｌ２を作成する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標に基づいて統合することで動作計画Ｌを作成する。統合動作計画部１０３は校正優先度算出部１０４を備える。校正優先度算出部１０４は、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度を算出する。
　図１に示すカメラ位置姿勢校正装置１００の各部機能について、さらに詳細に説明する。
　タスク動作計画部１０１は、画像とロボット２００の位置姿勢のデータから、ロボット２００に設定されたタスクを実行するためのタスク動作計画Ｌ１を作成する。ピッキングロボット２００において、タスク動作計画部１０１は、カメラ２０１によって撮影されたワーク２２１の画像と、ピッキングロボット２００の位置姿勢のデータから、ワーク２２１を持ち上げて所定の場所に並べるための動作であるタスク動作計画Ｌ１を作成する。たとえば、タスク動作計画部１０１は、公知のパターンマッチング手法や機械学習手法により、画像からワーク２２１の位置姿勢を推定することで、ワーク２２１を持ち上げることが可能なピッキングロボット２００の位置姿勢を算出する。複数のワーク２２１が認識された場合には、それぞれのワーク２２１に対応した、ワーク２２１を持ち上げることが可能なピッキングロボット２００の位置姿勢を算出する。
　なおタスク動作計画部１０１が計画した位置姿勢のタスク動作計画の時の動作軌跡の一例を、図３においてはＬ１として示している。なお図３には複数のワーク２２１のうちワーク２２１Ａに対するタスク動作計画をＬ１として示しており、他のワークに対するタスク動作計画も別途求められる。
　校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作（校正動作計画Ｌ２）を計画する。なおカメラ位置姿勢の校正に必要な動作について図３で例示すると、カメラ位置姿勢の校正にはＸ点における画像とロボットの位置姿勢の情報が必要であり、現在位置からＸ点に向かわせて、新たにＸ点における画像とロボットの位置姿勢の情報を取得するための計画が校正動作計画Ｌ２である。校正動作計画処理の詳細は、図４から図７を用いて後述する。
　統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標に基づいて統合することで動作（動作計画Ｌ）を計画する。統合動作計画処理の詳細は図１０、図１１を用いて後述する。
　なお、ロボット２００とカメラ位置姿勢校正装置１００の接続は、ＵＳＢやイーサネット(登録商標)といった有線接続でも良いし、無線ネットワークを介した無線接続でも良い。また、カメラ位置姿勢校正装置１００はロボット２００内に設けられても良いし、ロボット２００と接続されたＰＣやサーバ内に設けられても良い。
　次に図４～図７を用いて、校正動作計画部１０２における処理の内容について具体的に説明する。
　校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作である校正動作計画Ｌ２を計画する。校正動作計画部１０２は、取得済み動作データＤ１を記憶する取得済み動作データベースＤＢ１と、校正動作データＤ２を記憶する校正動作データベースＤＢ２を備えている。また、校正動作計画部１０２は校正に必要な動作（校正動作計画Ｌ２）を校正動作計画表ＴＢ１として出力する。
　最初に、取得済み動作データベースＤＢ１を形成することについて説明する。取得済み動作データベースＤＢ１は、ロボット稼働中の適宜の時点において、様々な位置姿勢で画像を取得し、記憶内容を補充される。
　図４は取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１の一例を示す図である。取得済み動作データベースＤＢ１の各行に示す取得済み動作データＤ１は、カメラ２０１が撮影した画像データＤ１１と画像撮影時のロボット２００の位置姿勢データＤ１２がペアとして記憶されたものである。ペアの情報には、個別に識別符号ＩＤが付与されている。なおロボット２００の位置姿勢は、３パラメータの位置と３パラメータの姿勢により、合計６パラメータで表現される。以降では、取得済み動作データベースＤＢ１の各行の識別子をｉとする。
　なお図４の例では、ｉ＝１（ＩＤ＝１）の画像取得時における、位置と姿勢による合計６パラメータで表現される位置姿勢ｐ１は（０、０、０、０、０、０）であり、ｉ＝２（ＩＤ＝２）の画像取得時における位置姿勢ｐ２は（－５、５、１０、０、０、０）であり、ｉ＝ｉ（ＩＤ＝ｉ）の画像取得時における位置姿勢ｐｉは（－１０、１０、３０、１０、０、０）である。この取得済み動作データＤ１によれば、各画像を取得したときの位置姿勢データがペアとして順次蓄積、記憶されている。
　図４の取得済み動作データＤ１は、カメラ２０１とロボット２００の相対位置姿勢に変更がない場合には、過去と同じ場所で画像を取得したときの位置姿勢のデータは、過去取得の位置姿勢のデータと同じ値を示すはずであるが、カメラ２０１とロボット２００の相対位置姿勢に変化が生じた場合には、この関係が失われることになる。つまり過去と同じ場所から画像取得していても、位置姿勢が相違することになる。
　このため、取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１は、カメラ２０１とロボット２００の相対位置姿勢に変化が生じたと考えられる場合に、全ての画像と位置姿勢を消去し、初期化され、再度ロボット２００の様々な位置姿勢で画像を取得し、形成されることになる。たとえば、タスクに失敗した場合や、カメラ２０１およびロボット２００が周辺環境の物体などに接触した場合には、カメラ２０１とロボット２００の相対位置姿勢に変化が生じたと判定する。
　ピッキングロボット２００において、タスクの失敗は、ワーク２２１の持ち上げ失敗にあたり、たとえば、エンドエフェクタ２１５として吸着パッドを用いる場合には、吸着パッド内の圧力によって持ち上げ成否を判定する。また、カメラ２０１およびロボット２００の周辺環境の物体などへの接触は、たとえば、画像を用いた周辺環境の認識結果において、ロボット２００のタスクにより変化すると想定される変化以外が生じた場合に、接触が発生したと判定する。
　図５は校正動作データベースＤＢ２に記憶された校正動作データＤ２の一例を示す図である。校正動作データベースＤＢ２の各行に記憶された校正動作データＤ２は、以前に実施した１回の校正において用いたロボット２００の複数の位置姿勢データＤ２１と、校正結果による各作業対象Ｗ（図３のワーク２２１）に対するタスクの成否データＤ２２で構成されている。
　各作業対象Ｗに対するタスクの成否において、「○」は成功を、「×」は失敗を、「－」は該当の作業対象Ｗに対するタスクを試みていないことを示す。ピッキングロボット２００において作業対象Ｗはワーク２２１であり、タスクの成否はワーク２２１の持ち上げ成否である。以降では、校正動作データベースＤＢ２の各行の識別子をｊ、各行に含まれる位置姿勢の識別子をｐｊ、作業対象Ｗの識別子をｗｊとする。なお、校正動作データベースＤＢ２の作成・更新方法については後述する。
　図５の表記について具体的に説明すると、ＩＤ＝１に示される位置姿勢ｐ１（０、０、０、０、０、０）、位置姿勢ｐ２（１０、０、０、０、０、０）、位置姿勢ｐｊ（１０、１０、１０、１０、１０、４５）の一連の位置姿勢情報群Ｄ２１は、１組のパターンを形成しており、この位置姿勢のパターンを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行した場合に作業対象Ｗa、Ｗb、Ｗｊに対するタスクが全て成功したことをここでは表している。
　同様にＩＤ＝２のパターンでは、ここに記述された一連の位置姿勢情報群Ｄ２１のデータを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行したところ、作業対象Ｗaに対するタスクに成功したが、作業対象Ｗbに対するタスクは失敗した、作業対象Ｗjに対するタスクはまだ実行していない、ことを表している。
　ＩＤ＝jのパターンでは、ここに記述された一連の位置姿勢情報群Ｄ２１のデータを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行したところ、作業対象Ｗjに対するタスクに成功したが、作業対象Ｗbに対するタスクは失敗した、作業対象Ｗaに対するタスクはまだ実行していない、ことを表している。
　なお図５に表記された位置姿勢のデータｐには、図４の取得済み動作データベースＤＢ１に格納された取得済み動作データＤ１と、画像とロボット２００の位置姿勢のペア情報としては未だ取得されず図４の取得済み動作データＤ１に格納されていないデータとがある。この場合に、取得すべき画像と位置姿勢データのペア情報を決定、計画したものが校正動作計画表ＴＢ１に記述されており、画像と位置姿勢データのペア情報を取得するための動作（図３のＸ点への移動）を示しているのが、校正動作計画Ｌ２である。
　次に、校正動作データベースＤＢ２を形成することについて説明する。校正動作データベースＤＢ２は、推定したカメラ２０１とロボット２００の相対位置姿勢およびタスク動作計画部１０１による動作計画を用いてタスクを実施し、タスクに成功したか否かを判定し、いずれかの作業対象に対するタスクに成功した場合には、校正に用いた位置姿勢とタスクの成否を校正動作データベースＤＢ２に加えることで形成される。
　なおここで、実際にタスクを実施し、タスクの成否を判定するには、時間が必要である。そこで、校正動作データベースＤＢ２の作成時間を短縮するために、たとえば、最初は、取得済み動作データベースＤＢ１に記憶した十分な数の画像と位置姿勢のペア（取得済み動作データＤ１）を用いて校正を実施し、上述の方法で校正動作データベースＤＢ２に登録するのがよい。具体的には例えば、作業環境の６自由度空間を等間隔のグリッドで分割し、各グリッドに対応する位置姿勢でデータを取得する。次に、さきほど校正に用いた画像と位置姿勢のペア（取得済み動作データＤ１）から、ランダムにいくつかのペアを抽出し、抽出したペアのみを用いて校正を実施する。全てのペアを用いた場合の校正結果と、抽出したペアのみを用いた校正結果を、後述する校正動作計画部１０２の処理ステップＳ５４０と同様の方法で比較し、２つが一致している場合には、校正動作データベースＤＢ２に抽出したペアに含まれる位置姿勢と、全てのペアを用いた場合のワークの持ち上げ結果を加える。これにより、実際のタスクの試行回数を減らしながら、校正動作データベースＤＢ２に含まれる動作の種類を増やすことができる。
　校正動作データベースＤＢ２は、カメラ位置姿勢推定部３００による校正が実施されると更新される。具体的には、カメラ位置姿勢推定部３００が校正に用いた取得済み動作データベースＤＢ１に含まれる位置姿勢と、カメラ位置姿勢推定部３００によって推定されたカメラ位置姿勢を用いたタスクの成否を校正動作データベースＤＢ２の１行として加える。
　図６は校正動作計画表ＴＢ１の一例を示す図である。校正動作計画表ＴＢ１の各行は校正動作データベースＤＢ２の各行に対応しており、校正動作データベースＤＢ２の各行に含まれる複数の位置姿勢の内、取得済み動作データベースＤＢ１に存在しない位置姿勢と、各作業対象に対するタスクの成否が含まれている。
　なお、各作業対象に対するタスクの成否は、校正動作データベースＤＢ２と校正動作計画表ＴＢ１で同一である。なお校正動作計画表ＴＢ１の位置姿勢において「－」は位置姿勢が含まれないことを表す。すなわち、図６の例では、校正動作計画表ＴＢ１の２行目には１つの位置姿勢しか含まれない。
　図７は、校正動作計画部１０２において実行される処理を示すフローチャートである。
　校正動作計画部１０２における最初の処理ステップＳ５００では、校正動作計画表ＴＢ１を初期化する。具体的には、位置姿勢ｐの情報はひとつも含まず空欄とし、各作業対象Ｗに対するタスクの成否は、校正動作データベースＤＢ２と同じとなるように、校正動作計画表ＴＢ１を作成する。この時に作成された校正動作計画表ＴＢ１は、図８に示す内容のものである。
　処理ステップＳ５１０ａでは、校正動作データベースＤＢ２の各行ｊに対するループ処理を開始する。ループ処理は、処理ステップＳ５１０ａと処理ステップＳ５１０ｂの間で条件を変更して繰り返し実行される。
　処理ステップＳ５２０ａでは、校正動作データベースＤＢ２のｊ行に含まれる各位置姿勢ｐｊに対するループ処理を開始する。ループ処理は、処理ステップＳ５２０ａと処理ステップＳ５２０ｂの間で条件を変更して繰り返し実行される。
　処理ステップＳ５１０ａ、処理ステップＳ５２０ａにおける繰り返し処理により、図５の校正動作データベースＤＢ２の各位置姿勢ｐの情報を順次呼び出すことになる。例えば、位置姿勢ｐ１、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）、位置姿勢ｐ２、ＩＤ＝１の位置姿勢情報（１０、０、０、０、０、０）、位置姿勢ｐｊ、ＩＤ＝１の位置姿勢情報（１０、１０、１０、１０、１０、４５）、位置姿勢ｐ１、ＩＤ＝２の位置姿勢情報（０、０、０、１０、０、－５）、位置姿勢ｐ２、ＩＤ＝２の位置姿勢情報（－１０、－５、５、０、１０、１０）、位置姿勢ｐｊ、ＩＤ＝２の位置姿勢情報（０、５、０、２０、－２０、５）の順番での呼び出しを行う。
　処理ステップＳ５３０ａでは、取得済み動作データベースＤＢ１の各行ｉに対するループ処理を開始する。ループ処理は、処理ステップＳ５３０ａと処理ステップＳ５３０ｂの間で条件を変更して繰り返し実行される。これにより図４の取得済み動作データベースＤＢ１のペア情報（画像情報Ｄ１１と位置姿勢情報Ｄ１２）が順次呼び出される。
　処理ステップＳ５４０では、校正動作データベースＤＢ２のｊ行に含まれる位置姿勢ｐｊと取得済み動作データベースＤＢ１のｉ行に含まれる位置姿勢を比較する。位置姿勢が一致していない場合はループ処理を続け、位置姿勢が一致している場合は、取得済み動作データベースＤＢ１の各行ｉに対するループ処理をぬけ、次の校正動作データベースＤＢ２のｊ行に含まれる各位置姿勢ｐｊに対するループ処理に進む。
　上記の例では最初に、図５の位置姿勢ｐ１、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）について、図４の位置姿勢ｐ、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）と比較し、次に位置姿勢ｐ、ＩＤ＝２の位置姿勢情報（－５、５、１０、０、０、０）と比較するといった処理を全ての組み合わせについて順次実行する。
　位置姿勢の一致判定は、たとえば、２つの位置の差が事前に設定した閾値以内、かつ、２つの姿勢の差が事前に設定した閾値以内の場合に、２つの位置姿勢が一致していると判定する。
　処理ステップＳ５５０では、校正動作計画表ＴＢ１のｊ行に位置姿勢ｐｊを追加する。このようにして最終的に作成されたものが図６の校正動作計画表ＴＢ１である。この事例では、図５のＩＤ＝１の位置姿勢ｐ１（０、０、０、０、０、０）について図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在したので、校正動作計画表ＴＢ１のＩＤ＝１の行には記述せず、同様に位置姿勢ｐ２（１０、０、０、０、０、０）、位置姿勢ｐｊ（１０、１０、１０、１０、１０、４５）について、図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在しないので、校正動作計画表ＴＢ１のＩＤ＝１の行に記述した。
　同様に位置姿勢ｐ１、ＩＤ＝２の位置姿勢ｐ１（０、０、０、１０、０、－５）、位置姿勢ｐ２（－１０、－５、５、０、１０、１０）について図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在したので、校正動作計画表ＴＢ１のＩＤ＝２の行には記述せず、取得済みデータベースＤＢ１に存在しない位置姿勢ｐｊ（０、５、０、２０、－２０、５）を校正動作計画表ＴＢ１のＩＤ＝１の行に記述した。
　図６の校正動作計画表ＴＢ１に抽出された位置姿勢情報ｐは、いずれも図４の取得済み動作データベースＤＢ１には存在しない情報である。このため、これら不存在の位置姿勢データについて、画像と位置姿勢のペアである取得済み動作データＤ１を新たに入手して取得済み動作データベースＤＢ１に補充、記憶する。そのうえで、図５の入手済み位置姿勢情報ｐを含むパターンについて、図２のカメラ位置姿勢推定部３００において推定を行い、校正データを用いてタスク実行することにより、タスク成功の確率を向上させることができる。
　なお、校正動作データベースＤＢ２に記憶される校正動作データＤ２は、カメラ位置姿勢校正装置１００を最初に使用する際に作成し、その後、カメラ位置姿勢を校正するたびに更新する。
　次に、統合動作計画部１０３における処理の内容について説明する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標から優先度算出部１０４によって算出された校正優先度に基づいて統合することで動作計画Ｌを作成するものである。
　なお、統合動作計画部１０３における処理の内容を理解するうえでは、優先度算出部１０４によって算出される校正優先度の算出手法を理解しておくのがよいので、以下においてはまず優先度算出部１０４についての説明を行う。
　図９、図１０、図１１を用いて、校正優先度算出部１０４における処理の内容について説明する。校正優先度算出部１０４は、タスクの状況および目標に基づいて、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度を算出する。
　タスクの状況および目標は、タスク状況目標取得部３１０が与えるタスク状況目標表ＴＢ２で管理される。図９は、タスク状況目標表ＴＢ２の一例を示す図である。タスク状況目標表ＴＢ２の各行はロボット２００が処理するタスクの状況および目標であり、ロボット２００が処理する順番に並んでいる。たとえば、タスクの目標は作業対象の管理番号Ｄ３１、終了目標時間Ｄ３２であり、タスクの状況は達成度Ｄ３３、終了予定時間Ｄ３４、タスク失敗回数Ｄ３５である。
　図９のタスク状況目標表ＴＢ２の事例によれば、ＩＤ＝１のタスクについて、１０：００までに終了すべきところ、９：５５に終了予定であり、現在時点において５０％ほどの処理が完了しており、かつ過去に１度の失敗をしていることがわかる。なお、ＩＤ＝１以外のタスクは全て今後実施予定のものである。
　タスク状況目標表ＴＢ２は、タスク状況・目標取得部３１０により更新される。たとえば、ピッキングロボット２００においては、タスクの目標である作業対象の管理番号Ｄ３１、終了目標時間Ｄ３２は、オペレータや生産管理システムにより入力される。
　タスクの状況である達成度Ｄ３３や失敗回数Ｄ３５は、ピッキングロボット２００によるピッキング動作の結果に基づいて更新する。終了予定時間Ｄ３４は、たとえば、過去の動作時間と達成度から計算する。達成度Ｄ３３はワークの総数および残数などから計算する。また、終了予定時間Ｄ３４は、タスク動作計画部１０１が出力したタスク動作計画Ｌ１から、動作に必要な時間を算出してもよい。
　校正優先度算出部１０４は、たとえば、出力動作種類と校正使用可能時間を、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度として算出する。ここで出力動作種類は、統合動作計画部１０３が出力する動作計画の種類であり、校正動作計画Ｌ１もしくは統合動作計画Ｌである。なお統合動作計画Ｌは、タスク動作計画Ｌ１と校正動作計画Ｌ２を実行するための計画である。校正使用可能時間は、校正動作にかけることが可能な時間である。
　図１０は校正優先度算出部１０４において実行される処理を示すフローチャートである。校正優先度算出部１０４の最初の処理ステップＳ６００では、ロボット２００に現在設定されているタスクに対する余裕時間である作業余裕時間を算出する。具体的には、現在のタスクに対応するタスク状況目標表ＴＢ２の終了目標時間Ｄ３２から終了予定時間Ｄ３５を引いた結果を作業余裕時間とする。図９のＩＤ＝１の場合、タスク状況目標表ＴＢ２の終了目標時間Ｄ３２（１０：００）から終了予定時間Ｄ３５（９：５５）を引いた結果である５分を作業余裕時間とする。ただし、終了目標時間Ｄ３２から終了予定時間Ｄ３５を引いた結果が０より小さい場合には、作業余裕時間は０とする。
　処理ステップＳ６１０では、ロボット２００に現在設定されているタスクに対する失敗回数Ｄ３５が、事前に設定された閾値以上の場合は処理ステップＳ６２０に、そうでない場合は処理ステップＳ６３０に進む。
　処理ステップＳ６２０では、出力動作種類を校正動作計画Ｌ１に設定し、校正使用可能時間を作業余裕時間に設定する。これにより、校正作業を実施せずにタスクを継続することはできないと判断し、作業余裕時間を用いて校正動作を実施するように設定する。先の例では、５分間で校正動作を実施するように設定する。
　処理ステップＳ６３０では、作業余裕時間が０より大きい場合は処理ステップＳ６４０に、そうでない場合は処理ステップＳ６５０に進む。
　処理ステップＳ６４０では、出力動作種類を統合動作計画Ｌに設定し、校正使用可能時間を作業余裕時間に設定する。これにより、タスクを継続しながら、作業余裕時間を用いて統合動作を実施するように設定する。先の例では、１０：００までにタスクが完了するように、校正動作とタスク動作による統合動作を実施するように設定する。
　処理ステップＳ６５０では、校正作業を実施することなくタスクを継続した場合に、タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出する。たとえば、現在ロボット２００に設定されているタスクに対する達成度、失敗回数から、今後タスクに失敗する回数を算出する。最後に、過去の実績から算出した１動作にかかる時間の平均、もしくは、タスクに失敗した際の動作時間の平均、と失敗回数から、推定失敗時間を算出する。
　処理ステップＳ６６０では、出力動作種類を統合動作計画Ｌに設定し、校正使用可能時間を推定失敗時間に設定する。これにより、作業余裕時間が０の場合においても、校正作業を実施することなくタスクを継続した場合にタスク失敗によって失う時間よりも短い時間で実行可能な統合動作を実施するように設定する。
　図１１は、統合動作計画部１０３において実行される処理を示すフローチャートである。統合動作計画部１０３の最初の処理ステップＳ７００では、優先度算出部１０４によって算出された出力動作計画が統合動作計画Ｌの場合には処理ステップＳ７１０に進み、それ以外の場合には処理ステップＳ７３０に進む。
　処理ステップＳ７１０では、校正動作計画表ＴＢ１に含まれる校正動作計画毎に、統合動作計画Ｌと、統合動作計画Ｌによってタスク動作計画Ｌ１よりも余分に必要となる時間である校正動作時間を算出する。処理の詳細は後述する。
　処理ステップＳ７２０では、校正動作計画表ＴＢ１と、校正動作計画表ＴＢ１に含まれる各校正動作に対応する統合動作および校正動作時間から、統合動作計画部１０３が出力する統合動作を選択する。
　たとえば、まず、校正使用可能時間よりも短い校正動作時間の統合動作を選択候補とする。次に校正動作計画表ＴＢ１とタスク状況目標表ＴＢ２を参照し、選択候補のうち、最も先のタスクで対象となる作業対象Ｗに対してタスクを成功可能な統合動作を出力する。なお、最も先のタスクまで成功可能な選択候補が複数あった場合には、校正動作時間が最も短い統合動作を出力する。また、選択候補が１つも存在しない場合には、統合動作ではなく、タスク動作計画部１０１が出力したタスク動作を出力する。
　処理ステップＳ７３０は校正動作計画表ＴＢ１に含まれる校正動作計画毎に、校正動作に必要となる校正動作時間を算出する。処理の詳細は後述する。
　処理ステップＳ７４０は、校正動作計画表ＴＢ１と、校正動作計画表ＴＢ１に含まれる各校正動作に対応する校正動作時間から、統合動作計画部１０３が出力する校正動作を選択する。
　たとえば、まず、校正使用可能時間よりも短い校正動作時間の校正動作を選択候補とする。次に校正動作計画表ＴＢ１とタスク状況目標表ＴＢ２を参照し、選択候補のうち、最も先のタスクで対象となる作業対象に対してタスクを成功可能な校正動作を出力する。なお、最も先のタスクまで成功可能な選択候補が複数あった場合には、校正動作時間が最も短い校正動作を出力する。また、選択候補が１つも存在しない場合には、現在のタスクを成功可能な校正動作のうち、校正動作時間が最も短い校正動作を出力する。
　図１２は処理ステップＳ７１０において実行される処理を示すフローチャートである。
　処理ステップＳ７１１では、Ｓ７１１ａとＳ７１１ｂの間で校正動作計画表ＴＢ１の各行ｋに対するループ処理を実行する。
　処理ステップＳ７１２では、Ｓ７１２ａとＳ７１２ｂの間で校正動作計画表ＴＢ１のｋ行に含まれる各位置姿勢ｐｋのタスク動作計画への割当パターンに対するループ処理を開始する。
　校正動作計画表ＴＢ１のｋ行には複数の位置姿勢ｐｋが含まれている。また、タスク動作計画部１０１は複数のタスク動作計画を出力している。すなわち、複数の位置姿勢ｐｋを、複数のタスク動作計画と統合する際には、位置姿勢ｐｋとタスク動作計画の組み合わせが複数パターン存在する。また、ひとつのタスク動作計画に対して、複数の位置姿勢ｐｋを統合する場合には、位置姿勢ｐｋを統合する順番にも複数のパターンが存在する。処理ステップＳ７１２は、これら全てのパターンに対するループ処理を開始する。
　処理ステップＳ７１３では、校正動作計画表ＴＢ１に含まれる位置姿勢ｐｋとタスク動作計画を統合した統合動作を計画する。具体的には、処理対象のパターンにおいて、校正動作計画表ＴＢ１に含まれる位置姿勢ｐｋが割り当てられたタスク動作計画毎に、公知の動作計画手法により、ロボット２００の基準位置姿勢からスタートし、校正動作計画表ＴＢ１に含まれる位置姿勢ｐｋを経由し、タスク動作計画部１０１によって動作計画として出力されたタスクを実行可能な位置姿勢に到達する動作を計画する。
　たとえば動作計画手法としては、Ｊ．Ｊ．Ｋｕｆｆｎｅｒ,ａｎｄ　Ｓ．Ｍ．ＬａＶａｌｌｅ,ＲＲＴ－ｃｏｎｎｅｃｔ：Ａｎ　ｅｆｆｉｃｉｅｎｔ　ａｐｐｒｏａｃｈ　ｔｏ　ｓｉｎｇｌｅ－ｑｕｅｒｙ　ｐａｔｈ　ｐｌａｎｎｉｎｇ,Ｉｎｔ．Ｃｏｎｆ．　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ、　ｐｐ．９９５－１００１（２０００）を用いることができる。
　処理ステップＳ７１４では、校正動作にかかる時間を算出する。具体的には、校正動作計画表ＴＢ１に含まれる位置姿勢ｐｋが割り当てられたタスク動作計画毎に、処理ステップＳ７１３で計画された統合動作をロボット２００が実施した際に必要な時間を、ロボット２００の仕様に基づいて算出する。
　また、校正動作を経由しない場合のロボット２００の動作時間、すなわち、ロボット２００の基準位置をスタートし、タスクを実行可能な場所に到達する動作を計画し、動作にかかる時間を算出する。
　次に、統合動作をロボット２００が実施した際に必要な時間と、校正動作を経由しない場合のロボット２００の動作時間の差を、校正動作にかかる時間とする。最後に、校正動作計画表ＴＢ１に含まれる位置姿勢ｐｋが割り当てられたタスク動作計画毎に計算した校正動作にかかる時間を合計することで、処理対象のパターンに対する校正動作時間とする。
　処理ステップＳ７１５は、処理ステップＳ７１２で開始された、校正動作計画表ＴＢ１のｋ行に含まれる各位置姿勢ｐｋのタスク動作計画への割当パターンに対するループ処理で算出された全ての校正動作時間から、最も短い校正動作時間と、対応する統合動作を選択する。
　図１１に戻り、処理ステップＳ７３０の処理を説明する。処理ステップＳ７３０は処理ステップＳ７１０と同様の処理により、校正動作計画と校正動作時間を算出する。ただし、処理ステップＳ７１０とは異なり、校正動作計画とタスク動作計画の統合は実施せず、校正動作のみからなる校正動作計画と校正動作時間を算出する。
　具体的には、校正動作計画表ＴＢ１の行毎に、行に含まれる複数の位置姿勢の順番の全てのパターンに対して、ロボット２００の基準位置から位置姿勢に達する動作計画を、公知の動作計画手法により計画し、校正動作計画とする。また、ロボット２００の仕様から校正動作計画を実施するために必要な時間を校正動作時間とする。最後に、校正動作計画表ＴＢ１の行毎に、最も短い校正動作時間と対応する校正動作を選択する。
　上述した本発明の実施例１によれば、次の作用効果が得られる。
　（１）カメラ位置姿勢校正装置１００は、タスク動作計画部１０１と、校正動作計画部１０２と、統合動作計画部１０３と、を備える。タスク動作計画部１０１は、画像とロボット２００の位置姿勢のデータから、ロボット２００に設定されたタスクを実行するための動作を計画する。校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作を計画する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画と校正動作計画部１０２によって計画された校正動作計画を統合することで動作を計画する（図１）。そのため、タスク動作計画と校正動作計画が統合され、ロボット２００に設定されているタスクを中断することなく、カメラ２０１とロボット２００の相対位置姿勢を校正することができる。また、カメラ２０１とロボット２００の相対位置姿勢の校正に必要となる時間を短くすることができる。
　（２）校正動作計画部１０２は、取得済みの画像とロボット２００の位置姿勢を含む取得済み動作データベースＤＢ１と、校正に用いたロボット２００の位置姿勢および校正結果によるタスクの成否を含む校正動作データベースＤＢ２に基づいて校正に必要な動作を計画し、校正動作計画表ＴＢ１として出力する（図４～図８）。そのため、取得済み動作が考慮されることにより、カメラ２０１とロボット２００の相対位置姿勢を校正に必要となる時間を短くすることができる。
　（３）統合動作計画部１０３は、校正優先度算出部１０４を備える。校正優先度算出部１０４は、タスクの状況および目標を含むタスク動作目標表ＴＢ２に基づいて、タスク動作計画に対する校正動作計画の優先度を算出する。統合動作計画部１０３は、校正優先度算出部１０４によって算出された校正動作計画の優先度に基づいて、タスク動作計画部１０１によって計画されたタスク動作計画と校正動作計画部１０２によって計画された校正動作計画を統合することで動作を計画する（図９～図１２）。そのため、タスクの状況および目標を考慮しながら、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。
　（４）校正優先度算出部１０４は、タスクの終了目標時間と終了予定時間から、タスクに対する余裕時間である作業余裕時間を算出し、作業余裕時間に基づいて、校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、作業余裕時間を考慮しながら、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。
　（５）校正優先度算出部１０４は、ロボット２００に設定されているタスクに対する失敗回数に基づいて、統合動作計画部１０３が出力する動作計画の種類である出力動作種類と校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、タスクに対する失敗回数を考慮し、失敗が多い場合には校正動作を実施し、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。
　（６）校正優先度算出部１０４は、ロボット２００に設定されているタスクに対する達成度、失敗回数から校正作業を実施することなくタスクを継続した場合に、タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、推定失敗時間に基づいて、校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、推定失敗時間を考慮し、推定失敗時間よりも短い時間でカメラ位置姿勢を校正できる場合には、カメラ位置姿勢を校正することで、校正を実施しない場合よりも短い時間でタスクを完了することができる。
　（７）統合動作計画部１０３は、タスク動作計画部１０１によって計画された各タスク動作計画と、校正動作計画部１０２によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することでタスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択する（図１１、図１２）。そのため、カメラ２０１とロボット２００の相対位置姿勢の校正に必要となる時間を短くすることができる。
　（８）統合動作計画部１０３は、校正動作データベースＴＢ１に含まれる校正結果によるタスクの成否と、タスク状況目標表ＴＢ２に含まれるロボット２００にタスクとして与えられる作業対象の予定とに基づいて、統合動作を選択する（図９、図１１）。そのため、作業対象の予定が考慮されることにより、長期的にみて短い時間でタスクを完了することができる。
　なお、本発明の実施例１を実現するに当たり、さらに以下の変形実施例とすることができる。
　まず、校正動作データベースＤＢ２はカメラ位置姿勢校正装置１００を最初に使用する際に、様々なロボット２００の位置姿勢で画像を取得し、取得した画像とロボット２００の位置姿勢を用いた校正結果によるタスクを実施することで作成した。しかし、校正動作データベースＤＢ２の作成方法はこれに限定されない。
　また統合動作計画部は、タスク動作計画部によって計画された各タスク動作計画と、校正動作計画部によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することでタスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択するものであってもよい。
　また統合動作計画部は、校正動作データベースと、複数の予定されているタスクの目標とを用いて統合動作を選択するものであってもよい。
　さらに校正動作データベースをシミュレーションにより作成するものであってもよい。
　校正動作データベースＤＢ２は、ロボット２００を動作させることなく、シミュレーションにより作成しても良い。たとえば、ロボット２００の構成、配置や、作業対象の形状などからシミュレーション環境を作成し、仮想的に画像とロボット２００の位置姿勢のペアを複数取得し、実データを用いる場合と同様の方法で校正動作データベースＤＢ２を作成しても良い。また、シミュレーションと実環境を一致させるために、実際の作業環境の画像を取得し、画像を用いた三次元復元手法や物体認識手法によって、実環境を模擬したシミュレーション環境を作成し、校正動作データベースＤＢ２の作成に用いても良い。
　上述した変形例１によれば、次の作用効果が得られる。すなわち、カメラ位置姿勢校正装置１００は、シミュレーションにより、校正動作データベースＤＢ２を作成する。そのため、校正動作データベースＤＢ２の作成にかかる時間が短くなることにより、カメラ位置姿勢校正装置を早期に使用開始することができる。
　以下、図１３～図１５を参照して、本発明の実施例２に係るカメラ位置姿勢校正装置について説明する。なお以下の説明では、実施例１と同じ構成要素には同じ符号を付して相違点を主に説明する。特に説明しない点については、実施例１と同じである。本実施例では、ロボット２００が自動運転車の場合を対象とする。
　実施例２におけるカメラ位置姿勢校正装置の構成は、基本的に図１と同じであり、優先度算出部１０４の処理が異なる点でのみ相違する。
　実施例２では、ロボット２００は自動運転車である。図１３は自動運転車２３０を駐車スペースに駐車する状況を示す図である。自動運転車２３０は、駐車場内において駐車スペースである目標位置姿勢２４１へ自動的に走行する。自動運転車２３０はカメラ２３１を搭載しており、カメラ２３１により周辺環境を認識し、自動運転車２３０を制御することで、自動運転車２３０を目標位置姿勢２４１へ移動させる。
　図１３に示す例では、自動運転車２３０は、他の車両２４２が駐車していない駐車スペースを認識することで目標位置姿勢２４１とし、自動で走行することで自動駐車を実現する。なお、自動運転車２３０の構成は任意である。たとえば、自動運転車２３０は複数のカメラ２３１を搭載しても良いし、ＬｉＤＡＲやレーダーなど他のセンサを搭載しても良い。また、自動運転車２３０は、通信装置により監視センタと通信しても良い。
　自動運転車２３０において、図２のロボットにおける位置姿勢センサ２０２は、たとえば、タイヤとステアリングの回転量を計測するセンサである。自動運転車２３０の位置姿勢としては、車両の制御に重要となる後輪車軸中心の位置姿勢を用いる。タイヤとステアリングの回転量を計測することで、自動運転車２３０の幾何モデルに基づき、公知の手法であるホイールオドメトリにより自動運転車２３０の位置姿勢を得ることができる。
　自動運転車２３０において、設定されたタスクは目標位置姿勢２４１まで走行することである。タスクの状況は、たとえば、目標位置姿勢２４１の種類や形状、目標位置姿勢２４１までの距離などである。タスクの目標は、たとえば、目標位置姿勢２４１に到達する時間などである。
　自動運転車２３０において、図１のタスク動作計画部１０１は、カメラ２３１によって撮影された周辺環境の画像と、自動運転車２３０の位置姿勢のデータから、自動運転車２３０を目標位置姿勢２４１まで移動させるための動作を計画する。たとえば、タスク動作計画部１０１は、公知のパターンマッチング手法や機械学習手法により、駐車スペースの位置姿勢を推定することで、駐車可能な位置姿勢を算出し、目標位置姿勢２４１とする。
　複数の駐車スペースが認識された場合には、それぞれの駐車スペースに対応した、駐車可能な位置姿勢を目標位置姿勢２４１とする。なお、目標位置姿勢２４１は駐車スペースに限らず、ユーザが求める任意の位置姿勢でも良い。また、カメラ２３１に加えて他のセンサや地図情報を用いて目標位置姿勢２４１を算出しても良い。
　次にロボットが自動運転車２３０である場合の校正優先度算出部１０４の動作、処理内容について、図１４、図１５を用いて説明する。
　図１４は、自動運転車２３０におけるタスク状況目標表ＴＢ２の一例を示す図である。図９に示した実施例１のタスク状況目標表ＴＢ２の項目に搭乗者の人数Ｄ３０が追加されている。
　自動運転車２３０におけるタスク状況目標表ＴＢ２において、作業対象Ｄ３１は目標位置姿勢２４１である。また、タスクの失敗Ｄ３５は、目標位置姿勢２４１への走行の失敗にあたり、たとえば、ドライバが周辺環境の物体への接触の危険性を察知し、ブレーキを踏んだ場合には失敗と判断する。また、超音波センサなどにより、周辺環境の物体への接触の危険性を検知した場合にも失敗と判断する。
　図１４のタスク状況目標表ＴＢ２は、タスク状況・目標取得部３１０により更新される。たとえば、自動運転車２３０においては、タスクの目標である作業対象の管理番号、終了目標時間Ｄ３２は、ユーザにより入力される、もしくは、自動運転システムによって設定される。タスクの状況である達成度Ｄ３３や失敗回数Ｄ３５は、自動運転車２３０による走行結果に基づいて更新する。終了予定時間Ｄ３４は、たとえば、過去の動作時間と達成度から計算する。達成度Ｄ３３は目標位置姿勢２４１までの距離および走行距離から計算する。また、終了予定時間Ｄ３４は、タスク動作計画部１０１が出力した動作計画から、動作に必要な時間を算出してもよい。
　図１５は、校正優先度算出部１０４において実行される処理を示すフローチャートである。実施例２においては、図１に示す実施例１の校正優先度算出部１０４において実行される図１０のフローチャートに、処理ステップＳ９７０および処理ステップＳ９８０が追加されている。
　処理ステップＳ９７０では、搭乗者が０の場合には処理ステップＳ９８０に進み、それ以外の場合は処理ステップＳ６５０に進む。
　処理ステップＳ９８０では、出力動作種類を統合動作計画Ｌ、校正使用可能時間を事前に設定した値に設定する。搭乗者が０の場合には、校正使用可能時間を事前に設定した値に設定することで、終了目標時間にタスクを終えることよりも、校正作業を優先する。
　実施例２によれば、次の作用効果が得られる。すなわち、校正優先度算出部１０４は、自動運転車２３０の搭乗者の数に基づいて校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する。そのため、搭乗者の数が０のときに校正を実施することにより、搭乗者がいる場合における校正動作時間を短くすることで、校正を実施することによる搭乗者への負担を小さくすることができる。
　なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。
１００：カメラ位置姿勢校正装置、１０１：タスク動作計画部、１０２：校正動作計画部、１０３：統合動作計画部、１０４：校正優先度算出部、２００：ロボット（ピッキングロボット）、２０１：カメラ、２０２：位置姿勢センサ、２２０：作業台、２３０：自動運転車、３００：カメラ位置姿勢推定部、３１０：タスク状況・目標取得部、ＤＢ１：取得済み動作データベース、ＤＢ２：校正動作データベース、ＴＢ１：校正動作計画表、ＴＢ２：タスク状況目標表

Claims (15)

  1. 　ロボットに搭載されたカメラにより撮影された画像と前記ロボットの位置姿勢から、前記ロボットに設定されたタスクを実行するための動作を計画するタスク動作計画部と、
    　前記画像と前記ロボットの位置姿勢から、前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画する校正動作計画部と、
    　前記タスク動作計画部によって計画されたタスク動作計画と前記校正動作計画部によって計画された校正動作計画を統合することで動作を計画する統合動作計画部と、
    を備え、前記ロボットを操作することを特徴とするカメラ位置姿勢校正装置。
  2. 　請求項１に記載のカメラ位置姿勢校正装置であって、
    　前記校正動作計画部は、複数組の取得済みの前記画像と前記ロボットの位置姿勢を含む取得済み動作データと、複数組の前記ロボットの位置姿勢と前記ロボットの位置姿勢を用いた校正結果による前記タスクの成否を含む校正動作データと、
    を用いて前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画することを特徴とするカメラ位置姿勢校正装置。
  3. 　請求項１または請求項２に記載のカメラ位置姿勢校正装置であって、
    　前記タスクの状況および目標を与えるタスク状況・目標取得部と、前記タスク状況・目標取得部からの前記タスクの状況および目標に基づいて、前記タスク動作計画に対する前記校正動作計画の優先度を算出する校正優先度算出部を備え、
    　前記統合動作計画部は、前記校正優先度算出部によって算出された前記優先度に基づいて、前記タスク動作計画と前記校正動作計画を統合することで動作を計画することを特徴とするカメラ位置姿勢校正装置。
  4. 　請求項３に記載のカメラ位置姿勢校正装置であって、
    　前記校正優先度算出部は、前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間を用いて、校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
  5. 　請求項３または請求項４に記載のカメラ位置姿勢校正装置であって、
    　前記校正優先度算出部は、前記タスクに対する失敗回数を用いて、前記統合動作計画部が出力する動作計画の種類である出力動作種類と校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
  6. 　請求項３ないし請求項５のいずれか一項に記載のカメラ位置姿勢校正装置であって、
    　前記校正優先度算出部は、前記タスクに対する達成度および失敗回数から校正作業を実施することなく前記タスクを継続した場合に、前記タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、前記推定失敗時間を用いて校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
  7. 　請求項３ないし請求項６のいずれか一項に記載のカメラ位置姿勢校正装置であって、
    　前記校正優先度算出部は、前記ロボットの搭乗者の数に基づいて校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
  8. 　請求項１ないし請求項７のいずれか一項に記載のカメラ位置姿勢校正装置であって、
    　前記統合動作計画部は、前記タスク動作計画部によって計画された各タスク動作計画と、前記校正動作計画部によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することで前記タスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択することを特徴とするカメラ位置姿勢校正装置。
  9. 　請求項１ないし請求項８のいずれか一項に記載のカメラ位置姿勢校正装置であって、
    　前記統合動作計画部は、前記校正動作計画と、複数の予定されている前記タスクの目標とを用いて統合動作を選択することを特徴とするカメラ位置姿勢校正装置。
  10. 　請求項１ないし請求項９のいずれか一項に記載のカメラ位置姿勢校正装置であって、
    　前記校正動作計画をシミュレーションにより作成することを特徴とするカメラ位置姿勢校正装置。
  11. 　請求項１ないし請求項９のいずれか一項に記載のカメラ位置姿勢校正装置により操作されるロボット。
  12. 　ロボットに搭載されたカメラにより撮影された画像と前記ロボットの位置姿勢から、前記ロボットに設定されたタスクを実行するための動作を計画したタスク動作計画と、前記画像と前記ロボットの位置姿勢から、前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画した校正動作計画と、前記タスク動作計画と前記校正動作計画を統合した統合動作計画と、前記タスクの状況および目標を得、
    　前記タスクの状況および目標に基づいて、前記タスク動作計画に対する前記校正動作計画の優先度を算出し、前記優先度に基づいて、動作計画を選択し、選択した動作計画により前記ロボットを操作することを特徴とするカメラ位置姿勢校正方法。
  13. 　請求項１２に記載のカメラ位置姿勢校正方法であって、
    　前記タスクに失敗した場合に、前記校正動作計画を優先し、前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間を用いて、校正動作を実施することを特徴とするカメラ位置姿勢校正方法。
  14. 　請求項１２に記載のカメラ位置姿勢校正方法であって、
    　前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間があるとき、前記統合動作計画を優先し、タスクを継続実施することを特徴とするカメラ位置姿勢校正方法。
  15. 　請求項１２に記載のカメラ位置姿勢校正方法であって、
    　前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間がないとき、前記タスクを継続した場合に、前記タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、推定失敗時間よりも短い時間で実行可能な前記統合動作計画を実施することを特徴とするカメラ位置姿勢校正方法。
PCT/JP2018/021497 2018-06-05 2018-06-05 カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット WO2019234814A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
JP2020523873A JP7064586B2 (ja) 2018-06-05 2018-06-05 カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット
US17/055,821 US11865730B2 (en) 2018-06-05 2018-06-05 Camera position/attitude calibration device, camera position/attitude calibration method, and robot
PCT/JP2018/021497 WO2019234814A1 (ja) 2018-06-05 2018-06-05 カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2018/021497 WO2019234814A1 (ja) 2018-06-05 2018-06-05 カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット

Publications (1)

Publication Number Publication Date
WO2019234814A1 true WO2019234814A1 (ja) 2019-12-12

Family

ID=68770913

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2018/021497 WO2019234814A1 (ja) 2018-06-05 2018-06-05 カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット

Country Status (3)

Country Link
US (1) US11865730B2 (ja)
JP (1) JP7064586B2 (ja)
WO (1) WO2019234814A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021216864A1 (en) * 2020-04-22 2021-10-28 Gm Cruise Holdings Llc Eliminating sensor self-hit data

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7337495B2 (ja) * 2018-11-26 2023-09-04 キヤノン株式会社 画像処理装置およびその制御方法、プログラム
JP2020196059A (ja) * 2019-05-31 2020-12-10 セイコーエプソン株式会社 ロボット

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06134691A (ja) * 1992-10-23 1994-05-17 Hitachi Ltd 位置検出方法およびこれを用いたフレキシブル生産システム
JPH06270020A (ja) * 1993-03-24 1994-09-27 Nissan Motor Co Ltd 位置測定装置
JP2012240174A (ja) * 2011-05-23 2012-12-10 Mitsubishi Electric Corp キャリブレーション装置及びキャリブレーション方法
JP2015150636A (ja) * 2014-02-13 2015-08-24 ファナック株式会社 ビジュアルフィードバックを利用したロボットシステム
US20170136626A1 (en) * 2015-11-16 2017-05-18 Abb Technology Ag Facilitating robot positioning
US20170312918A1 (en) * 2016-04-27 2017-11-02 Quanta Storage Inc. Programming Method of a Robot Arm
WO2018043525A1 (ja) * 2016-09-02 2018-03-08 倉敷紡績株式会社 ロボットシステム、ロボットシステム制御装置、およびロボットシステム制御方法
JP6301045B1 (ja) * 2017-03-09 2018-03-28 三菱電機株式会社 ロボット制御装置およびキャリブレーション方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4831549A (en) 1987-07-28 1989-05-16 Brigham Young University Device and method for correction of robot inaccuracy
US5297238A (en) * 1991-08-30 1994-03-22 Cimetrix Incorporated Robot end-effector terminal control frame (TCF) calibration method and device
JP2007061979A (ja) 2005-09-01 2007-03-15 Sharp Corp ロボットアームの視覚センサ補正方法およびコンピュータプログラム
US9102055B1 (en) * 2013-03-15 2015-08-11 Industrial Perception, Inc. Detection and reconstruction of an environment to facilitate robotic interaction with the environment
JP6415190B2 (ja) * 2014-09-03 2018-10-31 キヤノン株式会社 ロボット装置、ロボット制御プログラム、記録媒体、およびロボット装置の制御方法
EP3458919B1 (en) * 2016-05-19 2022-08-24 Deep Learning Robotics Ltd. Robot assisted object learning vision system
JP6396516B2 (ja) * 2017-01-12 2018-09-26 ファナック株式会社 視覚センサのキャリブレーション装置、方法及びプログラム
US20200016757A1 (en) 2017-03-09 2020-01-16 Mitsubishi Electric Corporation Robot control apparatus and calibration method
CA3030734C (en) * 2017-06-16 2023-01-10 Robotiq Inc. Robotic arm camera system and method

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06134691A (ja) * 1992-10-23 1994-05-17 Hitachi Ltd 位置検出方法およびこれを用いたフレキシブル生産システム
JPH06270020A (ja) * 1993-03-24 1994-09-27 Nissan Motor Co Ltd 位置測定装置
JP2012240174A (ja) * 2011-05-23 2012-12-10 Mitsubishi Electric Corp キャリブレーション装置及びキャリブレーション方法
JP2015150636A (ja) * 2014-02-13 2015-08-24 ファナック株式会社 ビジュアルフィードバックを利用したロボットシステム
US20170136626A1 (en) * 2015-11-16 2017-05-18 Abb Technology Ag Facilitating robot positioning
US20170312918A1 (en) * 2016-04-27 2017-11-02 Quanta Storage Inc. Programming Method of a Robot Arm
WO2018043525A1 (ja) * 2016-09-02 2018-03-08 倉敷紡績株式会社 ロボットシステム、ロボットシステム制御装置、およびロボットシステム制御方法
JP6301045B1 (ja) * 2017-03-09 2018-03-28 三菱電機株式会社 ロボット制御装置およびキャリブレーション方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021216864A1 (en) * 2020-04-22 2021-10-28 Gm Cruise Holdings Llc Eliminating sensor self-hit data
US11726188B2 (en) 2020-04-22 2023-08-15 Gm Cruise Holdings Llc Eliminating sensor self-hit data

Also Published As

Publication number Publication date
JPWO2019234814A1 (ja) 2021-02-12
JP7064586B2 (ja) 2022-05-10
US11865730B2 (en) 2024-01-09
US20210229290A1 (en) 2021-07-29

Similar Documents

Publication Publication Date Title
WO2019234814A1 (ja) カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット
CN109807882B (zh) 把持系统、学习装置和把持方法
JP6586532B2 (ja) ロボットの把持のための深層機械学習方法および装置
CN110039542B (zh) 具有速度方向控制的视觉伺服跟踪控制方法及机器人系统
JP2022544005A (ja) リモートクライアントデバイスからの入力に基づく効率的なロボットの制御
JP7386652B2 (ja) ロボット制御ための方法及び装置
US11230004B2 (en) Robot system and robot control method
JP7221183B2 (ja) 機械学習方法、フォークリフト制御方法、及び機械学習装置
JP2017094438A (ja) 学習による教示作業の簡易化及び動作性能向上機能を備えたロボットシステム
CN104924309A (zh) 机器人系统、机器人系统的校准方法及位置修正方法
CN113412178A (zh) 机器人控制装置、机器人系统以及机器人控制方法
EP4400274A1 (en) Command value generating device, method, and program
CN113305845B (zh) 一种多机械臂的协作方法
JP7473005B2 (ja) 情報処理システム、情報処理装置、情報処理方法、及び、プログラム
CN115958589A (zh) 用于机器人的手眼标定的方法和装置
US20230286143A1 (en) Robot control in working space
CN111263685B (zh) 机器人方法和系统
WO2021199305A1 (ja) 制御装置、制御システム、制御方法、および、制御プログラムが記録された記録媒体
CN115249333A (zh) 抓取网络训练方法、系统、电子设备及存储介质
CN115136093A (zh) 信息处理装置、信息处理方法、计算机程序和移动机器人
US20200156242A1 (en) Robot apparatus, robot system, robot control method, and storage medium
CN116300972B (zh) 一种机器人作业规划方法、系统及其应用
JP2604073B2 (ja) ロボットによる組み立て装置
US20220388167A1 (en) Method, computer program product and robot controller for configuring a robot-object system environment, and robot
CN115979270B (zh) 基于车库场景内导航点与车位号自动绑定的导航控制系统

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18921382

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020523873

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18921382

Country of ref document: EP

Kind code of ref document: A1