JP6941956B2 - Calculation method for changing the arrangement of the external state sensor - Google Patents

Description

INDUSTRIAL APPLICABILITY The present invention is a change in the arrangement mode of an external state sensor, which is attached to a moving body such as a robot and for detecting a state in the moving body by scanning the environment of the moving body, relative to the moving body. Regarding the technique of calculating the quantity.

A technique for calibrating the displacement of the sensor based on the observation result of the calibration marker by the sensor has been proposed (see Patent Document 1). A method of adjusting the position and inclination of a recognition object has been proposed based on the comparison of the distortion of the shape of the two-dimensional pattern and the change of the light-dark ratio detected by using the three-dimensional visual sensor (see Patent Document 2). A technique for attaching a white correction plate to the arm of a robot has been proposed (see Patent Document 3).

Japanese Unexamined Patent Publication No. 2009-014481 Japanese Unexamined Patent Publication No. 09-096506 Patent Gazette No. 3868896

However, when the arrangement mode of the sensor attached to the moving body such as a robot changes relative to the moving body, the amount of change is calculated based on the observation result of the calibration marker separate from the moving body. It's difficult to do.

Therefore, it is an object of the present invention to provide a method for easily calculating the amount of change in the arrangement mode of the external state sensor with respect to the moving body.

In the arrangement mode change calculation method of the present invention, an external state sensor that emits a scanning beam rotating around each of the first axis and the second axis orthogonal to each other in the sensor coordinate system is attached to the moving body. In addition, in a state where the marker element configured so that the feature amount detected by the external world state sensor changes according to the location is formed or arranged on the moving body, the sign element is applied to the external world state sensor. By scanning, the first factor derived from the correlation between the rotation angle representing the direction with respect to the external state sensor and the feature quantity by the labeling element with respect to the specified sampling point among the plurality of sampling points. Based on the detection process that detects The marker element comprises a process, and is characterized in that the marker element is composed of a plurality of strip-shaped regions that are arranged side by side in a predetermined direction of a moving body coordinate system as pattern markers and have different inclinations. And.

According to the arrangement mode change calculation method of the present invention, the feature amount of one or a group of sampling points among a plurality of sampling points by the external state sensor and the other sampling points are determined by the marker element formed or arranged on the moving body. There is a difference from the feature amount. By using the correlation between the feature amount and the direction of each sampling point with respect to the external state sensor, which reflects the difference, the amount of change in the arrangement mode of the external state sensor with respect to the moving object can be easily calculated. NS.

Explanatory drawing about the configuration of a robot. Explanatory drawing about the structure of the control device of a robot. Explanatory drawing about the 1st scanning range by an external state sensor. Explanatory drawing about the 2nd scanning range by an external state sensor. Explanatory drawing about attitude change of a sensor coordinate system. The explanatory view about the initial detection mode of the exponent of the designated point in the marker element of 1st Embodiment. Explanatory drawing about the latest detection mode of the index of a designated point. The explanatory view about the calculation method of the sensor posture change amount in 1st Embodiment. The explanatory view about the initial detection mode of the exponent of the designated point in the marker element of 2nd Embodiment. Explanatory drawing about the latest detection mode of the index of a designated point. The explanatory view about the calculation method of the sensor posture change amount in 2nd Embodiment. The explanatory view about the initial detection mode of the exponent of the designated point in the marker element of 3rd Embodiment. Explanatory drawing about the latest detection mode of the index of a designated point. The explanatory view about the calculation method of the sensor posture change amount in 3rd Embodiment. Explanatory drawing about the structure of the robot as another embodiment of this invention.

(First Embodiment)
(composition)
The robot 1 as a mobile body according to the first embodiment of the present invention shown in FIG. 1 is a leg-type mobile robot. The moving body may be any device having a moving function such as a vehicle. Similar to humans, the base 10, the head 11 provided on the upper part of the base 10, the left and right arm bodies 12 (first limbs) extending from both the upper left and right sides of the base 10, and the tip of the arm body 12 The hand 13 (hand part) provided in the base 10, the left and right leg bodies 14 (second limb body) extending downward from the lower part of the base 10, and the foot flat part 15 attached to the tip of the leg body 14. And have. The robot 1 uses the force transmitted from the actuator to perform the arm body 12 and the leg body 14 in a plurality of joint mechanisms corresponding to a plurality of joints such as a human shoulder joint, elbow joint, wrist joint, hip joint, knee joint, and ankle joint. Can be bent and stretched.

