JP7249221B2 - SENSOR POSITION AND POSTURE CALIBRATION DEVICE AND SENSOR POSITION AND POSTURE CALIBRATION METHOD - Google Patents

Description

The present invention relates to a sensor position/orientation calibration device and a sensor position/orientation calibration method.

In recent years, the declining working population has increased the need for substitution and automation of human work. Devices that meet these needs include mobiles, such as robots, combined with sensors (eg, cameras) attached to the mobiles to tackle various tasks. Taking a robot as an example, there is an industrial robot arm that automates picking work in distribution warehouses.

An industrial robot arm recognizes the position of an object to be picked by a sensor attached to the robot, and moves the robot arm to the recognized position to perform a picking operation. At this time, the position of the object to be picked has two representations, the position recognized by the camera on the camera coordinate system and the position on the robot arm coordinate system to which the robot arm moves.

In order for the robot arm to correctly perform the picking operation, transformation parameters representing the relative position and orientation of the camera coordinate system and the robot arm coordinate system (hereafter referred to as relative position and orientation parameters) are used to convert the position of the expression in both coordinate systems. need to be converted to Techniques for estimating the relative position of a moving object and a sensor are used in various fields. For example, picking robots used in the industrial field and automobiles equipped with sensors for the purpose of automatic driving and driver support, which were mentioned earlier, can be mentioned.

As a general method of estimating the relative position, for example, there is a method of calculating from setting data of the robot and the sensor. Alternatively, an object whose shape and appearance information is known in advance (for example, a board printed with a checkered pattern) is photographed in various postures with a camera attached to a robot arm. There is a method of estimating from the position and posture of the robot calculated from the command value of the motor. Methods for estimating these relative positions are collectively called calibration.

With conventional calibration, if the accuracy of calculating the position and orientation of the moving object is poor due to the effects of distortion and deflection of the shape of the moving object, the calibration may fail or the accuracy of estimating the relative position and orientation parameters may decline. I may invite Since it is difficult to detect the factors that lead to such deterioration in accuracy, there is a problem that countermeasures require a lot of time.
Therefore, there is a demand for a technique for detecting and correcting the deterioration in the accuracy of the calibration results due to the deterioration in the positional accuracy of the moving body.

In Patent Document 1, a sensor that detects the position of a target that is a movement destination and a sensor that detects the position of a hand of a robot arm are used to measure the error between the hand and the target. Techniques for moving the fingertips to the torso are described.

JP 2016-52699 A

The technique described in Patent Literature 1 requires the introduction of a new sensor for detecting the positions of the robot arm and the target, which complicates the configuration. In addition, in the technique described in Patent Document 1, the position of the robot is moved by feedback control while measuring the positional error between the hand and the target. There is a problem that the accuracy is not uniform and cannot be dealt with when it changes dynamically.
As described above, in conventional calibration, there are many cases where the accuracy of calibration results is not appropriate, but it has been difficult to detect a decrease in accuracy.

SUMMARY OF THE INVENTION It is an object of the present invention to provide a sensor position/orientation calibration device and a sensor position/orientation calibration method capable of detecting a decrease in accuracy in determining the position and orientation of a moving object.

In order to solve the above problems, for example, the configurations described in the claims are adopted.
The present application includes a plurality of means for solving the above problems. To give one example, a sensor position/orientation calibration device that converts a coordinate system set at an arbitrary point of a sensor into a coordinate system of a moving body. a relative position/orientation parameter acquisition unit that acquires the relative position/orientation of the moving object and the sensor; a shape information acquisition unit that acquires shape information of the moving object; a virtual sensor data generation unit that generates virtual sensor data that virtually reproduces the data acquired by the sensor; a sensing unit that outputs the sensor data acquired by the sensor; and a comparison between the virtual sensor data and the sensor data. A matching unit that determines whether or not there is a difference, and a matching result evaluation unit that evaluates the output of the matching unit and calculates the amount of error between the virtual sensor data and the sensor data.
Further, the virtual sensor data generation unit generates moving body posture shape information representing the posture shape of a virtual moving body based on the shape information and the posture information representing the posture of the moving body. a sensor viewpoint generation unit that receives moving body posture shape information and relative position/posture parameters as input and generates sensor viewpoint shape information converted into a sensor viewpoint; and a sensor viewpoint shape information and sensor-specific parameters as inputs: and a generator that virtually generates data acquired by the sensor.
Then, the matching result evaluation unit changes at least one or more parameters among the respective values of the posture information to create a plurality of pieces of posture information and input them, thereby determining the correspondence between the plurality of pieces of posture information and the amount of error. It was recorded, and analysis processing was performed for the correspondence between the posture information and the amount of error.

According to the present invention, it becomes possible to detect a decrease in the determination accuracy of the position and attitude of a moving object.
Problems, configurations, and effects other than those described above will be clarified by the following description of the embodiments.

FIG. 2 is a diagram for explaining the configuration of a robot arm and relative position/orientation parameters in the robot arm according to the first embodiment of the present invention; 1 is a block diagram showing a configuration example of a sensor position/orientation calibration device according to a first embodiment of the present invention; FIG. 1 is a block diagram showing an example of hardware of a sensor position/orientation calibration device according to a first embodiment of the present invention; FIG. It is a figure which shows the example of the shape information by the example of the 1st Embodiment of this invention. It is a block diagram of the virtual sensor data production|generation part by the 1st Embodiment of this invention. 9 is a flow chart showing an example of processing by a moving body shape simulating unit according to the first embodiment of the present invention; FIG. 5 is a diagram for explaining an example of a procedure for generating moving body posture shape information according to the first embodiment of the present invention; FIG. 4 is a flow chart showing an example of processing by a sensor viewpoint generation unit according to the first embodiment of the present invention; FIG. FIG. 4 is a diagram showing an example of sensor viewpoint shape information according to the first embodiment of the present invention; FIG. 5 is a diagram showing an example in which the generation unit according to the first embodiment of the present invention generates virtual sensor data as an image; FIG. 5 is a diagram showing an example in which the generation unit according to the first embodiment of the present invention generates virtual sensor data as coordinates; FIG. 10 is a block diagram showing a configuration example of a sensor position/orientation calibration device according to a second embodiment of the present invention; FIG. 11 is a flow chart showing an example of processing by a matching result evaluation unit according to the second embodiment of the present invention; FIG. It is a figure for demonstrating the matching result evaluation part by the 2nd Embodiment of this invention. It is a figure for demonstrating the matching result evaluation part by the 2nd Embodiment of this invention. It is a figure for demonstrating the evaluation method using the matching result evaluation part by the 2nd Embodiment of this invention. It is a figure which shows the example of the data which the matching result evaluation part by the 2nd Embodiment of this invention records. It is a figure which shows the example of the data which the matching result evaluation part by the 2nd Embodiment of this invention records.

