JP2008286772A - Attitude recognition sensor - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To solve the following problem of an attitude sensor which used to be used in a joint section of a robot: since its measurable parameters are limited and other coordinate transformation parameters are ignored, an error at an end part becomes an impermissible value if the number of joint sections increases. <P>SOLUTION: This sensor is composed of a group of laser light sources fixed to one rigid body of a joint section to emit three or more beams of light linear and independent mutually, an area sensor such as a CCD fixed to the other rigid body to receive the beams of light, and an arithmetic unit for computing coordinate transformation parameters from the coordinates of light receiving points on the area sensor. Thereby, all coordinate transformation parameters can be measured by one sensor with high accuracy. High-accuracy parameter measurement becomes possible by providing a movable half mirror which rotates around two orthogonal axes in front of the area sensor, and measuring the rotation angle of the half mirror and variations of beams reflected by the half mirror from reference values. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a posture recognition sensor attached to a joint portion for determining the posture and position of a robot that is connected to bendable at the joint portion.

  One of the backgrounds requiring the present invention is to rescue victims left behind in an environment where rubble, burnt earth, liquid, gaseous or other mixture is present in disaster areas such as earthquakes. Relied on rescue efforts by rescue teams or olfaction of trained dogs and other subjects. Therefore, even if it was possible to determine whether there was a victim or not, it was difficult to specify the position of the victim, and it was difficult to grasp the situation of the victim.

  Cutting-edge excavator and various sensors such as cameras, odor sensors, infrared sensors, etc. to search for victims, confirm location, confirm status of victims, secure food and water supply channels, and ensure communication There is a need for a device that can be moved forward by attaching a thermometer and sewing through the gaps in rubble or in clay.

  And when a victim is found, in a tubular robot that has a space that can secure the lifeline connecting the victim and the rescue team, each joint has a small and high-precision posture sensor, A means for accurately grasping the position of the tip was required.

  Conventionally, a posture sensor embedded in a robot joint or the like is a sensor that measures a specific parameter in accordance with the degree of freedom of the boundary portion. For example, a sensor capable of measuring only a rotation angle around one axis or two axes, or a sensor capable of measuring only a deviation (length) in the axial direction has been used.

Or, in a snake-like multi-joint robot, some parameters of coordinate transformation cannot be measured, so an error occurs, and because of the multi-joint, errors accumulate and the required accuracy cannot be obtained overall, especially the position of the tip There was a problem that the error of became large.

  If a technical solution is planned for such a problem, it is expected that the present technology can be used in various fields other than the above-described rescue means. For example, there is a request to construct a route for delivering goods, information, etc. in a place where it is considered impossible for a human animal to dive, but in order to do so, it is necessary to pioneer a preliminary route that is to be used in advance. Since it will be relatively easy to switch to the construction of this route after that, it is necessary to establish a prior art for constructing a backup route.

Since conventional posture sensors used for joints have limited parameters that can be measured, errors have occurred due to ignoring other parameters as minute ones.
For this reason, when there are many joints, there is a problem in that errors accumulate and become unacceptable values at the end. In addition, in order to obtain all parameters necessary for coordinate conversion with high accuracy, it is necessary to provide a plurality of sensors, and there is a problem that the sensors are complicated and large.

  Furthermore, the data of a plurality of sensors was sent to a central processing unit, and conversion into world coordinates or conversion into local coordinates was performed. Therefore, there has been a problem that the calculation algorithm becomes complicated.

