JP2006004412A - Moving object - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a moving object capable of inexpensively and precisely moving along a set predeterminate guide route in a closed activity area. <P>SOLUTION: A moving object is capable of automatically moving in a prescribed closed activity area and is provided with a map information storage part 12 having map information of the activity area stored therein, a guide route setting part 13 for setting a predeterminate guide route of the moving object 1 correspondingly to the map information stored in the map information storage part 12, and a control part 17 for guiding and controlling the moving object 1 on the basis of the predeterminate guide route set by the guide route setting part 13. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a mobile body that automatically travels according to a predetermined guidance route, and particularly to a mobile body that automatically travels according to a guidance route in a closed activity area set in accordance with map information.

  In recent years, unmanned vehicles and mobile robots have been actively developed due to soaring labor costs and labor shortages. However, for practical use, it is required to move the activity area with high accuracy and speed and cost. Is not required to be high.

  However, since the teaching / playback method is generally adopted as a route guidance technique for these mobile objects, it cannot be used for applications that require high-precision and speedy guidance. For example, speedy and high-precision movement is required for precision agriculture and lawn mowing for golf courses for outdoor use and for transporting luggage in a warehouse for indoor use. The fact is that the means to satisfy these requirements cannot be realized due to cost issues.

  For example, taking an automatic lawn mower at a golf course as an example, in more detail, as a conventional guidance method, an operator actually moves a machine in advance in a planned guidance route in an activity area such as a green, and the route is determined. A so-called teaching / playback system is generally used in which teaching (teaching) is performed and movement is performed based on the teaching.

  However, in this method, the position coordinates are obtained using means such as GPS every time a certain time or a certain distance moves, so the route is stored as a point sequence, and this point sequence is traced during actual driving. Since playback is performed, the following errors are generated.

(1) When the path is a curved line such as a green edge, an error occurs in the arc portion when the point sequence is moved linearly.
(2) Teaching errors occur due to variations in human operations.
(3) When the moving body changes its azimuth angle by a curve or the like, it takes time to calculate the amount of change after receiving a direction change instruction, and the control circuit issues a command to the drive unit to actually start the moving body. Causes an error.
(4) The position accuracy indicated by GPS has an error of several centimeters even with RTK-GPS, which is said to have the highest accuracy at present, so in applications where high accuracy is required, such as green edges, teaching using GPS is It is inappropriate. In other words, there is an error due to GPS in teaching, and there is a possibility that errors may overlap and the error may be doubled by using GPS even during actual driving.

  The present invention has been made in order to solve the above-described problems, and provides a moving body that can move along an established guided route in an active area that is inexpensive, highly accurate, and speedily closed. The purpose is to provide.

  In order to solve the above-described problems, the present invention is a moving body that automatically moves in a closed predetermined active area, and stores the map information of the active area and the map storage means. Guidance route setting means for setting a guidance route for the moving body corresponding to the map information, and control means for guiding and controlling the mobile body based on the guidance route set by the guidance route setting means. It becomes.

  Here, the guidance route setting means divides the planned guidance route into a plurality of sections, and expresses each section by a mathematical expression using parameters, and the control means uses the mathematical expression set by the guidance route setting means. Based on this, the moving body is guided and controlled for each section. According to this configuration, a continuous planned route using a mathematical formula can be obtained, and it is easier to control compared to a discrete route as in the case of using GPS as a reference, and the accuracy can be improved. In the embodiment, the control means includes a control unit 17, an angular velocity sensor 14, and a rotary encoder 15.

  The map information is three-dimensional map information including information on a plane direction and a height direction, and the control means is based on the planned guidance route set by the guidance route setting means and the map information. The control means acquires supplementary information used for guidance control of the mobile object, and performs guidance control of the mobile object based on the supplementary information. According to this configuration, for example, the state of the planned route can be known before traveling, and it is possible to prepare for handling such as drive control, thereby improving responsiveness.

  Further, an error for calculating an error between the sensor for detecting the position of the moving body, the planned guidance route set by the guidance route setting means, and the actual guidance route obtained based on the detection signal of the sensor. A calculating means and a correcting means for correcting the planned guidance route based on the error calculated by the error calculating means can be provided. In the embodiment, the sensor is constituted by a position detector (three-point marker detector), but a GPS such as RTK-GPS may be used.

  The correction means detects an error based on the detection signal of the sensor each time the moving body travels through each section of the planned guidance route that is divided into a plurality of sections, and based on the error, from the mathematical expression in the next section The resulting parameters can be corrected. According to this configuration, it is possible to prevent the error between the planned guidance route based on the travel control and the actual travel route that actually travels from increasing, and it is possible to perform inexpensive and extremely high-accuracy travel control.

  Further, the apparatus includes azimuth detecting means (for example, a geomagnetic sensor) for detecting the azimuth of the moving body, and the error calculating means is a lateral deviation amount, an azimuth error, and a travel distance between the planned guidance route and the actual guidance route at every predetermined timing. Each of the errors may be calculated, and the correction unit may correct the planned guidance route based on the lateral deviation amount, the azimuth error, and the travel distance error.

  In this case, the correction means corrects the distance error Xe occurring in the direction of the planned route as the travel distance error by shifting the position of the subsequent planned route by the error Xe with respect to the planned route. be able to.