<1. First Embodiment>
A first embodiment of the present invention will be described below with reference to FIGS. 1 to 11. FIG.
[1-1. Configuration of robot arm]
First, referring to FIG. 1, the configuration of a robot arm 100, which is an application example of the sensor position/orientation calibration device 1 according to the first embodiment of the present invention, will be described. In this embodiment, an example applied to a robot arm will be described. For example, it can be applied to various things.

The robot arm 100 has a base 111 , joints 112 a , 112 b , 112 c , links 113 a , 113 b , an arm 114 and a hand 115 .
One end of a link 113a is connected to the base 111 via a joint 112a, and one end of a link 113b is connected to the other end of the link 113a via a joint 112b. One end of an arm 114 is connected to the other end of the link 113b via a joint 112c, and a hand 115 is attached to the tip of the arm 114. As shown in FIG. Each joint 112a, 112b, 112c can freely adjust the angle of the connected link 113a, 113b or the arm 114, and the hand 115 at the tip can be moved to any position within the movable range.
The hand 115 is called an end effector and has a mechanism for pinching an object. However, as the hand 115, a mechanism for pinching an object as shown in FIG. 1 is an example, and a mechanism for sucking an object by air pressure using a sucker-like part, for example, may be used. Alternatively, the robot arm 100 may be one without an end effector.

Here, a camera 22, which is a sensor for acquiring an image, is attached to the arm 114, the camera 22 captures an image of the vicinity of the hand 115, and outputs a camera image as a sensor output. In FIG. 1, a range A1 around the camera 22 on the arm 114 and the hand 115 is shown in detail as an enlarged range A2.
As shown in the enlarged range A2 in FIG. 1, the hand 115 has a hand coordinate system 115b with a point 115a as an origin (hand coordinate system origin). The hand coordinate system 115b is indicated by three axes of x-axis, y-axis, and z-axis. Operations such as gripping an object by the hand 115 of the robot arm 100 are performed based on the hand coordinate system 115b.

On the other hand, the camera 22 has a camera coordinate system (sensor coordinate system) 22b with a point 22a as an origin (camera coordinate system origin). The camera coordinate system 22b is also indicated by three axes of x-axis, y-axis and z-axis. The z-axis of the camera coordinate system 22b is aligned with the optical axis of the lens attached to the camera 22b.
In FIG. 1, the hand coordinate origin 115a is set at the vertex of the hand 115, but if the hand coordinate origin 115a is a point that moves in conjunction with the movement of the robot arm 100, the hand coordinate system 115b can be set arbitrarily. can do. Also, although the camera coordinate origin 22a is set at the optical center of the camera 22 in FIG. 1, the camera coordinate origin 22a can be set arbitrarily as long as the relative position and orientation with respect to the camera 22 are unchanged. is.

The camera coordinate system 22b can be transformed into the hand coordinate system 115b by the relative position and orientation parameter information 2a. The sensor position/posture calibration device 1 of this embodiment has this relative position/posture parameter information 2a.

[1-2. Configuration of Sensor Position and Orientation Calibration Device]
FIG. 2 shows a configuration example of the sensor position/orientation calibration device 1 according to the first embodiment of the present invention.
The sensor position/orientation calibration device 1 virtually generates data acquired by a sensor and compares it with data actually acquired by the sensor to detect a decrease in accuracy in determining the position and orientation of a moving object. It is something to do. In this embodiment, a robot arm 100 shown in FIG. 3, which will be described later, is used as a moving body, and a camera 22 attached to the robot arm 100 is used as a sensor.

The sensor position/posture calibration device 1 includes a relative position/posture parameter acquisition unit 2 , shape information acquisition unit 3 , virtual sensor data generation unit 4 , sensing unit 5 , and matching unit 6 .
The relative position/posture parameter acquisition unit 2 acquires the hand coordinate system 115b capable of expressing the position and posture of the robot arm 100 and the camera coordinate system 22b capable of representing the position and posture of the camera 22 (relative position/posture parameter acquisition process). The parameters obtained by this relative position/posture parameter acquisition process are the relative position/posture parameter information 2a shown in FIG.

The shape information acquisition unit 3 acquires shape information of a moving object measured by a 3D sensor or camera, appearance information, and shape information of a moving object created by a general 3D model generation tool such as CAD (Computer Aided Design). Acquire (shape information acquisition processing).
The virtual sensor data generator 4 receives the relative position/orientation parameter information 2a between the robot arm 100 and the camera 22 and the shape information of the robot arm 100, and uses the position/orientation information of the robot arm 100 to generate an image captured by the camera 22. The image is converted into an image of the hand coordinate system 115b. The virtual sensor data generation unit 4 outputs the converted image of the hand coordinate system 115b as virtual sensor data (virtual sensor data generation processing). When the virtual sensor data generator 4 generates the image of the hand coordinate system 115b from the captured image of the camera 22, for example, a general perspective projection method is used.

The sensing unit 5 acquires a captured image, which is a sensor output, from the camera 22 attached to the robot arm 100 (sensing processing).
The matching unit 6 receives the virtual sensor data generated by the virtual sensor data generation unit 4 and the captured image output by the sensing unit 5, and evaluates the difference between the two data.
The sensor position/orientation calibration device 1 shown in FIG. 2 can be configured by a computer.

