JP6947592B2 - Map generator and map generator and control device - Google Patents

Description

The present technology relates to a device and a method for generating a map used for controlling the movement of a moving body toward a target point, and a control device for controlling the moving body.

In order for a moving body such as an unmanned vehicle to move autonomously, the moving body is provided with a map generator, a route generator, and a control unit. The map generator generates a map that incorporates the current position of the moving object, the target point of movement, and the position of an obstacle. Based on this map, the route generation unit generates a movement route from the current position of the moving body to the target point without interfering with obstacles. The control unit controls the moving body so that the moving body moves along the generated movement path (for example, Patent Document 1).

The target point is, for example, a waypoint through which the moving body passes next among a plurality of waypoints through which the moving body passes before reaching the final target position. The plurality of waypoints are set in the map stored in the storage unit of the map generator, for example.

The position of the obstacle is detected by an obstacle sensor provided on the moving body. The obstacle sensor is a laser radar that detects the position of an obstacle by, for example, scanning a laser beam. The location of the obstacle detected by the obstacle sensor is incorporated into the map described above.

The current position of the moving body is determined by a position detecting device provided on the moving body. For example, the position detecting device includes a radio wave receiving unit, a processing unit, an internal sensor, and an estimating unit. The radio wave receiving unit receives radio waves from a positioning satellite such as a GPS satellite. The processing unit obtains the position and orientation of the moving body based on the received radio wave. The internal sensor detects the operating state of the moving body (for example, the rotational speed of the traveling wheel and the orientation of the moving body). The estimation unit estimates the current position of the moving body by a Kalman filter based on the position and orientation of the moving body obtained by the processing unit and the operating state detected by the internal sensor (for example, Patent Document 2).
The position detection device may obtain the current position of the moving object based on the radio wave from the positioning satellite without using the internal sensor.

The current position of the moving object obtained as described above is also incorporated into the map. In this map, the route generation unit generates a movement route from the current position of the moving body to the target point without interfering with obstacles.

Japanese Unexamined Patent Publication No. 2010-039839 Japanese Unexamined Patent Publication No. 2014-228495

However, depending on the reception status of radio waves from the positioning satellite, the accuracy of the current position required by the position detection device may decrease. For example, the radio wave receiving unit may receive both radio waves transmitted from the positioning satellite and reflected by a structure (for example, a building) and radio waves directly propagated from the positioning satellite without such reflection. .. In this case, since the current position of the moving body is obtained based on both radio waves, the accuracy is lowered.

As a result, in the above-mentioned map, the accuracy of the relative position between the current position of the moving body and the obstacle may decrease.

Therefore, the purpose of this technology is to accurately determine the relative positional relationship between the moving object and the obstacle in the map when generating a map that incorporates the current position of the moving object, the target point of movement, and the position of the obstacle. It is to make it possible to express well.

In order to achieve the above object, the map generator according to the present technology is a device that generates a map used for controlling the movement of a moving object toward a target point, and is an internal sensor that detects the operating state of the moving object. The obstacle sensor that detects the position of the obstacle from the moving body, the first detection device that obtains the first current position of the moving body based on the operating state, and the relative position between the first current position and the position of the obstacle. A map generator that generates a map representing It includes a relative position detection unit. The map generation unit sets the target point on the map based on the obtained relative position.

In order to achieve the above object, the map generation method according to the present technology is a method of generating a map used for controlling the movement of a moving object toward a target point, and the following processes (a) to (g) are performed. conduct.
(A) The operating state of the moving body is detected by an internal sensor provided on the moving body.
(B) Detect the position of an obstacle from a moving body.
(C) The first current position of the moving body is obtained based on the operating state.
(D) Generate a map showing the relative position between the first current position and the position of the obstacle.
(E) Obtain the second current position of the moving object based on the radio waves from the positioning satellite.
(F) Find the relative position between the second current position and the given target point.
(G) A target point is set on the map based on this relative position.

According to the present technology, the map is generated based on the detection by the obstacle sensor and the internal sensor provided in the moving body. That is, the map is generated based on the information that is not affected by the reception status of the radio waves from the positioning satellite. In this respect, the relative positional relationship between the current position of the moving body and the obstacle can be accurately represented on the map.

On the other hand, when radio waves from positioning satellites are not used, if there is an error in the detection of the internal sensor, an error will occur in the current position of the moving object based on the detection of the internal sensor, and as a result, the current position will be displayed on the map. The accuracy of the position relative to the target point is reduced.
On the other hand, according to the present technology, the relative position between the current position of the moving body and the target point based on the radio wave from the positioning satellite is obtained, and the target point is set on the map based on this relative position. As a result, even if an error occurs in the detection of the internal sensor, it is possible to suppress the influence of this error on the relative position between the current position of the moving body and the target point on the map.

The moving body provided with the map generator according to the 1st Embodiment is shown. It is a block diagram which shows the structure of the map generation apparatus by 1st Embodiment. It is a flowchart which shows the map generation method and route generation processing by 1st Embodiment. It is a figure for demonstrating an example of the effect of 1st Embodiment. It is a block diagram which shows the structure of the map generation apparatus by 2nd Embodiment. It is a block diagram which shows the structure of the 1st detection apparatus in 2nd Embodiment. It is a flowchart which shows the correction process in 2nd Embodiment.

An embodiment of the present technology will be described with reference to the drawings. In addition, the same reference numerals are given to common parts in each figure, and duplicate description is omitted.

[First Embodiment]
FIG. 1 shows a mobile body 20 provided with a map generation device 10 according to a first embodiment of the present technology. FIG. 2 is a block diagram showing the configuration of the map generator 10.

The moving body 20 may be a vehicle that has wheels 1 for traveling and travels on the ground surface 2 by being rotationally driven by the wheels 1. Alternatively, the moving body 20 may be a traveling device traveling on the ground by a crawler, or another moving device.

