JP2005114722A - Method and device for calibrating three-dimensional position - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a calibration method capable of finding a three-dimensional position of a plurality of receivers serving as references, when finding the three-dimensional position. <P>SOLUTION: This method comprises a first step of reading out a data, wherein the respective three-dimensional positions of the plurality of receivers at respective positions are calculated, using a plurality of ultrasonic transmitters attached to this calibration device, at the plurality of positions where the calibration device with the plurality of ultrasonic transmitters mounted with a prescribed positional relation is moved; a second step of selecting the receiver capable of calculating the three-dimensional position from the plurality of positions out of the receivers, capable of calculating the three-dimensional position in the each position of the plurality of positions; a third step of finding the three-dimensional position of the calibration device by an optimization method, using the specified point as world coordinates, based on three-dimensional position coordinates of the selected receiver to prepare a coordinate transformation matrix; and a fourth step of transforming the position of the receiver, found in the each position, into a world coordinate system, using the coordinate transformation matrix found, so as to be integrated. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

  The present invention relates to a three-dimensional position calibration method and a calibration apparatus for obtaining three-dimensional positions of a plurality of receivers that are used as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus.

  In order to establish technology for observing and recognizing human behavior in everyday space, it is necessary to be able to observe human behavior with sufficient accuracy, in real time, and not to affect human natural behavior. It is necessary to be constrained and to easily realize the recognition function regardless of the surrounding environment. One method for this purpose is “object sensorization”. “Sensorization of an object” here refers to the property as a virtual sensor that reliably detects with sufficient accuracy the accompanying actions that occur when using the object while maintaining the original functions and properties of the object. Refers to adding to an object. In order to measure human activity, for example, by using an ultrasonic three-dimensional position measuring device, “sensorization of an object” is performed. Therefore, the position of an ultrasonic transmitter attached to the object to be sensorized is determined. Measure using an ultrasonic receiver with a known position on the wall or ceiling of the room.

  For this purpose, the position of the ultrasonic receiver attached to the wall or ceiling of the room must be accurately grasped. In other words, 3D position measurement is performed on the assumption that the position of the ultrasonic receiver attached to the wall or ceiling of the room is known, so the position of the ultrasonic receiver attached to the wall or ceiling of the room is accurately determined. In order to capture, a three-dimensional position measurement calibration is performed. Otherwise, the ultrasonic receiver must be accurately mounted and attached to the wall or ceiling of the room, which involves considerable difficulty.

  Conventionally, as a technique related to three-dimensional position calibration for accurately obtaining a three-dimensional position as a reference point for three-dimensional measurement, a “wide area three-dimensional position measuring method and apparatus” as disclosed in Patent Document 1 is known. It is. According to this technology, a jig that fixes multiple cameras that have already been calibrated for position and orientation and internal parameters in the camera coordinate system, and a jig that can be assembled with multiple characteristic objects with known positional relationships are installed at the measurement site. By doing so, it is possible to easily calibrate the conversion parameter between the camera coordinate system and the measurement area coordinate system at the measurement site, and to easily measure the three-dimensional position of the measurement target.

