JP2017166897A - Object detection system, abnormality determination method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To precisely grasp abnormality in a measurement device that detects an object in an object detection system that moves while detecting an object present around.SOLUTION: The object detection system comprises: measurement devices 4a-4d, moving along with a mobile entity, for measuring a coordinate value that represents the positions of feature points L1-L4 of an object present around the mobile entity as seen from the measurement devices 4a-4d; a first change amount calculation unit 53 for calculating a first change amount M1 that represents a change of the position and posture of the measurement devices 4a-4d on the basis of a change between first coordinate values P1-P4 and second coordinate values P1'-P4': a second change amount calculation unit 54 for calculating a second change amount that represents a change of the position and posture of the measurement devices 4a-4d on the basis of a change of the position and posture of the mobile entity before and after movement; and a determination unit 55 for comparing the first change amount M1 and the second change amount M2 and thereby determining abnormality in the measurement devices 4a-4d.SELECTED DRAWING: Figure 3A

Description

  The present invention relates to a system, method, and program for determining an abnormality of an object detection apparatus in an object detection apparatus that is attached to a moving body and detects an object existing around the object while moving.

  2. Description of the Related Art Conventionally, a system that detects an object existing around a vehicle (moving body) such as an automobile and controls the movement of the moving body based on a positional relationship between the detected object and the moving body is known. . In such a system, in order to accurately determine the positional relationship between the detected object and the moving body, it is important to grasp the mounting state of the measuring device that detects the object on the moving body.

  For example, Patent Document 1 discloses a method for detecting a deviation in the direction in which laser light is projected with respect to an object detection device attached to an automobile. In this method, the distance between the object detection device and the overhead marker at the moment when the laser beam is no longer reflected by the overhead marker and cannot be detected, and the object detection device is based on the calculated distance. The degree of deviation from the mounting angle is detected.

JP 2003-43147 A

In the detection method of the above-mentioned Patent Document 1, since the projected laser beam is not reflected by the overhead marker and the laser beam cannot be detected, for example, when the overhead marker is continuously present It was difficult to detect a shift in the mounting angle. In addition, it is difficult to accurately detect the timing at which the laser beam can no longer be detected while the moving body is moving, and thus it is difficult to accurately grasp the current mounting angle.
In addition, since the detection method of Patent Document 1 focuses on overhead signs, when there is no overhead sign in front and there is only an object on the side of the road, or when the road is curved rather than straight ahead Could not be used.

  An object of the present invention is to accurately grasp an abnormality of a measuring apparatus that detects an object in an object detection system that moves while detecting an object existing around the object.

Hereinafter, a plurality of modes will be described as means for solving the problems. These aspects can be arbitrarily combined as necessary.
An object detection system according to an aspect of the present invention includes a moving body, a measurement device, a coordinate prediction unit, and a determination unit. The moving body is a movable object. The measuring device is attached to the moving body and moves together with the moving body. The measuring device measures a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device.
The coordinate prediction unit is configured to determine the position of the object viewed from the moved moving body based on the first coordinate value, the position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the moving. A predicted coordinate value representing the position is calculated. The first coordinate value represents the position of the object measured before the moving body moves. The second coordinate value represents the position of the object measured after the moving body is moved.

In the above object detection system, during the movement of the moving body, the measurement apparatus uses the position viewed from the object measurement apparatus at a predetermined time (before the movement of the moving body) as the first coordinate value from the predetermined time. The position of the object viewed from the measuring device after a certain amount of time has elapsed (after the moving body has moved) is measured as the second coordinate value. Next, the first coordinate value representing the position of the object measured before the moving body moves, the position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement. Based on this, a predicted coordinate value representing the position of the object viewed from the moving body after movement is calculated.
Thereafter, the determination unit compares the predicted coordinate value with the second coordinate value to determine an abnormality occurring in the measurement apparatus.

  In the object detection system described above, the amount of change in the position and orientation of the measurement device calculated from the change in the measurement value of the object position by the measurement device is not information that is difficult to grasp, such as the timing at which the object is no longer observed by the measurement device. In addition, the abnormality of the measuring device is determined based on easy-to-understand information such as the amount of change in the position and posture of the measuring device calculated from the change in the position and posture of the moving body. Thereby, abnormality of a measuring device can be grasped more correctly.

  The object detection system may further include a moving body control unit. The moving body control unit controls the movement of the moving body. In this case, changes in the position and posture of the moving body before and after the movement are calculated based on the control amount of the moving body in the moving body control unit. Thereby, the change of the position and attitude | position of a moving body can be calculated comparatively easily.

  The object detection system may further include a movement amount measuring device. The movement amount measuring apparatus measures an amount representing movement of the moving body before and after movement. Thereby, the change of the position and attitude | position of the moving body before and behind a movement can be determined based on the measured value regarding a movement.

  The change in the position of the moving body before and after the movement may be expressed as a change in the reference position fixed with respect to the moving body before and after the movement of the moving body. In this case, the second change amount is calculated based on the change in the reference position before and after the movement of the moving body, and the relative relationship between the attachment position and orientation of the measurement device with respect to the reference position. Thereby, the second change amount can be calculated relatively easily.

  The positional relationship between the attachment position and the reference position may be constant. Thus, the second change amount can be calculated on the assumption that the mounting position of the measuring device is fixed. As a result, the first change amount and the second change amount can be easily compared.

  An object detection system according to another aspect of the present invention includes a moving body, a first parameter calculation unit, a second parameter calculation unit, a storage unit, and a determination unit. The moving body is a movable object. The measuring device is a measuring device that is attached to a moving body and moves together with the moving body. The measuring device measures a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device. The first parameter calculation unit calculates a first parameter. The first parameter is a parameter that represents a change in the position and orientation of the measurement device that is calculated based on a change between the first coordinate value and the second coordinate value. The first coordinate value represents the position of the object measured before the moving body moves. The second coordinate value represents the position of the object measured after the moving body is moved.

  The second parameter calculation unit calculates a second parameter. The second parameter is a parameter representing changes in the position and posture of the moving body before and after movement. The storage unit stores the third parameter. The third parameter is a parameter that represents the position and orientation of the measuring device with respect to the moving body at a certain time before the moving body moves. A determination part determines the change with respect to a certain time of the attachment position and attitude | position of a measuring apparatus based on a 1st parameter, a 2nd parameter, and a 3rd parameter.

  In the object detection system described above, based on information that is easy to grasp, such as the first parameter, the second parameter, and the third parameter, rather than information that is difficult to grasp such as the timing at which the object is no longer observed by the measurement device, The change of the attachment position and posture with respect to a certain point is determined. Thereby, the change of the attachment position and attitude | position of a measuring apparatus can be grasped | ascertained more accurately.

  The determination unit may calculate the presence / absence of the change or the degree of change of the attachment position and orientation of the measurement device with respect to a certain time, thereby determining how much the attachment state of the measurement device has changed.

  The determination unit may calculate the presence / absence of change or the degree of change based on the value of the determinant of the matrix generated from the first parameter, the second parameter, and the third parameter. Thereby, how much the mounting state of the measuring apparatus has changed can be quantified.

  The change in the position of the moving body before and after the movement may be expressed as a change in the reference position fixed with respect to the moving body before and after the movement of the moving body. Thereby, the second parameter can be calculated relatively easily.

An abnormality determination method according to still another aspect of the present invention is an abnormality determination method for a measuring apparatus attached to a movable body that is a movable object. The abnormality determination method includes the following steps.
A step of causing the measuring device to measure the first coordinate value representing the position of the object existing around the moving body viewed from the measuring device before the moving body moves.
A step of causing the measuring device to measure the second coordinate value representing the position of the object viewed from the measuring device after the moving body is moved.
Based on the first coordinate value, the position information of the measuring device relative to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement, a predicted coordinate value representing the position of the object viewed from the moving body after movement is calculated. And steps to
A step of determining an abnormality of the measuring device by comparing the predicted coordinate value and the second coordinate value.

  In the above-described abnormality determination method, the amount of change in the position and orientation of the measurement device calculated from the change in the measurement value of the object position by the measurement device is not difficult information such as the timing at which the object is no longer observed by the measurement device. In addition, the abnormality of the measuring device is determined based on easy-to-understand information such as the amount of change in the position and posture of the measuring device calculated from the change in the position and posture of the moving body. Thereby, abnormality of a measuring device can be grasped exactly.