The map generation device 10 is provided on the moving body 20. The map generation device 10 includes an internal sensor 3, an obstacle sensor 5, a first detection device 7, a map generation unit 9, a second detection device 13, and a relative position detection unit 15.

The internal sensor 3 detects the operating state of the moving body 20. In the present embodiment, the operating state is the information detected in the moving body 20 and is the information used to obtain the moving direction (direction) and the moving distance of the moving body 20. The operating state does not have to include information received from the outside of the mobile body 20 (for example, a positioning satellite of a satellite navigation system). In the present embodiment, the operating state includes the moving speed (hereinafter simply referred to as speed) and the angular velocity of the moving body 20. In the present embodiment, the angular velocity may mean the rotational speed of the moving body 20 around the axis fixed to the moving body 20. In the present embodiment, the angular velocity is the angular velocity of the moving body 20 in the yaw direction (around the yaw axis), but the present technology is not limited to this.

In the present embodiment, the internal sensor 3 includes a speed detection unit 3a and an angular velocity detection unit 3b. The speed detection unit 3a detects the speed of the moving body 20. In one example, the speed detection unit 3a measures the rotation speed of the wheel 1 described above, and detects the speed of the moving body 20 based on the rotation speed and the known outer diameter of the wheel 1. The angular velocity detection unit 3b detects the angular velocity of the moving body 20. The angular velocity detection unit 3b may be configured by using a gyro sensor.

The obstacle sensor 5 detects the position of the obstacle from the moving body 20. In the present embodiment, the obstacle sensor 5 acquires obstacle data indicating the position of an obstacle existing in the measurement range by performing measurement with respect to the measurement range seen from the moving body 20. The measurement range may be, for example, a range on the traveling direction side of the moving body 20 (a range surrounded by a broken line in FIG. 1B), or may be around the moving body 20. In the present embodiment, the obstacle data is data in which the coordinates of the position of the obstacle are represented by the sensor coordinate system fixed to the moving body 20, and the origin of the sensor coordinate system is, for example, the obstacle data acquired. It is the position of the moving body 20 at the time of (hereinafter, simply referred to as measurement). The acquisition of obstacle data by the obstacle sensor 5 is repeated during the movement of the moving body 20.

The obstacle sensor 5 is, for example, a laser radar. The laser radar 5 scans the laser beam (for example, pulsed laser beam) with respect to the measurement range, and acquires obstacle data representing the position of the obstacle based on the reflected laser beam from the reflection point on the surface of the object. The laser radar 5 may be, for example, a device called a lidar (Light Detection and Ranger) or an LRF (Laser Range Finder). The obstacle sensor 5 may acquire image data by imaging the measurement range, and may acquire obstacle data indicating the position of the obstacle by processing the image data.

The first detection device 7 obtains the current position of the moving body 20 (hereinafter referred to as the first current position) based on the operating state detected by the internal sensor 3. This first current position is represented by the first coordinate system Sr described later. In the present embodiment, the first current position of the moving body 20 is obtained based on the speed detected by the speed detection unit 3a and the angular velocity detected by the angular velocity detection unit 3b. For example, the first detection device 7 obtains the traveling direction of the moving body 20 from the time integral value of the angular velocity, and based on the traveling direction obtained at each time point and the speed detected by the speed detection unit 3a at each time point. , The first current position of the moving body 20 is obtained.

The map generation unit 9 is a map Mr. showing the relative position between the first current position detected by the first detection device 7 and the position of the obstacle based on the position of the obstacle detected by the obstacle sensor 5. To generate. More specifically, the map generation unit 9 determines the position of the obstacle and the first current position from the position of the obstacle detected by the obstacle sensor 5 and the first current position obtained by the first detection device 7 at the time of this detection. The relative position with respect to the current position is acquired, and the position of the obstacle is set in the map Mr based on the relative position and the first current position. Therefore, the map Mr shows the position of the obstacle detected by the obstacle sensor 5 at the time of measurement and the first current position detected by the first detection device 7 at the time of the measurement. The map generation unit 9 includes a map update unit 9a, a map storage unit 9b, and a position update unit 9c.

Each time the obstacle sensor 5 acquires the above-mentioned obstacle data, the map updating unit 9a converts the coordinates of the position of the obstacle represented by the obstacle data into coordinates and stores the map in the map storage unit 9b. Add to Mr. This coordinate conversion is a conversion from the above-mentioned sensor coordinate system to the coordinate system used in the first detection device 7 (that is, the first coordinate system Sr described later). Further, this coordinate conversion is performed based on the first current position at the time of acquiring the obstacle data and the orientation of the moving body 20 obtained from the operating state detected by the internal sensor 3. As a result, the map updating unit 9a updates the map Mr stored in the map storage unit 9b in the first coordinate system Sr. Therefore, at the present time, the map Mr of the map storage unit 9b incorporates the coordinates of the position of the obstacle represented by the obstacle data acquired at each time in the past. In the initial state, the map Mr of the map storage unit 9b may not incorporate the coordinates of the position of the obstacle, or may incorporate the position of a known obstacle (for example, a building). ..

The position update unit 9c represents the first current position of the moving body 20 detected by the first detection device 7 from moment to moment on the map Mr stored in the map storage unit 9b. As a result, the position update unit 9c represents the information including the first current position, the position of the obstacle, and the target point described later as one map Mr, and outputs the information to the route generation unit 11. In this map Mr, the relative position between the first current position and the position of the obstacle is shown.
Alternatively, the position update unit 9c converts the above-mentioned map Mr into a map of a coordinate system centered on the first current position (for example, the origin), and from the converted map, the first current position and the next target point (that is, that is). A local range including a target point (described later) may be extracted as a local map Mc, and this local map Mc may be output to the route generation unit 11.