Moreover, as an example of another known technique, a “three-dimensional position measuring device using a stereo method and its measuring method” as disclosed in Patent Document 2 is known. According to the technology of this three-dimensional position measuring apparatus, image data captured by a plurality of imaging devices is input, and coordinate values of a plurality of points corresponding to each pattern in the monitor coordinate system in the imaging device are obtained by image processing of the image data. A curved surface polynomial that approximates each curved surface formed by a plurality of points in the monitor coordinate system corresponding to the plate at each separated position is calculated based on the calculated coordinate values. Then, each coefficient of the curved surface polynomial corresponding to each separated position is stored, and the coordinate value in the monitor coordinate system of the measurement point is calculated at the actual position measurement. The world coordinate position of the measurement point is calculated by interpolation based on the positional relationship between the coordinate value and the curved surface on both sides of the measurement point 7 in the monitor coordinate system among the plurality of curved surfaces by the stored coefficients. measure. This provides a three-dimensional position measuring apparatus and a measuring method using a stereo method capable of highly accurate and simple three-dimensional position measurement.
JP-A-11-160021 JP-A-10-221066

  However, all the techniques of the conventional three-dimensional position measurement apparatus perform three-dimensional position measurement using an image obtained from a television camera and perform calibration, and can be applied to an ultrasonic three-dimensional position measurement apparatus. It was not a thing.

  Accordingly, an object of the present invention is to provide a three-dimensional position calibration method and a calibration apparatus for obtaining three-dimensional positions of a plurality of receivers that are used as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus. There is. In addition, the ultrasonic three-dimensional position measuring device can be made portable so that it can be used even in a wider variety of environments, and can be easily calibrated so that the three-dimensional position measuring device can be used quickly. Another object of the present invention is to provide a three-dimensional position calibration method and calibration apparatus.

  In order to achieve the above object, the three-dimensional position calibration method according to the first aspect of the present invention includes a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measuring apparatus. A three-dimensional position calibration method for obtaining a three-dimensional position, in which a plurality of calibration devices mounted with a plurality of ultrasonic transmitters in a predetermined positional relationship are moved to a plurality of points attached to the calibration device. A first step of reading out data obtained by calculating a three-dimensional position of each of a plurality of receivers at each point by an ultrasonic signal transmitted from the ultrasonic transmitter; and a three-dimensional position at each of the plurality of points A second step of selecting a receiver that can calculate a three-dimensional position from a plurality of points among receivers that can calculate A third step of creating a coordinate transformation matrix by obtaining the three-dimensional position of the calibration device by the optimization method using a specific point as the world coordinates based on the three-dimensional position coordinates, and using the obtained coordinate transformation matrix, And a fourth step of converting and integrating the three-dimensional position of the receiver obtained from the points into the world coordinate system.

  The calibration apparatus according to the second aspect of the present invention is a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers that are used as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus. The calibration apparatus used in the above-mentioned is characterized in that it comprises at least three or more ultrasonic transmitters mounted on a substrate in a predetermined positional relationship.

  The calibration device according to the third aspect of the present invention is a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers that are used as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement device. The calibration device used in the above-mentioned is provided with at least three ultrasonic transmitters mounted in a predetermined positional relationship at each end and center position of a tripod-shaped position fixture.

  Further, the three-dimensional position calibration method according to the fourth aspect of the present invention obtains the three-dimensional positions of a plurality of ultrasonic receivers serving as a reference when obtaining the three-dimensional position in the ultrasonic three-dimensional position measuring apparatus. A method for calibrating a three-dimensional position, wherein a first step of measuring three-dimensional positions of three or more ultrasonic receivers and storing the measurement data, and ultrasonic reception in which the measured three-dimensional positions are known The second step of calculating the three-dimensional position of the ultrasonic receiver at each point of one ultrasonic transmitter moved using the transmitter, and the ultrasonic wave according to the calculated three-dimensional position of the ultrasonic transmitter. And a third step of calculating the three-dimensional position of the receiver.

  In addition to the above configuration, the calibration apparatus according to the fifth aspect of the present invention may further include a stereo camera mounted on the substrate. The ultrasonic three-dimensional position measuring apparatus is a calibration apparatus used in a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers that are used as a reference when obtaining a three-dimensional position. It is good also as a structure which equips the stereo camera comprised with a camera with the at least 3 or more ultrasonic transmitter mounted by the predetermined positional relationship.

  According to the three-dimensional position calibration method of the present invention, first, in the first step, the calibration device at a plurality of points where a calibration device having a plurality of ultrasonic transmitters mounted in a predetermined positional relationship is moved. Read out the calculated data of the three-dimensional position of each of the plurality of receivers at each point by the ultrasonic signals transmitted from the plurality of ultrasonic transmitters attached to, and in the second step, A receiver that can calculate a three-dimensional position from a plurality of points is selected from the receivers that can calculate a three-dimensional position at each of a plurality of points. Then, in the next third step, the coordinate transformation matrix is generated by obtaining the three-dimensional position of the calibration device by the optimization method using the specific point as the world coordinate based on the three-dimensional position coordinate of the selected receiver. In the next fourth step, the position of the receiver obtained at each point is transformed into the world coordinate system using the obtained coordinate transformation matrix and integrated. The processing of each step here is executed by computer program processing.

  According to the three-dimensional position calibration method of the present invention, first, in the first step, the three-dimensional positions of three or more ultrasonic receivers are measured, the data is stored, and in the second step One ultrasonic transmitter is moved, and three or more three-dimensional positions that have already been measured based on the ultrasonic transmitter at each moved point are measured by the ultrasonic receivers 3 of the ultrasonic transmitters. The dimension position is calculated and measured. Further, in the third step, the position of the ultrasonic receiver is calculated and measured from the three-dimensional position of the ultrasonic transmitter. The processing of each step here is executed by computer program processing.

  By these processes, it is possible to obtain the three-dimensional positions of a plurality of receivers that are used as a reference when obtaining the three-dimensional position in the ultrasonic three-dimensional position measuring apparatus. In addition, since the three-dimensional position of the ultrasonic receiver attached at an arbitrary position on the boundary such as a wall or ceiling can be accurately measured, calibration for the reference position can be easily performed.

  The calibration device according to the present invention is configured to include a stereo camera mounted on a substrate. According to the calibration device having such a configuration, when performing a three-dimensional position calibration method for obtaining the three-dimensional positions of a plurality of receivers serving as a reference when obtaining the three-dimensional position in the ultrasonic three-dimensional position measurement device, In this case, the three-dimensional position of the stereo camera mounted on the calibration device is known, and the position of the object in the three-dimensional space can be measured by performing information processing on the stereo-viewed image by the stereo camera. Become.

