Distance calculation device and calculation program
It is intended to accurately calculate the distance between a movable body and a subject on the basis of various quantities of state, such as the azimuth angle between the movable body and the subject, velocity, and yaw rate. A calculation device includes a unit for calculating the azimuth angle between movable body and a subject, a unit for calculating the distance traveled by the movable body between two points of time, a unit for calculating a change in angle in the direction of movement, and a unit for calculating the linear distance from the movable body to the subject by using the unit.
1. A distance calculation device which calculates a distance between a movable body and a fixed subject, comprising:
an azimuth angle calculating unit calculating as an azimuth angle an angle made by a direction connecting the movable body to the subject and a traveling direction of the movable body on a horizontal plane;
a traveled distance calculation unit calculating a traveled distance between two points of time of the movable body;
an amount-of-angular-change calculating unit calculating a rotation angle of a movable body on a vertical axis passing through a centroid of a movable body between two points of time as an amount of angular change in a traveling direction; and
a linear distance calculating unit calculating a linear distance between the movable body and the subject using output of the azimuth angle calculating unit, the traveled distance calculation unit, and the amount-of-angular-change calculating unit.
2. The device according to claim 1 , further comprising:
a current position sensor detecting a current position of the movable body;
a database unit outputting map information including a road around the current position depending on output of the current position sensor; and
a running distance calculation unit calculating a distance on a traveling orbit from the current position to the subject using output of the database unit, the azimuth angle calculating unit, and the linear distance calculating unit.
3. The device according to claim 1 , further comprising
a subject recognition unit recognizing the subject from an image including the subject, and providing a recognition result for the azimuth angle calculating unit.
4. The device according to claim 3 , further comprising:
a subject recognition monitor unit monitoring whether or not output of the subject recognition unit has deviated a predetermined range; and
a subject recognition amendment unit amending output of the subject recognition unit and providing amended output for the azimuth angle calculating unit when the subject recognition monitor unit detects the deviation.
5. The device according to claim 3 , further comprising:
a current position sensor detecting a current position of the movable body;
a database unit outputting map information including a road around the current position depending on output of the current position sensor;
a velocity sensor providing a velocity of the movable body for the traveled distance calculation unit;
a roll angle calculation unit calculating as an angle of roll a rotation angle on a traveling direction axis of the movable body using output of the database unit and the velocity sensor; and
a roll angle amendment unit amending coordinates as a recognition result of the subject using output of the roll angle calculation unit and the subject recognition unit, and providing an amendment result for the azimuth angle calculating unit.
6. The device according to claim 1 , further comprising:
a yaw rate sensor detecting a rotation angular velocity on a vertical axis of the movable body, and providing the velocity for the amount-of-angular-change calculating unit; and
a distance estimation unit estimating a distance on a traveling orbit from the movable body to the subject using the yaw rate sensor and the linear distance calculating unit.
7. The device according to claim 6 , further comprising:
a yaw rate sensor monitor unit monitoring whether or not an output value of the yaw rate sensor and/or a change rate of an output value has deviated a predetermined range of an output value, and/or a range of a change rate of an output value; and
a yaw rate amendment unit amending an output value of the yaw rate sensor based on past output of the yaw rate sensor and a value of a velocity of the movable body when the yaw rate sensor monitor unit has detected the deviation, and providing an amendment result for the amount-of-angular-change calculating unit.
8. A distance calculation device which calculates a distance between a movable body and a fixed subject, comprising:
an elevation angle calculation unit calculating as an elevation angle an angle made by a direction connecting the movable body to the subject and a horizontal plane;
a traveled distance calculation unit calculating a traveled distance between two points of time of the movable body;
a pitching angle calculation unit calculating as a pitching angle a rotation angle of the movable body on a vertical plane depending on a traveling direction and a vertical direction of the movable body; and
a traveling direction distance calculation unit calculating a component in a traveling direction of a linear distance from the movable body to the subject using output of the elevation angle calculation unit, the traveled distance calculation unit, and the pitching angle calculation unit.
9. The device according to claim 8 , further comprising:
a current position sensor detecting a current position of the movable body;
a database unit outputting map information including a road around the current position depending on output of the current position sensor;
a azimuth angle calculating unit calculating as an azimuth angle an angle made by a direction connecting the movable body and the subject and a traveling direction of the movable body on a horizontal plane; and
a running distance calculation unit calculating a distance on a traveling orbit from the current position to the subject using output of the database unit, the azimuth angle calculating unit, and the traveling direction distance calculation unit.
10. The device according to claim 8 , further comprising
a subject recognition unit recognizing a subject from an image containing the subject, and providing the elevation angle calculation unit with the recognition result.
11. The device according to claim 10 , further comprising:
a current position sensor detecting a current position of the movable body;
a database unit outputting map information including a road around the current position depending on output of the current position sensor;
a velocity sensor providing a velocity of the movable body for the traveled distance calculation unit;
a roll angle calculation unit calculating as an angle of roll a rotation angle on a traveling direction of the movable body using output of the database unit and the velocity sensor; and
a roll angle amendment unit amending coordinates as the recognition result of the subject using output of the roll angle calculation unit and the subject recognition unit, and providing the corrected coordinates for the elevation angle calculation unit.
12. The device according to claim 8 , further comprising:
a current position sensor detecting a current position of the movable body;
a database unit outputting map information including a road around the current position depending on output of the current position sensor; and
an inclination angle calculation unit calculating an inclination angle in a traveling direction around a current position of the movable body using output of the database unit, wherein
the pitching angle calculation unit calculating a pitching angle using output of the inclination angle calculation unit.
13. The device according to claim 12 , further comprising
an acceleration sensor detecting acceleration in a traveling direction of the movable body, wherein
the pitching angle calculation unit calculates a pitching angle using output of the acceleration sensor in addition to output of the inclination angle calculation unit.
14. The device according to claim 8 , further comprising
an acceleration sensor detecting acceleration in a traveling direction of the movable body, wherein
the pitching angle calculation unit calculates a pitching angle using output of the acceleration sensor.
15. A computer-readable storage medium storing a program used to direct a computer which calculates a distance between a movable body and a fixed subject to perform a process, comprising:
calculating as an azimuth angle an angle made by a direction connecting the movable body to the subject and a traveling direction of the movable body on a horizontal plane;
calculating a traveled distance between two points of time of the movable body;
calculating a rotation angle of a movable body on a vertical axis passing through a centroid of a movable body between two points of time as an amount of angular change in a traveling direction;
calculating a linear distance between the movable body and the subject using the calculated azimuth angle, traveled distance, and amount of angular change; and
storing the calculated linear distance.