JPH06331400A - Sensor fusion method - Google Patents

Abstract

PURPOSE:To clarify a sensor fusion method which enables correction at higher accuracy than ever by utilizing various characteristics a plurality of sensors have for sensor fusion. CONSTITUTION:Various characteristics of sensors are evaluated with one evaluation element or a plurality of evaluation elements to define a certainty function indicating the degree of certainty of a sensor output with the evaluation elements as variables. Then, the current value of the evaluation element is inputted into the certainty function for each sensor to calculate the degrees of certainty about the individual evaluation elements. The degrees of certainty thus obtained are synthesized by weighting to calculate synthesized degrees of certainty B1 and B2 of the sensors (steps S1-S4). Thereafter, a weighting is applied to output data of the sensors based on the synthesized degrees of certainty to calculate a correction factor for measuring data to correct the measuring data by the correction factor (S5, S6).

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a sensor fusion method for fusing or integrating information obtained from a plurality of sensors to obtain new recognition information.

[0002]

2. Description of the Related Art According to a sensor fusion method, more accurate data can be obtained based on information obtained from a plurality of sensors of the same type or different types, and its application is being studied in various fields. Volume 113, February 1993 131-1
(See page 38).

For example, in running control of a floor mobile robot, the rotation angle of a measuring wheel that rotates in contact with the floor is detected, and at the same time, the distance to the wall surface is measured using an ultrasonic sensor or an optical sensor provided in the robot body. Is measured, the current position of the robot calculated from the rotation angle of the measuring wheel is corrected by the output of the distance sensor, and the movement path of the robot is controlled with high accuracy.

In the case where the basic measurement data (the current position of the robot) is corrected based on a plurality of sensor outputs as described above, in the conventional sensor fusion method,
Multiple rules are defined. For example, the current position of the robot is predicted based on a map held by the robot, the predicted data is compared with the output of the distance sensor, and if the difference is within a certain range, the output of the distance sensor is relied on. . The required number of such rules are defined, and when all of these rules are satisfied, the movement control of the robot is performed using the output of the distance sensor instead of the measurement data.

[0005]

However, the conventional sensor fusion method has a problem that a huge number of rules are required depending on the case and the processing time becomes long. Further, regarding each sensor, whether or not the correction is performed using the sensor output, that is, whether the correction is “1” or “0” is performed only. The characteristics of 1 are not fully utilized in sensor fusion, and the correction by sensor fusion cannot be said to be highly accurate.

An object of the present invention is to clarify a sensor fusion method capable of utilizing various characteristics of the sensor in the sensor fusion and thereby performing correction with higher accuracy than ever before. .

[0007]

According to the sensor fusion method of the present invention, in the first step,
The characteristics such as the characteristics and performance of the sensor are evaluated by one or a plurality of evaluation elements, and a reliability function that represents the reliability (reliability) of the sensor output is defined with each evaluation element as a variable. In the second step, for each sensor, the first
The current value of the evaluation element corresponding to the measurement data is input to the plurality of certainty functions defined in step to calculate the certainty for each evaluation element. In the third step, for each sensor, the plurality of certainty factors calculated in the second step are combined by weighting to calculate the combined certainty factor of each sensor. In the fourth step, the output data of each sensor is weighted based on the combined certainty factor to calculate a correction amount for the measurement data, and the measurement data is corrected by the correction amount.

[0008]

In the first step, the characteristics such as the characteristics and performance of each sensor are expressed in a certainty factor function having the horizontal axis as the evaluation element and the vertical axis as the certainty factor. For example, with respect to the ultrasonic sensor, the inclination angle of the measurement target surface, the measurement distance, and the like are adopted as the evaluation elements, and the confidence decreases as the inclination angle increases or the measurement distance increases. In the second step, the current value of the evaluation element corresponding to the measurement data, for example, the measurement data such as the measurement distance for the ultrasonic sensor is input to each confidence function, and the confidence of the measurement data is calculated. In the third step, the certainty factors for each sensor are combined by weighting. The weighting coefficient at this time is defined in advance according to the importance of the evaluation element for the sensor. The combined certainty factor for each sensor obtained in this step represents to what ratio the sensor should contribute to the correction of the measurement data with respect to the other sensors. In the fourth step, for example, assuming that the combined certainty factor is Bs, the correction amount (= Bs × (X′now−Xnow )) Is obtained, and the measurement data is corrected by adding this correction amount to the measurement data Xnow.