The map Mr stored in the map storage unit 9b is data representing the position coordinates of obstacles in the first coordinate system Sr. In the present embodiment, the first coordinate system Sr is referred to as a relative precision priority coordinate system. The relative precision priority coordinate system Sr means a coordinate system that represents the relative positions of the moving body 20 and the obstacle without being affected by the reception status of the radio waves from the positioning satellite. The position of the obstacle in the map Mr of the map storage unit 9b is based on the obstacle data representing the position of the obstacle as seen from the moving body 20. Further, the first current position of the moving body 20 on the map Mr is based on the operating state of the moving body 20. Therefore, it can be said that the map Mr accurately represents the positional relationship between the moving body 20 and the obstacle in that it is not affected by the reception status of the radio waves from the positioning satellite.

The radio wave receiving unit 12 is provided on the moving body 20 and receives radio waves from a positioning satellite of a satellite navigation system (for example, GPS).

The second detection device 13 obtains the current position of the moving body 20 (hereinafter referred to as the second current position) based on the radio wave received by the radio wave receiving unit 12. In the present embodiment, the second detection device 13 obtains the second current position of the moving body 20 in the second coordinate system Sa fixed to the ground surface 2. In the present embodiment, the second coordinate system Sa is referred to as an absolute precision priority coordinate system. The absolute precision priority coordinate system Sa means a coordinate system representing the current position of the moving body 20 so that the accumulation of information errors by the internal sensor 3 is not affected (or the accumulation of the errors is suppressed). The second detection device 13 is a Bayes filter (Bayes filter) based on the position and orientation of the moving body 20 obtained based on the radio waves received by the radio wave receiving unit 12 and the above-mentioned operating state detected by the internal sensor 3. For example, a Kalman filter) may be used to determine the second current position of the moving body 20.

The relative position detection unit 15 obtains a relative position between the second current position obtained by the second detection device 13 and one or a plurality of target points given in advance in the absolute precision priority coordinate system Sa. The target point is the position where the moving body 20 will pass from now on. In one example, the target point is the waypoint that the moving body 20 will pass next. That is, a plurality of waypoints that the moving body 20 passes through before reaching the final target position are predetermined in the absolute precision priority coordinate system Sa, and among these a plurality of waypoints, the moving body 20 is next. The passing point is the target point.

In order to determine the target point, in one example, as shown in FIG. 2, the map generation device 10 is provided with a storage device 17 and an input device 19. The storage device 17 stores a map Ma (for example, a wide area map) represented by the absolute precision priority coordinate system Sa. A person can set one or more target points on the map Ma by operating the input device 19. The input device 19 may use, for example, a GUI (graphical user interface).

In the present embodiment, the relative position detection unit 15 obtains the relative position between the second current position and the target point at a set frequency. Here, "obtaining by the set frequency" means that every time a certain time elapses, every time the moving body 20 moves a certain distance, or the moving body 20 passes through a certain number of target points (via points). It may mean that the relative position detection unit 15 obtains the relative position each time. The setting frequency may be appropriately set according to the specific application of the present technology. For example, the setting frequency may be less than the frequency with which the map updating unit 9a updates the map Mr. In this case, the setting frequency may be 1/3 or less, 1/5 or less, 1/10 or less, or 1/20 or less of the frequency at which the map updating unit 9a updates the map Mr, but is not limited thereto.

The target point setting unit 9d of the map generation unit 9 sets one or a plurality of target points on the map Mr. based on the relative position each time the relative position detection unit 15 obtains the above-mentioned relative position. In the present embodiment, assuming that the first current position and the second current position are the same on the map Mr, the target point setting unit 9d is based on the relative position obtained by the relative position detection unit 15 and the first current position. Set one or more target points on the map Mr. That is, the target point setting unit 9d has the relative position obtained by the relative position detection unit 15 based on the first current position obtained by the first detection device 7, and the second current position coincides with the first current position. As described above, the map Mr. is converted to the relative position of the relative accuracy priority coordinate system Sr, and one or a plurality of target points are set in the map Mr. based on the converted relative position. This coordinate transformation is performed based on the first current position and the relationship between the orientation of the coordinate system Sa and the orientation of the coordinate system Sr (described in detail in S61 to S63 described later).

The moving body 20 is controlled to move toward the next target point set in the map Mr or the local map Mc extracted from the map Mr. Therefore, in the present embodiment, the moving body 20 is provided with a route generation unit 11 and a control unit 21. The route generation unit 11 generates a movement route of the moving body 20 toward the next target point based on the map Mr or the local map Mc generated by the map generation device 10 (that is, the position update unit 9c). That is, the route generation unit 11 generates a movement route toward the next target point in the map Mr or the local map Mc without interfering with obstacles.

The control unit 21 controls the movement of the moving body 20 based on the generated movement path. That is, the control unit 21 controls the moving body 20 so that the moving body 20 moves along the generated movement path. In this control, for example, the control unit 21 controls the operation of a plurality of actuators that operate the accelerator, brake, steering, transmission, and the like of the vehicle as the moving body 20. The moving body 20 may be configured to move autonomously by the control unit 21, or may have another configuration. The moving body 20 may be temporarily remotely controlled (for example, in the moving direction) from the outside of the moving body 20.
The route generation unit 11 and the control unit 21 constitute a control device 30 for the mobile body 20.

FIG. 3 is a flowchart showing a map generation method and a route generation process using the map generation device 10 described above. The map generation method is performed during the movement of the moving body 20. The map generation method includes a map generation process of FIG. 3A, a current position detection process of FIG. 3B, and a target point setting process of FIG. 3C.