FIG. 3 shows a hardware configuration example when the sensor position/orientation calibration device 1 is configured by a computer.
A computer 10 shown in FIG. 3 includes a CPU (Central Processing Unit) 11, a ROM (Read Only Memory) 12, and a RAM (Random Access Memory) 13, which are respectively connected to a bus. Computer 10 further comprises non-volatile storage 14 , network interface 15 , input device 16 , display device 17 , robot interface 18 and image interface 19 .

The CPU 11 is an arithmetic processing unit that reads out from the ROM 12 a program code of software that executes processing of the sensor position/orientation calibration device 1 and executes the program code.
The RAM 13 is temporarily written with variables, parameters, etc. generated during the arithmetic processing.

For the nonvolatile storage 14, for example, a large-capacity information storage medium such as HDD (Hard Disk Drive) or SSD (Solid State Drive) is used. The nonvolatile storage 14 records programs for functions executed by the sensor position/orientation calibration device 1 .

For example, a NIC (Network Interface Card) or the like is used for the network interface 15 . The network interface 15 transmits and receives various information to and from the outside via a wireless LAN (Local Area Network) or the like.

A keyboard, a mouse, or the like is used as the input device 16, for example.
The display device 17 is, for example, a liquid crystal display monitor, and the display device 17 displays the result of execution by the computer 10 , that is, the calibration result of the sensor position/orientation calibration device 1 .

The robot interface 18 acquires posture data of the robot arm 100 from the controller of the robot arm 100 (the robot arm controller 21). The image interface 19 acquires image data captured by the camera 22 .

Note that the configuration of the sensor position/orientation calibration device 1 with the computer 10 shown in FIG. 3 is an example, and may be configured with an arithmetic processing device other than the computer. For example, some or all of the functions performed by the sensor position/orientation calibration device 1 may be realized by hardware such as FPGA (Field Programmable Gate Array) or ASIC (Application Specific Integrated Circuit).

[1-3. Relative position posture parameter]
Next, with reference to FIG. 1 again, the relative position/posture parameter information 2a acquired by the relative position/posture parameter acquisition unit 2 will be described.
The joints 112a, 112b, 112c of the robot arm 100 shown in FIG. 1 are provided with actuators such as servo motors. The control values of the joints 112a, 112b, 112c of the robot arm 100 represent the rotation angles of the joints 112a, 112b, 112c. However, the use of the rotation angle as the control value is an example, and other types of values may be used as the control values as long as the states of the joints 112a, 112b, and 112c can be represented.
The posture of the robot arm 100 is determined by a combination of control values of actuators of all joints 112a, 112b, 112c.

The relative position/orientation parameter information 2a is a parameter representing the relative position/orientation between the hand coordinate system 115b and the camera coordinate system 22b. The relative position/orientation parameter information 2a may be represented by a general affine transformation, Euler angles representing rotation angles about each axis, or quaternions representing a rotation axis and a rotation angle around the rotation axis. Anything that can express the position and orientation of the system can be used.
The relative position/orientation parameter information 2a is, for example, parameter information that determines the direction of conversion from the hand coordinate system 115b to the camera coordinate system 22b. It may be a direction to the coordinate system 115b.

FIG. 4 is a diagram for explaining the shape information 3a acquired by the shape information acquisition unit 3. As shown in FIG.
In the example of FIG. 4, four housing components 3a-1 to 3a-4 are registered as the shape information 3a. That is, a housing component 3a-1 composed of the base 111, a housing component 3a-2 composed of the link 113a and the joint 112a, and a housing component 3a- composed of the link 113b and the joint 112b. 3, and a housing component 3a-4 composed of a link 113c, an arm 114, and a hand 115. As shown in FIG. The posture of the robot arm 100 as a whole can be expressed by combining these housing components 3a-1 to 3a-4.

In this embodiment, the shape information 3 a is a three-dimensional model of the robot arm 100 , but the shape information 3 a may include information capable of expressing the shape of the robot arm 100 . Also, the shape information 3 a may include color information and texture information about the robot arm 100 .
The shape information 3a is a three-dimensional model measured using a general three-dimensional sensor and measurement method such as a laser sensor and a stereo camera, a general three-dimensional model creation tool such as CAD, URDF (Unified Robot Description Format) It is a three-dimensional model representing the shape of the robot arm 100 created in a description format such as.

[1-4. Processing of Virtual Sensor Data Generation Unit]
FIG. 5 shows the virtual sensor data generator 4. As shown in FIG.
The virtual sensor data generator 4 has a function of virtually reproducing the robot arm 100 and the camera 22 attached thereto and generating a photographed image obtained from the camera 22 .
The virtual sensor data generation unit 4 includes a mobile body posture shape simulation unit 4a. The moving body posture shape simulating unit 4a receives the shape information 3a acquired by the shape information acquiring unit 3 and the posture information 4b of the robot arm 100 as input, and generates a moving body posture shape that virtually reproduces the posture indicated by the posture information 4b. Output information 4c.

The virtual sensor data generator 4 also includes a sensor viewpoint generator 4d. The sensor viewpoint generation unit 4d receives the relative position/orientation parameter information 2a as an input, rotates and translates the moving body posture shape information 4c, and converts it into an observation from the viewpoint of the virtually mounted camera 22, Generate sensor viewpoint shape information 4e.
Furthermore, the virtual sensor data generator 4 includes a generator 4g. The generator 4g receives the sensor parameter information 4f and projects the sensor viewpoint shape information 4e onto the image plane of the camera 22 to generate a virtually arranged image.

The moving body posture shape simulating unit 4a, the sensor viewpoint generating unit 4d, and the generating unit 4g will be described in more detail below.
The posture information 4b supplied to the mobile body posture shape simulating unit 4a is a parameter for determining the posture of the robot arm 100, and is a parameter for determining the posture of the robot arm 100. Three-dimensional position coordinates in an arbitrary coordinate system.
Alternatively, the posture information 4b may be control values of actuators provided in the joints 112a to 112c of the robot arm 100. FIG. In the following description, an arbitrary coordinate system indicating the three-dimensional position coordinates indicated by the posture information 4b will be referred to as a robot coordinate system. This robot coordinate system corresponds to, for example, the robot coordinate system 100a shown in FIG.