Therefore, when the sensor output X'now and the measurement data Xnow match or the combined confidence Bs is zero, the measured data Xnow is used as it is for control without being corrected, and the combined confidence is calculated. When Bs is 1, the sensor output X'now is used for control instead of the measurement data Xnow. When the combined certainty factor Bs is 0 <Bs <1, the sensor output X'now and the measurement data Xnow are integrated (sensor fusion) according to the value of the combined certainty factor Bs, and highly accurate correction data is obtained. It will be.

[0010]

According to the sensor fusion method of the present invention, since the characteristic peculiar to each sensor is reflected in the sensor fusion as a certainty function, it is possible to perform the correction with higher accuracy than before.

[0011]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment in which the present invention is applied to the traveling control of a floor cleaning robot will be described in detail with reference to the drawings. First, the configuration of the floor surface cleaning robot will be described with reference to FIGS. 3 to 5. As shown in FIGS. 3 and 4, a self-propelled carriage
A pair of driving wheels (17) (1
8), equipped with caster type auxiliary wheels (19) at the end in the backward direction, and each driving wheel (17) (18) has a DC servo motor (2) (21).
Are connected. On the side of the driving wheels (17) (18),
Measuring wheel encoder (2) for measuring the moving distance of (1)
2) (23) has been deployed.

A head housing (12) is provided on the carriage (1), and a rotary housing (11) is rotatably in a horizontal plane between the carriage (1) and the head housing (12). It has been deployed. Also, a cleaning liquid tank is provided in the head housing (12).
In addition to (15), various electronic circuits (not shown) such as a power supply circuit and a control circuit are housed, and a sewage recovery tank (16) is provided in the carriage (1).
Is housed.

A window (1) opened on the rear wall of the rotary housing (11)
Electric power is supplied to the cleaning robot by connecting the power cord (10) drawn from 1a) to an AC power source.

The carriage (1) is provided with a pair of left and right cleaning unit housings (13) (13) having an opening at the front in front of the vehicle (1). Each cleaning unit housing (13) has a motor (shown in the figure). (Omitted)
Two rotating brushes (14) (14) each driven by
A cleaning mechanism housing (13) containing a cleaning mechanism (20) including
The lower end of the brush is slightly exposed from below.

The cleaning liquid tank (15) is a rubber hose (not shown).
The cleaning liquid is introduced into the cleaning unit housings (13) and (13) through the cleaning liquid and is supplied to the four rotating brushes (14) arranged in a line.
Further, a suction port of a squeegee (not shown) arranged behind the rotating brush (14) is connected to the sewage recovery tank (16) via a flexible tube and a pump (both not shown).

During movement of the carriage (1), four rotating brushes (1
At the same time as the rotation of (4), the cleaning liquid is supplied from the cleaning liquid tank (15) to the rotating brush (14), whereby the floor surface is cleaned. Further, the sewage generated by this is sucked into the squeegee as the self-propelled carriage (1) moves forward, and is collected in the sewage collection tank (16).

The head housing (12) has 4
Two long-distance ultrasonic sensors (31) (32) (33) (34), and two long-distance ultrasonic sensors (51) (52) and (61) (6) to each side.
2), 2 long-distance ultrasonic sensors facing backwards (41) (42)
Is provided. In addition, proximity ultrasonic sensors (35) (35) are arranged in the cleaning unit housings (13) (13) toward the front. Further, the carriage (1) is provided with two proximity light sensors (53) (54) and (63) (64), respectively, to the both sides.

Thus, as shown in FIGS. 3 and 4, four long-distance ultrasonic sensors (31) (32) (33) (34) and 2 facing forward are provided.
The front distance detector (3) is composed of two proximity ultrasonic sensors (35) (36), and the rear distance detector (4) is composed of two far distance ultrasonic sensors (41) (42) facing rearward. To be done. Also, two long distance ultrasonic sensors (51) (52) and two proximity light sensors (53) facing the right side in the traveling direction.
The right side distance detector (5) is constituted by (54), and the left side distance detector (61) (62) and the proximity light sensors (63) (64) facing the left side are detected by the left side distance detector ( 6) is constructed.