<Map generation process>
The map generation process includes steps S1 and S2. In step S1, the above-mentioned obstacle data is acquired by the obstacle sensor 5. In step S2, the map updating unit 9a updates the map Mr stored in the map storage unit 9b based on the obstacle data acquired in step S1 as described above. The map generation process is repeated in the map generation cycle.

<Current position detection process>
The current position detection process includes steps S3 and S4. In step S3, the operating state of the moving body 20 is detected by the internal sensor 3. In step S4, the first detection device 7 obtains the first current position of the moving body 20 in the relative precision priority coordinate system Sr (map Mr) based on the operating state detected in step S3. The current position detection process is repeated every moment. Therefore, in step S4, the moving distance and moving direction of the moving body 20 from the time when the previous step S4 is performed are obtained based on the operating state, and these moving distances and moving directions, and the first obtained in the previous step S4 are obtained. The new first current position is obtained based on the current position.

<Target point setting process>
The target point setting process includes steps S5 to S7 and is executed by the relative position detection unit 15.
In step S5, the second detection device 13 obtains the second current position of the mobile body 20 based on the radio waves from the positioning satellite.
In step S6, the relative position detection unit 15 obtains the relative position between the second current position obtained in step S5 and the target point given in advance.
In step S7, the relative position obtained in step S6 is converted to the relative position of the relative accuracy priority coordinate system Sr based on the first current position detected by the first detection device 7, and is converted to the converted relative position. Based on this, the target point setting unit 9d sets the target point in the map Mr stored in the map storage unit 9b. At this time, if there is a target point set in the past on the map Mr, the target point is deleted or is not used for future movement route generation.
The target point setting process (that is, steps S5 to S7) is repeated at the above-mentioned setting frequency.

Step S6 includes steps S61 to S63.
In step S61, in the absolute precision priority coordinate system Sa, the relative position ΔPa between the second current position Pa obtained in step S5 and the target point Pwa given in advance is obtained.

In step S62, the relative position ΔPa of the absolute precision priority coordinate system Sa is converted to the relative position ΔPm in the mobile coordinate system Sm. Here, the moving body coordinate system Sm is a coordinate system fixed to the current moving body 20, and the position of the current moving body 20 (that is, the latest second current position) is the origin. The transformation of ΔPa in step S62 may be a coordinate transformation of a vector indicating ΔPa, and is a transformation combining translation and rotation. Regarding this, the translation is a movement in which the latest second current position (starting point of the vector ΔPa) obtained in step S5 is made to match the origin of the moving body coordinate system Sm, and the rotation is a movement in the absolute accuracy priority coordinate system Sa. This is done based on the orientation of the body coordinate system Sm. The orientation of the moving body coordinate system Sm is obtained in the absolute accuracy priority coordinate system Sa based on the radio waves from the positioning satellites received by the two radio wave receiving units 12 provided at two locations apart from each other in the moving body 20. It may be there.

In step S63, the relative position ΔPm of the moving body coordinate system Sm is converted into the relative position ΔPr in the relative accuracy priority coordinate system Sr. This transformation may be a coordinate transformation of a vector indicating ΔPm, and is a transformation that combines translation and rotation. Regarding this, the translation is a movement in which the latest second current position (starting point of the vector ΔPm) obtained in step S5 is matched with the latest first current position in the relative precision priority coordinate system Sr, and the rotation is a moving object. This is performed based on the difference in orientation between the coordinate system Sm and the relative precision priority coordinate system Sr. For example, when the relative accuracy priority coordinate system Sr is a coordinate system fixed to the moving body 20 at the past time point, the difference in orientation between the moving body coordinate system Sm and the relative accuracy priority coordinate system Sr is the past time point. It may be obtained based on the direction of the moving body 20 in the above and the time integral value of the angular velocity detected by the angular velocity detecting unit 3b from the past time point to the present.

In step S7 described above, the target point setting unit 9d sets the target point Pwr on the map Mr based on the relative position ΔPr obtained in step S63. At this time, when the second current position, which is the start point of the vector representing ΔPr, is matched with the latest first current position on the map Mr, the position that becomes the end point of the vector may be set as the target point.

<Route generation process>
The route generation process includes steps S8 and S9 as shown in FIG. 3 (D). In step S8, the first current position of the latest moving body 20 obtained in the current position detection process (step S4) is displayed on the latest map Mr of the map storage unit 9b updated by the map generation process. This process is performed by the position update unit 9c.
In step S9, in the latest map Mr (or the above-mentioned local map Mc obtained from the map Mr) whose first current position is represented by step S8, the moving body 20 moves from the first current position to the next target point. The route generation unit 11 generates a movement route for heading. The route generation process is repeated in the route generation cycle.

<Control of moving body>
The movement route generated in the route generation process is used for controlling the moving body 20 until it is newly generated in the next route generation process. For example, the control unit 21 controls the movement of the moving body 20 from moment to moment based on the movement path, the speed detected by the speed detection unit 3a, and the angular velocity detected by the angular velocity detection unit 3b. As a result, the moving body 20 is controlled to move on the moving path. For example, the control unit 21 repeats the following processes (i) to (iii).
(I) The traveling direction of the moving body 20 is obtained from the time integral value of the angular velocity detected by the angular velocity detecting unit 3b, the traveling direction, the traveling direction obtained at each time point, and the speed detected by the speed detecting unit 3a at each time point. Based on the above, the current position of the moving body 20 is obtained.
(Ii) The deviation between the current position of the obtained moving body 20 and the above-mentioned moving path is obtained.
(Iii) The moving body 20 is controlled so that the obtained deviation is eliminated, that is, the moving body 20 moves along the above-mentioned movement path.

(Effect of the first embodiment)
The following is an example of the effect of the first embodiment, and does not limit the present technology.