The same applies to the case where the calibration device is configured to include at least three ultrasonic transmitters mounted in a predetermined positional relationship on a stereo camera including two cameras. In this case, when the calibration is performed, the three-dimensional position of the stereo camera is known. For example, the position of the object in the three-dimensional space can be measured by performing information processing on the image viewed in stereo by the stereo camera. .

  Hereinafter, an embodiment for carrying out the present invention will be described with reference to the drawings. FIG. 1 is a diagram showing the configuration of a system that implements the calibration method of the present invention in one mode. The system shown in FIG. 1 constitutes an ultrasonic three-dimensional position measuring device (ultrasonic three-dimensional tag system). This ultrasonic three-dimensional position measuring device is a system for using a small-sized ultrasonic transmission device as a three-dimensional tag, and is transmitted from one ultrasonic transmission device (10) as a three-dimensional tag. In this system, ultrasonic pulses are received together by at least three ultrasonic receivers (20) to determine the three-dimensional position of the ultrasonic transmitter (ultrasonic three-dimensional tag). In this case, in order for this system to correctly determine the three-dimensional position of the ultrasonic three-dimensional tag, the three-dimensional positions (three-dimensional position coordinates) of at least three receiving devices (20) have not been correctly determined. Must not.

  As shown in FIG. 1, an ultrasonic three-dimensional tag system includes an ultrasonic transmitter (ultrasonic three-dimensional tag) 10 that transmits an ultrasonic signal at a predetermined timing, an ultrasonic receiver that receives the ultrasonic signal, and amplification. From the ultrasonic receiver 20 comprising the device, the distance measuring device 34 for measuring the distance from the phase difference of the received ultrasonic signal, the wireless ultrasonic wave transmission control device 33 for controlling the timing of transmitting the ultrasonic signal, and the distance measuring device The system includes a data collection device 32 that collects the obtained data and a data processing device (personal computer: PC) 31 that performs data processing for three-dimensional calibration.

  The calibration device 11 here is configured such that at least three ultrasonic transmitters 10 have their relative positional relationships fixed. Each ultrasonic transmission device 10 constituting the calibration device 11 transmits an ultrasonic pulse.

  The ultrasonic pulse transmitted from the ultrasonic transmitter 10 is received by the ultrasonic receiver 20. The ultrasonic receiver 20 includes an ultrasonic receiver unit and an amplifying device. The ultrasonic receiver 20 receives and amplifies the ultrasonic pulse, and supplies a reception signal to the distance measuring device 34. The distance measuring device 34 measures the time until the ultrasonic pulse emitted from the ultrasonic transmitter 10 reaches the ultrasonic receiver 20 and measures the distance therebetween. By receiving ultrasonic pulses emitted from the same ultrasonic transmitter 10 by different ultrasonic receivers 20, the three-dimensional position of the ultrasonic transmitter 10 is obtained.

  In the ultrasonic receiver 20, in order to distinguish the plural ultrasonic transmitters 10 from each other and receive the ultrasonic pulses from the respective ultrasonic transmitters 10, the transmission of the ultrasonic pulses of the respective ultrasonic transmitters 10. Are controlled individually.

  Therefore, the wireless ultrasonic transmission control device 33 is provided, and the wireless ultrasonic transmission control device 33 sends a synchronization signal and an identification code ID to the high frequency signal (VHF) for each of the plurality of ultrasonic transmitters 10. : 314.9 MHz), and the transmission timing of the ultrasonic pulse of each ultrasonic transmitter 10 is individually controlled. The wireless ultrasonic transmission control device 33 transmits the identification code ID to the ultrasonic transmitter 10 wirelessly and transmits a synchronization signal to the entire system.

  The data collection device 32 collects distance data corresponding to the time measured by the distance measurement device 34 and transmits it to a data processing device (personal computer: PC) 31. The data processing device 31 calculates a three-dimensional position based on the distance between the ultrasonic transmitter 10 and the ultrasonic receiver 20. Further, as will be described later, the three-dimensional positions of a plurality of receivers 20 serving as a reference when obtaining the three-dimensional position in the ultrasonic three-dimensional position measuring apparatus are obtained and the calibration process is executed. Thereby, since the three-dimensional position of the ultrasonic receiver 20 attached to an arbitrary position on the boundary can be accurately measured, calibration for the reference position can be easily performed.

  FIG. 2 is a flowchart showing a processing flow of the first calibration processing executed in the data processing device of the three-dimensional position measuring device. When starting the processing here, as a pre-processing, as will be described later, the calibration device 11 in which a plurality of ultrasonic transmitters 10 are mounted in a predetermined positional relationship is set at points A, B, C, D, Are respectively received by a plurality of ultrasonic receivers 20 to be calibrated, and at a plurality of points A, B, C, D,. Ten three-dimensional positions are measured in advance. The measured data is stored separately for each point. And the calibration process here is started. Hereinafter, the ultrasonic receiver 20 and the ultrasonic transmitter 10 will be simply referred to as the receiver 20 and the transmitter 10, respectively.

  When the process is started, in the three-dimensional position calibration process, first, in step 101, the receivers at the respective points of the plurality of receivers 20 of the calibration apparatus 11 moved to the respective points A, B, C,. Data for which 20 three-dimensional positions are calculated is read. Next, in step 102, a receiver 20 that can calculate a three-dimensional position from a plurality of points is selected from among the receivers 20 that can calculate a three-dimensional position at each of the plurality of points. Then, in step 103, a coordinate transformation matrix is created by obtaining the three-dimensional position of the calibration device 11 by the least square method based on the three-dimensional position coordinates of the selected receiver 20 as a world coordinate, In step 104, the three-dimensional position of the receiver 20 obtained at each point is converted into a world coordinate system using the obtained coordinate transformation matrix and integrated. Thus, the three-dimensional position of each receiver 20 to be calibrated can be accurately obtained and calibrated.

  FIG. 3 is a flowchart showing a processing flow of the second calibration processing executed in the data processing device of the three-dimensional position measuring device. In the calibration process here, the calibration process can be executed by one transmitter. When starting the process here, as a pre-process, it is necessary to measure the three-dimensional positions of at least three ultrasonic receivers, as will be described later. Next, one transmitter is moved, and the three-dimensional position of the moved one transmitter can be measured by a receiver whose three-dimensional position is known at each moved point. Since the transmitter moves at each point, there are a plurality of transmitters whose three-dimensional positions are measured, and by using them, the three-dimensional position of the receiver can be accurately obtained and calibrated. .

  When the processing is started, in step 201, the three-dimensional positions of three or more ultrasonic receivers are measured, the measurement data is stored, and in step 202, one ultrasonic transmitter is moved and moved. The three-dimensional position of the ultrasonic transmitter is calculated by an ultrasonic receiver having three or more three-dimensional positions already measured by the ultrasonic transmitter at each point. In step 203, the three-dimensional position of the ultrasonic receiver is calculated using the measurement data obtained by the ultrasonic transmitter from which the three-dimensional position has been obtained. The processing of each step here is executed by computer program processing.

  FIG. 4 is a diagram for explaining the principle of the calibration method according to the present invention. The calibration here refers to obtaining the three-dimensional position of the receiver 20 as a reference when obtaining the three-dimensional position in the ultrasonic three-dimensional position measuring apparatus. As shown in FIG. 4, the ultrasonic wave is solved by solving equations relating to the positions of the receiver 20 and the transmitter 10 from the three-dimensional positions of a plurality of known ultrasonic receivers 20 (hatched circles at the top of the figure). The measurement is to obtain the three-dimensional position of the transmitter (circle in the lower part of the figure) 10. On the contrary, ultrasonic waves are obtained from the three-dimensional positions of a plurality of known ultrasonic transmitters (circles in the lower part of the figure) 10. Calibration is to determine the three-dimensional position of the receiver 20 (hatched circle at the top of the figure), and both are essentially the same problem.

  FIG. 5 is a diagram for explaining an example of the configuration of the calibration apparatus according to the present invention. As shown in FIG. 5, the calibration device 11 used when performing the three-dimensional position calibration here includes, for example, at least three or more transmitters 10 mounted in a predetermined positional relationship on a rectangular substrate. It has become a thing. A plurality of transmitters (ultrasonic transmitters) 10 provided on the substrate with respect to the receiver (ultrasonic receiver) 20 are fixed in relative positional relationship, and are on the same plane. Is provided.

  FIG. 6 is a diagram for explaining another configuration example of the calibration device according to the present invention. The calibration device 11 here may be configured such that the plurality of transmitters 10 provided therein are fixed in a three-dimensional positional relationship, for example, as long as the relative positional relationship is fixed. For example, as shown in FIG. 6, the calibration device 12 includes at least three transmitters 10 mounted in a predetermined positional relationship at each end and center position of a tripod-shaped position fixture. May be.

  Next, a calibration method using these calibration apparatuses 11 will be specifically described. First, the calibration method will be specifically described in the case where the arrangement of the transmitters 10 of the calibration device 11 is the same plane and the number of the transmitters 10 is three or more.