In addition, the correction means, for the direction error Θe and the lateral deviation amount Ye, is Vd, com which is the difference between the speeds commanded to the left and right wheels, Vr is the specified speed of the moving body, and the specified angular speed when traveling on the planned route at the specified speed Is ωr, the difference (ωr−ωa) between the specified angular velocity ωr and the actual traveling angular velocity ωa is ωe, the wheel width is W, and KΘ, Ky, Kω are gain coefficients for correcting each error,
Vd, com / W = ωr + KΘ · Θe + Ky · Ye / Vr + Kω · ωe
It can correct | amend so that a control means may perform drive control of the wheel of a mobile body so that it may satisfy | fill. Note that the correction timing and the gain coefficient can be changed and set as appropriate depending on the usage environment of the moving object.

  The moving body may be provided with a camera that captures an image of a predetermined area around the moving body, and the control unit may perform guidance control of the moving body based on an image captured by the camera.

  As described in detail above, according to the present invention, it is possible to provide a moving body that can move along an established guided route in an active area that is inexpensive, highly accurate, and speedily closed. .

Embodiments of the present invention will be described below with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a moving body in an embodiment of the present invention.
The mobile body 1 includes an operation unit 11 for inputting a planned guidance route that is a route on which the mobile body 1 is intended to travel, a map information storage unit 12 based on three-dimensional digital parameters, and a map of the planned guidance route. A guidance route setting unit 13 that is set according to information, an angular velocity sensor 14 that detects an angular velocity, a rotary encoder 15 that is a distance sensor that detects a moving distance, a position detector 16, a planned guidance route, and a detected angular velocity. A control unit 17 that corrects a future (previous) guidance route of the mobile body by the correction unit 17a based on the detection position of the position detector 16 while guiding the mobile body based on the detection distance, and the control unit 17 And a drive unit 18 for driving the moving body in response to the control signal from.

  The control unit 17 performs rotation drive control and course direction change control (direction change by steering control of the steering wheel or direction change by rotation speed change control of the left and right drive wheels), not shown, and the drive unit 18 drives or steers the wheel.

  As the position detector 16, for example, a three-point marker detector is employed. The three-point marker detector, which will be described later with reference to FIG. 2, is provided with three marks that can be optically detected at a predetermined location in the vicinity of the planned guidance route, and a reference point is set by these marks, and these marks are optically detected. By detecting automatically, the relative position with respect to the reference point is detected. Of course, an absolute position detector such as RTK-GPS that can be expressed by latitude and longitude may be adopted as the reference point for such relative position detection.

  FIG. 2 is a diagram illustrating an example of a planned guidance route set in the guidance route setting unit. In FIG. 2, the starting point S (x0, y0), the azimuth angle θ, the straight line length Sn, the arc radius Rn, and the arc length Cn are indicated. The planned guidance route shown in FIG. 2 has a loop shape and is divided into a plurality of arc portions C and straight portions S.

  Here, the guidance route shown in FIG. 2 corresponds to the position detection principle by the position detector (three-point marker detector) 16 described above at three points (for example, Q1, Q2, etc.) in a predetermined location near the active region. Q3) is provided and a coordinate system QR (xR, yR) is used with a predetermined point derived from these markers (for example, the intersection of X and Y axes shown in the figure) as a reference point. Of course, it can also be set by an absolute coordinate system such as latitude and longitude. In this case, it is easy to cope with a detection signal such as RTK-GPS. However, if a reference point is provided by providing a marker in the vicinity of the active area and a coordinate system is configured as in the present embodiment, for example, the position detector (three-point marker detector) 16 in the coordinate system. Position detection can be performed with extremely high accuracy.

  The control unit 17 expresses the route for each partial section divided in this way by a mathematical expression, and generates a control signal based on the detection signals of the angular velocity sensor 14 and the rotary encoder 15 so that the traveling route of the moving body follows the mathematical expression. Form and give to drive. Here, for convenience of explanation, for example, a case where a part of the route shown in FIG.