An abnormality determination method according to still another aspect of the present invention is an abnormality determination method of an attachment state of a measuring device attached to a movable body that is a movable object. The abnormality determination method includes the following steps.
A step of causing the measuring device to measure the first coordinate value representing the position of the object existing around the moving body viewed from the measuring device before the moving body moves.
A step of causing the measuring device to measure the second coordinate value representing the position of the object viewed from the measuring device after the moving body is moved.
A step of calculating a first parameter representing a change in the position and orientation of the measuring device based on a change between the first coordinate value and the second coordinate value.
A step of calculating a second parameter representing a change in the position and posture of the moving body before and after the moving body moves.
A step of reading, from the storage unit, a third parameter that is stored in the storage unit at a certain time before the mobile unit moves and that represents the position of the measuring device with respect to the mobile unit.
A step of determining a change in the mounting position and posture of the measuring device with respect to a certain time point based on the first parameter, the second parameter, and the third parameter.

  In the above abnormality determination method, based on information that is easy to grasp, such as the first parameter, the second parameter, and the third parameter, rather than information that is difficult to grasp such as the timing at which the object is no longer observed by the measuring device, The change of the attachment position and posture with respect to a certain point is determined. Thereby, the change of the attachment position and attitude | position of a measuring apparatus can be grasped | ascertained correctly.

  A program according to still another aspect of the present invention is a program that causes a computer to execute the abnormality determination method.

  In an object detection system that moves while detecting surrounding objects, it is possible to accurately grasp the abnormality of the measuring apparatus that detects the surrounding objects.

The figure which shows the structure of an object detection system. The figure which shows the structure of a measuring apparatus. The figure which shows the structure of a control part. The figure which shows the structure of the control part in other embodiment. The flowchart which shows operation | movement of an object detection system. The flowchart which shows the abnormality determination process of a measuring device. The figure which shows an example of real space. The figure which shows an example of the position data before a movement. The figure which shows an example of the position data after a movement. The figure which shows an example of the attachment position of a measuring apparatus. The figure which shows an example of the state which the attachment state of the measuring apparatus changed. The figure which shows an example of the 2nd position data acquired after the attachment state of a measuring apparatus changed.

1. First Embodiment (1) Configuration of Object Detection System A configuration of an object detection system 100 according to the first embodiment will be described below with reference to FIG. FIG. 1 is a diagram illustrating a configuration of an object detection system. The object detection system 100 according to the first embodiment is a system that assists an operation by a driver of the moving body 1 that is, for example, various vehicles such as an automobile.

  The object detection system 100 includes a moving body 1. The moving body 1 is a movable object, for example, a vehicle. The moving body 1 includes wheels 2a, 2b, 2c, and 2d. The wheels 2a and 2b can rotate around an axis via a speed reduction mechanism on the output rotation shaft of a power function (for example, an engine and / or an electric motor) at the front of the moving body 1 in the straight traveling direction (FIG. 1). It is attached. On the other hand, the wheels 2c and 2d are attached to the rear part of the moving body 1 in the straight traveling direction so as to be rotatable around the axis.

  The object detection system 100 includes four measuring devices 4a, 4b, 4c, and 4d. As shown in FIG. 1, the measuring device 4 a is attached to the forefront part of the moving body 1 in the straight traveling direction and detects an object existing in front of the moving body 1. The measuring device 4 b is attached to the rearmost part of the moving body 1 in the straight traveling direction, and detects an object existing behind the moving body 1. The measuring device 4 c is attached to the left side surface of the moving body 1 in the straight traveling direction, and detects an object existing on the left side of the moving body 1. The measuring device 4d is attached to the right side surface of the moving body 1 in the straight traveling direction, and detects an object present on the right side of the moving body 1. Since the four measuring devices 4a to 4d are attached to the moving body 1, when the moving body 1 moves, the four measuring apparatuses 4a to 4d move together with the movement of the moving body.

  In the present embodiment, the measuring devices 4a to 4d are TOF (Time Of Flight) sensors that measure the distance to the object B to be detected. However, the measurement apparatus is not limited to this, and for example, a stereo camera that measures the distance from the image difference between the left and right cameras, a laser range finder (laser range finder, LRF), or another type of distance measurement sensor may be used. Good. The configuration of the measuring devices 4a to 4d in the present embodiment will be described in detail later.