FIG. 7 is a diagram for specifically explaining the calibration method when the arrangement of the transmitters 10 of the calibration apparatus 11 is the same plane and the number of the transmitters 10 is three or more. As described above, the procedure for calibrating the three-dimensional positions of the plurality of receivers 20 is performed according to each procedure of procedure 1, procedure 2, procedure 3, and procedure 4 as follows.
Procedure 1: Ultrasonic waves transmitted from the transmitter 10 using a plurality of transmitters 10 attached to the calibration device 11 at, for example, three points of points A, B, and C appropriately moved. The three-dimensional position of each receiver 20 is calculated from the signal.
Procedure 2: The receiver 20 in the region 22 in which the three-dimensional position can be calculated from a plurality of points is selected from the receivers 20 in which the three-dimensional position can be calculated at each point.
Procedure 3: Based on the three-dimensional coordinates of the receiver 20 in the selected region 22, the three-dimensional position (coordinate transformation matrix) of the calibration device 11 is obtained by the least square method with the point A as the world coordinates.
Procedure 4: Using the obtained coordinate transformation matrix, the three-dimensional position of the receiver 20 obtained at each point is transformed into the world coordinate system and integrated.

  Thereby, the calibration device 11 can calibrate the three-dimensional position of the receiver 20 located in the region 22. Further, for the receiver 20 located in the region 21 here, the three-dimensional position can be calculated from a plurality of points by shifting the positions of the points A, B, and C of the calibration device 11. These receivers 20 can also be calibrated by making the area suitable.

Next, a method for determining the position of the receiver 20 using the distance data measured at a certain point (procedure 1) will be described. The ultrasonic receiver 20 is in a state where i of the transmitters 10 are on a plane (FIG. 5) and the positions (x ₁ , y ₁ , z ₁ ) to (x _i , y _i , z _i ) are known. To explain the basis of the principle of measuring the position (x, y, z), first, it is necessary to select three transmitters 10 not on a straight line from the i transmitters 10. These positions are defined as (x ₁ , y ₁ , z ₁ ), (x ₂ , y ₂ , z ₂ ), (x ₃ , y ₃ , z ₃ ). Assuming that the distances between the three transmitters 10 and the receiver 20 are l ₁ , l ₂ , and l _3, as shown in FIG. Then, there are three surfaces connecting the intersections of the spheres. The intersection of the three surfaces is determined as one, and the position (x, y, z) of the receiver 20 is obtained. However, it cannot be obtained when the three transmitters 10 are in a straight line. The equations for each sphere are as shown in equations (1) to (3).
The equation of the surface including the intersection line of the spheres by the equations (1) and (2) becomes the equation (4),
Similarly, equations (1), (3), and (2) and (3) also derive the following equations (5) and (6) respectively for the surface.
When the equations (4), (5), and (6) are represented by a matrix, the following equation (7) is obtained.
By solving the equation (7), the three-dimensional position (x, y, z) can be obtained.

Next, a method of obtaining the position of the receiver 20 using the least square method based on the distance information of three or more transmitters 10 and the receiver 20 will be described by taking the case of nine transmitters as an example.
From the equations (8) and (9), a plane equation including the intersection line of the spheres is obtained as follows.
Therefore, if i and j are appropriately selected, simultaneous equations such as the following equations (11) to (14) are obtained.
However, since the receivers (FIG. 5) 20 have the same height in the z-axis, z ₂ −z ₁ = 0, z ₃ −z ₁ = 0, z ₄ −z ₁ = 0,. Does not have Therefore, simultaneous equations such as the following equations (15) to (18) are obtained.
P ′ is obtained by the least square method. Specifically:

In general, there is no inverse matrix when it is not an n-order square matrix. Therefore, Equation (15) cannot obtain P ′ by applying an inverse matrix of A ′ from the left side of both sides. Therefore, by multiplying both sides of equation (15) by A ′ transpose matrix A ′ ^T from the left side, and further multiplying by (A ′ ^T A ′) ⁻¹ from the left side of both sides, P ′ can be obtained as follows. it can.
Thereby, since the value of the position (x, y) of the transmitter 10 is obtained, the value of (x, y) is substituted into the equation (8). Considering that z at the position of the transmitter 10 is positive from the calibration device 11, nine values of z are obtained. Let this average value be z of the transmitter 10. Thus, the position (x, y, z) of the transmitter 10 is obtained.

  Next, the position of the calibration device 11 is obtained based on the position of the receiver 20 that can be measured simultaneously from two points (procedure 3). In this case, as shown in FIG. 9, calibration is performed using nine transmitters 10 and 27 receivers 20 of the calibration device 11.

  FIG. 9 is a diagram for specifically explaining calibration performed using the nine transmitters 10 and the twenty-seven receivers 20 of the calibration apparatus 11. First, the three-dimensional position of each receiver 20 is measured using the nine transmitters 10 of the calibration device 11 at the point A. Depending on the orientation of the transmitter 10, there may be cases where only fewer than 27 receivers 20 can be measured.