The arm body 12 has a first arm link connected to the base 10 via a shoulder joint mechanism, one end connected to the end of the first arm link via an elbow joint mechanism, and the other end connected to a hand via a wrist joint. It is provided with a second arm link connected to the base of 13. The shoulder joint mechanism has two degrees of freedom of rotation around each of the yaw axis and the pitch axis. The elbow joint mechanism has one degree of freedom of rotation around the pitch axis. The wrist joint mechanism has two degrees of freedom of rotation around each of the roll axis and the pitch axis.

The hand 13 includes one or a plurality of finger mechanisms (movable members) that are movable with respect to the palm portion and the palm portion, and by the operation of the finger mechanism, between one or a plurality of finger mechanisms and the palm portion, or a plurality of fingers. It is configured so that the object can be gripped between the finger mechanisms of the.

The leg body 14 has a first leg link connected to the base 10 via a hip joint mechanism, one end connected to the end of the first leg link via a knee joint mechanism, and the other end via an ankle joint. It is provided with a second leg link connected to the portion 15. The hip joint mechanism has three degrees of freedom of rotation around each of the yaw axis, pitch axis and roll axis. The knee joint mechanism has one degree of freedom of rotation around the pitch axis. The ankle joint mechanism has two degrees of freedom of rotation around each of the pitch axis and the roll axis. The robot 1 can move autonomously by the movements of the left and right leg bodies 14 that are accompanied by repeated getting out of bed and landing.

_{A pair of external state sensors S 1} are arranged on each of the left side and the right side of the head portion 11. The external state sensor S ₁ is composed of a two-dimensional laser range finder fixed on a rotary table. The two-dimensional laser range finder irradiates the outside with a slit-shaped laser beam and detects the reflected light to detect the distance to an object existing in the irradiation direction (scanning direction) of the laser beam and the brightness (reflector value) of the object. Is detected as a feature quantity.

The rotary table constituting the external state sensor S ₁ , and thus the slit-shaped laser beam, is configured to rotate around the z-axis of the sensor coordinate system (x, y, z). As a result, for example, as shown in FIG. 3A, _{the first scanning range R 1} of the laser beam in the xy plane by _{the external state sensor S 1} extends in a fan shape (for example, the central angle is 270 °). The first scanning range R ₁ may have inversion symmetry with respect to the x-axis line, or may be asymmetric.

The external state sensor S ₁ is configured to change the height of the vector from the origin to the sampling point in the sensor coordinate system in the z-axis direction. As a result, as shown in FIG. 3B, for example _{, the second scanning range R 2} (or slit-shaped laser beam) of the laser beam in the x-z plane by _{the external state sensor S 1} has a fan shape (for example, a central angle of 150). It spreads to °). The second scanning range R ₂ may have inversion symmetry with respect to the x-axis line or may be asymmetric.

A concave curved surface 111 is formed on each of the left side and the right side of the head 11 along the side surface portion of a cylinder having a central axis parallel to the Z axis direction of the head coordinate system (X, Y, Z) (FIG. 3A and FIG. 4). A part of the concave curved surface 111 _{is included in the scanning range of the external state sensor S 1} , and the pattern marker 112 is arranged in the part. In the present embodiment, the single region of the concave curved surface 111 _{is a part of the first scanning range R 1} in the xy plane, the first designated range r ₁ (see FIG. 3A), and the second scanning in the xx plane. It is included in the second designated range r ₂ (see FIG. 3B), which is a part of the range R _2. The pattern marker 112 may be composed of a paint applied to the concave curved surface 111, or may be composed of a curved plate-shaped member attached to the concave curved surface 111 and a paint applied thereto.

The concave curved surface 111 constitutes a first marker element that constitutes a distance boundary in which the "distance" as the first feature amount is discontinuous. The pattern marker 112 includes a luminance boundary (a boundary between regions having different paint types and a region with paint and a region without paint) in which the “brightness (or reflector value)” as the second feature amount is discontinuous. ) Consists of the second marker element. At least one of one side edge, upper edge and lower edge portion of the pattern marker 112 may coincide with at least one of one side edge, upper edge and lower edge portion of the concave curved surface 111.