The object detection system 100 includes a control unit 5. The control unit 5 includes a CPU (Central Processing Unit), a storage device (RAM (Random Access Memory), ROM (Read Only Memory), SSD (Solid State Drive), HDD (Hard Disk Drive, etc.) and the like. For example, a computer system provided with an A / D, a D / A converter, and the like.
The control unit 5 inputs detection signals from the measuring devices 4a to 4d, and determines the positional relationship between the object B and the moving body 1 existing around based on the detection signals. The control unit 5 is connected to the moving body control unit 3, and the moving body control unit 3 further determines the braking function, power function, and / or moving direction of the moving body 1 provided on the wheels 2a and 2b. It is connected to the travel function such as the rotation function of the steering wheel to be changed. The control unit 5 outputs positional relationship information between the object B and the moving body 1, and the moving body control unit 3 that has received the information outputs a traveling function on behalf of the driver of the object detection system 100 as necessary. Control. The configurations of the control unit 5 and the moving body control unit 3 will be described in detail later.

The object detection system 100 includes a movement amount measuring device 6. The movement amount measuring device 6 measures an amount representing movement of the moving body 1. As the moving amount measuring device 6, for example, a speed sensor that measures the speed of the moving body 1, an acceleration sensor that measures acceleration, and / or an encoder that measures the amount of rotation of the wheels 2a to 2d, and the angular velocity of the moving body 1 are used. A gyro sensor to be measured can be used. In addition, as the movement amount measuring device 6, a GPS (Global Positioning System) device may be used.
Since the object detection system 100 includes the movement amount measuring device 6, changes in the position and posture of the moving body 1 before and after the movement can be determined based on the actually measured values related to the movement.

  The controller 5 calculates the measurement devices 4a to 4a before and after the movement of the moving body 1 calculated based on the position data D1 and D2 generated from the data acquired by the measurement devices 4a to 4d before and after the movement of the moving body 1. Measurement devices 4a to 4d before and after the movement of the moving body 1 calculated based on the amount of change in the position and orientation of 4d (referred to as the first change amount M1) and the change in the position and orientation of the moving body 1 before and after the movement. Based on the comparison with the change amount of the position and orientation (referred to as the second change amount M2), the abnormality of the measuring devices 4a to 4d is determined.

Examples of the measuring devices 4a to 4d determined in the present embodiment include a change in the mounting state of the measuring devices 4a to 4d on the moving body 1 and a failure of the measuring devices 4a to 4d.
Examples of the change in the attachment state of the measuring devices 4a to 4d to the moving body 1 include, for example, changes in the mounting position and the mounting angle of the measuring devices 4a to 4d on the moving body 1 and the moving bodies of the measuring devices 4a to 4d. There is a fixed degree to 1.
On the other hand, as the failure of the measuring devices 4a to 4d to be determined, for example, the displacement of the mounting position of the output unit 41 and the detection surface DS (described later) inside the measuring devices 4a to 4d, or the detection windows of the measuring devices 4a to 4d ( There are dirt, scratches, etc. on a window that transmits measurement light Lm and reflected light Lr described later.
The abnormality determination method of the measuring devices 4a to 4d in the control unit 5 will be described in detail later.

  By providing the above configuration, the object detection system 100 can assist the driving of the moving body 1 by the driver based on the positional relationship between the object B and the moving body 1 detected by the measuring devices 4a to 4d. Further, in the object detection system 100, the abnormality of the measuring devices 4a to 4d can be determined while the moving body 1 is traveling.

(2) Configuration of Measuring Device Next, the configuration of the measuring devices 4a to 4d will be described with reference to FIG. FIG. 2 is a diagram illustrating the configuration of the measurement apparatus. Since the four measuring devices 4a to 4d have the same configuration, the configuration of the measuring device 4a will be described below as an example.
The measuring device 4 a has an output unit 41. The output unit 41 is, for example, a light source that outputs measurement light Lm in the infrared region toward the object B that is a detection target. As shown in FIG. 2, in the present embodiment, the output unit 41 preferably outputs the measurement light Lm that has spread over a wide space in which the object detection system 100 moves. This is because the measuring device 4a can simultaneously detect the object B in a wide range of the space in which the object detection system 100 moves.

  The measuring device 4a includes a plurality of detection units 43-1, 43-2, ... 43-n. Each of the plurality of detection units 43-1 to 43-n is disposed at a predetermined position on the detection surface DS (semiconductor substrate), for example, and is reflected light that is generated when the measurement light Lm is reflected by the object B. Lr is detected. The detection units 43-1 to 43-n are, for example, charge coupled devices (Charge Coupled Devices) or CMOS (Complementary MOS) devices.

As shown in FIG. 2, the plurality of detection units 43-1 to 43-n are arranged in the vertical direction and the horizontal direction on the detection surface DS to form an array. Accordingly, the plurality of detection units 43-1 to 43-n can form a CCD image sensor or a CMOS image sensor on the detection surface DS.
A switching element (for example, a MOS-FET) for connecting / disconnecting the detection unit and the external control unit 5 is connected to each of the detection units 43-1 to 43-n. In addition, an address line is connected to the switching element, and when a signal is applied to the address line, the switching element is turned on, and the detection unit connected to the turned on switching element and the control unit 5 include Signal transmission / reception becomes possible.

  The measuring device 4a has a lens 45. The lens 45 condenses the reflected light Lr in the detection surface DS in a region where the plurality of detection units 43-1 to 43-n are formed. Thereby, an image of the object B in a wide range can be formed in an area where the plurality of detection units 43-1 to 43-n are formed.

  By having the above configuration, the measuring device 4a can acquire data by projecting the object B in the real space on which the object detection system 100 moves onto predetermined coordinates.

(3) Configuration of Control Unit The configuration of the control unit 5 of the object detection system 100 according to the present embodiment will be described below with reference to FIG. 3A. FIG. 3A is a diagram illustrating a configuration of a control unit. Part or all of the functions of the elements of the control unit 5 described below may be realized as a program that can be executed by a computer system that configures the control unit 5. At this time, the program may be stored in a storage area formed in a storage device of the computer system. In addition, some or all of the functions of each element of the control unit 5 may be realized in hardware by a custom IC or the like.
The control unit 5 includes a storage unit 51. The storage unit 51 stores various data, for example, a part of a storage area provided in a storage device of a computer system.

The control unit 5 includes a position data generation unit 52. The position data generation unit 52 generates position data D1 and D2. The position data D1 and D2 are generated as a set of coordinate values representing the relative position of the object B detected by the measuring device 4a as viewed from each measuring device.
Specifically, the position data generation unit 52 generates the position data D1 and D2 as follows. Hereinafter, a case where the position data D1 and D2 of the measuring device 4a are generated will be described as an example.
The position data generation unit 52 first connects the first detection unit 43-1 and the position data generation unit 52 by applying a signal to the address line corresponding to the first detection unit 43-1. A signal (for example, a current or voltage signal) indicating whether or not the detection unit 43-1 has detected the reflected light Lr is input. After inputting the signal, the position data generation unit 52 detects the difference between the time when the signal indicating whether or not the reflected light Lr is detected and the time when the output unit 41 outputs the measurement light Lm. Calculated as information T1.
If the reflected light Lr is not detected, the signal detection information T1 is set to infinity (or a very large value) because no signal is input from the detection unit 43-1.

Next, the position data generation unit 52 generates position information (x1, y1, T1). The positional information can be generated by associating the coordinate values (x1, y1) defining the arrangement position on the detection surface DS of the detection unit 43-1 with the signal detection information T1.
Thereafter, the position data generation unit 52 executes the above-described process for all the other detection units 43-2 to 43-n by sequentially changing the address lines to which the signal is applied, so that n positions are obtained. A set of information (x1, y1, T1), (x2, y2, T2),... (Xn, yn, Tn) is generated.

After that, the position data generation unit 52 converts each of the n pieces of position information aggregates (x1, y1, T1), (x2, y2, T2),... (Xn, yn, Tn), The position data D1 and D2 are generated by converting into coordinate values in a coordinate system representing the real space in which the moving body 1 moves.
Specifically, taking the case where the p-th position information (xp, yp, Tp) is converted into the above coordinate value as an example, the position data generation unit 52 firstly sets the signal detection information Tp (time unit). Information) is converted into a distance zp (= c * Tp / 2, c: speed of light) from the detection surface DS to the object B existing in the real space, with the time when the measurement light Lm is output from the output unit 41 as 0. . Next, the position data generation unit 52 enlarges the coordinate value (xp, yp) defined on the detection surface DS at an enlargement rate determined based on, for example, the distance zp or the focal length (of the lens 45). Thus, the coordinate value of the object B in the coordinate system representing the real space with the measuring device 4a as the origin is calculated as (Xp, Yp). Further, assuming that the coordinate value in the Z-axis direction is Zp = zp, the coordinate value of (Xp, Yp, Zp) can be finally calculated.
The position data D1 and D2 are obtained by executing the conversion of the coordinate values for all the n pieces of position information.

The control unit 5 includes a first change amount calculation unit 53 (an example of a first parameter calculation unit). The first change amount calculation unit 53 includes a first coordinate value that is a coordinate value of the object measured by the measuring device 4a before the moving body 1 moves, and an object measured by the measuring device 4a after the moving body 1 moves. A first change amount M1 representing a change in the position and orientation of the measuring device 4a before and after the movement of the moving body 1 is calculated based on a change between the second coordinate value and the second coordinate value.
Specifically, the first change amount calculation unit 53 selects the first coordinate value from the first position data D1 generated before the moving body 1 moves, and is generated before the moving body 1 moves. A second coordinate value is selected from the second position data D2, and a change amount between the first coordinate value and the second coordinate value is calculated as a first change amount M1. The calculation method of the first change amount M1 in the first change amount calculation unit 53 will be described in detail later.

The control unit 5 includes a second change amount calculation unit 54 (an example of a second parameter calculation unit). The second change amount calculation unit 54 is a second change that represents a change in the position and orientation of the measuring device 4a before and after the movement of the moving body 1 based on changes in the position and orientation of the moving body 1 before and after the movement of the moving body 1. The amount M2 is calculated.
Specifically, the second change amount calculation unit 54 changes the reference position fixed with respect to the moving body 1 before and after the movement of the moving body 1, and the attachment positions Ps1 to Ps4 and the postures of the measuring devices 4a to 4d. The second change amount M2 is calculated based on the relative positional relationship with respect to the reference position.

In the present embodiment, the relative position of the mounting position and / or posture (mounting angle) of the measuring device 4a used for calculating the second change amount M2 with respect to the reference position is determined at the time of mounting and does not change unless there is an abnormality. . In the present embodiment, the second change amount calculation unit 54 moves the moving body 1 based on the measured values of acceleration, speed, and / or position of the moving body 1 measured by the movement amount measuring device 6. The amount of movement before and after is calculated. Thereby, the movement amount before and after the movement of the mobile body 1 can be calculated based on the measured physical quantity.

  In addition, the second change amount calculation unit 54 is configured to move the vehicle 1 (the object detection system 100) such as the accelerator opening, the brake strength, and / or the steering angle of the steering wheel input to the mobile body control unit 3 (described later). ) And / or the movement amount of the moving body 1 based on the control amount relating to the movement of the moving body 1 determined by the moving body control unit 3 based on the positional relationship between the object B and the moving body 1. May be calculated. Thereby, the movement amount of the moving body 1 can be calculated relatively easily. A method of calculating the second change amount M2 in the second change amount calculation unit 54 will be described in detail later.

