JP6780942B2 - Steering support system - Google Patents

Description

The present invention relates to a system that assists in steering a moving body that intends to move along a moving path.

When implementing autonomous control, automatic driving, or semi-automatic driving of moving objects such as automobiles, ships, aircraft, and robots, the positions of roads and other moving routes on which the moving objects can move are stored in a database in advance, and the moving objects are used. It is required to realize movement along an appropriate route.

The latitude and longitude of the current position of the moving object are grasped using GPS (Global Positioning System), the current position is shown on the map, and passages (roads) and buildings within a certain distance from the current position are shown. A car navigation system that displays information such as the above is known (see, for example, Patent Document 1 below). However, ready-made car navigation systems only have vector data (vector image data) as map image data for screen display to be presented to passengers, and autonomous control and automatic control of moving objects. It is not always useful for driving or semi-automatic driving.

On the other hand, the lane markings (particularly the white lines painted on the road surface) laid on the road on which the moving body travels are sensed to calculate the optimum traveling line of the moving body, and the steering device of the moving body is automatically set. An automatic driving technique for operating has already been developed (see, for example, Non-Patent Document 1 below).

Japanese Unexamined Patent Publication No. 2005-339101

Toyota Motor Corporation, "Toyota Motor Corporation announces development status of autonomous driving technology | TOYOTA Global Newsroom", [online], 2014, Toyota Motor Corporation, [Search January 25, 2016], Internet <URL : Http://newsroom.toyota.co.jp/en/detail/3932729>

In order to use it for autonomous control of moving objects, automatic driving or semi-automatic driving, if the position and shape of movement paths and obstacles existing in the space are precisely converted into data and accumulated, the amount of data will be enormous and There is a concern that the process for grasping the distance and direction from the moving body using the data will become unnecessarily complicated, the amount of calculation will increase, and the process will be delayed.

An object of the present invention is to express the movement path that the moving body intends to move in the form of lightweight and easy-to-use data, and to contribute to the detection of the path existing around the moving body and the control of the moving body. And.

In order to solve the above-mentioned problems, the data is obtained by dividing the path on which the moving body is going to move into a plurality of parts and obtaining the latitude and longitude of a predetermined position in the width direction intersecting the extension direction of the movement path for each part. The route point data storage unit that stores the route point data, the current position acquisition unit that repeatedly acquires the latitude and route of the current position of the moving body, and the moving body that is stored in the path point data storage unit will be from now on. Read multiple path point data for the part of a plurality of continuous paths that will enter within a certain time, apply a straight line or curve that connects the latitudes and paths of the multiple path point data, and perform the straight line. Alternatively, the stretching direction of the curve is obtained, and the stretching direction and the moving direction of the moving body determined from the time series of the latitude and longitude of the current position of the moving body acquired by the current position acquisition unit are moved so as to be substantially parallel to each other. The steering amount or steering direction required for operating the steering device provided by the body or for the extension direction and the moving direction of the moving body to be substantially parallel is output in a manner appealing to the visual or auditory sense of the passenger of the moving body. The movement route of the moving body is a road, and the route point data for each part of the route stored in the route point data storage unit includes the latitude and longitude of the road marking line. And each path point data is the path point data for each part when the movement path is divided at equal intervals for each standard movement distance per second or fractional second on the movement path of the moving body. A steering support system is configured in which the control unit reads a plurality of path point data for a plurality of path portions that will enter in the next few seconds from the path point data storage unit and uses them for control .

In addition, path point data, which is data obtained by dividing the path on which the moving body intends to move into a plurality of parts and obtaining the latitude and longitude of a predetermined position in the width direction intersecting the extension direction of the movement path for each part, is stored. The path point data storage unit, the current position acquisition unit that repeatedly acquires the latitude and route of the current position of the moving body, and the moving body stored in the path point data storage unit will enter within a certain time from now on. Read a plurality of path point data for a portion of a plurality of continuous paths to be performed, fit a straight line or a curve connecting the latitudes and paths of the plurality of path point data, and determine the extension direction of the straight line or the curve. A steering device provided by the moving body so that the extending direction thereof and the moving direction of the moving body determined from the time series of the latitude and longitude of the current position of the moving body acquired by the current position acquisition unit are substantially parallel to each other. It is equipped with a control unit that operates or outputs the steering amount or steering direction required for the extension direction of the moving body to be substantially parallel to the moving direction of the moving body in a manner that appeals to the sight or hearing of the passengers of the moving body. However, the movement route of the moving body is a road, the route point data for each part of the route stored in the route point data storage unit includes the latitude and longitude of the road marking line, and the control unit is described by the control unit. The latitude and path of the current position of the moving body are acquired via the current position acquisition unit, and the path point data including the latitude and coordinates closest to the current position is read from the path point data storage unit, and the moving body is currently located. While obtaining the latitude and longitude values of the lane markings on the part of the road, the position of the lane markings laid on the road is detected using the sensor mounted on the moving body, and the moving body and the lane markings are detected. The latitude and longitude of the lane marking read from the path point data storage unit and the distance from the moving body to the lane marking are used to calculate the latitude and longitude of the current position of the moving body and correct it. A steering support system was constructed.