The measuring range of the above sensors is 300 to 9000 mm for long distance ultrasonic sensors, 100 to 1000 mm for near ultrasonic sensors, and 100 to 50 for near optical sensors.
It is set to 0 mm.

FIG. 5 shows a robot control system, in which a front distance detector (3), a rear distance detector (4) and a right side distance detection are provided at an input port of a main control circuit (7) composed of a microcomputer. The device (5) and the left side distance detector (6) are connected, and the right measuring wheel encoder (22) and the left measuring wheel encoder (23) are connected. The output values of both encoders are stored as the actual movement trajectory of the robot.

In addition, an input port of the main control circuit (7) includes a map storage section (71) storing a map of a moving space and a predetermined traveling route represented by XY coordinates on the map. It is also connected to the travel route storage unit (72).

On the other hand, at the output port of the main control circuit (7),
Cleaning mechanism (20), right wheel motor (2) and left wheel motor (21)
Is connected to control the start and stop of the cleaning operation, and the carriage traveling control is performed by the forward and reverse rotations of both driving wheels.

In trolley traveling control, correction (sensor fusion) based on the output of an ultrasonic sensor or an optical sensor is applied to measurement data obtained from the measurement wheel encoder (hereinafter, simply referred to as measurement wheel) while the trolley is traveling. Then, the current position of the robot is recognized.

First, sensor fusion using only the ultrasonic sensor will be described. For ultrasonic sensors,
When the robot travels in parallel to the side wall to be irradiated with ultrasonic waves, that is, when the posture data θ is 0 °, the incident angle of the ultrasonic wave on the side wall is 90 °, and the highest accuracy is obtained. Since the certainty factor decreases as the data θ increases, the certainty factor function f1 (θ) as shown in FIG. 2A can be defined using the posture data θ as an evaluation element.
Here, when the attitude data θ is 0, the certainty factor is 1, when the attitude data θ is ± 10 °, the certainty factor is 0.5, and the two are connected by a straight line. When the attitude data θ exceeds ± 10 °, The confidence level is set to 0.

Further, in the ultrasonic sensor, the measuring range is 300 mm to 9000 mm as described above, and an error of about 1% of the measuring distance occurs, so that the evaluation element is the distance data x shown in FIG. 2 (b). Confidence function f of distribution as shown in
2 (x) can be defined.

Further, in the floor cleaning robot, it is possible to predict the current position of the robot based on the traveling route stored in the traveling route storing section (72),
When the measurement data of the current position by the ultrasonic sensor deviates greatly from the predicted value, it is considered that the output of the ultrasonic sensor contains a large amount of noise, and the measurement data is unreliable. Therefore, using the absolute value of the positional deviation (x-Xref) between the distance data x and the prediction data Xref as an evaluation element, the confidence factor function f3 (abs (x
-Xref)) can be defined. If the positional deviation is 300 mm or less, the certainty factor is 1 and the positional deviation is 50.
When it exceeds 0 mm, the certainty factor is set to 0, and the lines are connected.

Each of the three certainty functions defined as shown in FIGS. 2 (a) (b) (c) has a current value (θnow, θnow,) of the evaluation element (posture data, distance data, position deviation) corresponding to the measurement data.
x, abs (x-Xref)) is input, the confidence factor for each evaluation element is calculated, and further the combined confidence factor f (x, θ) is calculated by the following formula 1. The current value of the posture data θnow
Can be obtained from the output data of the measuring wheel, but can also be calculated based on the outputs from the two ultrasonic sensors.

[0028]

[Equation 1]

Here, c ₁ , c ₂ and c ₃ are weighting coefficients, which are defined in advance according to the importance of the three evaluation elements. By the formula 1, the combined confidence degree obtained by combining the confidence degrees for the three evaluation elements is obtained.

After that, sensor fusion based on the output of the ultrasonic sensor is applied to the measurement data obtained from the measurement wheel. Here, the composite confidence factor for the ultrasonic sensor calculated by Equation 1 is Bs, the current position of the robot calculated based on the output of the ultrasonic sensor is X'now, and the current position of the robot obtained from the measurement wheel is If Xnow is used, the correction equation for Xnow is expressed by the following equation 2.