The control unit 5 includes a determination unit 55. The determination unit 55 is calculated based on a change between the first coordinate value and the second coordinate value before and after the movement of the moving body 1 (that is, a change in the position of the object viewed from the measurement device 4a before and after the movement). Specifically, the determination unit 4a determines an abnormality of the measuring device 4a by comparing the first change amount with a second change amount calculated based on changes in the position and orientation of the moving body 1 before and after the movement. 55 is a value calculated as a difference between the first change amount M1 and the second change amount M2 in consideration of a measurement error of the measurement device 4a or the movement amount measurement device 6 or the like, It is determined that an abnormality has occurred in the measuring device 4a. On the other hand, if the value calculated as the difference between the first change amount M1 and the second change amount M2 is smaller than the threshold δ, it is determined that the measuring device 4a is operating normally.

If it is determined that an abnormality has occurred in the measurement device 4a, the determination unit 55 may display on the display (not shown) of the object detection system 100 that an abnormality has occurred in the measurement device 4a (abnormal alarm). Or the determination part 55 may notify the said abnormality alarm with an audio | voice etc. using the speaker with which the object detection system 100 is equipped. Thereby, the abnormality of the measuring apparatus 4a can be notified with respect to the driver | operator etc. of the moving body 1 (object detection system 100).
An abnormality determination method of the measuring device 4a based on the difference between the first change amount M1 and the second change amount M2 in the determination unit 55 will be described in detail later.

The control unit 5 includes a real space data output unit 56. The real space data output unit 56 outputs real space data VD. The mobile body control unit 3 (FIG. 1) receives the real space data VD, and the control amount (for example, accelerator opening, brake strength, and / or steering angle of the steering wheel, etc.) by the driver of the mobile body 1 (object detection system 100). ) And output to the second change amount calculation unit 54. In addition, the moving body control unit 3 controls the control amount (accelerator opening degree, brake strength, and / or steering angle, etc.) of the moving body 1 (object detection system 100) based on the positional relationship between the moving body 1 and the object B. ).
For example, if the moving body control unit 3 determines that the object B is present in the vicinity of the moving body 1, the moving body control unit 3 gives a command to increase the brake strength and the accelerator opening (or the engine and / or the electric motor output control mechanism). A command to change to 0 and / or a command to change the steering angle of the steering wheel in a direction away from the object B are output to a driving function (not shown) such as a power function, a brake, and / or a steering wheel. Thereby, it can avoid that the mobile body 1 (object detection system 100) collides with the object B. FIG.
Moreover, the mobile body control part 3 may output the instruction | command which cut | disconnects the clutch between an engine and / or an electric motor (power function) and the wheels 2a and 2b as needed.

  By having said structure, the control part 5 determines whether abnormality, such as the attachment state of the measuring apparatus 4a, and a failure generate | occur | produced, and the mobile body control part 3 is the position of the mobile body 1 and the object B. While controlling the movement of the moving body 1 (the object detection system 100) based on the relationship, the driver or the like can be notified when there is an abnormality in the measuring device 4a.

(4) Operation of Object Detection System (4-1) Overall Operation Hereinafter, the operation of the object detection system 100 will be described. First, the overall operation of the object detection system 100 will be described with reference to FIG. FIG. 4 is a flowchart showing the operation of the object detection system.
When the object detection system 100 starts operation, first, the position data generation unit 52 generates the position data D1 and D2 as described above, and stores them in the storage unit 51 (step S1). At this time, the storage unit 51 stores the first position data D1 generated before the movement of the moving body 1 (position data generated last time) and the second position data D2 generated after the movement of the moving body 1 (current time). Generated position data).
Further, the second change amount calculation unit 54 obtains the control amount of the moving body 1 from the acquisition of the first position data D1 to the acquisition of the second position data D2, and the movement amount measuring device 6 and / or the moving body. Obtained from the control unit 3 and stored as movement amount data D3.

  After acquiring the position data D1 and D2, the control unit 5 determines an abnormality occurring in the measurement device 4a using the position data D1 and D2 and the movement amount data D3 (step S2). The control unit 5 measures the first change amount M1 representing a change in the position and orientation (mounting angle) of the measurement device 4a calculated based on the position data D1 and D2, and the measurement device calculated based on the movement amount data D3. Based on the comparison with the second change amount M2 representing the change in the position and orientation of 4a, the abnormality of the measuring device 4a is determined. The abnormality determination process of the measuring device 4a in step S2 will be described in detail later.

As a result of executing the above step S2, when it is determined that an abnormality has occurred in the measuring device 4a (in the case of “No” in step S3), the moving body control unit 3 determines that the object B and the moving body 1 It is determined that the positional relationship cannot be accurately grasped. As a result, the process proceeds to step S7 without detecting the object B.
If it is determined that an abnormality has occurred in the measurement device 4a, the determination unit 55 notifies the driver of the object detection system 100 to that effect. The driver of the moving body 1 that has recognized the abnormality of the measuring apparatus 4a, for example, stops the movement of the moving body 1 and adjusts the mounting position of the measuring apparatus 4a as necessary, for example, in the measuring apparatus 4a. You can eliminate the abnormalities.

Further, as an abnormality of the measuring device 4a, for example, a window that transmits the measuring light Lm and reflected light Lr of the measuring device 4a is dirty, water droplets are attached, or there are scratches on the window. is there. This is because if the window that transmits the measurement light Lm or the reflected light Lr is dirty or has water droplets attached thereto, or if the window is damaged, the optical path of the measurement light Lm or the reflected light Lr changes from the normal optical path. It is.
In such a case, for example, the measurement device 4a can be operated normally by removing dirt or water droplets from the window of the measurement device 4a or replacing the measurement device 4a that has caused an abnormality.

  On the other hand, when it is determined that the measuring device 4a is normal (in the case of “No” in step S3), the moving body control unit 3 determines that the positional relationship between the object B and the moving body 1 can be accurately grasped. . In this case, the moving body control unit 3 performs detection of the object B in the vicinity of the moving body 1 using the second position data D2 generated after the movement of the moving body 1 (step S4). Specifically, the moving body control unit 3 confirms whether there is a coordinate value included in the second position data D2 that has a Z coordinate value equal to or less than a predetermined value.

  When it is determined that a coordinate value having a Z coordinate value equal to or smaller than a predetermined value exists in the second position data D2 and the object B exists in the vicinity of the moving body 1 (in the case of “Yes” in step S5), For example, the moving body control unit 3 issues a command to apply a brake, a command to set the accelerator opening to 0, and / or a command to change the steering wheel angle so as to avoid a collision with the object B in the vicinity of the moving body 1. , Output to a driving function such as a power function, a brake, and / or a steering wheel (step S6). Thereby, the mobile body 1 can avoid the collision with the object B existing in the vicinity.

  When it is determined that no coordinate value having a Z coordinate value equal to or smaller than a predetermined value exists in the second position data D2 and the object B does not exist in the vicinity of the moving body 1 (in the case of “No” in step S5) ) The process proceeds to step S7 without the mobile body control unit 3 performing control on the mobile body 1 so as to avoid the collision with the object B.

After performing the detection of the object B and the avoidance control as necessary, the control unit 5 determines whether or not to stop the operation of the object detection system 100 (step S7). For example, when the stop of the object detection system 100 is instructed by the driver of the moving body 1 or the like (in the case of “Yes” in step S7), the control unit 5 stops the operation of the object detection system 100.
On the other hand, when the stop of the object detection system 100 is not instructed (in the case of “No” in step S7), the above steps S1 to S6 are continuously executed.

By executing steps S1 to S7 described above, the object detection system 100 determines whether or not the object B exists in the vicinity of the moving body 1 when there is no abnormality in the measurement device 4a. If the object B exists, the moving body 1 can be controlled to avoid a collision with the object B.
On the other hand, when an abnormality has occurred in the measuring device 4a, the object detection system 100 can notify the driver of the moving body 1 or the like of an abnormality alarm. As a result, the driver can recognize the abnormality of the measuring device 4a and adjust the measuring device 4a as necessary to eliminate the occurring abnormality.