The map Mr is generated based on the detection by the obstacle sensor 5 and the internal sensor 3 provided on the moving body 20. That is, the map Mr is generated based on the information that is not affected by the reception status of the radio waves from the positioning satellite. In this respect, the map Mr can accurately represent the relative positional relationship between the current position of the moving body 20 and the obstacle. Therefore, the movement route generation based on such an accurate map Mr. is realized.

FIG. 4 is an explanatory diagram showing an example of this effect. FIG. 4A shows a map incorporating the current position of the mobile body 20 based on radio waves from the positioning satellite, and FIG. 4B shows a map Mr in the case of the present embodiment. In FIGS. 4A and 4B, the plurality of triangular marks indicate the current positions of the moving bodies 20 at the time points t1 to t3 surrounded by broken lines, and the square marks indicate the next target points.

In FIG. 4A, the current position of the moving body 20 is correct at the time point t1 and deviates from the correct current position (solid line triangle mark) at the next time point t2 due to deterioration of the radio wave reception condition (broken line triangle). Mark), and at the next time point t3, it is assumed that the signal is correct due to the normalization of the radio wave reception status. In this case, the same obstacle is detected as separate obstacles at the time point t1 and the time point t2 and incorporated into the map Mr. As a result, at time t3, the movement path, which should originally be a straight line, becomes curved so as to avoid an obstacle on the left side that is erroneously incorporated.

In FIG. 4B, it is assumed that the original position of the moving body 20 and the position of the obstacle are the same as those in FIG. 4A. In FIG. 4B, since the current position of the moving body 20 is correct at any of the time points t1, t2, and t3, a moving path that goes straight to the target point is generated at the time point t3.

In the present embodiment, further, the relative position between the second current position of the moving body 20 based on the radio wave from the positioning satellite and the target point is obtained, and the target point is set on the map based on this relative position. As a result, even if an error occurs in the detection of the internal sensor 3, the influence of this error can be suppressed.

In other words, the present embodiment compensates for the advantages of one coordinate system and the disadvantages of the other coordinate system for the relative precision priority coordinate system Sr and the absolute precision priority coordinate system Sa. The details are as follows (1) and (2).

(1) The relative accuracy priority coordinate system Sr representing the map Mr has an advantage that the accuracy of the relative position between the first current position of the moving body 20 detected by the first detection device 7 and the obstacle is high. On the other hand, in this relative accuracy priority coordinate system Sr, if the error of the first current position of the moving body 20 is accumulated due to the detection error of the internal sensor 3, the accuracy of the relative position between the moving body 20 and the target point is lowered. It is disadvantageous because it may be done. This disadvantage is suppressed by setting the target point on the map Mr based on the relative position between the moving body 20 and the target point obtained in the absolute precision priority coordinate system Sa.

(2) In the absolute precision priority coordinate system Sa used by the second detection device 13, the relative position between the second current position of the moving body 20 and the target point detected by using the radio waves from the positioning satellite is represented. .. Therefore, this relative position has an advantage that there is no or little influence of the detection error of the internal sensor 3. On the other hand, the absolute accuracy priority coordinate system Sa is disadvantageous because if the reception condition of the radio wave from the positioning satellite deteriorates, the accuracy of the relative position between the moving body 20 and the obstacle number may decrease. This disadvantage can be suppressed by incorporating the relative position between the first current position of the moving body 20 and the position of the obstacle obtained in the relative accuracy priority coordinate system Sr into the map Mr.

Further, the relative position detection unit 15 obtains the relative position between the second current position of the mobile body 20 based on the radio wave from the positioning satellite and the target point at a set frequency. Each time the relative position is obtained, the map generation unit 9 converts the relative position into the relative position of the relative accuracy priority coordinate system Sr based on the first current position detected by the first detection device 7. The target point is set in the map Mr based on the relative position after conversion. Therefore, it is possible to prevent the relative position between the first current position and the target point on the map Mr from being significantly deviated from the corresponding actual relative position due to the accumulation of the error of the first current position based on the operating state.

As described above, the relative position detection unit 15 obtains the relative position ΔPa between the second current position P2a and the target point Pta in the absolute accuracy priority coordinate system (second coordinate system) Sa, and sets ΔPa as the current moving object. It is converted to the relative position ΔPm in the moving body coordinate system Sm fixed to 20, and ΔPm is converted to the relative position ΔPr in the relative accuracy priority coordinate system (first coordinate system) Sr. By such coordinate transformation, ΔPr indicating the target point Pta with respect to the second current position P2a of the moving body 20 can be obtained in the relative precision priority coordinate system Sr representing the map Mr. Therefore, the map generation unit 9 can set a target point on the map Mr based on ΔPr based on the radio wave from the positioning satellite.

[Second Embodiment]
FIG. 5 is a block diagram showing a map generation device 10 according to a second embodiment of the present technology. The second embodiment is the same as the first embodiment in that it is not described below.
In the second embodiment, the configuration of the first detection device 7 is different from that of the first embodiment. FIG. 6 is a block diagram showing the configuration of the first detection device 7. In the second embodiment, the first detection device 7 corrects the speed detected by the speed detection unit 3a, corrects the angular velocity detected by the angular velocity detection unit 3b, and the moving body 20 is based on the corrected speed and the angular velocity. The first current position is detected.

The first detection device 7 includes a speed acquisition unit 7a, a speed correction data calculation unit 7b, a speed correction unit 7c, a direction change acquisition unit 7d, an angular velocity correction data calculation unit 7e, an angular velocity correction unit 7f, and a position calculation unit 7g.

The speed acquisition unit 7a obtains the speed of the moving body 20 based on the radio wave from the positioning satellite received by the radio wave receiving unit 12. This speed may be obtained without relying on the information from the internal sensor 3.