[1-5. Processing of Moving Object Posture Shape Simulation Unit]
FIG. 6 is a flow chart showing the processing of the moving body posture shape simulator 4a.
First, the moving object posture shape simulator 4a sets a robot coordinate system in an arbitrary space (step S11), receives shape information 3a (step S12), and receives posture information 4b (step S13).

Then, the moving body posture shape simulator 4a arranges the housing constituent elements included in the shape information 3a in the robot coordinate system indicated by the posture information 4b (step S14). After that, the moving body posture shape simulator 4a confirms whether or not all of the housing constituent elements included in the shape information 3a or the necessary housing constituent elements set in advance out of the shape information 3a have been arranged ( step S15). In step S15, when it is determined that there is an unarranged housing component (no in step S15), the moving body posture shape simulator 4a returns to the process of step S14. Then, in step S15, when it is determined that all housing components have been arranged (yes in step S15), moving body posture shape simulator 4a obtains moving body posture shape information 4c.

Next, the details of the process of arranging the housing constituent elements in the robot coordinate system indicated by the posture information 4b, which is performed in step S14, will be described with reference to FIGS. 4 and 7. FIG.
In step S14, the mobile body posture shape simulator 4a arranges the housing constituent elements registered in the shape information 3a in the robot coordinate system. The place where the housing component is arranged is obtained by referring to the attitude information 4b. In the example of FIG. 4, four housing constituent elements 3a-1 to 3a-4 are registered as the shape information 3a, and the posture of the robot arm 100 can be expressed by combining these elements. The number and details of the configuration of the housing components 3a-1 to 3a-4 are not necessarily limited to those shown in FIG. 4 as long as the shape of the robot arm 100 can be represented.

FIG. 7(a) shows moving body posture shape information 4c obtained by changing the position and posture of the housing component 3a-2 among the housing components 3a-1 to 3a-4.
FIG. 7(a) shows moving body posture shape information 4c obtained by changing the positions and postures of housing components 3a-3 and 3a-4 among housing components 3a-1 to 3a-4. ing.

In the example of FIG. 7, the orientation information 4b is set with the rotation angles θ1, θ2, and θ3 of the three joints 112a to 112c, which are movable parts, among the housing components 3a-1 to 3a-4. Then, in step S14, for example, by rotating and moving each joint 112a to 112c according to the posture information 4b from the posture indicated by the initial value of the actuator, the moving body posture shape simulator 4a generates the moving body posture shape information 4c. .

The posture information 4b may be the control values of the actuators provided in the joints 112a to 112c of the robot arm 100. In this case, the moving body posture shape simulating unit 4a uses the control values of the actuators as displacement amounts (for example, Equipped with a simulator that converts to rotation angle). Then, the moving body posture shape simulator 4a may create the moving body posture shape information 4c using this simulator.
Alternatively, the orientation information 4b may be registered as information that directly expresses the positions and orientations of the housing components 3a-1 to 3a-4 in the robot coordinate system.

A sensor viewpoint generation unit 4d to which the relative position/posture parameter information 2a and the moving body posture shape information 4c are input performs viewpoint transformation of the three-dimensional model possessed by the observed moving body posture shape information 4c. Then, it converts to the one observed from the viewpoint of the camera coordinate system 22b and outputs the sensor viewpoint shape information 4e.

[1-6. Processing of Sensor Viewpoint Generation Unit]
FIG. 8 is a flowchart showing the processing of the sensor viewpoint generation unit 4d.
First, the sensor viewpoint generation unit 4d receives the moving body posture shape information 4c (step S21), and also receives the relative position/posture parameter information 2a (step S22). Then, the sensor viewpoint generation unit 4d sets the hand coordinate system 115b in the mobile body posture shape information 4c (step S23).

After that, the sensor viewpoint generation unit 4d sets the camera coordinate system 22b using the relative position/orientation parameter information 2a expressed as the relative position from the hand coordinate system 115b (step S24).
Then, the sensor viewpoint generation unit 4d rotates and translates the moving body posture shape information 4c to match the starting point with the camera coordinate origin 22a, and acquires the sensor viewpoint shape information 4e (step S25).

Next, the processing in steps S23, S24, and S25 in the flowchart of FIG. 8 will be described in detail.
In step S23, the hand coordinate origin 115a and the hand coordinate system 115b are set on the moving body posture shape information 4c. A hand coordinate system 115b having a hand coordinate origin 115a as a point that moves in conjunction with the movement of the robot arm 100 changes its position in the robot coordinate system 100a depending on the posture of the robot arm 100, but the relative position and posture with respect to the end effector is Since it is invariable, it can be uniquely set on the mobile body posture shape information 4c.

In step S24, the camera coordinate origin 22a and the camera coordinate system 22b are set on the mobile body posture shape information 4c.
The relative position/orientation parameter information 2a is a parameter representing the relative position and orientation of the hand coordinate system 115b and the camera coordinate system 22b. Since the relative position/posture parameter information 2a uniquely obtains the geometric position/posture from the preset hand coordinate system 115b, it is possible to set the camera coordinate origin 22a and the camera coordinate system 22b.

The directions of expression of the relative positional relationship between the hand coordinate system 115b and the camera coordinate system 22b represented by the relative position/orientation parameters are the direction from the hand coordinate system 115b to the camera coordinate system 22b and the direction from the camera coordinate system 22b to the hand coordinate system 115b. Two ways are conceivable. Since the positions and orientations of the camera coordinate origin 22a and the camera coordinate system 22b are the same and can be uniquely determined in any direction, the relative position/posture parameter information 2a may be expressed in any direction.

FIG. 9 is a diagram for explaining the processing in step S25.
In step S25, the camera coordinate system 22b is set in the mobile body posture shape information 4c. Since the moving body posture shape information 4c is a three-dimensional model, the moving body posture shape information 4c can be easily converted into a model observed from an arbitrary viewpoint by combining rotation and translation. Obtainable.

FIG. 9 shows an example of sensor viewpoint shape information obtained by moving the viewpoint to the camera coordinate system 22b set like the example shown on the right side of step S24 in FIG. In the example shown in FIG. 8, the housing component 3a-4 is oriented toward the origin of the robot coordinate system 100a, and the camera coordinate system 22b is located above the housing component 3a-4. It is set to face the same direction as 3a-4.