(4-2) Measurement Device Abnormality Determination Processing Next, the abnormality determination processing of the measurement device 4a executed in step S2 will be described with reference to FIG. FIG. 5 is a flowchart showing an abnormality determination process of the measuring apparatus.
In the following description, as shown in FIG. 6, the moving object 1 is a real space in which an object B1 that is a building that is long in the horizontal direction and an object B2 that is a pedestal arranged in front of the object B1 exist. A case will be described as an example in which moves between time t1 and time t2. FIG. 6 is a diagram illustrating an example of real space.
In the example shown in FIG. 6, the reference position O of the moving body 1 at the time t1 moves to the reference position O ′ at the time t2, and the moving body 1 changes the moving direction (that is, the posture) by a predetermined angle. . In the following, a case where an abnormality of the measuring device 4a is determined will be described as an example.

  When the abnormality determination process is started, first, the first change amount calculation unit 53 uses the first position data D1 generated at time t1 to call the object coordinate values at time t1 (referred to as first coordinate values P1 to P4). Is acquired (step S21).

Specifically, for example, when the first position data D1 as shown in FIG. 7 is acquired at time t1, the first change amount calculation unit 53 first has four characteristic points L1 shown in FIG. Select ~ L4. FIG. 7 is a diagram illustrating an example of position data before movement. Feature point candidates can be extracted using point detection methods such as the Harris method, KLT method, principal curvature method, etc. by expanding the position information into two-dimensional coordinates set on the detection surface DS. Here, the reason for selecting the four feature points L1 to L4 is that, as will be described later, the first change amount calculation unit 53 sets the first change amount M1 (first parameter S _M ) as a matrix of 4 rows and 4 columns. It is for calculating.

In the example shown in FIG. 7, two structures attached to the upper part of the object B1 that is a building are feature points L1 and L2, and two structures on the object B2 that is a base are feature points L3 and L4. Is selected.

The first change amount calculation unit 53 acquires the coordinate values of the selected feature points as the first coordinate values P1 to P4. For example, the first change amount calculation unit 53 sets the first coordinate value P1 to (X ₁ , Y ₁ , Z ₁ ), the first coordinate value P2 to (X ₂ , Y ₂ , Z ₂ ), and the first coordinate It is assumed that the value P3 is determined as (X ₃ , Y ₃ , Z ₃ ) and the first coordinate value P4 is determined as (X ₄ , Y ₄ , Z ₄ ).

Next, the first change amount calculation unit 53 searches the feature point corresponding to the feature point selected in the first position data D1 from the second position data D2 generated at the time t2 by image recognition or the like, The coordinate values of the corresponding feature points found by the search are acquired as the second coordinate values P1 ′ to P4 ′ after movement of the moving body 1 in the same manner as in step S21 (step S22).
For example, the second coordinate value P1 ′ is (X ₁ ′, Y ₁ ′, Z ₁ ′), the second coordinate value P2 ′ is (X ₂ ′, Y ₂ ′, Z ₂ ′), and the second coordinate value. 'the _{(X 3' P3, Y 3} ', Z 3') and 'a _{(X 4'} second coordinate value _{P4, Y 4 ', Z 4} ') and was determined to.

After obtaining the first coordinate values P1 to P4 and the second coordinate values P1 ′ to P4 ′, the first change amount calculation unit 53 obtains the obtained first coordinate values P1 to P4 and the second coordinate values P1 ′ to P4 ′. The first change amount M1 representing the change in the position and orientation of the measuring device 4a before and after the moving body 1 is moved is calculated (step S23).
The first position data D1 and the second position data D2 are respectively expressed as coordinate values of the objects B1 and B2 in the coordinate system (that is, the coordinate system viewed from the measurement device 4a) set in the measurement device 4a. Therefore, with the movement of the moving body, the position of the measuring device 4a changes from the attachment position Ps1 to the attachment position Ps1 ′ between time t1 and time t2, as shown in FIG. Further, as the moving body 1 moves, the Z axis representing the normal direction of the detection surface DS of the measuring device 4a changes to the Z ′ axis as shown in FIG.
As described above, by changing the position and orientation of the measuring device 4a, the images of the objects B1 and B2 indicated by the first position data D1 (dotted lines in FIGS. 7 and 8) are the second position data D2. It changes as shown by the solid line in FIG. FIG. 8 is a diagram illustrating an example of the position data after movement.

However, as shown in FIGS. 6 to 8, the moving direction of the measuring device 4a (moving body 1) is opposite to the moving direction of the image indicated by the first position data D1 and the second position data D2. Become. For example, as shown in FIG. 6, the moving body 1 moves to the right in the drawing, while the images of the objects B1 and B2 indicated by the second position data D2 are the objects B1 and B2 indicated by the first position data D1. It is moving to the left of the page.
Accordingly, the first change amount calculation unit 53 of the present embodiment moves the coordinate values of the second coordinate values P1 ′ to P4 ′ to the first coordinate values P1 to P4 (that is, indicated by the second position data D2). an image of an object B1, B2 which is a mobile and deformed to coordinate conversion to the image of the object B1, B2 shown in a first position data D1) for calculating a first parameter S _M.

In this embodiment, the first change amount calculation unit 53, by using the homogeneous coordinate representation in three-dimensional space to calculate a first parameter S _M. Thereby, the rotational motion and the translational motion of the three-dimensional space can be expressed by one matrix.
Specifically, first, (X ₁ , Y ₁ , Z ₁ , 1), (X ₂ , Y ₂ , Z ₂ , 1) are respectively obtained from the first coordinate values P1 to P4 acquired in step S22. ), (X ₃ , Y ₃ , Z ₃ , 1) and (X ₄ , Y ₄ , Z ₄ , 1) are generated. Further, from the second coordinate values P1 ′ to P4 ′, (X ₁ ′, Y ₁ ′, Z ₁ ′, 1), (X ₂ ′, Y ₂ ′, Z ₂ ′, 1), (X ₃ ), respectively. Four-dimensional coordinates of ', Y ₃ ', Z ₃ ', 1) and (X ₄ ', Y ₄ ', Z ₄ ', 1) are generated.

The first parameter _{S M,} the second coordinate value P1'~P4 ', since it is defined as a matrix for moving to the first coordinate value P1 to P4, the movement of coordinate values using the first parameter _{S M} Four formulas shown: (X ₁ , Y ₁ , Z ₁ , 1) ^T = _SM (X ₁ ′, Y ₁ ′, Z ₁ ′, 1) ^T , (X ₂ , Y ₂ , Z ₂ , 1) ^T = _SM (X ₂ ′, Y ₂ ′, Z ₂ ′, 1) ^T , (X ₃ , Y ₃ , Z ₃ , 1) ^T = _SM (X ₃ ′, Y ₃ ′, Z ₃ ′, 1) ^T and (X ₄ , Y ₄ , Z ₄ , 1) ^T = _SM (X ₄ ′, Y ₄ ′, Z ₄ ′, 1) ^T can be defined. Note that the superscript “T” in the above formula represents transposition of a matrix. When the above four formulas are combined into one formula, the following formula 1 is obtained.

Next, in Equation 1 above, by multiplying both sides by the inverse matrix of the matrix that is not the first parameter S _M on the right side, the first parameter S _M (4 × 4 matrix) as shown in Equation 2 below. Can be calculated as the first change amount M1.
Since the right side of Equation 2 includes only the known coordinate values calculated in Steps S21 and S22, the first change amount calculation unit 53 uses Equation 2 to express a specific value represented as a numerical value. The first parameter S _M (first change amount M1) can be calculated. Note that the inverse matrix on the right side of Equation 2 can be calculated by a method of calculating an inverse matrix such as a sweep-out method. The first change amount calculation unit 53, a first parameter S _M calculated as specific numerical values in the storage unit 51 as the first change amount M1.

Calculating a first parameter S _M using equation 2 as described above, after storage in the storage unit 51 as the first change amount M1, the second change amount calculation unit 54 calculates a second change amount M2. The second change amount calculation unit 54 assumes that the mounting position and the mounting angle of the measuring device 4a have not changed before and after the movement of the moving body 1 (that is, between the time t1 and the time t2). Based on changes in position and orientation due to movement of the moving body 1 during t2, and relative relations of the mounting positions Ps1, Ps1 ′ and the mounting angle of the measuring device 4a to the reference positions O, O ′ of the moving body 1. Thus, the second change amount M2 is calculated.