The measured position of the receiver 20 is (x _a1 , y _a1 , z _a1 ), (x _a2 , y _a2 , z _a2 ),..., (X _a18 , _{ya 18} , z _a18 ), Position (x _A1 , y _A1 , z _A1 ), (x _A2 , y _A2 , z _A2 ) of the transmitter 10 in local coordinates (coordinate system based on the calibration device that is initially placed),. (X _A9 , y _A9 , z _A9 ) are known, and in the world coordinates (X _A1 , Y _A1 , Z _A1 ), (X _A2 , Y _A2 , Z _A2 ),..., (X _A9 , Y _A9 , Z _A9 ). Next, the transmitter 10 is moved to the point B, and the three-dimensional position of the receiver 20 is measured. Similarly, the position of the receiver 20 in this case _{(x b1, y b1, z} b1), (x b2, y b2, z b2), ······, (x b18, y b18, z b18 ), The position of the transmitter 10 in the local coordinates is (x _B1 , y _B1 , z _B1 ), (x _B2 , y _B2 , z _B2 ),..., (X _B9 , y _B9 , z _B9 ) And (X _B1 , Y _B1 , Z _B1 ), (X _B2 , Y _B2 , Z _B2 ),..., (X _B9 , Y _B9 , Z _B9 ) in the world coordinates.

As can be seen from FIG. 9, when the three-dimensional position of the receiver 20 is measured at the point A and when the three-dimensional position of the receiver 20 is measured at the point B, the overlapping portion, that is, the point A and the point B There is a receiver 23 that can measure a three-dimensional position in both. The positions of the receivers 20 in this part are (x _a9 , y _a9 , z _a9 ), (x _a10 , _{ya 10} , z _a10 ),..., (X _a18 , _{ya 18} , z _a18 ) and _{(x b1, y b1, z} b1), (x b2, y b2, z b2), ······, (x b9, y b9, z b9) as the rotation and the role of translation here When the transformation matrix M of 4 × 4 matrix having the following is considered, it can be expressed as the following equations (20) to (23).

M _BtoA is obtained by the method of least squares. Expression (20) is obtained by multiplying both sides by the P _B transposed matrix P _B ^T from the right side, and further multiplying (P _B P _B ^T ) ⁻¹ from the right side of both sides, _{thereby obtaining} M _BtoA as in the following Expression (24). Can be requested.
When the _MBerA of only rotation / translation is obtained using the Nelder-Mead simplex method, there is a case where enlargement / reduction is lost and distortion is lost.
By _obtaining this M _BtoA, the coordinates (x ′ _B1 , y ′ _B1 , z ′ _B1 ) of the transmitter 10 at the point B in the local coordinates at the point A are obtained as shown in the following equation (25). (X ′ _B2 , y ′ _B2 , z ′ _B2 ),..., (X ′ _B18 , y ′ _B18 , z ′ _B18 ) are obtained.
Further, the position of the receiver 20 in the local coordinate system B can be converted into the world coordinate system (local coordinate system A) using _MBToA .

Similarly, M _BtoA , M _CtoB , M _DtoC, etc. are obtained,
By calculating the equations (26) to (27), the position of the receiver 20 can be expressed using the local coordinate system A as the world coordinates.

  Next, a calibration method in the case where the number of transmitters 10 in which the array of transmitters 10 of the calibration apparatus 12 is not on the same plane is four or more will be described. As described above, the same procedure as the procedure (procedure 1, procedure 2, procedure 3, procedure 4) for calibrating the three-dimensional positions of the plurality of receivers 20 is performed.

  FIG. 10 is a diagram for specifically explaining a calibration method when the number of transmitters 10 in which the arrangement of the transmitters 10 of the calibration device 12 is not in the same plane is four or more. As described above, the procedure for calibrating the three-dimensional positions of the plurality of receivers 20 is performed according to each procedure of Procedure 1, Procedure 2, Procedure 3, and Procedure 4. As the calibration device 12, for example, four or more transmitters 10 that are not on the same plane as shown in FIG. 6 are used for calibration. The calibration itself is the same as described above.

FIG. 11 is a diagram for explaining a calibration method in the case where calibration is performed by freely moving one transmitter 10. Next, a case where calibration is performed by moving freely using only one transmitter 10 will be described. In this case, first, the lengths of the three sides of the three receivers 24 arbitrarily selected manually are measured, and the positions of the receivers 24 are determined. Using the three receivers 24 whose positions are known, the three-dimensional position of one moved transmitter 10 is obtained, and using the transmitter 10 where the position of each obtained point is known, The positions of a plurality of other receivers 20 are determined. In this case, the procedure for calibrating the three-dimensional positions of a plurality of receivers is performed according to each procedure of procedure 1, procedure 2, and procedure 3 as follows.
Procedure 1: The lengths of the three sides of the three receivers 24 are measured, and the three-dimensional positions of the three receivers (upper part in the figure) 24 are obtained.
Procedure 2: Move one transmitter (lower part in the figure) 10 to move the transmitter (lower part in the figure) 10 moving from three receivers (upper part in the figure) 24 whose positions are known. A three-dimensional position is calculated.
Procedure 3: Select four or more moving transmitters 10 (lower part in the figure), and calculate the three-dimensional position of the receiver 20 in the same manner as described above.

  When it is known that the receivers 20 are in the same plane, the calibration method is performed as described above, but the calibration is performed taking into consideration that all the receivers 20 are on the plane. FIGS. 12 and 13 are diagrams for explaining the principle of the calibration method for obtaining the position of the plane-constrained receiver 20 when there is one transmitter and when there are four or more transmitters.