[0031]

[Equation 2]

According to Equation 2, the output data of the measuring wheel is corrected (sensor fusion) by the output data of the ultrasonic sensor.

Next, sensor fusion based on the ultrasonic sensor and the optical sensor will be described. For the optical sensor, three confidence functions can be defined as in the ultrasonic sensor. Here, the composite confidence factor for the ultrasonic sensor is B1, the composite confidence factor for the optical sensor is B2,
Let Xnow1 be the current position of the robot calculated based on the output of the ultrasonic sensor, Xnow2 be the current position of the robot calculated based on the output of the optical sensor, and Xnow be the current position of the robot obtained from the measurement wheel. Sensor fusion between the sound wave sensor and the optical sensor is performed by the following equation 3.

[0034]

[Equation 3]

The Xnow correction equation is expressed by the following equation 4.

[0036]

[Equation 4]

According to the equations (3) and (4), the output data of the measuring wheel is corrected by the output data of the ultrasonic sensor and the optical sensor (sensor fusion).

FIG. 1 shows the procedure of sensor fusion using an ultrasonic sensor and an optical sensor. Step S
1, the present position of the robot, that is, the position Xnow in the X-axis direction and the position Yn in the Y-axis direction is calculated based on the output data of the measuring wheel.
ow and the posture θnow, which is the tilt angle of the robot, are calculated.

Next, in step S2, the data from the ultrasonic sensor is input and the data is input from the optical sensor, and in step S3, the composite confidence factor B1 for the ultrasonic sensor is calculated and The current position Xnow1 of the robot is calculated based on the output of the sound wave sensor. Furthermore, in step S4, the composite confidence factor B for the optical sensor is
2 is calculated, and the current position Xnow2 of the robot is calculated based on the output of the optical sensor.

Then, in step S5, the sensor fusion between the ultrasonic sensor and the optical sensor is performed by the above equation 3, and in step S6, the current position Xnow is corrected by X'now by the above equation 4 (sensor fusion). ) Is performed.

As a result, a highly accurate current position Xnow is obtained, and the traveling control of the robot is performed based on this. Further, the same can be applied to the sensor fusion based on three or more sensors.

According to the above-described sensor fusion method of the present invention, the characteristic peculiar to each sensor is quantitatively evaluated by the certainty factor function, and the result of the evaluation is reflected in the correction, so that the accuracy is higher than in the prior art. Measurement data can be obtained.

The above description of the embodiments is for explaining the present invention, and should not be construed as limiting the invention described in the claims or limiting the scope. The configuration of each part of the present invention is not limited to the above-mentioned embodiment, and it goes without saying that various modifications can be made within the technical scope described in the claims. For example, when a sensor whose confidence factor changes over time is used, time is defined as one of the evaluation factors.

[Brief description of drawings]

FIG. 1 is a flowchart showing a procedure of a sensor fusion method according to the present invention.

FIG. 2 is a graph showing three confidence functions.

FIG. 3 is a perspective view of the floor cleaning robot as viewed from the front.

FIG. 4 is a perspective view of the same as seen from the rear side.

FIG. 5 is a block diagram showing a control system of a floor cleaning robot.

[Explanation of symbols]

 (1) Self-propelled carriage (17) Right driving wheel (18) Left driving wheel (19) Auxiliary wheel (22) Right measuring wheel encoder (23) Left measuring wheel encoder

Claims (1)

[Claims] 1. A sensor fusion method for correcting basic measurement data based on one or a plurality of sensor outputs, wherein one or a plurality of characteristics of the sensor such as characteristics and performance of the sensor are evaluated. A first step of defining a certainty factor function that represents the certainty factor of the sensor output by using each evaluation element as a variable by evaluating each element, and for each sensor, a plurality of certainty factor functions defined in the first step. A second step of inputting the current value of the evaluation element corresponding to the measurement data to calculate the certainty factor for each evaluation element, and for each sensor, weighting the plurality of certainty factors calculated in the second step. And a third step of calculating the combined confidence of each sensor, and weighting the output data of each sensor based on the combined confidence. It calculates the correction amount for the data,
And a fourth step of correcting the measurement data with the correction amount.