_{A part of the external state sensor S 1} is arranged so as to be close to the central portion of the head 11 by utilizing the space defined by the concave curved surface 111. As a result, the amount of protrusion of the head 11 of the external state sensor S ₁ to the left and right is suppressed.

FIG. 5A shows the pattern marker 112 in a developed state on the ab plane. In the initial state where the Z-axis of the head coordinate system and the z-axis of the sensor coordinate system are parallel, "a" _{corresponds to the rotation angle or azimuth θ z} of the laser beam around the z-axis line, and "b" is the laser beam. _{Corresponds to the elevation angle θ y} with respect to the xy plane of (see FIG. 4). _{A = a 1} (b ₁ <b <b ₂ ) (or a = a ₂ (b ₁ <b <b ₂ )) corresponding to a part of the edge of the concave curved surface 111 on the right side (or left side) of the head portion 11. , B = b ₁ (a ₁ <a <a ₂ ) and b = b ₂ (a ₁ <a <a ₂ ) constitute the distance boundary. The cross-shaped line segments a = 0 (b ₁ <b <b ₂ ) and b = 0 (a ₁ <a <a ₂ ) that divide the four rectangular regions of the pattern marker 112 constitute the luminance boundary.

Since the laser beam is rotated around the z-axis and the y-axis by the external state sensor S _{1, the scanning lines of the laser beam ‥ L i1} ‥ L _i2 ‥ are a as shown in FIG. 5A. It is tilted in the −b plane. The angle of inclination of the scanning line L _i is for z-axis of the turning speed of the laser beam is represented by the ratio of y-axis of the rotating speed of the laser beam. The scanning line L _i is specified by a first exponent i representing an azimuth angle θ _{z (discrete value).} The sampling point on the scanning line L _i by the external state sensor S ₁ is specified by the second index j representing the _{elevation angle θ y} (discrete value) in addition to the first index i.

(Control device configuration)
The control device 2 shown in FIG. 2 is composed of a programmable computer or an electronic control unit (composed of a CPU, ROM, RAM, I / O circuit, etc.) mounted on the robot 1. The control device 2 is configured _{to recognize the values of various state variables based on the output signals of the external state sensor S 1} and the internal state sensor S ₂ , and to control the operation of each actuator based on the recognition result. Has been done.

In addition to the external state sensor S ₁ , other external state sensors such as an active sensor using infrared light mounted on the substrate 10 may be provided.

The internal state sensor S ₂ includes a GPS measuring device or an acceleration sensor for measuring the position (center of gravity position) of the robot 1, a gyro sensor for measuring the posture of the base 10, and each axis of each joint mechanism. It includes a rotary encoder that measures the joint angle and the like, a 6-axis force sensor that measures the external force acting on the hand 13, and the like.

The control device 2 includes a storage device 20, a first factor detection element 22, a second factor calculation element 24, and an motion generation element 28.

Storage device 20, etc. represent the arrangement of the structure in to the world coordinate system obtained through external state sensor S ₁ "environment information", and stores and holds information necessary for the operation generating the robot 1. The first factor detection element 22 causes the external state sensor S ₁ to scan the pattern marker 112 (second marker element) arranged on the concave curved surface 111 (first indicator element). As a result, among a plurality of sampling points, the first factor derived from the correlation between the rotation angle representing the direction with _{respect to the external state sensor S 1 and the feature amount by the marking element is detected for the specified sampling point.} .. The second detection element 24 represents the amount of change in the relative arrangement mode of the _{external state sensor S 1} with respect to the robot 1 based on the time-series change mode of the first factor detected by the first detection element 22. Calculate the factors.

Motion generation element 28 based on the inner bounds condition of robotic 1 detected by the external conditions and inner bounds state sensor S ₂ of the robot 1 detected by the external state sensor S _1, the operation of the arms 12 and the hand 13, as well as , Generates the movement or gait of the robot 1 including the movement of the leg body 14 that changes the position / posture of the base 10.

A single processor (arithmetic processor) may function as the plurality of elements 22, 24 and 28, or the plurality of processors (multi-core processors) cooperate with each other by intercommunication and the plurality of elements 22, 24 and 28. May function as.

(Action)
The pattern marker 112 is scanned by the external state sensor S ₁ while changing the azimuth angle θ _z and the elevation angle θ _y of the laser beam (see the double-headed arrow in FIG. 3A and the double-headed arrow in FIG. 3B). In this process, the combination of the respective values of the first index i and the second index j at the designated point of the pattern marker 112 is detected and stored in the storage device constituting the control device 2 (FIG. 6 / STEP12). ).