The speed correction data calculation unit 7b obtains speed correction data to be used for speed correction based on the speed detected by the speed detection unit 3a of the internal sensor 3 and the speed obtained by the speed acquisition unit 7a. The speed correction data may represent a difference or ratio between the speed detected by the speed detection unit 3a and the speed obtained by the speed acquisition unit 7a.

The speed correction unit 7c corrects the speed detected by the speed detection unit 3a of the internal sensor 3 based on the speed correction data obtained by the speed correction data calculation unit 7b.

The orientation change acquisition unit 7d obtains a change in the orientation of the moving body 20 based on the radio waves from the positioning satellite received by the two radio wave receiving units 12 provided at two positions apart from each other in the moving body 20. This change in orientation may be obtained without relying on the information from the internal sensor 3.

The angular velocity correction data calculation unit 7e corrects the angular velocity based on the angular velocity detected by the angular velocity detection unit 3b of the internal sensor 3 and the change in orientation obtained by the orientation change acquisition unit 7d (for example, the angular velocity of the moving body 20). Obtain the angular velocity correction data to be used. The angular velocity correction data may represent the difference or ratio between the angular velocity detected by the angular velocity detecting unit 3b and the angular velocity obtained by the orientation change acquisition unit 7d.

The angular velocity correction unit 7f corrects the angular velocity detected by the angular velocity detection unit 3b of the internal sensor 3 based on the angular velocity correction data obtained by the angular velocity correction data calculation unit 7e.

The position calculation unit 7g calculates the first current position of the moving body 20 based on the speed corrected by the speed correction unit 7c and the angular velocity corrected by the angular velocity correction unit 7f. The position calculation unit 7g inputs the calculated first current position to the map generation unit 9 (position update unit 9c).

FIG. 7 is a flowchart showing the correction process in the second embodiment. This correction process includes steps S11 to S17 for correcting the speed, steps S21 to S27 for correcting the angular velocity, and steps S28 for obtaining the first current position from the corrected speed and the angular velocity.

<Speed correction processing>
Step S11 and step S12 are performed in parallel. In step S11, the speed acquisition unit 7a obtains the speed of the moving body 20 based on the radio waves from the positioning satellite. In step S12, the speed of the moving body 20 is obtained by the speed detection unit 3a of the internal sensor 3.

In step S13, it is determined whether the condition is satisfied when the condition related to the operation of the moving body 20 (hereinafter referred to as the operating condition) is satisfied, and the condition is satisfied when the steps S11 and S12 are performed. That is, it is determined whether or not the operating conditions are satisfied when step S11 and step S12 are performed.

The operating conditions are preset and are conditions for indicating that the operation of the moving body 20 is stable. The operating conditions include the following (1) to (3). In the present embodiment, the operating conditions include the following (1) to (3). Hereinafter, the time period during which steps S11 and S12 are performed is referred to as a sample acquisition time.
(1) The amount of change in the speed of the moving body 20 during the sample acquisition time (for example, 1 second) is equal to or less than a predetermined reference value for speed change.
(2) The amount of change in the orientation of the moving body 20 during the sample acquisition time is equal to or less than a predetermined reference value for orientation change.
(3) The speed of the moving body 20 (the minimum value of the absolute value of the speed at the sample acquisition time) is equal to or higher than the predetermined reference value for the speed.

The "velocity of the moving body 20" in the above (1) and (3) may be the speed detected by the speed detecting unit 3a or the speed detected based on the radio wave received by the radio wave receiving unit 12. You may. The "change amount of the direction of the moving body 20" in (2) above may be a time integral value of the angular velocity detected by the angular velocity detection unit 3b.

In step S13, if the operating conditions are satisfied, the process proceeds to step S14, and if not, steps S11 and S12 are performed again.

In step S14, the speed correction data calculation unit 7b obtains a sample of speed correction data used for speed correction based on the speed detected in step S11 and the speed obtained in step S12. In one example, the sample is a correction gain Gv expressed by the following equation.

Gv = L1 / L2

Here, L1 is a value obtained by integrating the speed detected by the speed acquisition unit 7a over the sample acquisition time. L2 is a value obtained by integrating the speed detected by the speed detection unit 3a of the internal sensor 3 over the sample acquisition time.

In step S15, it is determined whether the speed correction data calculation unit 7b has obtained the set number of the above samples. When a set number of samples (a number of 2 or more) is obtained, the process proceeds to step S16, and if not, the process returns to steps S11 and S12, and the above processing is repeated.

In step S16, the speed correction data calculation unit 7b obtains speed correction data based on a set number of samples. In the present embodiment, in step S16, the average value of the set number of samples is obtained as the speed correction data. However, the speed correction data may be obtained by using the robust averaging method. As a result, the speed correction data can be obtained without considering the set number of samples that deviate significantly from the average value of the set number of samples.

When step S16 is performed, the process proceeds to step S17, and steps S11 and S12 described above are performed again. That is, the process of steps S11 to S16 described above is repeated while performing step S17 as described later.

In step S17, the speed correction unit 7c corrects the speed detected by the speed detection unit 3a of the internal sensor 3 based on the speed correction data obtained in step S16. The corrected speed is input to the position calculation unit 7g. Regarding this, the speed correction unit 7c corrects and corrects the speed detected moment by moment by the speed detection unit 3a based on the speed correction data obtained in the immediately preceding step S16 until the next step S16 is performed. The later speed is input to the position calculation unit 7g every moment.

When the above-mentioned sample has the above-mentioned correction gain Gv, the speed correction unit 7c multiplies the speed detected by the speed detection unit 3a by the speed correction data (corresponding to the correction gain Gv) obtained in step S16. Corrects the speed.

<Angular velocity correction processing>
Step S21 and step S22 are performed in parallel. In step S21, the orientation change acquisition unit 7d obtains the orientation change of the mobile body 20 based on the radio waves from the positioning satellite. In step S22, the angular velocity of the moving body 20 is obtained by the angular velocity detection unit 3b of the internal sensor 3.