At this time, the sensor viewpoint shape information 4e obtained by converting the viewpoint in the direction indicated by the camera coordinate system 22b at the position of the camera coordinate origin 22a is, as shown in FIG. It becomes a viewpoint from which a part of the body component 3a-2 is observed. In the example shown in FIG. 9, with respect to the axes of the camera coordinate system 22b, the axis pointing toward the origin of the robot coordinate system 100a is set as the Z axis, and the sensor viewpoint shape information 4e is created so that the Z axis is in the distant direction. there is
However, the sensor viewpoint shape information 4e shown in FIG. 9 is only an example, and the direction for viewpoint conversion may be the same as that of the camera 22 that is actually attached. For example, in the case of a general optical camera, the direction of the viewpoint is the optical axis direction.

[1-7. Processing in which the generation unit generates virtual sensor data]
A generator 4g shown in FIG. 5 receives the sensor viewpoint shape information 4e and the sensor parameter information 4f and generates an image acquired by the camera 22 as virtual sensor data 4h. The virtual sensor data 4h is data generated when the robot arm 100 takes a posture according to the posture information 4b. In other words, sensor data acquired by the camera 22 attached to the robot arm 100 at the position indicated by the relative position/orientation parameter information 2a is virtually generated. This virtual sensor data 4h is ideal sensor data for each piece of input information.

FIG. 10 is a diagram for explaining a process of generating virtual sensor data by the generation unit 4g.
In the example of FIG. 10, the perspective projection model is used to project the subject in the three-dimensional space of the camera coordinate system 22b shown in FIG. 10(a) onto the image plane 4g-1 shown in FIG. 10(b).
In FIG. 10, the sensor viewpoint shape information 4e is arranged with respect to a camera coordinate system 22b having the optical center of the camera 22 as the camera coordinate origin 22a. The virtual sensor data 4h acquired by the camera 22 is obtained by projecting the sensor viewpoint shape information onto the image plane 4g-1.
For the perspective projection model of the camera 22, the parameters shown in [Equation 1] are used.

The parameters shown in [Equation 1] are generally called internal parameters of the camera 22 .
Among these internal parameters, (fx, fy) represents the focal length per pixel, which is the value obtained by dividing the focal length of the camera 22 by the vertical and horizontal physical spacing of the pixels of the camera 22 .
Further, among the internal parameters shown in [Equation 1], (cu, cv) is the intersection of the optical axis of the camera 22 and the image plane 4g-1 in the virtual sensor data coordinate system 4i in the image plane 4g-1, the image It represents the coordinates of the center 4j.

The method of projecting the point (x, y, z) on the camera coordinate system 22b onto the image plane 4g-1 is, for example, the calculation shown in the formula [Equation 2].

In the example shown in FIG. 10, the virtual sensor data 4h is virtually generated by projecting all possible areas of the sensor viewpoint shape information onto the image plane according to the above-described perspective projection model of the image captured by the camera 22. It is what I did. Alternatively, the virtual sensor data 4h may be obtained by projecting only a specific partial area of the sensor viewpoint shape information 4e onto the image plane 4g-1, and any region of the sensor viewpoint shape information may be projected onto the image plane 4g-1. .
Alternatively, as shown in FIG. 11, the virtual sensor data may be coordinate values or a set of coordinate values when projecting at least one specific point onto the image plane 4g-1. That is, the subject in the three-dimensional space of the camera coordinate system 22b shown in FIG. 11(a) may be a set of coordinate position values of points 1 to n as shown in FIG. 11(b).

In this embodiment, the sensor is the camera 22, but the sensor may be a general visual sensor. The virtual sensor data 4h may be replaced with, for example, moving images or three-dimensional data according to the type of sensor and the output format of the sensor data of the sensor.

[1-8. Processing of Sensing Section]
The sensing unit 5 shown in FIG. 2 acquires sensor data from the camera 22 actually attached to the robot arm 100 . The robot arm 100 assumes the posture indicated by the posture information 4b received as an input by the virtual sensor data generation unit 4, and the sensing unit 5 acquires sensor data from the camera 22 in this state.
Alternatively, the sensing unit 5 has a function of performing various arithmetic processing on sensor data, detects a specific area or point, such as a marker, from the sensor data, and outputs the coordinate value or a set of coordinate values. You may As the sensor, in addition to the camera 22, a general visual sensor may be used. Moreover, the sensor data acquired by the sensing unit 5 may be, for example, moving images or three-dimensional data. Furthermore, the sensor data acquired by the sensing unit 5 may be acquired in advance. For example, the information stored in a storage device such as a hard disk may be obtained, or it may be obtained from the outside using a communication method.

[1-9. Processing of Matching Unit]
The matching unit 6 shown in FIG. 2 compares the virtual sensor data 4h generated by the virtual sensor data generation unit 4 and the sensor data acquired by the sensing unit 5 to determine whether there is a difference.
In a robot system including an ideal robot arm 100 and camera 22, the virtual sensor data 4h and the sensor data are completely the same. However, in reality, due to acceptance of distortion and deflection of the robot housing, the determination accuracy of the position and attitude of the robot arm 100 is lowered, and the virtual sensor data 4h and the sensor data do not match, resulting in discrepancies.

As a matching method, the virtual sensor data 4h and the pixel value of each pixel of the sensor data are compared. Alternatively, a method of comparing values obtained by converting into general feature amounts such as SIFT (Scale-Invariant Feature Transform) or HOG (Histogram of Oriented Gradients) may be used. Alternatively, the matching may be performed only for a specific region such as the edge portion observed at the edge of the link 113a in the virtual sensor data 4h and the sensor data, and the matching target region is not particularly limited. In addition, the matching unit 6 stores a threshold that is an allowable amount of difference in advance, and when the virtual sensor data 4h and the sensor data do not completely match, if the amount of difference falls within the threshold, it is determined that there is no difference. may