_{For example, the upper end point (0, b 2} (> 0)) of the boundary line segment of the rectangular region in each of the first quadrant and the second quadrant in the ab coordinate system shown in FIG. 5A (brightness boundary and distance boundary). _{(Intersection) and the lower end point (0, b 1} (<0)) (intersection of the brightness boundary and the distance boundary) of the boundary line segment of the rectangular region in each of the third quadrant and the fourth quadrant are specified. For each of the designated points (0, b ₁ ) and (0, b ₂ ), (i, j) = (i ₁ , j ₁ ) and (i ₂ , j ₂ ) are detected as the first factor. In addition to the intersection of the luminance boundary and the distance boundary, one or any point of each of the luminance boundary and the distance boundary may be specified.

Subsequently, it is determined whether or not the designated condition is satisfied (FIG. 6 / STEP14). The "designated conditions" include that an external force of a specified value or more has acted on the robot 1, that the robot 1 has moved a predetermined distance or more since the last (i, j) detection (see FIG. 6 / STEP 12), and that the last (I, j) A predetermined time or more has passed since the detection (FIG. 6 / STEP12 or STEP16), the robot 1 has executed a task involving a predetermined interaction with an external structure more than a specified number of times, and the like. This includes the condition that an event has occurred in which the position / orientation of the sensor coordinate system with respect to the head coordinate system is likely to change. Occurrence or non-occurrence of the event is detected by at least some of the sensors of the example inner bounds condition sensor S _2.

If the determination result is negative (FIG. 6 / STEP14 ... NO), it is determined again after a certain period of time whether or not the designated condition is satisfied (FIG. 6 / STEP14). On the other hand, when the determination result is positive (FIG. 6 / STEP14 ... YES), the pattern marker 112 is scanned by the external state sensor S _{1 while changing the azimuth angle θ z} and the elevation angle θ _{y of the laser beam.} Will be done. Then, the combination of the respective values of the first index i and the second index j at the designated point of the pattern marker 112 is detected as the first factor, and is stored in the storage device constituting the control device 2 (FIG. 6). / STEP16). As a result, for example, (i, j) = (i ₁ ', j ₁ ') _{for each of the designated points (0, b 1} ) and (0, b ₂ ) in the ab coordinate system shown in FIG. 5B. , (I ₂ ', j ₂ ') are detected.

Then, based on the initial detection result (or the previous detection result) and the latest detection result (or the current detection result) of the first index i and the second index j at each designated point, the head coordinate system (or the moving body coordinate system) is subjected to. The amount of change in attitude of the sensor coordinate system (and thus the external state sensor S ₁ ) is calculated as the second factor (FIG. 6 / STEP 18).

For example, initial detection results and the average value and the unit azimuth angle corresponding to the spacing of adjacent scanning lines of the first index deviation at each of the most recent detection result (= i ₁ '-i ₁ and i _2' -i _{2) The result of multiplication with the deviation is calculated as the amount of change in attitude Δθ z} around the z-axis of the sensor coordinate system (see FIGS. 5A and 5B). The initial detection results and the average value and, in the z-axis direction of the sampling points in the same scan line interval of the deviation of the second index in each of the latest detection result (= j ₁ '-j ₁ and j _2' -j ₂₎ The result of multiplication with the unit elevation deviation corresponding to is calculated as the _{amount of change in attitude Δθ y around the y-axis of the sensor coordinate system.} Further, the ratio of the deviation of the second index to the deviation of the first index of the two designated points in the initial detection result (j _2- j ₁ ) / (i _2- i _{1 ) and the ratio (j 2} ) in the latest detection result. _{'-j 1') / (i} 2 '-i 1') and the deviation between the multiplication result of the unit angle of the phrase is calculated as the x-axis around the posture change amount [Delta] [theta] _x of the sensor coordinate system.

(Second Embodiment)
(composition)
Since the robot 1 according to the second embodiment of the present invention is the same as the first embodiment except for the configuration of the pattern marker 112, the description of the same configuration will be omitted.