The present invention is intended to solve the problems of such a conventional configuration.
By measuring all six elements necessary for coordinate transformation with one sensor, the error due to coordinate transformation is reduced, and the algorithm for coordinate transformation is simplified, so that each sensor unit can be connected to the local coordinate system and the world coordinate system. Mutual conversion is possible. And each sensor part grasps | ascertains the position in a world coordinate, and aims at speeding-up as a system.

  The configuration and operation of the posture recognition sensor of the present invention for achieving the above object will be described.

  The first problem-solving means includes: a light projecting unit having a beam emitting unit such as at least three laser light sources fixed to one rigid body; and a light projecting unit fixed to one rigid body. A pair of sensors is constituted by a light receiving unit having a planar screen fixed to a rigid body and having a light receiving point recognizing means by a photoelectric conversion device including a CCD.

  The plurality of sets of sensors are connected to each other via the joint portion, and the posture recognition sensor is configured such that a light beam from the light projecting unit intersects with the light receiving unit and is on a screen of the light receiving unit. It is characterized by comprising means for reading out the coordinates of a plurality of light receiving points and means for acquiring coordinate conversion parameters.

  The second problem solving means is to fix a light projecting portion having two laser light sources, one beam splitter and one screen to one rigid body of the joint portion, one screen and one half mirror. The light receiving part having a fixed structure is fixed to the other joint part.

  In such a posture recognition sensor, a light receiving unit half mirror is arranged so as to be substantially perpendicular to the optical axis of the first laser light source of the light projecting unit, and a first screen is arranged in parallel with the rear surface of the half mirror. A beam splitter that transmits the beam light of the first laser light source and bends the reflected light from the half mirror in a direction of about 90 degrees, and a second screen that receives the reflected light from the beam splitter.

  And a second light source having an optical axis incident on the first screen at an angle of 10 degrees to 80 degrees with respect to a normal line of the surface of the first screen. Means for reading out the light receiving point position coordinates of the light beams from the first laser light source and the second laser light source on one screen and the light receiving point position coordinates from the first light source on the second screen; The coordinate conversion parameter is obtained from the coordinates of the above.

    According to a third problem solving means, in the second problem solving means, the half mirror of the light receiving unit is provided with a movable means by electrical control, and the beam light from the first laser light source is the half light. A means for detecting the position of the light receiving point reflected on the mirror and made on the second screen of the light projecting unit through the splitter and the tilt of the movable half mirror is provided. From the coordinates of the three light receiving points and the tilt of the half mirror The present invention is characterized in that coordinate conversion parameters are obtained.

  The operation of the first problem solving means is as follows.

In general, three straight lines that are not linearly defined in the coordinate system A intersect with a plane defined in another coordinate system A ′, and the coordinate system A ′ in which the coordinates of these three intersection points define the plane. If it is already known, the six parameters for coordinate transformation can be obtained.

とあらわすことが出来る。A coordinate system in which the three light sources are fixed is A, and the other is A ′.
That is, the three straight lines are fixed in the coordinate system A, and the equation of each straight line is

It can be expressed.

ここでＴｘｙｚは３行３列の行列で、ｘ軸、ｙ軸、ｚ軸まわりの回転角α、β、γの３変数の

These straight lines intersect with the screen fixed in the coordinate system A ′, and have means for reading the coordinates of the intersections. For convenience, if the screen surface fixed to the coordinate system A ′ is an x′-y ′ plane with z ′ = 0, the following equation is established using a general equation for coordinate transformation.

Here, Txyz is a matrix of 3 rows and 3 columns, and three variables of rotation angles α, β, and γ around the x axis, the y axis, and the z axis.