In step S23, it is determined whether or not the above-mentioned operating conditions are satisfied when step S21 and step S22 are performed. In the angular velocity correction process, the above-mentioned sample acquisition time regarding the operating conditions means the time period during which steps S21 and S22 are performed.

In step S23, if the operating conditions are satisfied, the process proceeds to step S24, and if not, steps S21 and S22 are performed again. Note that step S23 and step S13 are not performed separately, and the above-mentioned step S13 may also serve as step S23.

In step S24, the angular velocity correction data calculation unit 7e obtains a sample of the angular velocity correction data based on the change in the orientation detected in step S21 and the angular velocity obtained in step S22. In one example, the sample is an offset amount θ _off expressed by the following equation.

θ _off = (Δθr−Δθa) / T

Here, θ _off represents the deviation (offset) of the origin of the value measured by the angular velocity detection unit 3b. Δθr is a value obtained by integrating the angular velocity detected by the angular velocity detection unit 3b of the internal sensor 3 over time over the sample acquisition time. Δθa is the difference between the orientation of the moving body 20 obtained by the orientation change acquisition unit 7d at the start of the sample acquisition time and the orientation of the moving body 20 obtained by the orientation change acquisition unit 7d at the end of the sample acquisition time. .. T is the sample acquisition time (length of time).

In step S25, it is determined whether the set number of the above samples has been obtained by the angular velocity correction data calculation unit 7e. When a set number of samples (a number of 2 or more) is obtained, the process proceeds to step S26, and if not, the process returns to steps S21 and S22, and the above processing is repeated.

In step S26, the angular velocity correction data calculation unit 7e obtains the angular velocity correction data based on the set number of samples. In the present embodiment, in step S26, the average value of the set number of samples is obtained as the angular velocity correction data. At this time, the angular velocity correction data may be obtained by using the above-mentioned robust averaging method.

When step S26 is performed, the process proceeds to step S27, and steps S21 and S22 described above are performed again. That is, the process of steps S21 to S26 described above is repeated while performing step S27 as described later.

In step S27, the speed correction unit 7c corrects the angular velocity detected by the angular velocity detection unit 3b of the internal sensor 3 based on the angular velocity correction data obtained in step S26. The corrected angular velocity is input to the position calculation unit 7g. Regarding this, the angular velocity correction unit 7f corrects and corrects the angular velocity detected moment by moment by the angular velocity detection unit 3b based on the angular velocity correction data obtained in the immediately preceding step S26 until the next step S26 is performed. The later angular velocity is input to the position calculation unit 7g every moment.

When the angular velocity correction data is the above-mentioned offset amount θ _off , the angular velocity correction unit 7f corrects the angular velocity by adding the _{offset amount θ off} obtained in step S26 to the angular velocity detected by the angular velocity detection unit 3b. do.

In step S28, the first current position of the moving body 20 is calculated momentarily based on the corrected speed input momentarily in step S17 and the corrected angular velocity input momentarily in step S27. do. The position calculation unit 7g inputs the calculated first current position as the first current position of the moving body 20 to the map generation unit 9 (position update unit 9c) every moment.

(Effect of the second embodiment)
In the second embodiment, in addition to the effects of the first embodiment, the following effects can be obtained. The following is an example of the effect of the second embodiment, and does not limit the present technology.

In the second embodiment, the speed correction data is obtained based on the speed detected by the speed detection unit 3a of the internal sensor 3 and the speed obtained by the speed acquisition unit 7a based on the radio wave from the positioning satellite. Based on this speed correction data, the speed detected by the speed detection unit 3a of the internal sensor 3 is corrected, and the first current position is calculated based on the corrected speed. Therefore, it is possible to suppress the influence of the detection error by the speed detection unit 3a on the first current position. For example, when the speed detection unit 3a obtains the speed of the moving body 20 based on the outer diameter of the wheel 1 and the rotation speed of the wheel 1, the outer diameter of the wheel 1 changes due to the decrease in the tire pressure of the wheel 1. A detection error occurs by the speed detection unit 3a. The influence of this error on the first current position can be suppressed by the above-mentioned correction.

Similarly, the influence of the detection error by the angular velocity detection unit 3b on the first current position can be suppressed by the correction by the above-mentioned angular velocity correction data.

As described above, since a plurality of speed correction data samples are obtained and the speed correction data is obtained from these plurality of samples, highly reliable speed correction data can be obtained.

Each sample was obtained when the above-mentioned operation conditions indicating that the operation of the moving body 20 was stable were satisfied. As a result, more reliable speed correction data can be obtained.
The same applies to the angular velocity correction data.

The present technology is not limited to the above-described embodiment, and it goes without saying that various changes can be made within the scope of the technical idea of the present technology.

1 Wheel, 2 Ground surface, 3 Internal sensor, 3a Speed detector, 3b Angle velocity detector, 5 Obstacle sensor (laser radar), 7 First detector, 7a Speed acquisition unit, 7b Speed correction data calculation unit, 7c Speed correction unit, 7d direction change acquisition unit, 7e angular velocity correction data calculation unit, 7f angular velocity correction unit, 7g position calculation unit, 9 map generation unit, 9a map update unit, 9b map storage unit, 9c position update unit, 9d target point Setting unit, 10 map generator, 11 route generator, 12 radio wave receiver, 13 second detector, 15 relative position detector, 17 storage device, 19 input device, 20 mobile unit, 21 control unit, 30 control device, Mr Relative Accuracy Priority Coordinate System Map, Ma Absolute Accuracy Priority Coordinate System Map, Sr 1st Coordinate System (Relative Accuracy Priority Coordinate System), Sa 2nd Coordinate System (Absolute Accuracy Priority Coordinate System), Sm Moving Body Coordinate System