As shown on the upper side of FIG. 7A and the upper side of FIG. 7B, the pattern markers 112 are arranged side by side so as to be separated from each other in the lateral direction (a-axis direction), and a plurality (for example, three) having different inclinations. It is composed of a strip-shaped region of. The width of the strip-shaped region and the mutual spacing are set sufficiently larger than the resolution in the a-axis direction of _{the external state sensor S 1 (the spacing between adjacent scanning lines).}

_{A = a 1} (b ₁ <b <b ₂ ) (or a = a ₂ (b ₁ <b <b ₂ )) corresponding to a part of the edge of the concave curved surface 111 on the right side (or left side) of the head portion 11. , B = b ₁ (a ₁ <a <a ₂ ) and b = b ₂ (a ₁ <a <a ₂ ) constitute the distance boundary. The boundary between the plurality of strip-shaped regions (low-luminance region) of the pattern marker 112 and the regions to the left and right of the strip-shaped region (low-luminance region) constitutes the luminance boundary.

(Action)
Scanning of the pattern marker 112 by the external state sensor S _{1 is executed.} In this process, first sampling point group first index i = i _1, i ₂ and i ₃ are designated (plural sampling points in each scanning line L _i1, L _i2 and L _i3), the second index The mode of change in luminance according to the change in the value of j is detected as the first factor, and is stored and held in the storage device constituting the control device 2 (FIG. 8 / STEP22). Thus, for example, for each of Figure 7A scan lines are shown in the upper L _i1, L _i2 and L _i3, the luminance corresponding to the change in the value of the second index j as shown in the drawing downward The mode of change is detected.

Next, it is determined whether or not the designated condition is satisfied (FIG. 8 / STEP24). If the determination result is negative (FIG. 8 / STEP24 ... NO), it is determined again after a certain period of time whether or not the designated condition is satisfied (FIG. 8 / STEP24). On the other hand, when the determination result is affirmative (FIG. 8 / STEP24 ... YES) _{, the first exponents i = i 1} , i ₂ and in the process of scanning the pattern marker 112 by the _{external state sensor S 1} the first sampling point group i ₃ is specified (plural sampling points in each scanning line L _i1, L _i2 and L _i3), variant of the luminance according to the change in the value of the second index j is first It is detected as a factor and stored in a storage device constituting the control device 2 (FIG. 8 / STEP26). Thus, for example, for each of the FIG. 7B scan lines are shown in the upper L _i1, L _i2 and L _i3, feature amount corresponding to the change in the value of the second index j as shown in the drawing downward The change mode of is detected.