In the path starting from the point S (x0, y0) on the coordinate form in FIG. 3, the mathematical formulas set by straight lines and arcs are as follows.
1) The equation for obtaining the coordinates (x, y) of the point where the moving body has moved S1 on the straight line between S and P1 is expressed as the following equation.

y = (y1-y0) / (x1-x0) * x + (y1-y0) / (x1-x0) * x0 (1)
(X−x0) ² + (y−y0) ² = S1 ² (2)

  The coordinates (x, y) of the point moved by the arbitrary distance S1 can be obtained from the expressions (1) and (2), and the control unit 17 follows the coordinates satisfying such an expression in the linear section. Control the drive. In this case, with respect to the course direction control, the steering wheel or the wheel angle is fixed so that the angular velocity becomes zero in the movement of the straight section.

2) The equation for obtaining the coordinates (x, y) of the point where the moving body has moved by C1 from P1 on the arc between P1 and P2 is expressed as the following equation.

(X1−xr1) ² + (y−yr1) ² = R1 ² (3)
(X−x1) 2+ (y−y1) 2 = (2R1sin (360 / (4πR1) × C1) (4)

  The coordinates of the moving distance C1 point are obtained from the above equations (3) and (4), and the control unit 17 controls the drive unit so as to follow the coordinates satisfying such a mathematical expression in the arc section. In this case, the steering angle is fixed so that the steering wheel is turned at a certain angle so that the angular velocity becomes V / R. Here, V is a moving speed, and R is a radius (curvature radius).

  In this way, the control unit 17 outputs a drive signal to the drive unit 18 so as to follow the route set in the guidance route setting unit 13, but in reality, the moving body gradually guides the guidance route due to various error factors. It will shift from. For example, the direction error is caused by the zero point shift or the angle error of the angular velocity sensor, and is shown in FIG. The distance error is caused by wheel slip or the like, and is shown in FIG. Therefore, in this embodiment, the direction error and the distance error are corrected every predetermined section.

Hereinafter, an example of such a correction method will be described.
In FIG. 4C, the first correction point set for the planned guidance route is A ″, the actual position reached (the point on the locus detected by the position detector 16) is the point A, and the position of the point A Is positioned in the reference coordinate system. As described above, positioning to the reference coordinate system is performed when the guidance route is set with respect to a coordinate system provided with a reference point (arbitrary, for example, three markers) in the vicinity of the active area. This is done by detecting a point marker. In the case where the coordinates are expressed in an absolute coordinate system such as latitude and longitude, the position is determined by RTK-GPS. Next, in the coordinate system, the origin S is a fulcrum, and the intersection with the planned route when the locus SA is shifted by θ degrees is A ′.

  If the position error is set to 5 mm or less, the point of A ″ is determined so that the section AA ′ ≦ 5 mm. Thereafter, assuming that the pitches are corrected to an equal pitch, the sections SA ″ = section A ″ B ″ = section B ″ C ″..., B ″, C ″ are set as correction points.

  When the moving body reaches the position A corresponding to the correction point A ″, the deviation from the correction point A ″ in the actual guidance route of the position A is determined based on the locus obtained by the position detector 16 as the guidance guidance route. By comparison (rotation shift until coincidence), the angle difference θ can be obtained. Therefore, the moving body corrects the direction in this setting in the next section (A ″ B ″) of the planned guidance route so as to eliminate the angle difference θ in order to correct the azimuth (route direction).

  Further, if the point at which the point A intersects the planned guidance route due to the shift of the locus (actual travel route) is A ′, the subsequent planned guidance route of the moving object from the point A (stored in the computer in the moving object) by the shift. The planned route on which the moving body moves after the point A should be taken from the point A ′ on the planned guidance route along with the angle correction. The next section of the set guidance route is corrected from the section A ″ B ″ to the section A′B ″. In other words, the correction of the distance is performed by moving the A ″ point of the initial next section A ″ B ″ backward by a distance error (a distance error corresponding to the planned guidance route) A′A ″. Will be done.

  That is, the actual distance deviation AA ″ is corrected simultaneously with the direction correction, and the starting point of the next scheduled route is reset to A ′ in order to follow the next scheduled route. Further, the set value of the next route section is corrected in order to return to the planned route starting from A ′. That is, the next route section is the section A ″ B ″, but this section is set as the section A ″ B ″, and the parameters of the section are changed according to mathematical formulas.

  For example, when the section A′B ″ is an arc having a radius R, the control unit outputs a control signal to the drive unit so that the moving body is driven at the angular velocity V / R + θ / t. Here, V is the moving speed, and t is the moving time of the section A′B ″. Further, when the section A′B ″ is a straight line, the steering wheel is steered so as to be θ / t.

The operation of the present embodiment will be described collectively with reference to FIG.
First, a guidance route is input (step S1). Next, this guidance route is divided into a plurality of sections as shown in FIG. 2, and each section is displayed as a mathematical expression (step S2). And a control part performs drive control based on the numerical formula in a 1st area (after that nth area) from a starting point (step S3, S4). Next, after the first section movement (n-th section movement), the movement distance and the direction error are calculated based on the detection signal from the sensor (position detector 16) (step S6), and the second section is calculated based on the calculation result. (Section n + 1) is corrected (steps S7 and S8), and drive control is performed according to the correction value (step S4). This process is performed until the end of the last section, and the guidance control is finished (step S5).

Embodiment 2. FIG.
The control unit guides the operation information incidental to the movement of the moving body, such as when there is an uphill or downhill ahead of the moving direction of the moving body, when entering a curve from a straight line, or when entering a straight line from a curve. The information can be acquired in advance based on the scheduled route, and the control signal can be output to the drive unit so as to smoothly enter the accompanying operation.

  In this case, for example, the time until the next operation is performed is calculated from the current speed and the remaining distance, the control signal is output before the time determined by the type of information and the response speed, and the next operation is smoothly performed. To be able to do so.

  For example, if there is an uphill, the engine output corresponding to the current speed obtained from the map information, the length of the ascending slope, the length of the engine, etc. are stored in a table, and the control unit Based on the incidental information, a parameter is extracted from this table immediately before the uphill slope and a drive signal is output.

  Similarly, when entering a curve from a straight line, or conversely entering a straight line from a curve, the control signal for the turning angle of the handle is extracted from the table based on the incidental information of the map information and output immediately before entering the operation. .

  In addition, when the moving body to be described later is a lawn mower, parameters related to the raising / lowering of the cutter with respect to the boundary of the turf area or the angle adjustment parameters are extracted from the table similarly prepared together with the above parameters, and the control signal thereof is the operation position. Output immediately before.

  According to such a configuration, even when the response speed is low, the operation can be smoothly performed at the operation position, and the functional accuracy of the moving body can be improved.

Embodiment 3 FIG.
Next, the case where the present invention is applied to an automatic lawn mower will be described with reference to FIGS.

(1 course setting)
Prior to work, the green keeper determines a work route (virtual trajectory) in advance. The setting of the work route will be described in detail below, but the green keeper determines the work procedure from the relation of the lawn and prepares several work patterns. Select one of them. The work procedure after selection is as follows.

(2 guide route setting)
First, as shown in FIG. 6A, the green edge that requires high accuracy is trimmed from the starting point S1 (previously set as an accurate reference position). The most important point to obtain high accuracy is the initial alignment (position and orientation). Therefore, the manager places the lawn mower in the correct position and orientation at the starting point.

(2-1 Initial alignment method)
(1) The position is marked in the vicinity of the actual green edge corresponding to the S point determined on the digital map. Alternatively, a reference point previously measured by precision surveying or the like is set on the green side. A GPS antenna is placed at this reference point to serve as a reference station. A mobile station GPS antenna is placed at a point that has a predetermined positional relationship with the reference station, and is initialized. The mobile station GPS antenna that has been initialized is mounted on the moving body, and the positioning is performed by driving the moving body so that the position coordinates indicated by the GPS coincide with the start position coordinates.

  (2) The direction may be determined by a setting jig in the vicinity of the green edge in the same manner as the alignment. The other method is to display the planned guidance route (scheduled route expressed by a mathematical expression) in relation to the reference part of the machine such as a cutter edge (for example, an on-board LCD) and show the actual green edge. Set to match the line display. As will be described later (Embodiment 7), when a geomagnetic sensor is used, it may be aligned with the direction of the planned route by the geomagnetic sensor.

(3 work)
Next, the lawn mower starts work along the planned route.
When moving, calculate the direction and distance with the angular velocity sensor and rotary encoder, and stop after traveling the specified distance. The position correction is performed by the method described in the first embodiment.
After the stop, the cutter is lifted, and once out of the green, moves along the virtual trajectory to the next starting point S2. The movement to S2 is guided according to the planned virtual trajectory.

  Next, the pattern as shown in FIG. 6B is trimmed from the point S2. At point S2, the cutter is lowered according to the instruction of the parameter data. As shown in FIG. 6B, the cutter advances and reaches the vicinity of the green edge.

  Outside the green, it rotates 180 degrees according to the set virtual trajectory, enters the green again, lowers the cutter, and starts cutting. The above operation is repeated, and the operation of FIG. During this time, the position and direction are corrected in the same manner as described above.

  In addition, the movement of the machine is stopped for a certain period of time when the cutter is moved up and down, that is, when the green edge enters and leaves, and the accuracy can be improved by correcting the zero (zero) point of the direction sensor.

  The setting operation for performing the work operation shown in FIG. 6B will be described with reference to the flowchart of FIG. 7. First, the starting point coordinates are input (step D1). That is, the starting point S2 is set according to the hunting pattern designated by the green keeper on the green digital map coordinates created in advance.

  Next, the green edge intrusion position coordinates are input (D2) and the green edge intrusion position coordinates are input (D3). That is, in order for the lawn mower to start trimming, the coordinates of the point A at the green edge entry point and the point B that exits the green edge are input.

  Next, a mathematical expression of the travel route (scheduled guidance route) is generated (D4). That is, a linear expression connecting point A and point B is calculated and stored. Based on this linear expression generation, the cutter is lowered to an arbitrary distance LA from point A and the position of LB before point B, and an instruction to ascend is input as incidental information.

  Next, the direction change start point / end point coordinates are input (D5), and the optimum turning radius (R) is calculated (D6). That is, the cutter is lifted at the position LB before point B, proceeds straight on the road, goes out of the green, inputs the start point and end point of 180 degree direction rotation, and simultaneously calculates the turning radius (R). .

  Next, the re-entry point in the green is obtained as an intersection with the green edge line by inputting a parallel movement condition of L-1 (L: cutter width, l: lapping) with respect to the straight line AB. When the re-entry point is determined, a virtual trajectory outside the green of the lawn mower is set with a mathematical formula of straight lines and arcs at a predetermined distance from the green edge (D7) and stored. The above operation is repeated to input goal coordinates and the process is terminated (D8).

(End of Work 4) After the work of FIG. 6B is finished, the process moves to S3 shown in FIG. This completes the work for one green and either waits for the manager to arrive or automatically moves to the next green.

Embodiment 4 FIG.
In the embodiment described above, the case where the vehicle can travel while correcting according to the planned guidance route has been described. However, depending on the case, the operator may instruct to change the course as appropriate to support the correction, or the position and direction at the start point may be adjusted. It is desirable that the operator can reliably perform the above.

  In the fourth embodiment, a case where a remote instruction can be given to a moving body assuming such a case will be described. FIG. 8 is a block diagram showing the configuration inside the moving body 1A and the monitor room 30 in the fourth embodiment. In the fourth embodiment, it is assumed that the structure shown in FIG. 8 is added to the structure shown in FIG.

  A moving body 1A shown in FIG. 8 is further provided at an appropriate position of the moving body, and can capture a predetermined range in the traveling direction, and a video signal processing unit 22 that processes a video signal captured by the camera 21. The transmission / reception unit 23 transmits the processed video signal and receives the remote instruction signal, and the control unit 17A controls the drive unit 18 based on the remote instruction signal received by the transmission / reception unit 23.

  On the other hand, in the monitor room 30, a transmission / reception unit 31 that performs transmission / reception with the mobile unit 1A, a monitor device 32 that displays a video signal transmitted from the mobile unit 1A, and an operation unit 33 that an operator issues an instruction to the mobile unit 1A, It has.

  In the above configuration, when the moving body 1A is at the starting point, the surrounding image is transmitted and displayed on the monitor device 32. The operator operates the moving body 1 </ b> A so that the position and direction of the moving body 1 </ b> A are adjusted from the image via the operation unit 33. In this case, for example, if the monitor device 32 side prepares another video when the position and the direction are correctly aligned, and the operator issues an instruction so that the overlap of the video matches, the position can be more accurately determined. And it becomes possible to perform direction alignment.

  Further, when the moving body 1A is traveling, the operator 33 can be operated so that the error converges more quickly while the operator monitors the video signal. Of course, in this case as well, the control unit 17A performs the correction operation shown in the first embodiment, but the traveling accuracy of the moving body 1A can be further improved by adding the operator's operation thereto. Furthermore, in this case, if the video signal processing unit 22 displays the course on which the predetermined part of the moving body is going to pass on the video, the operation of the operator becomes easier.

Embodiment 5. FIG.
In the fifth embodiment, a case will be described in which a configuration that enables automatic traveling is further added by performing pattern matching processing as necessary.
FIG. 9 is a block diagram showing a configuration added in the fifth embodiment.
In Embodiment 5, it has the reference pattern memory | storage part 24 and the pattern matching process part 25, a matching process is performed by comparing the pattern imaged with the camera 21, and a reference pattern, and based on the result, a control part 17B outputs to the drive unit 18. In this pattern comparison, the rotation of the pattern accompanying the azimuth change of the moving body 1B includes an azimuth detector 14A that detects the azimuth based on the detection signal of the angular velocity sensor 14 (see FIG. 1), and rotates using this azimuth. The pattern matching process is performed after correction.

  The reference pattern storage unit 24 stores video patterns acquired in advance along the planned route for guidance. These patterns are sequentially read out and used for matching processing as the mobile body 1B progresses. According to such a configuration, movement control with extremely high accuracy can be performed. Therefore, for example, in the case of a lawn mower, it is effective to apply it to lawn mowing around a green edge.

Embodiment 6 FIG.
In the fifth embodiment, the reference pattern based on the route is used in advance. However, the traveling direction and the travel distance of the moving body can be accurately detected using the pattern matching process without using the reference pattern.

  FIG. 10 is a block diagram showing a configuration in the sixth embodiment. This configuration includes a video storage unit (previous step video storage unit) 26 that temporarily stores the video of the camera, and stores the raw video (latest video) output from the camera 21 and the previous step video storage unit 26. The pattern matching processing unit 25A performs matching with the stored video of the previous step (for example, one or several frames before), so that the moving body 1C moves in which direction and how much distance between the two frames. This is to detect this. By repeating this detection process, the path can be detected following the movement of the moving body.

  In particular, when the moving body 1C is a lawn mower, the lawn is considered to be almost a random pattern. Therefore, a part of the image captured before one step (for example, a plurality of (for example, 1024) pixel data constituting the central portion of the image) It is possible to detect the moving distance and the direction very accurately by detecting where the image moved after one step by the matching process. The correction can be made based on the detection result by the direction detection unit.

Embodiment 7 FIG.
In the seventh embodiment, when a difference occurs between the actual route and the planned guidance route, the correction is performed by a method different from that in the first embodiment.

FIG. 11 is a block diagram showing a moving body in the seventh embodiment of the present invention.
In addition to the configuration of the first embodiment, the moving body 1D can further include a geomagnetic sensor 41 that detects the absolute direction of the moving body 1D.

  In the present embodiment, the control unit 17D detects the detection angle and the detection distance when the mobile body 1D travels based on the detection angle by the angular velocity sensor 14 and the detection distance by the rotary encoder 15, the detection position by the position detector 16, and Based on the detection direction of the geomagnetic sensor 41, the moving unit 1D is controlled to move while correcting the planned guidance route by the correction unit 17a-1 so that the moving unit 1D follows the planned guidance route.

  In FIG. 11, the same reference numerals as those in FIG. 1 denote the same or equivalent parts as described in FIG. 1, and the description thereof is omitted here.

  The control unit 17D expresses the route for each of the partial sections divided as described in the first embodiment, and forms a control signal so that the traveling route of the moving body follows the equation to generate the drive unit 18. To give.

  That is, in the seventh embodiment, the control unit 17D can obtain the coordinates (x, y) of the point moved by the arbitrary distance S1 from the expressions (1) and (2) described in the first embodiment, and the control unit 17D. In the straight section, the drive unit 18 is controlled so as to follow coordinates satisfying such a mathematical expression. In this case, with respect to the course direction control, in the movement of the straight section, the handle is fixed or the left and right wheel rotational speeds are made the same.

  Further, the coordinates of the point of the moving distance C1 are obtained from the above-described equations (3) and (4) in the first embodiment, and the control unit 17D is driven so as to follow the coordinates satisfying these equations in the arc section. The unit 18 is controlled.

  At the time of this driving, the control unit 17D instructs the driving unit so that the rotation speeds of the left and right wheels are constant and both wheels are set to the specified speed in the linear part. In the arc portion, the drive unit is instructed to have a predetermined angular velocity determined from the rotation radius and the moving body speed (the average value of the rotation speeds of the wheels).

  That is, the control unit 17D causes the guided route to travel (autonomous traveling) based on the parameters and the outputs of the angular velocity sensor 14 and the rotary encoder 15 so that the moving body 1D follows the route set in the planned guided route setting unit 13. . However, in actuality, as shown in FIG. 12, the planned route deviates due to various error factors such as an initial azimuth setting error and a left / right wheel rotation difference. Therefore, in the seventh embodiment, each of the detection position of the position detector 16 and the absolute direction of the geomagnetic sensor 41 is sampled at predetermined time intervals (for example, every 20 ms), and an error from the planned guidance route is detected. Correct as follows.

Hereinafter, the operation of the present embodiment will be described with reference to FIG.
About the distance error Xe (distance difference between the planned position on the planned guidance path and the intersection when the perpendicular is drawn from the moving body position to the planned guidance path) generated in the direction of the planned guidance path, the error Xe It is corrected by shifting (adding or subtracting) the position of the subsequent guidance route. For example, when the distance of the actual route is smaller than the planned guidance route, the position of the moving body 1D on the planned guidance route is returned by that distance.

  The correction of the direction error Θe and the lateral shift amount Ye can be performed by controlling the left and right wheel speeds. This correction will be described below.

If the difference in speed commanded to the left and right wheels is Vd, com,
Vd, com / W = ωr + KΘ · Θe + Ky · Ye / Vr + Kω · ωe (5)
Here, Vr is the specified speed of the moving body, and corresponds to the average speed of both the left and right wheels.
ωr represents a specified angular velocity when traveling on the planned route at a specified speed.
ωe represents the difference (ωr−ωa) between the specified angular velocity ωr and the actual traveling angular velocity ωa.
W is the wheel width.

  KΘ, Ky, and Kω are constants for correcting each error, and are gain coefficients that determine control sensitivity. If these constants are set to a large value, the correction control becomes sharp and the posture tends to zigzag by swinging left and right. Conversely, if it is set to a small value, control tends to be slow and not easily corrected.

  In the case of a lawn mower, since high accuracy is required especially for cutting the green edge, it must be corrected quickly without zigzag. Therefore, these constants are

  0 <KΘ, Ky, Kω <10

It is appropriate to set as follows.
Here, the time interval for correcting the distance error Xe and the time interval for correcting the direction error Θ and the lateral shift amount Ye may be made different from each other. For example, the actual path can be made smoother by making the time interval for correcting the distance error smaller than the time interval for correcting the direction error and the lateral deviation amount. It goes without saying that these can be appropriately changed depending on the usage state of the moving body 1D.

  Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and can be embodied by modifying the constituent elements without departing from the scope of the invention in the implementation stage. In addition, various inventions can be formed by appropriately combining a plurality of constituent elements disclosed in the above embodiments. For example, some components may be deleted from all the components shown in each embodiment. Furthermore, you may combine the component covering different embodiment suitably.

1 is a block diagram showing a first embodiment. It is a figure which shows an example of the guidance guidance path | route set to the guidance path | route setting part. It is a figure explaining the case where a part of path | route is approximated by numerical formula. It is a figure for demonstrating an error and the correction method. 3 is a flowchart showing an overall operation of the first embodiment. It is a figure explaining the operation | movement about the case where this invention is applied to an automatic lawn mower as this Embodiment 3. FIG. 10 is a flowchart for explaining the operation of the third embodiment. FIG. 10 is a block diagram illustrating configurations of a moving body and a monitor room in a fourth embodiment. FIG. 10 is a block diagram illustrating a configuration of a fifth embodiment. FIG. 10 is a block diagram illustrating a configuration of a sixth embodiment. FIG. 10 is a block diagram illustrating a configuration of a seventh embodiment. FIG. 20 is an explanatory diagram showing the operation of the seventh embodiment.

Explanation of symbols

  1, 1A, 1B, 1C, 1D moving body, 11 operation unit, 12 3D digital parameter map information storage unit, 13 guide route setting unit, 14 angular velocity sensor, 14A azimuth detection unit, 15 rotary encoder, 16 position detector, 17 , 17A, 17B, 17C control unit, 17a correction unit, 18 drive unit, 21 camera, 22 video signal processing unit, 23, 31 transmission / reception unit, 24 reference pattern storage unit, 25, 25A pattern matching processing unit, 26 previous step video Storage unit, 30 monitor room, 32 monitor device, 33 operation unit, 41 geomagnetic sensor.

Claims (9)

A moving body that automatically moves in a closed predetermined active area,
Storage means for storing map information of the active area;
Guidance route setting means for setting a planned guidance route for the mobile body corresponding to the map information stored in the storage means;
A moving body comprising: control means for guiding and controlling the moving body based on the guidance route set by the guidance route setting means.   The guidance route setting means divides the planned guidance route into a plurality of sections, and expresses each section by a mathematical expression using a parameter, and the control means is configured based on a mathematical expression set by the guidance route setting means. The mobile body according to claim 1, wherein the mobile body is guided and controlled for each section.   The map information is three-dimensional map information including information on a plane direction and a height direction, and the control means is based on the planned guidance route set by the guidance route setting means and the map information. The mobile body according to claim 1 or 2, wherein the mobile body acquires supplementary information used for guidance control of the mobile body, and performs guidance control of the mobile body based on the supplementary information. Position detecting means for detecting the position of the moving body;
An error calculation means for calculating an error between the planned guidance path set by the guidance path setting means and the actual guidance path obtained based on the detection signal of the position detection means;
The moving body according to any one of claims 1 to 3, further comprising a correction unit that corrects the scheduled route based on an error calculated by the error calculation unit.   The correction means detects an error based on the detection signal of the position detection means each time the mobile body travels through each section of the planned guidance route divided into a plurality of parts, and based on the error, the mathematical expression in the next section is calculated. The moving body according to claim 4, wherein the obtained parameter is corrected. Comprising azimuth detecting means for detecting the azimuth of the moving body,
The error calculation means calculates a lateral deviation amount, an azimuth error, and a travel distance error between the planned guidance route and the actual guidance route for each predetermined timing,
The moving body according to claim 4, wherein the correction unit corrects the planned guidance route based on the lateral deviation amount, the azimuth error, and the travel distance error.   The correction means corrects the distance error Xe generated in the direction of the planned guidance route as the travel distance error by shifting the position of the subsequent guidance route with respect to the planned guidance route by the error Xe. The moving body according to claim 6. The correction means, for the direction error Θe and the lateral shift amount Ye,
The difference between the speeds commanded to the left and right wheels is Vd, com, the specified speed of the moving body is Vr, the specified angular speed when traveling at the specified speed on the planned route is ωr, and the difference between the specified angular speed ωr and the actual traveling angular speed ωa (ωr− When ωa) is ωe, the wheel width is W, and KΘ, Ky, Kω are gain coefficients for correcting each error,
Vd, com / W = ωr + KΘ · Θe + Ky · Ye / Vr + Kω · ωe
The moving body according to claim 7, wherein correction is performed by causing the control means to perform drive control of wheels of the moving body so as to satisfy the above.   The camera according to any one of claims 1 to 5, wherein the moving body includes a camera that captures an image of a predetermined area around the moving body, and the control unit performs guidance control of the moving body based on an image captured by the camera. The moving body according to Crab.

Images