In FIG. 12, P _i is, for example, the coordinates of the receiver 20 in local coordinates of the point A, M is the transformation matrix, Pr _i are coordinates at point B after the conversion. Pt _j is the coordinate of the transmitter 10. L _ij is the distance between the transmitter 10 and the receiver 20. here
At a, E is determined using the Nelder-Mead simplex method Pr _i that minimizes.

In Figure 13, Pr _i are the coordinates of the receiver 20, Pt _j are coordinates of the transmitter 10. P0 is a point on the plane, and n is a normal vector of the plane. First, the position of the receiver 20 is measured and the plane is considered. for that reason,
A normal vector n is obtained by performing singular value decomposition on the equation (30). further,
From this equation (31), P0 is obtained by the method of least squares. Thereby, a plane equation can be obtained. Further, the distance d _ij of the transmitter 10 from the plane is obtained, and the distance L _ij between the receiver 20 and the transmitter 10 is measured, so that r _ij is obtained. Then, the intersection P _i = (x _i , y _i , z _i ) between the perpendicular line drawn from the transmitter 10 to the plane and the plane can also be obtained. Here, consider a sphere of radius r _i centered on P _i . This sphere can be considered as many as the number of transmitters 10, and it is considered that there is a receiver 20 at the intersection. If the position of the receiver 20 is (x, y, z), the sphere can be expressed by equations (32) to (34).
The formula of these sphere planes is a matrix and can be expressed as the following formulas (35) to (38).
Then, the position (x, y, z) of the receiver 20 is obtained by the method of least squares.

Next, the accuracy of calibration by the calibration method according to the present invention will be verified. By performing calibration, a receiver is measured, e ₁ the distance of the actual _receiver, e 2, _..., when the e _n, the following equation (39), a value for evaluating the error .

  14 and 15 are graphs showing the calibration results when the number of transmitters whose transmitters are not on the same plane is four or more. As shown in FIG. 14, when the number of transmitters whose transmitters are not on the same plane is four or more and the plane is not constrained, the error E according to the equation (39) is E = 45 to 70 mm. As shown in FIG. 15, when the plane constraint is performed, the equation (39) is used. When the plane is freely moved, when the plane constraint is not performed, the equation (39) is used as shown in FIG. The error E was E = 30-60 mm.

  Further, as the calibration device 12 as described with reference to FIG. 10, the calibration method described above is performed by using the calibration device 12 to which four ultrasonic transmitters in which the respective ultrasonic receivers are arranged at intervals of 250 mm are attached. Applied, the three-dimensional position of 80 receivers was determined. In this case, the calibration device 12 moved three times and measured at a total of four locations including the initial position. The result is shown in FIG.

  FIG. 17 is a graph showing the results of calibration experiments using four ultrasonic transmitters whose transmitters are arranged in a tripod shape. Circles in the figure indicate the results calculated based on the measurement of each position (point A, point B, point C, point D) of the calibration apparatus. From the figure, it is confirmed that the position of the receiver and the position of the calibration device after movement (point B, point C, point D) are obtained using the position of the first calibration device (point A) as the world coordinates. it can. The error was 8 cm or less.

  FIG. 18 is a diagram for explaining still another example of the configuration of the calibration apparatus according to the present invention. As shown in FIG. 18, the calibration device 13 here includes at least three transmitters 10 mounted on the substrate in a predetermined positional relationship. On the other hand, cameras 41 and 42 for configuring a stereo camera on a substrate in a predetermined positional relationship are provided. By using the cameras 41 and 42 in the calibration device 13 as a stereo camera, it is possible to measure the three-dimensional position of an object in a three-dimensional space by stereo vision.

  As described above, the calibration device 13 includes, for example, at least three transmitters 10 mounted in a predetermined positional relationship on a rectangular substrate, and calibration is performed using these transmitters 10. Perform the action. As a result, the plurality of transmitters 10 provided on the substrate of the calibration device 13 are determined as the three-dimensional position of the world coordinate system by performing the above-described calibration. As a result, the stereo camera mounted on the substrate of the calibration device 13 has a known three-dimensional position, and the position of the object in the three-dimensional space can be measured using the stereo camera whose installation position is known. .

  Using a stereo camera system, for example, by specifying interactively the feature points of an object in a stereo image with a mouse, the three-dimensional shape that is an important property of the object can be modeled into a simple shape. By doing so, "physical structure modeling" can be realized.

  In this case, when creating a three-dimensional model of an object using a stereo camera system, the object must be in the field of vision of both eyes, so it is very difficult to model a large room. However, by combining an ultrasonic three-dimensional position measurement device and a stereo camera, even if the stereo camera is moved to create a three-dimensional model, the model can be created on one coordinate system.

  Using the calibration apparatus shown in FIG. 18, that is, an ultrasonic three-dimensional position measuring apparatus using a two-lens stereo camera and six ultrasonic transmitters, a system is configured to measure the position of an object in a three-dimensional space. The method will be described.

The procedure is performed by the following procedure a, procedure b, procedure c, and procedure d.
Step a: Stereo camera calibration,
Step b: Register the three-dimensional positions of at least three ultrasonic transmitters attached to the stereo camera at the calibrated positions.
Procedure c: When the calibration device 13 is moved, each three-dimensional position of the ultrasonic transmitter 10 at that position is acquired, and the movement from the previous position to the current position is performed in the form of a rotation matrix and a translation vector. calculate.
Step d: Unify the three-dimensional model into one coordinate system using the obtained rotation matrix and translation vector.