Then, for each of the scanning lines L _i1, L _i2 and L _i3, early detection results of the feature quantity of variants in accordance with the change in the value of the second index j (or previous detection results) and the latest detection result (or sensing current based on the results), the initial (or at the latest detected from the previous time) (or chronological posture variation to this) of the sensor coordinate system with respect to the head coordinate system (and thus external state sensor S ₁₎ is calculated (Fig. 8 / STEP28).

For example, different 2 at a predetermined number of points before, after, or before, and after the inflection point of each feature amount curve of the _{scanning lines L i1 to} L _i3 shown in the lower side of FIG. 7A and the lower side of FIG. 7B, respectively. Based on the (or time-series) brightness deviation at one time point, the amount of change in attitude Δθ _x around the x-axis of the sensor coordinate system is calculated. The positive / negative distinction between the previous and current luminance deviations represents the positive / negative _{distinction of Δθ x} , and the magnitude of the previous and current luminance deviations represents _{the magnitude of Δθ x.} Such a qualitative relationship is stored in the storage device 20 as a table or a calculation formula, and Δθ _x is calculated based on the luminance deviation.

(Third Embodiment)
(composition)
Since the robot 1 according to the third embodiment of the present invention is the same as the second embodiment except for the configuration of the pattern marker 112, the description of the same configuration will be omitted.

As shown on the upper side of FIG. 9A and the upper side of FIG. 9B, the pattern markers 112 are arranged side by side so as to be separated from each other in the vertical direction (b-axis direction), and the inclinations are different from each other (for example, three). It is composed of a strip-shaped region of. The width of the band-shaped region and the mutual spacing are set sufficiently larger than the resolution in the b-axis direction of _{the external state sensor S 1 (sampling point spacing on the same scanning line).}

_{A = a 1} (b ₁ <b <b ₂ ) (or a = a ₂ (b ₁ <b <b ₂ )) corresponding to a part of the edge of the concave curved surface 111 on the right side (or left side) of the head portion 11. , B = b ₁ (a ₁ <a <a ₂ ) and b = b ₂ (a ₁ <a <a ₂ ) constitute the distance boundary. The boundary between the plurality of strip-shaped regions (low-luminance region) of the pattern marker 112 and the regions above and below it (high-luminance region) constitutes the luminance boundary.

(Action)
Scanning of the pattern marker 112 by the external state sensor S _{1 is executed.} In this process, the second index j (or the group of combinations of the first index i and the second index j) corresponds to the change of the first index i in each of the _{designated second sampling point groups G 1 to} G _3. The change mode of the brightness is detected as the first factor, and is stored in the storage device constituting the control device 2 (FIG. 10 / STEP32). The difference in the second index j of the sampling points having the same first index i, which constitutes each of the different second sampling point groups, may be constant or indefinite. As a result, for example, for each of the sampling point groups G ₁ , G ₂ and G ₃ shown on the upper side of FIG. 9A, the brightness corresponding to the change in the value of the first index i as shown on the lower side of the figure. The change mode of is detected.

Next, it is determined whether or not the designated condition is satisfied (FIG. 10 / STEP34). If the determination result is negative (FIG. 10 / STEP34 ... NO), it is determined again after a certain period of time whether or not the designated condition is satisfied (FIG. 10 / STEP34). Meanwhile, if the determination result is positive (Fig. 10 / STEP34 ‥ YES), in the process of scanning is performed in the pattern marker 112 by Hashigaikai state sensor S _1, the second index j is designated 2 The change mode of the brightness according to the change of the first index i in each of the sampling point groups G _{1 to} G ₃ is detected as the first factor, and is stored and stored in the storage device constituting the control device 2 (FIG. 10 / STEP36). As a result, for example, for each of the sampling point groups G ₁ , G ₂ and G ₃ shown on the upper side of FIG. 9B, the brightness corresponding to the change in the value of the first index i as shown on the lower side of the figure. The change mode of is detected.

Then, for each of the sampling point groups G ₁ , G ₂ and G ₃ , the initial detection result (or the previous detection result) and the latest detection result (or the current detection result) of the change mode of the brightness according to the change of the value of the first index i based on the results), posture variation of the sensor coordinate system (and thus external state sensor S ₁₎ with respect to the head coordinate system are calculated (Fig. 10 / STEP 38).

For example, the luminance deviation at inflection temae, a predetermined number of points after or before and after each of the luminance curve of the sampling point group G ₁ ~G ₃ shown in each of FIGS. 9A lower side and Figure 9B lower Based on, the amount of change in attitude Δθ _x around the x-axis of the sensor coordinate system is calculated. The luminance deviation at inflection temae a positive value, although the post-inflection point is a negative value, the previous and other positive and negative current luminance deviation represents a different positive and negative [Delta] [theta] _x, The magnitude of the brightness deviations of the previous time and this time represents _{the magnitude of Δθ x.} Such a qualitative relationship is stored in the storage device 20 as a table or a calculation formula, and Δθ _x is calculated based on the luminance deviation.

(Other Embodiments of the present invention)
Any combination of the pattern markers 112 in each of the first to third embodiments may be arranged on the concave curved surface 111. The arrangement mode such as the arrangement direction and the arrangement order of the pattern marker 112 may be arbitrarily changed.

In the above embodiment, the distance boundary is formed by a part of the edge of the concave curved surface 111, but as another embodiment, the distance boundary is formed by a part of the ridge line or the valley line derived from the shape of the robot 1 (moving body). It may have been done.

In the above embodiment, the distance of the sampling point in which at least one of the x-axis, y-axis, and z-axis with respect to the _{external state sensor S 1 is designated is detected as the first factor, and the detection result of the distance.} The amount of displacement of the external state sensor S ₁ with respect to the robot 1 in the at least one axial direction may be calculated as a second factor based on the deviation of.

In the above embodiment, the single region of the concave curved surface 111 _{is a part of the first scanning range R 1} in the xy plane, the first designated range r ₁ (see FIG. 3A), and the second scanning in the xx plane. Although it is included in the second designated range r ₂ (see FIG. 3B) which is a part of the range R ₂ and the pattern marker 112 is provided in the single area (see FIG. 3A), the robot 1 in another embodiment. The arrangement mode of the pattern marker 112 on the concave curved surface 111 may be variously changed depending on the shape, position / orientation of the concave curved surface 111 on the head portion 11 or the _{arrangement mode of the external world state sensor S 1 with respect to the concave curved surface 111.} For example, as shown in FIG. 11, each of the pair of regions of the concave curved surface 111 separated from each other is a part _{of the first scanning range R 1 in the xy plane. A} pattern marker 112 may be provided in each of the pair of regions included in 1. The pattern markers 112 or combinations thereof arranged in the pair of regions may be the same or different.

In the above embodiment, the sufficiency determination process of the designated conditions (see FIG. 6 / STEP14, FIG. 8 / STEP24, FIG. 10 / STEP34) is omitted, and periodically (for example, a cycle according to the clock frequency of the control device 2). Calculation of the amount of change in the attitude of the sensor coordinate system (see FIGS. 6 / STEP16, 18, FIG. 8 / STEP26, 28, FIG. 10 / STEP36, 38) may be executed.

1 Robot (moving body), 2 Control device (calculation device), 10 Base, 11 Head, 20 Storage device, 22 First factor detection element, 24 Second factor calculation element, 28 Operation Generator, 111 ... concave curved surface (first marker element), 112 ... pattern marker (second marker element), S ₁ ... external state sensor, S ₂ ... internal state sensor.

Claims (6)

An external state sensor that emits scanning beams that rotate around the first axis and the second axis that are orthogonal to each other in the sensor coordinate system is attached to the moving body and is detected by the external state sensor. In a state where a marker element whose feature amount is configured to change depending on the location is formed or arranged on the moving body.
By causing the external state sensor to scan the marking element, the rotation angle representing the direction with respect to the external world sensor and the feature amount by the marking element with respect to the designated sampling point among the plurality of sampling points. The detection process to detect the first factor derived from the correlation of
Based on the time series of the first factor detected by the detection process, a calculation process for calculating a second factor representing the amount of change in the relative arrangement mode of the external state sensor with respect to the moving body is provided .
The arrangement mode change calculation is characterized in that the marker elements are arranged as pattern markers in parallel in a predetermined direction of the moving body coordinate system so as to be separated from each other, and are composed of a plurality of strip-shaped regions having different inclinations. Method. In the arrangement mode change calculation method according to claim 1,
In the detection process, the second rotation angle around the second axis of the point on the boundary where the feature amount is discontinuous is detected as the first factor.
In the calculation process, based on the deviation of the second rotation angle, the amount of change in the angle of the external state sensor with respect to the moving body around the second axis is calculated as the second factor. Method of calculation. In the arrangement mode change calculation method according to claim 2,
In the detection process, the first rotation angle around the first axis of the point on the boundary where the feature amount is discontinuous is detected as the first factor.
In the calculation process, based on the deviation of the first rotation angle, the amount of change in the angle of the external state sensor with respect to the moving body around the first axis is calculated as the second factor. Method of calculation. In the arrangement mode change calculation method according to claim 2 or 3,
In the detection process, the first rotation angle around the first axis and the second rotation angle around the second axis of a pair of points on the boundary where the feature quantities are discontinuous are detected as the first factor. Then, in the calculation process, based on the deviation of the ratio of the deviation of the first rotation angle and the deviation of the second rotation angle between the pair of points, the first external state sensor with respect to the moving body. An arrangement mode change calculation method, characterized in that the amount of change in angle around a third axis perpendicular to each of the first axis and the second axis is calculated as the second factor. In the arrangement mode change calculation method according to any one of claims 1 to 4,
In the detection process, the said first sampling point group first rotation angle of the first axis line is specified, the feature quantity of variants in accordance with the change of the second rotation angle of the second axis around the Detected as one factor,
In the calculation process, the arrangement mode is characterized in that the amount of change in the angle around the second axis with respect to the moving body of the external state sensor is calculated as the second factor based on the deviation of the change mode of the feature amount. Change calculation method. In the arrangement mode change calculation method according to claim 5,
In the detection process, the change mode of the feature amount in response to the change in the first rotation angle of the second sampling point group in which the second rotation angle is specified is detected as the first factor.
In the calculation process, the arrangement mode is characterized in that the amount of change in the angle around the first axis with respect to the moving body of the external state sensor is calculated as the second factor based on the deviation of the change mode of the feature amount. Change calculation method.

Images