In addition, the route point data for each part of the route stored by the route point data storage unit includes the altitude data of the portion, and the control unit stores the route point data storage unit . The moving body reads out a plurality of path point data for the parts of a plurality of continuous paths that the moving body will enter within a certain time, and moves according to the ups and downs of the moving path found from the plurality of path point data. The amount or direction of operation of the output of the drive source required to control the output of the engine or motor that is the drive source of the vehicle, or to suppress fluctuations in the movement speed of the moving object, to the visual or auditory sense of the passengers of the moving object. It may be output in an appealing manner.

In order to further reduce the amount of data and the amount of calculation, the path point data storage unit moves every standard movement distance per second or fractional second on the movement path of the moving body. The route point data for each part when the route is divided is stored, and the control unit stores the route point data for a plurality of route parts that will enter in the next few seconds. It is preferable that the system is read from the unit and used for control.

According to the present invention, the movement path that the moving body intends to move can be expressed in the form of lightweight and easy-to-use data, which can contribute to the detection of the path existing around the moving body and the control of the moving body.

The figure which shows the hardware resource which the steering support system has in one Embodiment of this invention. The functional block diagram of the steering support system of the same embodiment. The figure which illustrates the route point data stored in the route point data storage part in the same embodiment. The figure which illustrates the relationship of the position of the moving body, the moving path and the part thereof, and the marking line in the same embodiment. The flow chart which shows the procedure example of the process which a computer executes according to a program in the same embodiment.

An embodiment of the present invention will be described with reference to the drawings. The steering support system of the present embodiment is mounted and operated on a moving body 1, typically an automobile or a robot. This steering support system may be referred to as a computer (electronic control unit) that controls an engine or a motor that is a steering device 11 mounted on the moving body 1 or a driving source 12 of the moving body 1. ) Is the main component.

The moving body 1 in the present embodiment includes a known steering device 11 capable of changing the direction of the traveling wheels, a known engine or motor serving as a drive source 12 for rotationally driving the wheels, and the movement of the moving body 1. A camera sensor (which may be a panoramic camera), a radar sensor, or an objective distance (distance measuring) sensor as a sensor 13 for detecting a lane marking line laid on the road surface of the route or an object around the moving body 1. (Laser radar, ultrasonic radar, infrared radar, etc.), passive infrared sensor (human sensor), etc., and navigation signal receiving device 14 of navigation satellite system (satellite positioning system) such as GPS and QZSS (Quasi-Zenith Satellite System). To be equipped. In addition, it is provided with a display 15 that displays and outputs information in a manner that appeals to the sight of the passenger on the moving body 1, a voice output device 16 that outputs information by voice in a manner that appeals to the hearing of the passenger, and the like. Sometimes it is.

As shown in FIG. 1, the computer that controls the operation of the mobile body 1 is hardware such as a CPU (Central Processing Unit) 1a, a main memory 1b, an auxiliary storage device 1c, a video codec 1d, an audio codec 1e, and a communication interface 1f. It is equipped with resources, and these are controlled by 1 g of a controller (system controller, I / O controller, etc.) and cooperate with each other.

The auxiliary storage device 1c is a flash memory, a hard disk drive, an optical disk drive, or the like. The video codec 1d temporarily stores GPU (Graphics Processing Unit), which generates a screen to be displayed based on a drawing instruction received from the CPU 1a and sends the screen signal to the display 15, and screen and image data. The video memory etc. to be stored is used as an element. The audio codec 1e decodes the encoded voice data and outputs the voice from the voice output device 16. Each of the video codec 1d and the audio codec 1e can be implemented as software instead of hardware. The communication interface 1f is a device for the computer to exchange information with an external device.

The program to be executed by the CPU 1a is stored in the auxiliary storage device 1c, and when the program is executed, the program is read from the auxiliary storage device 1c into the main memory 1b and decoded by the CPU 1a. The computer operates the hardware resources according to the program, and exerts functions as the path point data storage unit 101, the current position acquisition unit 102, and the control unit 103 shown in FIG.

The route point data storage unit 101 stores the route point data by using the required storage area of the main memory 1b or the auxiliary storage device 1c. FIG. 3 shows an example of the route point data stored in the route point data storage unit 101.