If a difference occurs between the virtual sensor data 4h and the sensor data as a result of matching by the matching unit 6, it is possible to detect a decrease in the determination accuracy of the posture. For example, the robot arm control device 21 (FIG. 3) is notified of a decrease in the determination accuracy of the detected posture. Alternatively, the information is displayed on the display device 17 (FIG. 3) included in the sensor position/orientation calibration device 1 for notification.

<2. Second Embodiment>
A second embodiment of the present invention will be described below with reference to FIGS. 12 to 18. FIG.
In this embodiment, a case where the virtual sensor data 4h and the sensor data are coordinate values of specific points will be described as an example.

[2-1. Configuration of Sensor Position and Orientation Calibration Device]
FIG. 12 shows a configuration example of a sensor position/orientation calibration device 1a according to a second embodiment of the present invention.
A sensor position/orientation calibration device 1a shown in FIG. It is the same as the sensor position/orientation calibration device 1 described in the example (FIG. 2).

The second embodiment differs from the first embodiment in that the sensor position/orientation calibration device 1a of the second embodiment includes a matching result evaluation unit 7. FIG.
In the second embodiment shown in FIG. 12 as well, similar to the first embodiment, the relative position/orientation parameter acquisition unit 2, the shape information acquisition unit 3, the virtual sensor data generation unit 4, and the sensing unit 5 Through the processing, virtual sensor data 4h and sensor data are acquired. The virtual sensor data 4h and the sensor data are processed by the matching unit 6 and sent to the matching result evaluation unit 7. The matching result evaluation unit 7 receives the result of the matching unit 6, and the difference between the virtual sensor data 4h and the sensor data is determined. If it is determined that there is, the amount of error is calculated.

[2-2. Processing of Matching Result Evaluation Unit]
FIG. 13 is a flow chart showing the processing of the matching result evaluation unit 7. As shown in FIG.
The matching result evaluation unit 7 receives the determination result of the matching unit 6 (step S31), and determines whether there is a difference between the virtual sensor data 4h and the sensor data from the determination result of the matching unit 6 (step S32).
If it is determined in step S32 that there is a difference between the virtual sensor data 4h and the sensor data (yes in step S32), the matching result evaluation unit 7 searches for an orientation that minimizes the error indicating the amount of difference (step S33). Then, the matching result evaluation unit 7 calculates and records the posture that minimizes the error and the position and posture error amounts of the posture information 4b (step S34).

Further, in step S32, when it is determined that there is no difference between the virtual sensor data 4h and the sensor data (no in step S32), the matching result evaluation unit 7 records the error amount indicating that there is no error amount (step S35).

The processing of steps S33, S34, and S35 will be described in detail below with reference to FIGS. 14 to 16. FIG.
FIG. 14 shows the processing in step S33.
In the example shown in FIG. 14, the virtual sensor data 4h are the coordinate values when the marker marked on the housing component 3b-1 is projected onto the image plane 4g-1. The example in FIG. 14 shows three markers: a star, a square, and a triangle. Each marker has known three-dimensional coordinates, and can be expressed as (x, y, z) coordinates in the robot coordinate system 100a, for example. That is, the reference coordinate system can be easily transformed. In this embodiment, an example in which markers are detected and their positions are used is shown, but similar to the first embodiment, matching of images and the like may be performed.

In the example shown in FIG. 14, three markers 7a-1, 7a-2, and 7a-3 are installed, but the shape and number of the markers may be different as long as the markers are identifiable. Also, in the example shown in FIG. 14, three markers are installed on the housing of the robot arm 100, but the positions and number of markers are not limited to those shown in the figure.

There are various methods such as pattern matching for detecting the markers 7a-1, 7a-2, and 7a-3 in the image projected onto the image plane 4g-1, but other methods may be used.
The virtual sensor data 4h-1 shown in FIG. 14(b) and the sensor data 5a-1 shown in FIG. 14(c) indicate the same marker coordinates.

That is, in the example shown in FIG. 14, (u41, v41) in FIG. 14(b) indicating the position of the marker 7a-1 in the virtual sensor data coordinate system 4i and (u51, v51) in FIG. 14(c) are handle. Also, (u42, v42) in FIG. 14(b) indicating the position of the marker 7a-2 corresponds to (u52, v52) in FIG. 14(c). Further, (u43, v43) indicating the position of the marker 7a-3 in FIG. 14(b) and (u53, v53) in FIG. 14(c) correspond to each other.

When it is determined that there is a difference in step S32 of FIG. 13, the matching result evaluation unit 7 determines that the coordinates of at least one of the markers 7a-1, 7a-2, and 7a-3 do not match, or It is judged that the above differences have occurred.
In both the virtual sensor data 4h-1 and the sensor data 5a-1, the positions of the markers 7a-1, 7a-2, and 7a-3 in the virtual sensor data coordinate system 4i are obtained by the above-described [Equation 2] formula: The combination of each marker position can be uniquely obtained from the position and orientation of the camera coordinate system 22b.