FIG. 19 is a diagram for explaining transformation of a coordinate system by a stereo camera that has been calibrated for three-dimensional position measurement by an ultrasonic three-dimensional position measurement apparatus. A specific conversion procedure will be described below with reference to FIG. In the following description, the symbols are those shown in FIG. A procedure for converting from the camera coordinate system (camera 2) placed at the point 2 to the camera coordinate system (camera 1) placed at the point 1 will be described. It is as follows when it writes with a formula.
P _c1 = M _c1c2 · P _c2
Also,
M _c1c2 = M ⁻¹ _c2u2 · M _u2u1 · M _c1u1
There is a relationship. First, a coordinate system is created with at least three ultrasonic transmitters on the calibration device (ultrasonic1, ultrasonic2). Since the relationship between this coordinate system and the coordinate system of the stereo camera does not change after calibration,
Write and _{M c1u1 = M c2u2 = M cu} , put the _{M cu.} Also,
This M _cu can be written as M _cu = M _wu1 · M _c1w = M _wu2 · M _c2w . Since M _cu does not change after calibration unless the relative position of the stereo camera and the ultrasonic transmitter in the apparatus 13 is changed, once M _cu is obtained, it can be changed again after changing the environment. It is possible to use M _cu .

Next, the matrix to convert from ultrasonic2 to ultrasonic1 can be written as:
M _u2u1 = M _wu1 · M ⁻¹ _wu2

With the above,

P _c1 = M _c1c2 · P _c2
= M ⁻¹ _c2u2 · M _u2u1 · M _c1u1 · P _c2
= (M ⁻¹ _cu · M _wu1 · M ⁻¹ _wu2 · M _cu ) · P _c2
It becomes.

  In this way, a system configuration using a calibration device equipped with a stereo camera is used, and by specifying interactively the characteristic corresponding points of the object in the left and right stereo images with the mouse, it is easy to align linear, polygonal, A simple three-dimensional shape such as a circle can be modeled. Also, the physical structure modeling can be realized by registering the number and identification information of the ultrasonic transmitters (ultrasonic three-dimensional tag) attached to the object in association with the model.

  A physical function of an object can be modeled by providing a virtual sensor / effector serving as a sensor function for determining the position of a three-dimensional space in a physical structure model created by modeling a physical phenomenon. Here, the virtual sensor / effector is for giving an attribute that can behave as a sensor on a computer. In the three-dimensional space expressed on the computer, the physical structure model to which the virtual sensor is attached is selected with the mouse, and the virtual sensor to be attached is selected, so that the attachment work can be performed interactively on the computer.

  The types of virtual sensors / effectors include, for example, “Angle Sensor” that detects rotation, “Bar Effector” that causes a touch event, and “Touch Sensor” that detects a touch event. By using these virtual sensors / effectors as sensors and writing the behavior semantic information corresponding to the phenomenon in the physical phenomenon model combination table created here, the physical phenomenon model output is associated with the action event. I do. For example, if the desk and the cup are in contact, write “put a cup on the desk” or the like, and if the notebook and the pen are in contact, write “write something” or the like.

  Moreover, action recognition can be performed by using the three-dimensional position measurement system here and the sensored object. For example, the position information obtained from the 3D position measurement system is input to the physical structure model / physical phenomenon model created as described above, and the correlation is performed based on the output of the physical phenomenon model obtained as a result. By referring to the action event, detection of human action can be extracted as semantic information.

  In order to verify the effectiveness of this method, we conducted a verification experiment using the constructed system in a sensor room simulating daily living space. The objects that were modeled during the verification experiment were four points: desk, cup, tissue box, and stapler. Ultrasonic three-dimensional tags (ultrasonic transmitters) were attached to three points other than the desk, which are likely to move when used by humans. A four-point object was modeled by a system using a calibration device into a simple physical structure in which a desk and a tissue box are square, a cup is circular, and a stapler is linear.

  Next, a physical phenomenon model is created by attaching virtual sensors and effectors such as “Touch Sensor” to the desk and “Angle Sensor” and “Bar Effector” to the cup, tissue box, and stapler. Was made into a sensor. Then, the output of the virtual sensor was associated with the action event. It was confirmed that an action event can be reliably detected by starting input of a three-dimensional coordinate value of an ultrasonic three-dimensional tag and lifting or rotating three points other than the desk. Through this verification experiment, we confirmed that the experimental system can realize a simple environmental modeling function and solve problems such as robustness, real-time performance, and ease of system construction.

  As described above, the calibration method according to the present invention makes the ultrasonic three-dimensional tag system portable and quickly constructs a system for observing human activities in various environments where human activities actually occur. Available.

  In addition, it can be used to quickly realize a behavior observation system by calibrating a three-dimensional position measurement apparatus at a site where human behavior occurs. Specifically, the behavior observation system includes 1) a function for modeling a physical structure of an object, 2) a function for modeling a physical phenomenon of an object, 3) a function for associating an output of a physical phenomenon model with an action event, 4 ) Consists of object recognition and position measurement functions. By integrating the functions 1) to 3), the object can be easily converted into a sensor so that the object can behave as a sensor for recognizing human behavior while maintaining the properties and functions of the object. Yes.

  Also, by integrating the function of 4) into the object sensor, a system that can obtain the output of the virtual sensor and observe / recognize human behavior is built using an ultrasonic 3D tag and a stereo camera. it can. The function of creating an abstracted physical structure model of an object can be realized by a calibration device that integrates an ultrasonic three-dimensional tag and a stereo camera. The physical phenomenon model creation function can be realized by attaching a virtual sensor / effector to the physical structure model. The action event detection function is performed by associating the output of the virtual sensor with a sentence representing the action. The object recognition / position measurement function allows an ultrasonic three-dimensional tag to have an ID. By linking the ID with the name of the object, the object recognition function is realized and the ID is obtained. By measuring the position of the ultrasonic three-dimensional tag, the position measuring function of the object can be realized. In this way, a simple environment modeling function and action event detection function can be realized.