The path point data is, at least, a predetermined point in the width direction that intersects (particularly, orthogonally) the extension direction of the movement path for each part α when the path that the moving body intends to move is divided into a plurality of parts α, in other words. If so, it includes the latitude and longitude data of the position on the virtual line extending parallel to the movement path. In this embodiment, an automobile or a robot traveling on a road is assumed as a moving body. Therefore, the route point data includes latitude and longitude data indicating the position of the lane marking in each part α of the road route which is the movement route of the moving body. A lane marking is a line painted on the road surface, a line drawn by road studs (cat's eye), stones, etc. installed on the road surface, including the following:
"Road outside line" A lane marking drawn near the road edge that marks the outermost boundary of the roadway. The area outside the road outside line is, in principle, a roadside zone where automobiles are prohibited. The area inside the road outside line is the vehicle lane where vehicles are allowed to travel.
"Road Chuo Line" A lane marking line drawn to divide the traffic of automobiles by direction. In reality, it is often represented not only by painted lines, but also by structures such as rumble strips, road studs, lane separation markers (center poles, post cones), and medians installed on the road. ..
"Lane boundary line" A boundary line that separates adjacent lanes when there are two or more lanes for vehicles traveling in the same direction.

As shown in FIGS. 3 and 4, in the present embodiment, the standard speed at which the moving body 1 travels on the road, that is, per second or a fraction of a second (for example, a half second, The movement path of the moving body 1 is divided according to the movement distance per (quarter second, etc.), and the start point (or end point, intermediate point, etc.) of each part α having the length of the distance, of each part α, Path point data is associated with the type of route (road), which is an identifier that identifies the movement route, using the latitude and longitude values indicating the position of the lane marking in the predetermined position along the extension direction of the movement route as the route point data. It is stored in the storage unit 101. Assuming that the moving route is an expressway and the standard moving speed of the moving body 1 traveling on the highway is 100 km / h, the standard moving speed of the moving body 1, that is, the moving distance per second is 27. 78m, the moving distance per half second is 13.89m, and the moving distance per quarter second is 6.945m. Therefore, when the movement route is an expressway, the expressway is divided into a plurality of parts α having a length (journey) of 27.78 m, 13.89 m, or 6.945 m, and each part α is a unit. The route point data is obtained and stored in the route point data storage unit 101.

In the illustrated example, the latitude and longitude of each of the left side roadway line L and the right side roadway outside line (which may be equal to the roadway center line) R at the starting point of each part α of the movement path are stored in a database. In addition to this, the latitude and longitude of the lane boundary line at the starting point of each part α of the movement route may be included in the route point data.

Further, the path point data for each part α of the movement route includes the latitude and longitude indicating the position of the road side wall LL on the left side at the start point (or end point) of the part α in addition to the latitude and route of the above lane marking. It may include the value of latitude and longitude indicating the position of the road side wall RR on the right side at the starting point of the portion α. Road side walls LL and RR are rumble strips, road studs, lane separation markers, median strips, which exist outside the road outside line or the lane center line and obstruct the movement, passage or passage of the moving body 1. Soundproof walls, guardrails, curbs, poles, medians, building structures, etc.

Further, the path point data for each part α of the movement route includes information on the altitude value which is the altitude of the start point (or end point) of the part α and the number of lanes included in the part α. There is. Here, the information on the number of lanes represents only the number of lanes of the road heading in the same direction by numerical values such as 1, 2, 3, ..., T2 (two lanes including the uphill lane), I2 (interchange lane). Including, two lanes), P2 (including parking area lane, two lanes), O2 (no overtaking, two lanes), G1 (one lane oncoming lane. In this case, one outbound route and one inbound route with opposite movement directions) It may be used to express additional information about the road in addition to the number of lanes, such as (may be treated as a road).

The latitude and longitude, which are the elements of the route point data, are each a value having a predetermined number of digits. When the number of effective digits of latitude and longitude is up to 6 digits after the decimal point, the resolution of those latitudes and longitudes in Japan is about 0.11 m in the latitude direction and about 0.09 m in the longitude direction.

Incidentally, the route point data may be generated only when the passenger on the moving body 1 specifies the destination of the moving body 1. For example, the passenger operates the car navigation system mounted on the moving body 1 to input the destination, and the car navigation system searches for and determines the moving route from the current position of the moving body 1 to the destination. After that, the computer of the steering support system divides the movement path into a plurality of portions α, generates path point data for each portion α, and executes a process of storing the path point data in the path point data storage unit 101.

The current position acquisition unit 102 uses the function of the navigation signal receiving device 14 included in the mobile body 1 to repeatedly acquire the latitude and longitude of the current position of the mobile body 1 at predetermined time intervals.

The control unit 103 includes path point data regarding a plurality of portions α on the movement path of the moving body 1 stored in the path point data storage unit 101, and the moving body 1 obtained through the current position acquisition unit 102. Steering of the moving body 1 and control of the moving speed are performed based on the transition of the current position.

As shown in FIG. 5, the control unit 103 first collects the route point data having the latitude and longitude values closest to the latitude and longitude indicating the current position of the moving body 1 in the route point data storage unit 101. Search from the inside (step S1). Here, an example of a method of calculating the geodesic length, which is the distance between the position of the moving body 1 and the target position, and the azimuth angle of the target position as seen from the position of the moving body 1 is shown. The calculation method described below is introduced on the Geographical Survey Institute website (http://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2stf.html).

Let the latitude of the position of the moving body 1 be φ ₁ and the longitude be L ₁ , the latitude of the target position be φ ₂ , and the longitude be L ₂ . However, the north latitude is positive, the south latitude is negative, the east longitude is positive, and the west longitude is (360 ° -longitude). When L = L _2- L ₁ is a negative value, the latitude of the position of the moving body 1 is φ ₂ , the longitude is L ₂ , the latitude of the target position is φ ₁ , and the longitude is L ₁ . Make L a positive value. Let a be the semimajor axis of the earth ellipsoid (reference ellipsoid) and f be the flattening.
L = L _2- L ₁ ; L'= 180 ° -L (however, 0 ° ≤ L ≤ 180 °)
Δ ＝ φ _{2 −} φ ₁ ； Σ ＝ φ ₁ ＋ φ ₂
u ₁ = tan ^-1 [(1-f) tan φ ₁ ]; u ₂ = tan ^-1 [(1-f) tan φ ₂ ]
Σ'= u ₁ + u ₂ ; Δ'= u ₂ -u ₁ (however, -180 ° ≤ Σ'≤ 180 °, -180 ° ≤ Δ'≤ 180 °)
ξ = cos (Σ'/2);ξ'= sin (Σ'/2)
η = sin (Δ'/2);η'= cos (Δ'/2)
x = sine ₁ sine ₂ ; y = cosu ₁ cosu ₂
c = ycosL + x
ε = [f (2-f)] / [(1-f) ² ]
After that, some calculation formulas differ depending on the value of c, and zone 1, when c ≧ 0 is satisfied, zone 2, c <-cos when 0> c ≧ −cos (3 ° cosu ₁ ) is established. The case where (3 ° cosu ₁ ) is satisfied is defined as zone 3.
The initial value of θ θ _(0), and in zone _{1 θ (0) = L (} 1 + fy), Zone 2, θ ₍₀₎ = a L '.
In zone 3, the calculation method is divided into zone 3 (a) where Σ = 0 and zone 3 (b) where Σ ≠ 0.
R = fπcos ² u ₁ [1- (1/4) f (1 + f) sin ² u ₁ + (3/16) f ² sin ⁴ u ₁ ]
d ₁ = L'cosu ₁ −R; d ₂ = | Σ'| + R
q = L'/ (fπ); f ₁ = (1/4) f [1 + (1/2) f]; γ ₀ = q + f ₁ q−f ₁ q ³
The calculation of θ ₍₀₎ in zone 3 (a) is
A ₀ = tan ^-1 (d ₁ / d ₂ ) (However, -90 ° ≤ A ₀ ≤ 90 °)
B ₀ = sin ^-1 [R / √ (d ₁ ² + d ₂ ² )] (However, 0 ° ≤ B ₀ ≤ 90 °)
ψ = A ₀ + B ₀ ; j = γ ₀ / cosu ₁
k = (1 + f ₁ ) | Σ'| (1-fy) / (fπy); j ₁ = j / (1 + ksecψ)
ψ'= sin ^-1 j ₁ (however, 0 ° ≤ ψ'≤ 90 °)
ψ'' = sin ^-1 (j ₁ cosu ₁ / cosu ₂ ) (However, 0 ° ≤ ψ'' ≤ 90 °)
θ ₍₀₎ = 2 tan ^-1 {tan [(ψ'+ ψ'') / 2] sin (| Σ'| / 2) / cos (Δ'/2)}
On the other hand, in zone 3 (b), when d ₁ > 0, θ ₍₀₎ = L'is set. The calculation of the geodesic line length and the azimuth angle when d ₁ ≤ 0 in zone 3 (b) will be described separately.

(Except when d ₁ ≤ 0 in zone 3 (b)) In the calculation of θ below, θ _(n) ≡ θ ₍₀₎ is set in the first calculation, and θ _{(n + 1 )} is set in the second and subsequent calculations. ₎ ≡ θ _(n) .
In zone 1, g = √ [η ² cos ² (θ _(n) / 2) + ξ ² sin ² (θ _(n) / 2)]
Except for zone 1, g = √ [η ² sin ² (θ _(n) / 2) + ξ ² cos ² (θ _(n) / 2)]
In zone 1, h = √ [η ' 2 cos 2 (θ (n) / 2) + ξ' 2 sin 2 (θ (n) / 2)]
In addition zone ^{1, h = √ [η '2} sin 2 (θ (n) / 2) + ξ' 2 cos 2 (θ (n) / 2)]
σ = 2tan ^-1 (g / h); J = 2gh; K = h ^2- g ²
γ = (y / J) sinθ _(n) ; Γ = 1-γ ² ; ζ = ΓK-2x; ζ'= ζ + x
D = (1/4) f (1 + f)-(3/16) f ² Γ
E = (1-DΓ) fγ {σ + DJ [ζ + DK (2ζ ^{2 −} Γ ² )]}
In zone 1, F = θ _(n) −LE
Except for zone 1, F = θ _(n) −L'+ E
G = fγ ² (1-2DΓ) + fζ'(σ / J) [1-DΓ + (1/2) fγ ² ] + (1/4) f ² ζζ'
θ _{(n + 1)} = θ _(n) −F / (1-G)
By repeating the above for the required number of times, θ is obtained. The iterative calculation is preferably continued until | F | <10 ^-15 .

(Except when d ₁ ≤ 0 in zone 3 (b)) The geodesic length s is
n ₀ = εΓ / [√ (1 + εΓ) +1] ² ; A = (1 + n ₀ ) [1+ (5/4) n ₀ ² ]
B = ε [1- (3/8) n ₀ ² ] / [√ (1 + εΓ) +1] ²
s = (1-f) aA (σ-BJ {ζ- (1/4) B [K (Γ ² -2ζ ² ) − (1/6) Bζ (1-4K ² ) (3Γ ² -4ζ ² ) ]}))
(Except when d ₁ ≤ 0 in zone 3 (b)) The azimuth angle α of the target position as seen from the position of the moving body 1 is
In zone 1, α = tan ^-1 [ξtan (θ / 2) / η] −tan ^-1 [ξ'tan (θ / 2) / η']
Other than zone 1, α = tan ^-1 [η'tan (θ / 2) / ξ'] − tan ^-1 [η tan (θ / 2) / ξ]
However, the latitude and longitude of the position of the moving body 1 and (φ _2, L _2), as long as are the latitude and longitude of the position of the target (phi _1, L _1),
In zone 1, α = 180 ° + tan ^-1 [ξtan (θ / 2) / η] + tan ^-1 [ξ'tan (θ / 2) / η']
In addition zone ^{1, α = 360 ° -tan -1 [} η'tan (θ / 2) / ξ '] - tan -1 [ηtan (θ / 2) / ξ]
The geodesic length s and the azimuth angle α when d ₁ = 0 in zone 3 (b) are
Γ = sin ² u ₁
n ₀ = εΓ / [√ (1 + εΓ) +1] ² ; A = (1 + n ₀ ) [1+ (5/4) n ₀ ² ]
s = (1-f) aAπ
α = 90 °
However, the latitude and longitude of the position of the moving body 1 and (φ _2, L _2), as long as are the latitude and longitude of the position of the target _{(φ 1, L 1),} α = 270 °
Further, the geodesic length s and the azimuth angle α when d ₁ <0 in zone 3 (b) are calculated according to the following. In the first operation, γ _(n) ≡γ _(0), and in the second and subsequent operations, γ _{(n + 1)} ≡γ _(n) .
Γ = 1-γ _(n) ² ; D = (1/4) f (1 + f)-(3/16) f ² Γ
γ _{(n + 1)} = q / (1-DΓ)
By repeating the above as many times as necessary, Γ and D are obtained. The iterative operation is preferably continued until | γ _(n) −γ _(n-1) | <10 ^-15 . And
n ₀ = εΓ / [√ (1 + εΓ) +1] ² ; A = (1 + n ₀ ) [1+ (5/4) n ₀ ² ]
s = (1-f) aAπ
m = 1-qsecu ₁ ; n = DΓ / (1-DΓ); w = mn + mn
When w ≦ 0, α = 90 °
When w> 0, α = 90 ° -2 sin ^-1 √ (w / 2)
However, the latitude and longitude of the position of the moving body 1 and (φ _2, L _2), as long as are the latitude and longitude of the position of the target (phi _1, L _1),
When w ≦ 0, α = 270 °
When w> 0, α = 270 ° + 2 sin ^-1 √ (w / 2)
The control unit 103 that searches the route point data storage unit 101 for the route point data closest to the current position of the moving body 1 then belongs to the same movement route as the searched route point data. The path point data for the portion α of the plurality of continuous moving paths to which the moving body 1 is about to enter is read from the path point data storage unit 101 (step S2). More specifically, one movement path (when generating path point data) of a part α of a plurality of paths that the moving body 1 will enter within a certain time (for example, within 10 seconds from now) (10 if separated by the length of travel distance per second) Path point data [position of road side wall LL on the left side, position of road outside line L on the left side, position of road outside line R on the right side, right side Read out the position, altitude, number of lanes, ...] of the road side wall RR.

Then, based on these path point data, the trajectory of the movement of the moving body 1 in the future, that is, an appropriate traveling line when the moving body 1 travels on the road is obtained. For example, the position of the left side road outer line L with respect to the portion α of the plurality of movement paths to which the moving body 1 is about to enter (needless to say, the position of the right side road outer line R, the position of the lane boundary line, etc.). Read the latitude and longitude, fit a straight line or curve (drawn by a dashed line in FIG. 4) that connects the positions of the plurality of road outer lines L, and obtain the extension direction of the straight line or curve (step). S3).

Further, the control unit 103 obtains the current moving direction of the moving body 1 based on the latitude and longitude of the current position of the moving body 1 that is repeatedly acquired via the current position acquiring unit 102 (step S4). .. The direction in which the moving body 1 is moving (drawn with a white arrow in FIG. 4) is known from the latitude and longitude of the latest moving body 1 position just acquired and the latest past. It becomes clear from the difference between the latitude and longitude of the position of the obtained moving body 1.

Then, the control unit 103 sets the steering direction and / or the steering amount so as to reduce the deviation between the vector of the moving direction of the moving body 1 and the vector of the extending direction of the moving path on which the moving body 1 intends to travel. It is determined and the steering device 11 is controlled (step S5).

The control unit 103 outputs information on the steering direction and / or steering amount required for reducing the deviation in a manner that appeals to the visual or auditory sense of the passenger of the moving body 1, that is, on the screen of the display 15. It is also preferable to display or output audio via the audio output device 16. This is appropriate when the passenger can receive a notice of the steering content by the control unit 103 during autonomous control, automatic driving or semi-automatic driving of the moving body 1, or when the passenger himself steers the moving body 1. It is possible to notify the passenger of a large amount of steering.

In addition, the control unit 103 is a drive source 12 of the moving body 1 based on the elevation value of each portion α included in the route point data of the portion α of the plurality of routes read from the route point data storage unit 101. Control the output of the engine or motor. By referring to the elevations of the portions α of the plurality of movement paths to which the moving body 1 is about to enter, it can be determined whether the moving body 1 is about to go uphill or downhill. Further, the height difference of the plurality of continuous portions α represents the slope of the road surface. Therefore, the control unit 103 obtains whether the portion α of the movement path that the moving body 1 is about to enter is an uphill or a downhill, and the slope of the road surface (step S6), and the moving body 1 responds accordingly. By increasing or decreasing the output of the mounted engine or motor (step S7), fluctuations in the moving speed of the moving body 1 due to the ups and downs of the moving path are suppressed.

Information on whether the control unit 103 increases or decreases the output of the engine or motor in order to suppress the fluctuation of the moving speed, and / or the amount of increase or decrease in the output required to suppress the fluctuation of the moving speed. Is output in a manner that appeals to the sight or hearing of the passenger of the moving body 1, that is, it is preferably displayed on the screen of the display 15 or output by voice via the voice output device 16. As a result, the passenger can receive a notice of the content of the output control by the control unit 103 during autonomous control, automatic driving or semi-automatic driving of the moving body 1, or it is appropriate when the passenger himself operates the accelerator pedal. It is possible to notify the passenger of the amount of operation.

In the present embodiment, the path that the moving body 1 intends to move is divided into a plurality of portions α, and the latitude and longitude of a predetermined position in the width direction intersecting the extending direction of the moving path are obtained for each portion α. The route point data storage unit 101 that stores the route point data, the current position acquisition unit 102 that repeatedly acquires the latitude and route of the current position of the moving body 1, and the movement stored in the route point data storage unit 101. The extension direction of the movement path found from the path point data for the portion α of the plurality of paths to which the body 1 is about to enter, and the latitude and longitude of the current position of the moving body 1 acquired by the current position acquisition unit 102. The steering device 11 provided in the moving body 1 is operated so that the moving direction of the moving body 1 determined from the time series is substantially parallel, or the extending direction of the moving path and the moving direction of the moving body 1 are substantially parallel. A steering support system including a control unit 103 that outputs a steering amount or a steering direction necessary for the operation in a manner that appeals to the sight or hearing of the passenger of the moving body 1 is configured. According to this embodiment, the movement path that the moving body 1 intends to move can be expressed in the form of lightweight and easy-to-use data. The system of the present embodiment is effective for detecting a route existing around the moving body 1 and thus controlling the moving body 1.

When the movement route of the moving body 1 is a road, if the route point data for each part α of the route stored in the route point data storage unit 101 includes the latitude and longitude of the road marking line. For example, it is possible to properly steer the moving body 1 along the lane markings laid on the road.

In addition, the route point data for each portion α of the route stored by the route point data storage unit 101 includes the altitude data of the portion α, and the control unit 103 informs the route point data storage unit 101. The output of the engine or motor that is the drive source 12 of the moving body 1 according to the ascending / descending of the moving path found from the path point data for the partial α of the plurality of paths to which the stored moving body 1 will enter. Outputs the operation amount or operation direction of the output of the drive source 12 necessary for operating the moving body 1 or suppressing the fluctuation of the moving speed of the moving body 1 in a manner appealing to the sight or hearing of the passenger of the moving body 1. If this is the case, it can contribute to the control of the moving speed as well as the steering of the moving body 1.

Path point data for each part α when the path point data storage unit 101 divides the movement path according to the standard movement distance per second or fractional second on the movement path of the moving body 1. Is stored, and the control unit 103 reads the route point data for the portion α of the plurality of routes that will enter in the next few seconds from the route point data storage unit 101 and uses it for control. If this is the case, the amount of route point data to be stored in the route point data storage unit 101 can be significantly reduced, the occupancy of the storage area of the main memory 1b or the auxiliary storage device 1c can be reduced, and the calculation load is also reduced. ..

The present invention is not limited to the embodiments described in detail above. For example, as an application of the system of the above embodiment, it is conceivable to correct the latitude and longitude values of the current position of the moving body 1 acquired via the navigation signal receiving device 14. The control unit 103 obtains the latitude and longitude values of the current position of the moving body 1 by using the function of the navigation signal receiving device 14 which is an element of the current position acquisition unit 102, and obtains the latitude and coordinates closest to the current position. The included route point data is searched from the route point data storage unit 101, and the lane marking or the like (road side wall LL) in the portion α of the road where the moving body 1 is currently located, which is described in the extracted route point data. , RR, road outer line L, R, or at least one of the lane boundaries) to obtain latitude and longitude values. On the other hand, the control unit 103 uses the function of the sensor 13 mounted on the moving body 1 to detect the position of the marking line or the like laid on the road, and between the moving body 1 and the marking line or the like. Measure the distance. Then, the latitude and longitude of the current position of the moving body 1 are calculated from the latitude and longitude of the marking line or the like read from the path point data storage unit 101 and the distance from the moving body 1 to the marking line or the like, and this is calculated. By comparing with the latitude and longitude of the current position of the moving body 1 obtained via the navigation signal receiving device 14, the error between the two can be obtained.

The moving body 1 targeted by the system according to the present invention is not limited to an automobile or a robot, and may be a ship, an aircraft, or the like.

In addition, the specific configuration of each part, the processing procedure, and the like can be variously modified without departing from the spirit of the present invention.

1 ... Moving body 11 ... Steering device 12 ... Drive source (engine or motor)
15 ... Display 16 ... Voice output device 101 ... Path point data storage unit 102 ... Current position acquisition unit 103 ... Control unit α ... Movement route part LL, L, R, RR ... Predetermined position in the width direction of the movement route (road side wall) , Road outside line)

Claims (5)

A path point that divides the path that the moving body intends to move into a plurality of parts and stores the path point data, which is data obtained by obtaining the latitude and longitude of a predetermined position in the width direction that intersects the extension direction of the movement path for each part. Data storage and
The current position acquisition unit that iteratively acquires the latitude and path of the current position of the moving object,
A plurality of path point data stored in the path point data storage unit are read out for a plurality of continuous path portions where the moving body will enter within a certain time from now on, and the plurality of path point data of the plurality of path point data. A straight line or curve that connects the latitude and path is fitted to obtain the extension direction of the straight line or curve, and it is found from the time series of the extension direction and the latitude and longitude of the current position of the moving body acquired by the current position acquisition unit. The steering device provided in the moving body is operated so that the moving direction of the moving body is substantially parallel to the moving direction, or the steering amount or steering direction required for the extension direction thereof and the moving direction of the moving body to be substantially parallel. It is equipped with a control unit that outputs data in a manner that appeals to the visual or auditory sense of a moving passenger .
The movement route of the moving body is a road, the route point data for each part of the route stored in the route point data storage unit includes the latitude and longitude of the road marking line, and each route point data moves. It is the path point data for each part when the movement path is divided at equal intervals for each standard movement distance per second or fractional second on the movement path of the body.
A steering support system in which the control unit reads a plurality of path point data for a plurality of path portions that will enter in the next few seconds from the path point data storage unit and uses the control unit for control . A path point that divides the path that the moving body intends to move into a plurality of parts and stores the path point data, which is data obtained by obtaining the latitude and longitude of a predetermined position in the width direction that intersects the extension direction of the movement path for each part. Data storage and
The current position acquisition unit that iteratively acquires the latitude and path of the current position of the moving object,
A plurality of path point data stored in the path point data storage unit are read out for a plurality of continuous path portions where the moving body will enter within a certain time from now on, and the plurality of path point data of the plurality of path point data. A straight line or curve that connects the latitude and path is fitted to obtain the extension direction of the straight line or curve, and it is found from the time series of the extension direction and the latitude and longitude of the current position of the moving body acquired by the current position acquisition unit. The steering device provided in the moving body is operated so that the moving direction of the moving body is substantially parallel to the moving direction, or the steering amount or steering direction required for the extension direction thereof and the moving direction of the moving body to be substantially parallel. It is equipped with a control unit that outputs data in a manner that appeals to the visual or auditory sense of a moving passenger.
The movement route of the moving body is a road, and the route point data for each part of the route stored in the route point data storage unit includes the latitude and longitude of the road marking line .
The control unit acquires the latitude and path of the current position of the moving body via the current position acquisition unit, and reads out the path point data including the latitude and coordinates closest to the current position from the path point data storage unit. While obtaining the latitude and longitude values of the lane markings on the part of the road where the moving object is currently located, the sensors mounted on the moving objects are used to detect the position of the lane markings laid on the road. The latitude and longitude of the lane marking read from the path point data storage unit by measuring the distance between the moving body and the lane marking, and the latitude and longitude of the current position of the moving body from the distance from the moving body to the lane marking. Steering support system that calculates and corrects longitude . The route point data for each part of the route stored by the route point data storage unit includes the altitude data of the portion.
The control unit reads out a plurality of path point data stored in the path point data storage unit for a plurality of continuous path portions that the moving body will enter within a certain time from now on. The output of the drive source required to operate the output of the engine or motor, which is the drive source of the moving body, or to suppress the fluctuation of the moving speed of the moving body according to the ascending / descending of the moving path found from the path point data of The steering support system according to claim 1 or 2, wherein the operation amount or operation direction of the above is output in a manner that appeals to the visual or auditory sense of a moving passenger. Path point data for each part when the path point data storage unit divides the movement path at equal intervals for each standard movement distance per second or fractional second on the movement path of the moving body. Stores and
The steering support system according to claim 2, wherein the control unit reads a plurality of path point data for a plurality of path portions to be entered in the next few seconds from the path point data storage unit and uses the control unit for control. A computer, which is used to construct the steering support system according to claim 1, 2, 3 or 4.
A path point that divides the path that the moving body intends to move into a plurality of parts and stores the path point data, which is data obtained by obtaining the latitude and longitude of a predetermined position in the width direction that intersects the extension direction of the movement path for each part. Data storage,
The current position acquisition unit that iteratively acquires the latitude and path of the current position of the moving object, and
A plurality of path point data stored in the path point data storage unit are read out for a plurality of continuous path portions where the moving body will enter within a certain time from now on, and the plurality of path point data of the plurality of path point data. A straight line or curve that connects the latitude and path is fitted to obtain the extension direction of the straight line or curve, and it is found from the time series of the extension direction and the latitude and longitude of the current position of the moving body acquired by the current position acquisition unit. The steering device provided in the moving body is operated so that the moving direction of the moving body is substantially parallel to the moving direction, or the steering amount or steering direction required for the extension direction thereof and the moving direction of the moving body to be substantially parallel. To function as a control unit that outputs in a manner that appeals to the visual or auditory sense of a moving passenger .
The movement route of the moving body is a road, the route point data for each part of the route stored in the route point data storage unit includes the latitude and longitude of the road marking line, and each route point data moves. It is the path point data for each part when the movement path is divided at equal intervals for each standard movement distance per second or fractional second on the movement path of the body.
A program in which the control unit reads a plurality of path point data for a plurality of path portions that will enter in the next few seconds from the path point data storage unit and uses them for control .

Images