In step S33, the matching result evaluation unit 7 searches for a posture (posture information 4b') that matches the virtual sensor data 4h and the sensor data 5a-1 by changing the posture information 4b. The search method may be a search method that comprehensively changes the position and orientation. Alternatively, a search method such as a gradient descent method may be used in which an approximate posture in which the virtual sensor data 4h-1 and the sensor data 5a-1 match is calculated in advance and the motion is repeated.

When searching for an orientation in which the virtual sensor data 4h-1 and the sensor data 5a-1 match by changing the orientation information 4b, the virtual sensor data 4h can be changed by changing the input of the virtual sensor data generator 4. or by moving the robot arm 100 to change the sensor data 5a-1.

For example, the markers 7a-1, 7a-2, 7a-3 and the camera 22 are positioned so that a plane passing through the markers 7a-1, 7a-2, 7a-3 faces the image plane 4g-1. , the differences between (u41, v41) and (u51, v51), (u42, v42) and (u52, v52), and (u43, v43) and (u53, v53) are (3, 0 ), in step S33, posture information 4b for parallel translation of the virtual sensor data 4h or sensor data 5a-1 by 3 pixels on the image plane 4g-1 in the direction of canceling the difference may be obtained. Movement of the posture information 4b may include rotation.

In step S34, the matching result evaluation unit 7 calculates and records the amount of error between the posture information 4b and the posture information 4b' (see FIG. 16) obtained in step S33. If the orientation information 4b, 4b' is a parameter representing the position and orientation, errors are recorded in a method of expressing differences according to the properties of the parameters. For example, when the orientation information 4b and 4b' are rotation/movement matrices, they are conversion matrices from the orientation information 4b to the orientation information 4b' or in the opposite direction. Alternatively, if the posture information 4b, 4b' is the control amount of the actuator, etc., the change in the control amount, for example, the change in the rotation angle may be recorded.

In step S35, when the matching result evaluation unit 7 determines in step S32 that there is no difference between the virtual sensor data 4h and the sensor data 5a-1, the matching result evaluation unit 7 expresses that the posture information 4b and the posture information 4b' have the same value. record. For example, when the orientation information 4b and 4b' are rotation/movement matrices, the expression indicating that there is no difference between the orientation information 4b and 4b' is a unit matrix. Alternatively, if the posture information 4b, 4b' is the control amount of the actuator, etc., 0 is recorded as the change in the control amount, for example, the change in the rotation angle.

FIG. 15 shows the case where there is only one marker 7a-1. In this case, in step S33, since there are a plurality of posture information 4b' (see FIG. 16) in which the virtual sensor data 4h-1 and the sensor data 5a-1 match, the matching result evaluation unit 7 can uniquely determine Can not. In step S33, a plurality of pieces of posture information 4b may be used to create a plurality of virtual sensor data 4h-1 and sensor data 5a-1.

[2-3. Processing for evaluating posture accuracy]
Next, a method for evaluating the posture accuracy of the robot arm 100 will be described with reference to FIG.
In the example shown in FIG. 16, the robot arm 100 includes three joints 112a-112c with actuators, and the pose information is the rotation angles of the three joints 112a-112c. However, the rotation angles of the three joints 112a to 112c are an example, and other information may be used.

First, as shown in FIG. 16A, arbitrary orientation information 4b is input to the sensor position and orientation calibration device 1a.
The left side of FIG. 16(b) shows the moving body posture shape information 4c when the posture information 4b is input. The sensor position/orientation calibration device 1a records the error amount corresponding to the orientation information 4b.

Next, the sensor position/orientation calibration device 1a inputs the orientation information 4b′ obtained by changing only the angle θ1 among the contents of the orientation information 4b, and similarly records the amount of error.
The right side of FIG. 16(b) shows moving body posture shape information 4c' corresponding to posture information 4b'.
At this time, if there is a difference between the two recorded error amounts, and the latter error amount becomes large when compared, the posture accuracy of the joint 112a controlled by the angle θ1, for example, is poor, or the link connected to it is It may be determined that distortion or bending occurs.

One or more pieces of orientation information 4b may be changed when evaluating orientation accuracy using the sensor position and orientation calibration device 1a of the present embodiment. Alternatively, the procedure shown in the flowchart of FIG. 13 may be executed multiple times to record the relationship between multiple patterns of posture information 4b and the amount of error.

FIG. 17 shows an example of a table recording the relationship between the attitude information 4b and the amount of error. From the relationship between the posture information 4b and the error amount, it is possible to obtain the elements of the posture information 4b that can increase or decrease the error amount or a combination of the elements of the posture information 4b by general analysis methods such as multiple regression analysis and logistic regression analysis. can.

Alternatively, the sensor position/orientation calibration device 1a may create an orientation accuracy map α as shown in FIG.
In the example shown in FIG. 18, since the sensor position/orientation calibration device 1a obtains the attitude information 4b of the robot arm 100 and the amount of error when the attitude is taken, for example, a region is set according to the degree of the amount of error and visualized. are doing.
That is, as shown in FIG. 18, when the movable range α of the hand 115 of the robot arm 100 is set, within the movable range α, there is an error-free range α1, a relatively small error range α2, and a large error range α2. The range α3 in which the error occurs is visualized and displayed. 18A is a side view of the movable range α of the hand 115, and FIG. 18B is a top view of the movable range α of the hand 115. FIG.

The posture accuracy map showing the movable range α shown in FIG. 18 may be output in a format that can be displayed on a general display device such as a display, or in the form of data representing the correspondence between coordinates and error amounts. can be output as

As described above, according to this embodiment, for example, by referring to the relationship between the posture information 4b and the error amount shown in FIG. 17 or the posture accuracy map shown in FIG. can be obtained. Therefore, by obtaining the output of the sensor position/orientation calibration device 1a, the control device for the robot arm 100 can perform control to avoid areas with poor orientation accuracy and control path generation with less deterioration in orientation accuracy. Alternatively, even if the arrival position of the robot arm 100 in the hand coordinate system, for example, is the same, when the combination of the posture information 4b is different, the posture information 4b with the highest accuracy can be selected to increase the posture accuracy of the robot arm 100. control becomes possible.

<3. Variation>
It should be noted that the present invention is not limited to the above-described embodiments, and includes various modifications. For example, each of the embodiments described above has been described in detail in order to explain the present invention in an easy-to-understand manner, and is not necessarily limited to those having all the described configurations.

For example, in each of the above-described embodiments, an example in which a robot arm is used as a moving object has been described. It is possible.
In addition, in each of the above-described embodiments, an example in which a camera is applied as a sensor has been described. It can be applied to various sensors as long as they are sensors.

In addition, in block diagrams such as FIGS. 3 and 5, only control lines and information lines that are considered necessary for explanation are shown, and not all control lines and information lines are necessarily shown on the product. . In practice, it may be considered that almost all configurations are interconnected. In addition, in the flowcharts shown in FIGS. 6 and 8, a plurality of processes may be executed simultaneously or the order of processes may be changed as long as the process results are not affected.

Further, the sensor position/orientation calibration devices 1 and 1a described in the embodiments are implemented by software, in which the processor interprets and executes programs that implement respective functions, as described with reference to FIG. good too. Information such as programs for realizing each function can be stored in a recording device such as a memory, a hard disk, an SSD (Solid State Drive), or a recording medium such as an IC card, an SD card, or an optical disc.

1, 1a... sensor position/orientation calibration device, 2... relative position/orientation parameter acquisition unit, 2a... relative position/orientation parameter information, 3... shape information acquisition unit, 3a... shape information, 4... virtual sensor data generation unit, 4a... 4b, 4b'... Attitude information 4c, 4c'... Moving body posture shape information 4d... Sensor viewpoint generation part 4e... Sensor viewpoint shape information 4f... Sensor parameter information 4g... Generation Part, 4h... Virtual sensor data, 5... Sensing part, 6... Matching part, 7... Matching result evaluation part, 10... Computer, 11... Central control unit (CPU), 12... ROM, 13... RAM, 14... Non-volatile Storage 15 Network interface 16 Input device 17 Display device 18 Robot interface 19 Image interface 21 Robot arm control device 22 Camera 22a Origin of camera coordinate system 22b Camera coordinate system , 100... Robot arm, 111... Base, 112a, 112b, 112c... Joint, 113a, 113b... Link, 114... Arm, 115... Hand, 115a... Origin of hand coordinate system, 115b... Hand coordinate system, α... Movable range, α1: no error range, α2: medium error range, α3: large error range

Claims (10)

A sensor position/orientation calibration device for converting a coordinate system set at an arbitrary point of a sensor into a coordinate system of a moving body,
a relative position/posture parameter acquisition unit that acquires the relative position/posture of the moving object and the sensor;
a shape information acquisition unit that acquires shape information of the moving body;
a virtual sensor data generation unit that generates virtual sensor data that virtually reproduces the data acquired by the sensor based on the information acquired by the parameter acquisition unit and the shape information acquisition unit;
a sensing unit that outputs sensor data acquired by the sensor;
a matching unit that compares the virtual sensor data and the sensor data and determines whether there is a difference;
A matching result evaluation unit that evaluates the output of the matching unit and calculates an error amount between the virtual sensor data and the sensor data,
The virtual sensor data generator,
a moving body posture shape simulating unit that generates moving body posture shape information representing a posture shape of a virtual moving body based on the shape information and the posture information representing the posture of the moving body;
a sensor viewpoint generation unit that receives the moving body posture shape information and the relative position and posture parameters as inputs and generates sensor viewpoint shape information converted into the viewpoint of the sensor;
a generation unit that virtually generates data acquired by the sensor based on the sensor viewpoint shape information and the parameter specific to the sensor,
The matching result evaluation unit changes at least one or more parameters among the respective values of the posture information to create a plurality of the posture information and inputs the plurality of posture information and the error amount. and perform analysis processing on the correspondence between the posture information and the error amount.
Sensor position and orientation calibration device. The moving body posture shape simulating unit includes:
arranging a housing component at a position indicated by the posture information in an arbitrary coordinate system;
referring to a command value included in the posture information, and generating the moving body posture shape information by virtually reproducing the position and posture of the housing component realized by the command value through rotation and movement;
2. The sensor position/orientation calibration device according to claim 1, wherein the control values virtually reproduce the operation of an actuator provided in the moving body. 2. The sensor viewpoint shape information according to claim 1 , wherein the sensor viewpoint shape information is generated by converting the moving body posture shape information into the viewpoint of the sensor by performing one or both of rotation and movement processing. sensor position and orientation calibration device. The generating unit virtually generates data acquired by the sensor for only a partial area of the mobile body posture shape information,
2. The sensor position/orientation calibration device according to claim 1 , wherein the virtually generated data acquired by the sensor acquires at least one or more pieces of position information of a specific portion of the mobile body posture shape information. The matching unit compares part or all of the output of the sensing unit with the virtual sensor data, and if the difference between the compared determination results is greater than a preset threshold,
The sensor position/orientation calibration device according to claim 1, wherein it is determined that there is a difference. The sensor position/orientation calibration device according to claim 1 , wherein the matching result evaluation unit calculates the orientation information that minimizes or reduces the error amount when there is a difference between the virtual sensor data and the sensor data. 2. The sensor position/orientation calibration according to claim 1 , wherein the matching result evaluation unit detects at least one or more markers and calculates an error amount from a difference in the positions of the markers in the virtual sensor data or the sensor data. Device. 2. The sensor according to claim 1 , wherein the matching result evaluation unit acquires the posture accuracy of the moving body by inputting a plurality of pieces of the posture information and creating and evaluating a plurality of the virtual sensor data and the sensor data. Position and attitude calibration device. The matching result evaluation unit has a function of creating and displaying a map showing correspondence between the posture information and the error amount,
2. The sensor position/orientation calibration device according to claim 1 , wherein correspondence between the orientation information and the error amount is recorded, and a list of the orientation information and the corresponding error amount is recorded. A sensor position/orientation calibration method for converting a coordinate system set at an arbitrary point of a sensor into a coordinate system of a moving body,
a relative position/posture parameter acquisition process for acquiring the relative position/posture of the moving object and the sensor;
Shape information acquisition processing for acquiring shape information of the moving body;
A virtual sensor data generation process for generating virtual sensor data that virtually reproduces the data acquired by the sensor based on the information acquired by the parameter acquisition process and the shape information acquisition process;
A sensing process for outputting sensor data acquired by the sensor;
A matching process for comparing the virtual sensor data and the sensor data to determine whether there is a difference;
A matching result evaluation process that evaluates the output of the matching process and calculates an error amount between the virtual sensor data and the sensor data,
Furthermore, the virtual sensor data generation process includes:
moving body posture shape simulation processing for generating moving body posture shape information representing a posture shape of a virtual moving body based on the shape information and posture information representing the posture of the moving body;
a sensor viewpoint generation process for generating sensor viewpoint shape information converted to the viewpoint of the sensor, using the moving body posture shape information and the relative position and posture parameters as inputs;
a generation process of virtually generating data acquired by the sensor using the sensor viewpoint shape information and parameters unique to the sensor as inputs;
In the matching result evaluation process, by changing at least one or more parameters among the respective values of the posture information to create a plurality of the posture information and inputting them, the plurality of posture information and the error amount are generated. and perform analysis processing on the correspondence between the posture information and the error amount.
Sensor position and orientation calibration method.

Images