The above-mentioned “relative relationship of the mounting positions Ps1 and Ps1 ′ of the measuring device 4a and the mounting angle to the reference positions O and O ′ of the moving body 1” refers to the reference positions O and O ′ in the present embodiment. It is defined as the amount of movement when translated to Ps1 and Ps1 ′. Since the normal direction of the detection surface DS of the measuring device 4a in the normal state is determined to be parallel to the straight direction of the moving body 1, the measuring device 4a has the reference positions O and O ′ of the moving body 1 at the mounting angle. There is no relative relationship to.
Therefore, for example, a matrix that translates the coordinate value (Ox, Oy, Oz, 1) of the reference position O of the moving body 1 to the coordinate value (Ps1x, Ps1y, Ps1z, 1) of the mounting position Ps1 of the measuring device 4a. Assuming that the matrix is U, the following equation is established: (Ps1x, Ps1y, Ps1z, 1) ^T = U (Ox, Oy, Oz, 1) ^T The matrix U that moves the reference positions O and O ′ to the attachment positions Ps1 and Ps1 ′ will be referred to as a third parameter U.

When the coordinate value of the mounting position of the measuring device 4a is (a, b, c) when the coordinate value of the reference position of the moving body 1 is (0, 0, 0), the third parameter U is It is defined as Equation 3 below. For example, as shown in FIG. 9, when the reference position is the origin (0, 0, 0), and the coordinate value of the mounting position of the measuring device 4a is (0, 0, c), Equation 3 Both a and b of the third parameter U indicated by are 0. FIG. 9 is a diagram illustrating an example of an attachment position of the measurement device.

  The third parameter U is defined as representing the position and orientation of the measuring device 4a with respect to the moving body 1 at a certain time before the moving body 1 moves. Furthermore, in this embodiment, it is assumed that the attachment position and the attachment angle of the measuring device 4a are not changed before and after the movement of the moving body 1 unless there is an abnormality. Therefore, the third parameter U is a constant value set in advance. That is, the element values a, b, and c of the third parameter U do not change after being set at a certain point in time. Therefore, the third parameter U is set in advance and stored in the storage unit 51.

After defining the third parameter U as described above, the second change amount calculation unit 54 performs an actual measurement value relating to movement from the movement amount measurement device 6 acquired as movement amount data D3 between time t1 and time t2, and Alternatively, the change in the position and orientation of the moving body 1 before and after the movement is calculated from the control amount of the moving body 1 output from the moving body control unit 3 (step S24).
For example, when an actual speed value is input from the movement amount measuring device 6, the movement amount data D3 as the speed acquired between the time t1 and the time t2 is accumulated, or the speed is integrated by time. The moving distance of the moving body 1 that has moved between time t1 and time t2 can be calculated. On the other hand, when the actual measured acceleration value is input from the movement amount measuring device 6 as the movement amount data D3, the movement distance is calculated by calculating the speed from the acceleration and then integrating the speed by accumulation or time. it can.
Further, the moving distance can be calculated as described above from the speed and acceleration calculated from the accelerator opening degree of the moving body 1 output from the moving body control unit 3.

In addition, when the movement amount measuring device 6 or the moving body control unit 3 inputs the change in the angle (direction) of the moving body 1 or the steering angle of the steering wheel as the movement amount data D3, the above moving distance is taken into consideration as necessary. Thus, the angle change of the moving body 1 from the time t1 to the time t2 can be calculated.
Further, when the position in the moving space at the time t1 and the position in the moving space at the time t2 are acquired as the movement amount data D3 from the movement amount measuring device 6, the movement is calculated from the difference between these two positions. Changes in the position and posture of the body 1 can be calculated.

The moving amount of the reference position of the moving body 1 is defined as a matrix T (referred to as a second parameter T) that moves the reference position O at time t1 to the reference position O ′ at time t2, and the calculated position of the moving body 1 When the change in the posture (angle) is reflected in each element of the second parameter T, the second parameter T is calculated as in the following Expression 4, for example.

In the second parameter T, ΔX, ΔY, and ΔZ of each element in the fourth column are respectively the coordinate value of the reference position O of the moving body 1 as ΔX in the X-axis direction and ΔY, Z-axis in the Y-axis direction. This corresponds to the movement in the direction by ΔZ. That is, if the coordinate value of the reference position O before movement is (Ox, Oy, Oz, 1), it means that the coordinate value of the reference position O ′ after movement is changed to (Ox + ΔX, Oy + ΔY, Oz + ΔZ, 1). doing. That is, ΔX, ΔY, ΔZ of each element in the fourth column corresponds to a change in the position of the moving body 1.
On the other hand, the other element of the second parameter T represents the rotation around the axis of the coordinate system with the reference positions O and O ′ as the origin, and corresponds to the change in the posture (movement direction) of the moving body 1. The second parameter T shown above is a matrix for rotating a coordinate system whose origin is the reference position O by α around the X axis, β around the Y axis, and γ around the Z axis (that is, the mobile body 1 is rotated). It is. As for the rotation angles α, β, and γ of the second parameter T, which direction (right rotation or left rotation around the axis) is positive according to the definition of the X, Y, and Z axes in the coordinate system with the reference position as the origin. The angle can be determined as appropriate. The second change amount calculation unit 54 stores the second parameter T calculated as described above in the storage unit 51.

After the second parameter T and the third parameter U are determined as in Equation 3 and Equation 4, respectively, the second change amount calculation unit 54 reads the second parameter T and the third parameter U from the storage unit 51. (Step S25), the second change amount M2 is calculated (step S26).
The second change amount M2 is, for example, (i) moving the attachment position Ps1 of the measurement device 4a before movement to the reference position O before movement, (ii) changing the reference position O before movement to the reference position after movement (Iii) The reference position O ′ after the movement is moved to the attachment position Ps1 ′ of the measurement apparatus 4a after the movement. Specifically, the second change amount M2 is calculated as follows.

First, an expression representing the above operations (i) to (iii) is generated using the second parameter T and the third parameter U.
Specifically, the operation (i) is an operation reverse to ^T = U (Ox, Oy, Oz, 1) ^T , which moves the reference position O to the mounting position Ps1 (Ps1x, Ps1y, Ps1z, 1). Therefore, using the inverse matrix (U ⁻¹ ) of the third parameter U, it can be expressed as (Ox, Oy, Oz, 1) ^T = U ⁻¹ (Ps1x, Ps1y, Ps1z, 1) ^T.
On the other hand, when the coordinate value of the reference position O ′ after the movement is (Ox ′, Oy ′, Oz ′, 1), the operation (ii) uses the second parameter T to (Ox ′, Oy ′, Oz). ', 1) ^T = T (Ox, Oy, Oz, 1) ^T.
Further, in the operation (iii), when the coordinate value of the attachment position Ps1 ′ after the movement is (Ps1x ′, Ps1y ′, Ps1z ′, 1), the third parameter U is used to (Ps1x ′, Ps1y ′, Ps1z). ', 1) ^T = U (Ox', Oy ', Oz', 1) can be expressed as ^T.

Next, an expression for moving the attachment position Ps1 to the attachment position Ps1 ′ is calculated using the expressions representing the operations (i) to (iii).
Specifically, first, the expression (Ps1x ′, Ps1y ′, Ps1z ′, 1) representing the operation (iii) ^T = U (Ox ′, Oy ′, Oz ′, 1) “(Ox ′, Oy ′” of ^T , Oz ′, 1) ^T ”by replacing the expression (Ox ′, Oy ′, Oz ′, 1) ^T = T (Ox, Oy, Oz, 1) ^T representing the operation (ii) with (Ps1x ′ , Ps1y ′, Ps1z ′, 1) ^T = UT (Ox, Oy, Oz, 1) ^T
Next, (Ps1x ′, Ps1y ′, Ps1z ′, 1) ^T = UT (Ox, Oy, Oz, 1) “(Ox, Oy, Oz, 1) ^T ” in the expression ^T is operated (i). (Ox, Oy, Oz, 1) ^T = U ⁻¹ (Ps1x, Ps1y, Ps1z, 1) By substituting ^T , finally (Ps1x ′, Ps1y ′, Ps1z ′, 1) ^T = UTU ⁻¹ (Ps1x, Ps1y, Ps1z, 1) ^T and an expression representing an operation for moving the attachment position Ps1 to the attachment position Ps1 ′ are calculated.