Therefore, by solving the above nine simultaneous equations, the six elements (α, β,

As described above, the light beam from the three light sources fixed in the coordinate system A of the light projecting unit reads the coordinates of the intersection formed on the screen fixed in the coordinate system A ′ of the light receiving unit, thereby converting the coordinates into the coordinate conversion parameters. All the rotation angles around the X axis, Y axis, and Z axis, and the three elements of the coordinate translation vector can be obtained with a very simple configuration. [0015]
The operation of the second problem solving means is as follows.

That is, the beam light from the first light source passes through the beam splitter and reaches the half mirror of the light receiving unit, and some light rays further pass through the half mirror and reach the first screen, and some light rays are reflected by the half mirror. .

At this time, when the surface of the half mirror is perpendicular to the input beam, the reflected beam passes through the same path as the input beam and is bent by approximately 90 degrees by the beam splitter to form a light receiving point on the second screen. With this light receiving point as a reference, if the adjacent coordinate system is tilted, the position of the light receiving point formed on the second screen by the reflected light from the first light source is shifted from the reference point. And the direction of tilt.

Further, the distance between the coordinate systems can be obtained in the manner of triangulation by the distance between the light receiving points on the first screen formed by the light beam of the first light source and the light beam of the second light source. Furthermore, the twist between coordinate systems can be obtained from the inclination of the line segment formed by the two light receiving points on the first screen formed by the first light source and the second light source.

  From these measurement results, it is possible to obtain the six elements of the rotation angle around the X axis, the Y axis, and the Z axis, and the coordinate translation vector, which are coordinate conversion parameters. Strictly speaking, six elements can be obtained by solving equations algebraically.

  The operation of the third problem solving means is as follows.

That is, the beam light from the first beam light source passes through the beam splitter and reaches the movable half mirror, a part of the light beam further passes through the movable half mirror and reaches the first screen, and a part of the light beam is reflected by the movable half mirror. The

  When the surface of the movable half mirror is perpendicular to the input beam, the reflected beam passes through the same path as the input beam and reaches the beam splitter, and the light path is bent to create a light receiving point on the second screen.

  The light projecting unit and the light receiving unit are fixed with a certain reference, and at this time, the position of the movable mirror is adjusted so that the surface of the movable half mirror and the input light from the first beam light source are substantially orthogonal to each other. The position of the light receiving point is used as a reference point. The reference point is set to be approximately in the center of the second screen.

  If the adjacent coordinate system A ′ is changed, the light beam from the first beam light source is reflected by the movable half mirror and the position of the light receiving point formed on the second screen is shifted from the reference point. The inclination of the light receiving point is controlled to return to the reference point, and the change in the inclination of the movable half mirror at this time is read out by an encoder or the like, so that two of the rotation angles around the three axes of the coordinate conversion parameters are obtained. Can be determined.

Further, the distance between the coordinate systems can be obtained from the distance between the light receiving points on the first screen formed by the first beam light source and the second beam light source and the inclination of the movable half mirror.

Furthermore, the twist between the coordinate systems can be obtained from the position of the line segment formed by the light receiving point on the first screen formed by the first beam light source and the second beam light source.
Therefore, from these measurement results, it is possible to obtain the six elements of the coordinate conversion parameters, that is, the rotation angles around the X, Y, and Z axes, and the coordinate translation vector.

  When the movable half mirror is driven by a digital driving device such as a step motor, the probability that the light receiving point on the second screen cannot be matched with the reference point increases. In this case, the rotation angle around the axis can be determined in an interpolation manner from the inclination of the half mirror and the magnitude and direction of the deviation of the light receiving point from the reference point.

  As described above, the posture recognition sensor of the present invention can measure six elements that are necessary and sufficient conditions for coordinate conversion with one small sensor, and can perform coordinate conversion with high accuracy.

  Furthermore, by providing a half mirror, it is possible to provide a sensor capable of measuring the rotation angle around the axis with higher accuracy.

  Furthermore, by making the half mirror movable, it is possible to provide a sensor capable of measuring a wider rotation angle with high accuracy.

  The leading rigid body is a bendable robot in which a plurality of subsequent rigid bodies are connected by joints, and a detecting means such as a voice, sound, color, odor sensor or the like is attached to the tip, or a direction detecting means is provided. It is configured to be installed and propelled. And since the attitude | position recognition sensor of this invention can be attached to each joint part of the robot and coordinate conversion can be performed one after another, it has the effect that a highly accurate position measurement can be performed.

  For example, a rigid body is connected and sent from a location starting from the current location, and when the tip reaches the target location, the position is grasped as accurate information via a plurality of posture recognition sensors. be able to.

  In addition, since it can be configured with high accuracy and a small size, for example, if each rigid body is a housing such as a tube that can secure a space inside, the posture recognition sensor of the present invention is housed for each rigid body, and the sensor It can be secured as a connection path for related equipment or as a path for required articles.

  Therefore, the posture recognition sensor of the present invention can be applied not only to the utility for searching in the disaster area described above but also to other wide industrial fields, and a large economic effect can be expected.

  As a specific example, in a robot that can bend by connecting a plurality of rigid bodies, the posture recognition sensor of the present invention is attached to the joint of the robot, and the six elements calculated by the arithmetic unit provided in each sensor are And sent to a central processing unit having a man-machine interface and arithmetic units provided in each other sensor via a communication network, and written and recorded in a storage circuit of each arithmetic unit.

  As a result, the calculation device of each sensor records the latest 6 elements of all the sensor units including the 6 elements collected by itself, and the position coordinates defined by an arbitrary coordinate system in the system are coordinated. It has the utility that it is possible to convert each joint part independently and in parallel by using a calculation circuit for position coordinates in another arbitrary coordinate system by sequentially applying the conversion formula.

  9 elements of a 3 × 3 coordinate conversion matrix used for coordinate conversion instead of the 6 elements and 3 elements of parallel movement are exchanged and stored between the sensors, thereby reducing the amount of calculation in the arithmetic circuit. In addition, it has many effects such as being capable of coordinate transformation at higher speed.

Hereinafter, embodiments of the present invention will be described with reference to FIGS. 3, 6, 7, and 8.

  In FIG. 6, the posture recognition sensor includes a sensor unit and a control / calculation unit 50, and the sensor unit is further divided into a light projecting unit 13 and a light receiving unit 14, and the light projecting unit 13 is adjacent to the joint unit 23. The light receiving unit 14 is fixed to the other rigid body 22.

  The light projecting unit 13 is configured by integrating the first laser light source 1, the beam splitter 5, the second CCD 3, and the second laser light source 2 on a chassis. The light receiving unit 14 includes a first CCD 4 and a movable half mirror 6 on the front surface thereof. The movable half mirror 6 is attached to a movable support plate 9 and can be tilted in an arbitrary direction by a step motor 8. The entire light receiving unit 14 is attached to and integrated with one chassis.

  The control / arithmetic unit 50 includes a CPU 51, a laser control circuit 52, a CCD readout circuit 53, a control circuit 54, a step motor drive circuit 55, and an encoder circuit 56.

  As shown in FIG. 8, the movable half mirror 6 is attached to a movable support plate 9 supported at three points, and the inclination angle can be changed with respect to the first CCD attachment plate 7. Of the three support points of the movable support plate, two points are linearly moved by the step motor, and the structure is provided with means for controlling the tilt angle and the tilt direction by the step motor.

  Next, the operation of the sensor will be described. 6 and 7, the laser light from the first laser light source 1 passes through the beam splitter 5, enters the vicinity of the center of the movable half mirror 6, reflects a part of the light, returns to the beam splitter 5, and returns to the optical path of approximately 90 degrees. The light is received by the second CCD 3 and the coordinates of the light receiving point are detected.

  The positional relationship between the light projecting unit 13 and the light receiving unit 14 is fixed with a certain reference, and at this time, the position of the movable half mirror 6 is adjusted so that the surface of the movable half mirror 6 and the input light from the first beam light source 1 are substantially orthogonal to each other. To do. At this time, the position of the light receiving point formed on the second CCD 3 is set as a reference point. This reference point is set so as to be approximately at the center of the second screen, and the bending of the joint at this time is set as the zero point.

  When the angle formed by the light projecting unit 13 and the light receiving unit 14 changes from the above-described zero point, the first CCD 3 detects a deviation between the light receiving point and the reference point, and the CPU 51 calculates the correction direction of the movable half mirror 6. Control the motor 8 so that the coordinate position of the light receiving point is returned to the reference point.

  When the light receiving point matches the reference point, the angle can be calculated by reading the position of the step motor 8 with an encoder. In the case of the step motor 8, it is probabilistic that the light receiving point and the reference point completely coincide with each other. In this case, by calculating the angle from the position of the step motor 8 and the deviation between the light receiving point and the reference point. Highly accurate measurement is possible.

  Part of the laser light that has reached the movable half mirror 6 from the first laser light source 1 passes through the movable half mirror 6 and reaches the first CCD 4, where the position is detected and the displacement of the rigid bodies adjacent to each other can be detected. .

  The laser light from the second laser light source 2 passes through the movable half mirror 6 and reaches the first CCD 4, and the coordinate position of the light receiving point is measured. From the light receiving point of the laser beam from the first laser light source 1 and the length of the line segment determined by the light receiving point of the laser light from the second laser light source 2 and the inclination of the movable half mirror 6 on the first CCD 4, The distance in the optical axis direction of the first laser light of the light receiving unit 14 can be calculated.

  Further, the twist around the optical axis of the first laser light source can be measured from the slope of the line segment determined by the laser light receiving point from the first laser light source 1 and the laser light receiving point from the second laser light source 2.

  As described above, the three elements of the rotation angle around the X axis, the Y axis, and the Z axis and the coordinate translation vector, which are necessary and sufficient conditions for the coordinate transformation for determining the mutual positional relationship between the two rigid bodies, are 6 in total. All the elements are obtained with one small sensor.

Other forms for carrying out the invention

  Hereinafter, another embodiment of the present invention will be described with reference to FIGS.

  1 and 2 show an embodiment of the first problem solving means. The posture recognition sensor includes a sensor unit and a control / arithmetic unit. The sensor unit includes a light projecting unit 13 and a light receiving unit 14. The light projecting unit 13 is fixed to one rigid body 105 adjacent to each other, and the light receiving unit 14 is fixed to the other rigid body. The light projecting unit 13 includes a first laser light source 10, a second laser light source 11, and a third laser light source 12. The light projecting unit 13 is integrated with the chassis, and the light receiving unit 14 includes a CCD 4. The control / arithmetic unit includes a CPU 51, a laser control circuit 52, and a CCD readout circuit 53.

  The operation of the above configuration will be described below. The optical axes of the first laser light source 11, the second laser light source 12, and the third laser light source 13 are not algebraically linearly related to each other, intersect the surface of the CCD 14, and the optical axes are mutually on the light receiving surface of the CCD 14. It shall not be exchanged.

  If the positional relationship between the rigid bodies adjacent to each other in the joint changes, the positional relationship between the light receiving points on the CCD surface changes, so that the positional relationship of the rigid bodies can be obtained.

  By reading the positions of the three light receiving points on the CCD 14 with the CCD reading circuit 53, determining the coordinates of the light receiving points, and solving the simultaneous equations with the arithmetic circuit, the six elements of the coordinate conversion parameters can be determined.

  Furthermore, another embodiment of the present invention will be described with reference to FIGS.

  4 and 5, the posture recognition sensor includes a sensor unit and a control / calculation unit 50, and the sensor unit includes a light projecting unit 13 and a light receiving unit 14. The light receiving unit 14 is fixed to the other rigid body 22 while being fixed to the adjacent one rigid body 21. The light projecting unit 13 is configured by integrally mounting the first laser light source 1, the beam splitter 5, the second CCD 3, and the second laser light source 2 on the chassis, and the light receiving unit 14 is half-mounted on the first CCD 4 and the front surface thereof. The mirror 6 is attached to and integrated with one chassis. The control / calculation unit 50 includes a CPU 51, a laser control circuit 52, and a CCD readout circuit 53.

  The operation of the above configuration will be described below. The laser light from the first laser light source 1 passes through the beam splitter 5, enters near the center of the half mirror 6, reflects a part of the light beam, returns to the beam splitter, and bends the optical path by approximately 90 degrees. The light is received and the position is detected.

  When the reflected light beam of the half mirror 6 takes the same optical path as the incident light beam, that is, when the half mirror 6 and the laser optical axis from the first laser light source 1 form a right angle, the light receiving point is substantially at the center of the second CCD 3. It has been adjusted in advance to come, and this is taken as the reference point.

  When the axes of the light projecting unit 13 and the light receiving unit 14 are not parallel, that is, when the surface of the half mirror 6 and the first laser optical axis are not perpendicular, the light receiving point 20 on the first CCD 3 is shifted from the reference point. Thus, the angle between the light projecting unit 13 and the light receiving unit 14 is detected by detecting this shift and reading the coordinates.

  A part of the laser light reaching the half mirror 6 from the first laser light source 1 passes through the half mirror 6 and reaches the first CCD 4, where the position is detected and the deviation of the light receiving unit 14 can be detected.

  The laser light from the second laser light source 2 passes through the half mirror 6 and reaches the first CCD 4, and the position of the light receiving point is measured. Based on the length of a line segment determined by the light receiving point of the laser light from the first laser light source 1 and the light receiving point of the laser light from the second laser light source 2 and the inclination of the half mirror 6, the first light projecting unit 13 and the first light receiving unit 14. The distance in the optical axis direction of the laser light can be calculated.

  Further, the twist about the main optical axis can be measured from the inclination of the line segment determined by the laser light receiving point from the first laser light source 1 and the laser light receiving point from the second laser light source 2.

  As described above, the three elements of the rotation angle around the X axis, the Y axis, and the Z axis and the coordinate translation vector, which are necessary and sufficient conditions for the coordinate transformation for determining the mutual positional relationship between the two rigid bodies, are 6 in total. All the elements are obtained with one small sensor.

External view of posture recognition sensor showing embodiment of the present invention Functional diagram of the sensor Joint mounting diagram of the sensor External view of posture recognition sensor showing another embodiment of the present invention Functional diagram of the sensor External view of posture recognition sensor showing another embodiment of the present invention Functional diagram of the sensor Structure of movable half mirror of the sensor

Explanation of symbols

1 1st laser light source 2 2nd laser light source 3 2nd CCD
4 First CCD
DESCRIPTION OF SYMBOLS 5 Splitter 6 Half mirror 7 Light-receiving part base plate 8 Step motor 9 Movable plate 10 Laser light source 11 Laser light source 12 Laser light source 13 Emitting part 14 Light-receiving part 20 Light-receiving point 21 Rigid body 22 Rigid body 23 Joint part 50 Calculation / control part 51 CPU
52 Laser control circuit 53 CCD readout circuit 54 Motor control circuit 55 Step motor drive circuit 56 Encoder circuit

Claims (3)

  A joint portion that links adjacent rigid bodies is a light beam path, a light projecting portion having light emitting means such as at least three laser light sources fixed to one rigid body, and a planar shape fixed to the other rigid body A posture recognition sensor in which one set is configured with a light receiving unit having a screen and a light receiving point recognition means by a photoelectric conversion device, and a plurality of the sets are connected via the joint portion, An attitude characterized by comprising means for reading out the coordinates of a plurality of light receiving points on the screen of the light receiving unit, and means for acquiring coordinate conversion parameters. Recognition sensor.   2. The light receiving unit according to claim 1, wherein a light projecting unit having two laser light sources, one beam splitter, and one screen is fixed to one rigid body of the joint unit, and includes one screen and one half mirror. Is a posture recognition sensor configured by fixing the part to the other of the joint part, and a half mirror of the light receiving part is arranged so as to be substantially perpendicular to the optical axis of the first laser light source of the light projecting part. A light receiving portion having a first screen arranged in parallel with the rear surface, a beam splitter that transmits the beam light of the first laser light source and bends the reflected light from the half mirror in a direction of about 90 degrees, and the reflection from the beam splitter A second screen for receiving light, and a second laser light source having an optical axis incident on the first screen at an angle of 10 to 80 degrees with respect to the normal of the surface of the first screen; Have And a light receiving point position coordinate of the beam light from the first laser light source and the second laser light source on the first screen and a light receiving point position coordinate from the first light source on the second screen. And a posture recognition sensor, characterized in that a coordinate conversion parameter is obtained from the coordinates of the three light receiving points.   The posture recognition sensor according to claim 2, wherein the half mirror of the light receiving unit includes movable means by electrical control, and the beam light from the first laser light source is reflected by the half mirror, Equipped with means to detect the position of the light receiving point created on the second screen of the light projecting unit through the splitter and the tilt of the movable half mirror, and obtain coordinate conversion parameters from the coordinates of the three light receiving points and the tilt of the half mirror A posture recognition sensor characterized by being configured to do so.

Images