Claims (11)

A device that generates a map used to control the movement of a moving object toward a target point.
An internal sensor that detects the operating state of the moving body and
An obstacle sensor that detects the position of an obstacle from the moving body,
A first detection device that obtains the first current position of the moving body based on the operating state, and
A map generation unit that generates a map showing a relative position between the first current position and the position of the obstacle.
A second detection device that obtains the second current position of the moving object based on radio waves from the positioning satellite, and
A relative position detecting unit for obtaining a relative position between the second current position and a given target point is provided.
The map generation unit is a map generation device that sets the target point on the map based on the obtained relative position. The map generator
From the position of the obstacle detected by the obstacle sensor and the first current position obtained at the time of the detection, the relative position between the position of the obstacle and the first current position is acquired, and the relative position and the relative position are obtained. Based on the first current position, the position of the obstacle is set on the map,
The map generation device according to claim 1, wherein the target point is set on the map based on the relative position obtained by the relative position detection unit and the first current position. The relative position detection unit obtains the relative position at a set frequency, and obtains the relative position.
The map generation device according to claim 1 or 2, wherein when the relative position is obtained, the map generation unit sets the target point on the map based on the relative position. The internal sensor includes a speed detection unit that detects the speed of the moving body, and the speed is included in the operating state.
The first detection device is
A speed acquisition unit that obtains the speed of the moving object based on the radio waves from the positioning satellite, and
A speed correction data calculation unit that obtains speed correction data based on the speed of the moving body detected by the speed detection unit and the speed obtained by the speed acquisition unit.
Based on the speed correction data, a speed correction unit that corrects the speed detected by the speed detection unit of the internal sensor, and a speed correction unit.
The map generator according to claim 1, 2 or 3, further comprising a position calculation unit that calculates the first current position of the moving body based on the corrected speed. The internal sensor includes an angular velocity detection unit that detects the angular velocity of the moving body, and the angular velocity is included in the operating state.
The first detection device is
A direction change acquisition unit that obtains a change in the direction of the moving body based on radio waves from the positioning satellite, and a direction change acquisition unit.
An angular velocity correction data calculation unit that obtains angular velocity correction data based on the angular velocity of the moving body detected by the angular velocity detection unit and the change in orientation obtained by the orientation change acquisition unit.
An angular velocity correction unit that corrects the angular velocity detected by the angular velocity detection unit of the internal sensor based on the angular velocity correction data.
The map generator according to claim 1, 2 or 3, further comprising a position calculation unit that calculates the first current position of the moving body based on the corrected angular velocity. When the condition regarding the operation of the moving body is satisfied, the condition is satisfied.
(A) When the conditions are satisfied, the speed detection unit detects the speed of the moving body, and the speed acquisition unit obtains the speed of the moving body.
(B) Based on the speed of the moving body detected by the speed detection unit in (A) and the speed obtained by the speed acquisition unit in (A), a sample of the speed correction data is obtained.
(C) When the conditions are satisfied, which are different from each other, the speed correction data calculation unit obtains a plurality of the samples by performing the above (A) and (B) a plurality of times.
(D) The map generation device according to claim 4, wherein the speed correction data calculation unit obtains the speed correction data based on the plurality of samples. When the condition regarding the operation of the moving body is satisfied, the condition is satisfied.
(A) When the conditions are satisfied, the angular velocity detection unit detects the angular velocity of the moving body, and the orientation change acquisition unit obtains a change in the orientation of the moving body.
(B) A sample of the angular velocity correction data based on the angular velocity of the moving body detected by the angular velocity detection unit in (A) and the change in orientation obtained by the orientation change acquisition unit in (A). Seeking,
(C) When the conditions are satisfied, which are different from each other, the angular velocity correction data calculation unit obtains a plurality of the samples by performing the above (A) and (B) a plurality of times.
(D) The map generation device according to claim 5, wherein the angular velocity correction data calculation unit obtains the angular velocity correction data based on the plurality of samples. The above conditions are
The amount of change in the speed of the moving body is equal to or less than the predetermined reference value for speed change.
The amount of change in the orientation of the moving body is less than or equal to the predetermined reference value for orientation change, and
The map generator according to claim 6 or 7, wherein the speed of the moving body is equal to or higher than a predetermined reference value for speed. The first detection device obtains the first current position P1r in the first coordinate system Sr representing the map based on the operating state.
The relative position detection unit
(A) In the second coordinate system Sa fixed to the ground surface, the relative position ΔPa between the second current position P2a and the target point Pta is obtained.
(B) The relative position ΔPa is converted into the relative position ΔPm in the moving body coordinate system Sm fixed to the current moving body.
(C) The relative position ΔPm is converted into the relative position ΔPr in the first coordinate system Sr.
The map generator
(D) The map generation device according to any one of claims 1 to 8, wherein the target point Ptr is set in the map in the first coordinate system Sr based on the relative position ΔPr. The map generator according to any one of claims 1 to 9 outputs the map or a local map obtained by extracting the local range including the first current position and the target point from the map.
A route generation unit that avoids obstacles and generates a movement route toward the target point based on the output map or the local map.
A control device including a control unit that controls the movement of the moving body based on the movement path. A method of generating a map used to control the movement of a moving object toward a target point.
(A) The operating state of the moving body is detected by an internal sensor provided on the moving body.
(B) The position of the obstacle is detected from the moving body,
(C) Obtaining the first current position of the moving body based on the operating state,
(D) A map showing the relative position between the first current position and the position of the obstacle is generated.
(E) Obtain the second current position of the moving object based on the radio waves from the positioning satellite.
(F) Find the relative position between the second current position and the given target point.
(G) A map generation method in which the target point is set on the map based on the obtained relative position.

Images