It is a figure which shows the structure of the system which implements the calibration method of this invention with one aspect | mode. It is a flowchart which shows the process flow of the 1st calibration process in which a process is performed in the data processor of a three-dimensional position measuring device. It is a flowchart which shows the process flow of the 2nd calibration process in which a process is performed in the data processor of a three-dimensional position measuring device. It is a figure explaining the principle of the calibration method concerning this invention. It is a figure explaining an example of composition of a calibration device concerning the present invention. It is a figure explaining another structural example of the calibration apparatus concerning this invention. It is a figure explaining concretely the calibration method in case the arrangement | sequence of the transmitter of a calibration apparatus is the same plane and the number of transmitters is three or more. It is a figure explaining the principle which calculates | requires a three-dimensional position in the calibration method concerning this invention. It is a figure explaining concretely the calibration performed using nine transmitters and 27 receivers of a calibration apparatus. It is a figure explaining a calibration method concretely when the number of transmitters where the arrangement of the transmitters of the calibration apparatus is not on the same plane is four or more. It is a figure explaining the calibration method in the case of performing calibration by moving one transmitter freely. It is a figure explaining the principle of the calibration method which calculates | requires the position of the plane constrained receiver when there are four or more receivers. It is a figure explaining the principle of the calibration method which calculates | requires the position of the receiver restrained by plane when a some receiver is on a plane. It is the 1st figure which displayed the calibration result in the case of the number of the transmitters which the arrangement | sequence of a transmitter is not on the same plane is four or more on a graph. It is the 2nd figure which displayed in a graph the calibration result in case the number of transmitters which the arrangement | sequences of a transmitter are not on the same plane are four or more. It is the figure which displayed the result of having performed calibration by moving only one transmitter freely. It is the figure which displayed the experimental result of the calibration by the graph by the four ultrasonic transmitters in which the arrangement of the transmitters is a tripod. It is a figure explaining another example of a structure of the calibration apparatus concerning this invention. It is a figure explaining the conversion of the coordinate system by the stereo camera which calibrated the three-dimensional position measurement with the ultrasonic three-dimensional position measurement apparatus.

Claims (8)

A three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus,
A plurality of ultrasonic transmitters mounted on the calibration device at a plurality of points where a calibration device having a plurality of ultrasonic transmitters mounted in a predetermined positional relationship is moved at each point by ultrasonic signals transmitted from the plurality of ultrasonic transmitters attached to the calibration device. A first step of reading the calculated data of the three-dimensional position of each of the plurality of receivers;
A second step of selecting a receiver capable of calculating a three-dimensional position from a plurality of points among receivers capable of calculating a three-dimensional position at each of the plurality of points;
A third step of creating a coordinate transformation matrix by obtaining a three-dimensional position of the calibration device by an optimization method using a specific point as a world coordinate based on the three-dimensional position coordinate of the selected receiver;
A four-dimensional position calibration method comprising: a fourth step of converting and integrating the three-dimensional position of the receiver obtained at each point using the obtained coordinate transformation matrix into the world coordinate system. A calibration apparatus for use in a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus,
A calibration apparatus comprising at least three or more ultrasonic transmitters mounted on a substrate in a predetermined positional relationship. A calibration apparatus for use in a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus,
A calibration device comprising at least three or more ultrasonic transmitters mounted in a predetermined positional relationship at each end and center position of a tripod-shaped position fixture. A three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of ultrasonic receivers that are used as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measuring apparatus,
A first step of measuring a three-dimensional position of three or more ultrasonic receivers and storing the measurement data;
A second step of calculating a three-dimensional position of the ultrasonic receiver at each point of one ultrasonic transmitter moved using an ultrasonic receiver whose measured three-dimensional position is known;
And a third step of calculating a three-dimensional position of the ultrasonic receiver based on the calculated three-dimensional position of the ultrasonic transmitter. The calibration device according to claim 2, further comprising:
A calibration apparatus comprising a stereo camera mounted on the substrate. A calibration apparatus for use in a three-dimensional position calibration method for obtaining three-dimensional positions of a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measurement apparatus,
A calibration apparatus comprising at least three ultrasonic transmitters mounted in a predetermined positional relationship on a stereo camera composed of at least two cameras. A computer-readable program for performing information processing for obtaining three-dimensional positions of a plurality of receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measuring device,
A plurality of ultrasonic transmitters mounted on the calibration device at a plurality of points where a calibration device having a plurality of ultrasonic transmitters mounted in a predetermined positional relationship is moved at each point by ultrasonic signals transmitted from the plurality of ultrasonic transmitters attached to the calibration device. A first step of reading the calculated data of the three-dimensional position of each of the plurality of receivers;
A second step of selecting a receiver capable of calculating a three-dimensional position from a plurality of points among receivers capable of calculating a three-dimensional position at each of the plurality of points;
A third step of creating a coordinate transformation matrix by obtaining a three-dimensional position of the calibration device by an optimization method using a specific point as a world coordinate based on the three-dimensional position coordinate of the selected receiver;
A computer-readable program for executing, by a computer, processing with the fourth step of converting and integrating the three-dimensional position of the receiver obtained at each point using the obtained coordinate transformation matrix into the world coordinate system. A computer-readable program for performing information processing for obtaining a three-dimensional position of a plurality of ultrasonic receivers serving as a reference when obtaining a three-dimensional position in an ultrasonic three-dimensional position measuring device,
A first step of measuring a three-dimensional position of three or more ultrasonic receivers and storing the measurement data;
A second step of calculating a three-dimensional position of the ultrasonic receiver at each point of one ultrasonic transmitter moved using an ultrasonic receiver whose measured three-dimensional position is known;
A computer-readable program for executing, by a computer, processing with the third step of calculating the three-dimensional position of the ultrasonic receiver based on the calculated three-dimensional position of the ultrasonic transmitter.