Expression (Ps1x ′, Ps1y ′, Ps1z ′, 1) representing the operation of moving the attachment position Ps1 to the attachment position Ps1 ′ ^T = UTU ⁻¹ (Ps1x, Ps1y, Ps1z, 1) In ^T , “UTU ⁻¹ ” Is a matrix for moving the mounting position Ps1 of the measuring device 4a to the mounting position Ps1 ', that is, the movement of the measuring device 4a before and after the moving body 1 is moved. It can be defined as a second change amount M2 representing the amount.

In the matrix with “UTU ⁻¹ ” that is the second change amount M2, the third parameter U and its inverse matrix U ⁻¹ are preset constant values. The second parameter T can be specifically calculated from the change in the position and posture of the moving body 1 before and after the movement in step S24.
Therefore, the second change amount calculation unit 54 is expressed as “UTU ⁻¹ ” using the third parameter U (U ⁻¹ ) and the second parameter T that has already been specifically calculated in step S24. The second change amount M2 can be calculated specifically. The second change amount calculation unit 54 stores the second change amount M2 calculated as described above in the storage unit 51.

  After calculating the first change amount M1 and the second change amount M2, the determination unit 55 determines whether or not an abnormality has occurred in the measurement device 4a based on the difference between the first change amount M1 and the second change amount M2. Is determined (step S27).

Hereinafter, the relationship between the abnormality of the measuring device 4a and the difference between the first change amount M1 and the second change amount M2 will be described.
When the attachment position and the attachment angle of the measuring device 4a to the moving body 1 are not changed before and after the movement, or when the attachment position and the attachment angle of the detection surface DS are not changed before and after the movement inside the measurement device 4a, The positional relationship between the detection surface DS and the reference positions O and O ′ of the moving body 1 and the angle of the detection surface DS with respect to the normal direction in the normal direction do not change before and after the movement of the moving body 1.
As a result, the first change amount M1 (first parameter S _M ) calculated as a matrix for moving the second coordinate values P1 ′ to P4 ′ to the first coordinate values P1 to P4 is the second parameter T and the third parameter value. The second change amount M2 (matrix UTU ⁻¹ ) calculated using the parameter U is matched.

On the other hand, for example, as shown in FIG. 10, the mounting position of the measuring device 4a after the moving body 1 is moved is changed to the mounting position Ps1 ″, and the mounting angle (normal direction of the detection surface DS) is the straight direction ( It is assumed that the angle changes from an angle parallel to the Z and Z′-axis directions to an angle inclined in the positive direction of the X-axis (X′-axis) (the right direction of the paper). FIG. 10 is a diagram illustrating an example of a state in which the mounting state of the measuring device is changed. In other words, it is assumed that the positional relationship of the detection surface DS of the measuring device 4a with respect to the reference position and the angle of the detection surface DS with respect to the normal direction in the straight line direction differ before and after the movement of the moving body 1.
The images of the objects B1 and B2 shown in the second position data D2 ′ acquired by changing the positional relationship of the detection surface DS and the angle in the normal direction before and after the movement are attached before and after the movement as shown in FIG. This is different from the images of the objects B1 and B2 (images indicated by dotted lines in FIG. 11) shown in the second position data D2 acquired when the state does not change. FIG. 11 is a diagram illustrating an example of the second position data acquired after the mounting state of the measuring device is changed.
Specifically, as shown in FIG. 10, the images of the objects B1 and B2 indicated in the second position data D2 ′ acquired when the mounting state of the measuring device 4a changes are acquired when the mounting state does not change. The image is shifted further to the left side of the paper than the images of the objects B1 and B2 indicated by the second position data D2.

As a result, the first parameter S _M ′ (first change) calculated as a matrix for moving the second coordinate values P1 ″ to P4 ″ selected from the second position data D2 ′ to the first coordinate values P1 to P4. The amount M1 ′) is different from the first parameter S _M (first change amount M1) calculated when the mounting state does not change. That is, when the mounting state of the measuring device 4a changes before and after the movement, the first change amount M1 ′ does not coincide with the second change amount M2.

Further, when there are dirt and scratches on an optical member such as a window that transmits the measurement light Lm and reflected light Lr in the measuring device 4a, the dirt and scratches may bend the optical path of the measurement light Lm and reflected light Lr, These lights are scattered (refracted). As a result, dirt and scratches on the optical member produce the same effect as changing the mounting position and mounting angle of the measuring device 4a from the state before the dirt and scratches are attached to the optical member.
Accordingly, whether or not the measuring device 4a is faulty or abnormal, such as contamination of the optical member, can be determined based on whether or not the first change amounts M1 and M1 ′ match the second change amount M2.

In the present embodiment, the determination unit 55, the first parameter _S M, _{S M} ', the second parameter T, and is generated from the third parameter U, the first parameter _S M representing a first change amount M1, _{S M} 'And a determinant (det (S _M -UTU ^-1 )) of a difference matrix S _M -UTU ^-1 which is a value calculated as a difference between the matrix UTU ^-1 representing the second change amount M2 and It is determined whether or not the first change amount M1 and the second change amount M2 match (that is, whether or not there is an abnormality in the measurement device 4a).

When the value of the determinant det (S _M −UTU ⁻¹ ) is smaller than the predetermined threshold δ (in the case of “Yes” in step S27), the determination unit 55 determines whether the first change amount M1 and the second change amount M2 are Are determined to match, and the measurement device 4a is determined to be normal. Thereafter, the abnormality determination process ends. The predetermined threshold value δ is appropriately determined in consideration of the measurement error of the measuring device 4a and the moving amount measuring device 6, the allowable mounting position and the mounting angle of the measuring device 4a, and the like. it can.

On the other hand, when the value of the determinant det (S _M −UTU ⁻¹ ) is equal to or greater than the predetermined threshold δ (in the case of “No” in step S26), the determination unit 55 determines the first change amount M1 and the second change amount. It is determined that M2 does not match, and it is determined that an abnormality has occurred in the measuring device 4a.
At this time, the determination unit 55, for example, sounds a warning sound using the speaker of the control unit 5, displays a message notifying that the mounting state has changed on the display of the control unit 5 or the like, or turns on a warning lamp. Or the driver is notified that the mounting state of the measuring device 4a has changed (step S28). Thereafter, the attachment state determination process ends.

During execution of step S28, the determination unit 55 may calculate the degree of abnormality occurring in the measurement device 4a using the determinant det (S _M -UTU ⁻¹ ). For example, the relationship between the change in the value of the determinant det (S _M -UTU ⁻¹ ) and the change in the value of the attachment position and the attachment angle of the measuring device 4a is acquired in advance and stored in the storage unit 51, and the matrix When the expression det (S _M −UTU ⁻¹ ) is equal to or greater than the predetermined threshold δ, the determination unit 55 determines whether the change in the value of the determinant det (S _M −UTU ⁻¹ ) and the attachment position of the measuring device 4a Using the relationship with the change in the value of the mounting angle, the mounting position of the measuring device 4a and the mounting angle from a certain point in time or the degree of abnormality of the measuring device 4a (for example, contamination of the optical member or adhesion of scratches) Degree) can be calculated.

By executing the above steps S21 to S27, the object detection system 100 is not information that is difficult to grasp, such as the timing at which the object is no longer observed by the measuring device 4a, but the first coordinate values P1 to P4 and the second coordinate values. Changes in the position and orientation of the measuring device 4a calculated from changes in measured values by the measuring device 4a of P1 ′ to P4 ′ (that is, first parameters S _M , S _M ′, first change amounts M1, M1 ′), The change in the position and orientation of the measuring device 4a calculated based on the change in the position and orientation of the moving body 1 before and after movement (ie, the second parameter T) and the third parameter U (ie, the second change amount M2). The abnormality of the measuring device 4a is determined based on information that is easily acquired by the object detection system 100 without difficulty. Thereby, the attachment state of the measuring apparatus 4a can be grasped more accurately.

In the above description, the method for determining the abnormality of the measuring device 4a has been described. However, the other measuring devices 4b to 4d can similarly determine the abnormality. However, when determining the abnormality of the other measuring devices 4b to 4d, the second change amount M2 (matrix UTU ⁻¹ ) represents the position and orientation of each measuring device 4b to 4d with respect to a predetermined moving body. Calculation is performed using the matrix U.

2. Other Embodiments Although one embodiment of the present invention has been described above, the present invention is not limited to the above embodiment, and various modifications can be made without departing from the scope of the invention. In particular, a plurality of embodiments and modifications described in this specification can be arbitrarily combined as necessary.
In the flowchart showing the determination process of whether or not the measurement apparatus 4a in FIG. 5 is abnormal, the order of the processes shown in the flowchart can be changed as appropriate. For example, after the process shown in steps S23 to S25 is first executed to calculate the second change amount M2, the process shown in steps S21 to S22 is executed to execute the first change amounts M1 and M1 ′ (first The parameter S _M ) may be calculated.

(A) Other Embodiments for Position Data In the first embodiment described above, one feature point is selected for each of four different objects in FIG. 7, but it is not necessary to be different objects. It is also possible to select four characteristic points for one object.

(B) Other Embodiments Regarding Conditions for Performing Abnormality Determination In the first embodiment, the control unit 5 always obtains the second parameter T and the second change amount M2 while the moving body 1 is moving. In addition, the abnormality determination of the measuring devices 4a to 4d is to be executed. However, the present invention is not limited to this. For example, when the speed is constant and the direction does not change, the second parameter T and the second change amount T2 are used as the second parameter T and the second change amount M2 used in the previous determination. The calculation of the two change amounts M2 may be omitted.

(C) Other Embodiments for Conditions for Performing Determination Abnormality Determination In the first embodiment, abnormality determination is performed using the first change amount M1 and the second change amount M2. The determination may be made by other methods. For example, instead of the first change amount calculation unit 53 and the second change amount calculation unit 54 (FIG. 3A) in the control unit 5 of the first embodiment, the control unit 5 has a coordinate prediction unit 57 as shown in FIG. 3B. You may have. FIG. 3B is a diagram illustrating a configuration of a control unit according to another embodiment.
The coordinate prediction unit 57 moves by using the first coordinate value representing the position of the object measured before the movement, the position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement. The position of the subsequent object is predicted (predicted coordinate value) and compared with the second coordinate value representing the actual position of the moved object. In this case, the difference may be calculated as a distance between two coordinate values. According to this method, since comparison is performed using coordinate values, it is easy to evaluate the comparison result, and the number of feature points to be selected does not have to be four.

  The present invention can be widely applied to abnormality determination of a measuring apparatus in an object detection system that detects an object existing around a moving body such as a vehicle.

DESCRIPTION OF SYMBOLS 100 Object detection system 1 Moving body 2a, 2b, 2c, 2d Wheel 3 Moving body control part 4a, 4b, 4c, 4d Measuring apparatus 41 Output part 43-1, 43-2, 43-n detection part 45 Lens 5 Control part 51 Storage Unit 52 Position Data Generation Unit 53 First Change Calculation Unit 54 Second Change Calculation Unit 55 Judgment Unit 56 Real Space Data Output Unit 57 Coordinate Prediction Unit 6 Movement Measurement Devices B, B1, B2 Object D1 First Position Data D2, D2 ′ Second position data D3 Movement amount data L1, L2, L3, L4 Feature points P1, P2, P3, P4 First coordinate values P1 ′, P2 ′, P3 ′, P4 ′ Second coordinate values P1 ′ ', P2 ″, P3 ″, P4 ″ Second coordinate values Ps1, Ps2, Ps3, Ps4, Ps1 ′ Mounting position M1, M1 ′ First variation M2 Second variation DS Detection surface Lm Measurement light Lr Reflection light O, O 'reference position _{S M, S} M' second Parameter T second parameter U third parameter _S M -UTU ^-1 difference matrix δ predetermined threshold

Claims (13)

A moving object that is a movable object;
A measuring device attached to the moving body and moving together with the moving body, measuring a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device, and the moving body The movement after movement based on the first coordinate value representing the position of the object measured before movement, the positional information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after movement. A coordinate prediction unit that calculates a predicted coordinate value representing the position of the object viewed from the body;
A determination unit that determines an abnormality of the measurement device by comparing the predicted coordinate value with a second coordinate value that represents the position of the object measured after the moving body is moved;
An object detection system comprising: A moving body control unit for controlling movement of the moving body;
Changes in the position and posture of the moving body before and after movement are calculated based on the control amount of the moving body in the moving body control unit.
The object detection system according to claim 1.   The object detection system according to claim 1, further comprising a movement amount measuring device that measures an amount representing movement before and after the movement of the moving body. The change in position before and after the movement of the moving body is expressed as a change in the reference position provided for the moving body before and after the movement of the moving body.
The object detection system in any one of Claims 1-3.   The object detection system according to claim 4, wherein a positional relationship between the attachment position and the reference position is constant. A moving object that is a movable object;
A measuring device attached to the moving body and moving together with the moving body, and measuring a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device;
Based on a change between a first coordinate value representing the position of the object measured before the movement of the moving body and a second coordinate value representing the position of the object measured after the movement of the moving body, A first parameter calculation unit for calculating a first parameter representing a change in the position and orientation of the measuring device;
A second parameter calculation unit for calculating a second parameter representing a change in position and posture of the moving body before and after movement;
A storage unit that stores a third parameter that represents the position and orientation of the measuring device with respect to the moving body at a certain time before the moving body moves;
Based on the first parameter, the second parameter, and the third parameter, a determination unit that determines a change in the attachment position and posture of the measurement device with respect to the certain time point;
An object detection system comprising:   The object detection system according to claim 6, wherein the determination unit calculates the presence or absence of a change or the degree of change in the attachment position and orientation of the measurement apparatus with respect to the certain time point. The determination unit calculates the presence or absence of the change or the degree of the change based on a determinant value of a matrix generated from the first parameter, the second parameter, and the third parameter.
The object detection system according to claim 7. The change in the position of the moving body before and after the movement is expressed as a change in the reference position fixed with respect to the moving body before and after the movement of the moving body.
The object detection system in any one of Claims 6-8. An abnormality determination method for a measuring device attached to a movable object that is a movable object,
Before the moving body moves, causing the measuring device to measure a first coordinate value representing a position of an object existing around the moving body as viewed from the measuring device;
After the movement of the moving body, causing the measurement device to measure a second coordinate value representing a position of the object viewed from the measurement device;
Prediction representing the position of the object viewed from the moving body after movement based on the first coordinate value, position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement Calculating a coordinate value; comparing the predicted coordinate value with the second coordinate value to determine an abnormality of the measuring device;
An abnormality determination method including A method for determining an abnormality in an attachment state of a measuring device attached to a movable object that is a movable object,
Before the moving body moves, causing the measuring device to measure a first coordinate value representing a position of an object existing around the moving body as viewed from the measuring device;
After the movement of the moving body, causing the measurement device to measure a second coordinate value representing a position of the object viewed from the measurement device;
Calculating a first parameter representing a change in position and orientation of the measuring device based on a change between the first coordinate value and the second coordinate value;
Calculating a second parameter representing a change in the position and orientation of the moving body before and after the movement of the moving body;
Reading from the storage unit a third parameter stored in the storage unit at a certain time before the mobile body moves and representing the position and orientation of the measuring device relative to the mobile body;
Determining a change in the attachment position and orientation of the measuring device with respect to the certain time point based on the first parameter, the second parameter, and the third parameter;
An abnormality determination method including   A program that causes a computer to execute the abnormality determination method according to claim 10.   The program which makes a computer perform the said abnormality determination method of Claim 11.