JP5618124B2 - Route search apparatus and moving system - Google Patents

Description

  The present invention relates to a technique for searching for a route from a start position to a goal position.

  Self-position estimation and map construction are important elements for autonomous movement of mobile robots. However, in the outdoor environment, the road surface condition changes due to the influence of the climate, etc., and the environment is likely to change dynamically. Therefore, there is a need for a route search method that can safely travel to the destination according to the environmental condition when moving. .

  Known autonomous movement methods according to environmental conditions include a method of determining the road surface area where the robot can travel from image information and traveling on the road surface, and a method of avoiding danger by detecting obstacles other than the road surface Yes. In addition, a method has been proposed in which an area map that can be moved safely is learned by learning an area with a large change in situation and an area with a high degree of danger by repeating environmental driving. However, there is a problem that generation of such a map requires much labor.

  Further, Non-Patent Document 1 creates a map in which a floor area swept by a person walking in an environment is regarded as a movable area by extracting obstacle and person position information from image data. A technique for performing route planning based on the created map is disclosed.

“One Method for Map Creation and Autonomous Movement Based on Pedestrian Observation by Mobile Robots” Tanaka, K., Okada, Nobuhiro, Kondo, Eiji, The Japan Society of Mechanical Engineers, Vol. 70, No. 693 (2004-5), Paper No. 03-0437

  However, according to the method of Non-Patent Document 1, the floor area swept by at least one person is regarded as a movable area regardless of the number of persons who have swept a certain floor area. It is not possible to evaluate how safe the floor area is. In other words, it is estimated that the floor area swept by many people is more likely to be safe. However, in the method of Cited Document 1, the safety of the floor area is taken into account by taking the number of swept people into consideration. Since no evaluation has been performed, there is room for further improvement in order to search for a safer route.

  An object of the present invention is to provide a route search device capable of searching for a safe and shortest route and a mobile system including the route search device.

(1) A route search device according to one aspect of the present invention includes an acquisition unit that acquires range data indicating a position to an object existing in the surroundings, and a two-dimensional coordinate space that acquires the range data acquired by the acquisition unit. To the map data generating means for generating the two-dimensional map data indicating the position of the object, and extracting the point cloud indicating each object existing in the surroundings from the map data, and out of the extracted point cloud A voting space is generated by dividing a point group satisfying a predetermined pedestrian condition as a point group indicating a pedestrian and a two-dimensional coordinate space indicated by the map data in a grid pattern, In each grid constituting the generated voting space, voting means for voting each range data constituting a point cloud indicating a pedestrian specified by the specifying means, and a grid having a higher voting result by the voting means, the safer High rate Grid showing the safety factor calculating means, the voting space where the safety factor is calculated, and set as the target grating start grid indicating a predetermined start position, the start position for calculating the safety factor of the grid so that The total cost is calculated as the sum of the value obtained by adding the costs of each grid from the grid to the adjacent grid adjacent to the target grid and the linear distance between the adjacent grid and the grid indicating the predetermined goal position. and a search unit that cost is searched minimum and ing path using a * algorithm, the cost of each grating is proportional to the linear distance between adjacent contact grid, and inversely proportional to the safety factor of the adjacent grating Have a value to

  According to this configuration, range data indicating the position of an object existing in the vicinity is acquired, the acquired range data is plotted in a two-dimensional coordinate space, and a point cloud indicating each object is extracted and extracted. A point cloud indicating a pedestrian is identified from the obtained point clouds. And, the voting space obtained by partitioning the two-dimensional coordinate space in a grid pattern is voted for the range data that constitutes the point cloud indicating the pedestrian, and the higher the voting result, the higher the safety factor Thus, the safety factor of each grid is calculated. As a result, the voting space becomes two-dimensional map data in which a higher safety factor is calculated for a grid through which many pedestrians pass. Then, using this voting space, a route is searched for such that the distance between the grid indicating the start position and the grid indicating the predetermined goal position is the shortest and the safety factor is the highest.

  Thereby, since many pedestrians pass, it is possible to search for a route that is highly safe and has the shortest distance from the start position to the goal position. Therefore, when this configuration is adopted in a moving system (robot) that automatically operates from the start position to the goal position, it is possible to avoid collision with an obstacle and to guide the moving body to the goal position along the shortest path.

  (2) It is preferable that the specifying unit adopts a moving speed and a shape of a point group as the pedestrian condition.

  According to this configuration, since the moving speed and shape of the point group are adopted as the pedestrian condition, the point group indicating the pedestrian can be accurately identified.

  (3) It is preferable that the shape of the point group in the pedestrian condition includes the number of points constituting the point group and the width of the point group.

  According to this configuration, since the number and width of the point group are adopted as the shape of the pedestrian condition, the point group can be specified with higher accuracy.

  (4) The range data is measured by irradiating a laser beam in a horizontal direction so that the height from the ground is below the pedestrian's knee, and the voting means It is preferable that the voting values of the surrounding grids are set by changing the voting values of the grids according to a predetermined two-dimensional distribution function around the grid where the range data constituting the point cloud indicating the person is voted.

  According to this configuration, voting can be performed in consideration of the size of the pedestrian's torso. Further, since the vote value is changed according to the two-dimensional distribution function, the vote value can be changed gently.

  (5) The specifying unit specifies a point group that does not satisfy the pedestrian condition as a point group indicating an obstacle, and the voting unit corrects a vote value for the range data of the point group indicating the pedestrian. It is preferable to vote with the voting value set to a negative value for the range data constituting the point cloud indicating the obstacle.

  According to this configuration, the safety factor can be calculated so that a point group indicating an obstacle and a point group indicating a pedestrian are clearly distinguished.

( 6 ) A GPS sensor and an orientation sensor are further provided, and the safety factor calculation means is based on position data indicating its current position measured by the GPS sensor and orientation data measured by the orientation sensor. The coordinate system of the voting space is converted into a global coordinate system, and the search unit sets a grid indicating the current position of the subject measured by the GPS sensor in the voting space, and sets the grid indicating the set current position. It is preferable to search for the route by setting the start position.

  According to this configuration, since the coordinate system of the voting space can be represented by the global coordinate system, the route to the goal position can be searched using the current position measured by the GPS sensor as the start position. Moreover, the goal position can be set by inputting position data (for example, latitude and longitude) in the global coordinate system. In addition, since the GPS sensor and the direction sensor are provided, it is possible to detect whether or not the current position and the traveling direction of the self are deviated from the searched route from the GPS sensor and the direction sensor.

  According to the present invention, it is possible to search for a route that is highly safe because many pedestrians pass and that has the shortest distance from the start position to the goal position.

1 shows an external configuration diagram of a mobile system according to an embodiment of the present invention. FIG. It is a block diagram which shows the electrical structure of the movement system shown in FIG. It is the block diagram which showed the functional structure of the mobile system shown in FIG. It is a flowchart which shows the operation | movement at the time of the mobile system by embodiment of this invention searching a path | route. It is the table | surface which showed the pedestrian condition. (A) is a schematic diagram which shows an example of the map data produced | generated by the map data production | generation part, (B) is a schematic diagram which shows the map data from which the point cloud which shows a pedestrian was extracted from the map data shown to (A). FIG. (A) shows the map data in which the resolution is lowered to the resolution of the voting space in the map data on which the range data is plotted, and (B) shows the voting space where the range data is voted. It is the schematic diagram which showed an example of the path | route searched by the search part. It is a flowchart which shows the process at the time of a movement system moving along the path | route searched by the search part. It shows the travel safe area map data obtained by the navigation experiment, the searched route, and the travel route of the mobile system. (A)-(D) are the figures which showed the mode of the moving system in driving | running | working.

  Hereinafter, a mobile system according to an embodiment of the present invention will be described. FIG. 1 is an external configuration diagram of a mobile system according to an embodiment of the present invention. As shown in FIG. 1, the movement system is a wheelchair type movement system, and includes a range sensor 1, a route search unit 2, a direction sensor 3, a tire unit 4, a base 5, a frame 6, an operation unit 7, and a GPS sensor. 8 is provided.

  The range sensor 1 is configured by, for example, a laser range finder, and measures range data indicating a distance and a direction to an object located around the movement system. Specifically, the range sensor 1 scans the laser beam by changing the laser beam emission direction into a fan shape on the horizontal plane that is about the height of the pedestrian below the pedestrian from the ground, and receives the reflected light from the object. By doing this, the range data up to the surrounding objects is measured. Here, the range sensor 1 scans the laser beam in a range of −90 degrees to +90 degrees and 180 degrees, for example, assuming that the clockwise direction on the horizontal plane is a positive direction. Measure area data.

  The range data includes direction data indicating the direction of the detected object when the direction in front of the range sensor 1 is used as a reference, and distance indicating the distance to the object when the range sensor 1 is used as a reference. Data and are included. Further, the irradiation angle of the laser beam is adopted as the direction data.

  In addition, since the range sensor 1 is mounted in front of the platform 5, it is located in a semicircular range having a radius of several tens of meters to several hundreds of meters (for example, a maximum of 80 meters) in front of the mobile system. The measurement data of the object to be measured is measured. In the present embodiment, as the range sensor 1, for example, an LMS 200 manufactured by SICK is used.

  The route search unit 2 is configured by a computer. In the present embodiment, for example, a notebook personal computer is employed. The azimuth sensor 3 is constituted by, for example, a three-dimensional magnetic azimuth sensor, and measures and measures the angle component of the yaw angle as an angle between the true north direction of the global coordinate system and the direct front direction of the moving system. The angle is output to the route search unit 2 as azimuth data. In the present embodiment, as the direction sensor 3, for example, TruePoint HMR3500 manufactured by Honeywell is adopted.

  The tire part 4 is comprised by the four tires attached to the lower part of the flame | frame 6, and moves a moving system along the ground. Of the four tires, the left and right two large radii tires provided at the rear are respectively attached with a left motor M1 and a right motor M2, and both tires receive driving force from the left and right motors M1 and M2. Receive and rotate. The two tires with a small radius provided in front are auxiliary wheels.

  The stand 5 is composed of a stand on which a computer such as the route search unit 2 is installed. The operation unit 7 is composed of, for example, a joystick and is used when the mobile system is operated manually.

Frame 6 is constituted by, for example, quadrangular prism member, to form a skeleton portion of the wheelchair, the tire is attached to the lower portion, the central portion is mounted platform 5, the operation unit 7 in the rear right hand attached ing.

  The GPS sensor 8 is composed of, for example, a differential GPS sensor (relatively-accurate GPS sensor), and is attached to the upper end of a column 81 standing on the front left hand side of the frame 6 to obtain position data in the global coordinate system of the mobile system. get. Here, the position data includes a latitude component and a longitude component. In the present embodiment, for example, A100 manufactured by hemisphere is employed as the GPS sensor 8.

  FIG. 2 is a block diagram showing an electrical configuration of the mobile system shown in FIG. As shown in FIG. 2, the movement system includes a range sensor 1, a route search unit 2, a direction sensor 3, an operation unit 7, a GPS sensor 8, a left motor drive unit 9 (an example of tire drive means), and a right motor drive unit. 10 (an example of tire driving means), a motor control unit 11 (an example of tire driving means), a left motor M1 (an example of tire driving means), and a right motor M2 (an example of tire driving means).

  The path search unit 2 includes an input device 21, a ROM (Read Only Memory) 22, a CPU (Central Processing Unit) 23, a RAM (Random Access Memory) 24, an external storage device 25, a display device 26, a recording medium driving device 27, And an input / output interface (I / F) 29. The input device 21, ROM 22, CPU 23, RAM 24, external storage device 25, display device 26, recording medium drive device 27, and I / F 29 are connected to a bus line, and various data and the like are input / output through this bus line. Various processes are executed under the control of the CPU 23.

  The input device 21 includes a keyboard, a mouse, and the like, and is used for a user to input various data. The ROM 22 stores a system program such as BIOS (Basic Input / Output System). The external storage device 25 is configured by a hard disk drive, an SSD (Solid State Drive), or the like, and stores a control program for causing a predetermined OS (Operating System) and a computer to function as the path search unit 2. The CPU 23 reads the OS and the like from the external storage device 25 and controls the operation of each block. The RAM 24 is used as a work area for the CPU 23.

  The display device 26 is composed of a liquid crystal display device or the like, and displays various images under the control of the CPU 23. The recording medium drive device 27 includes a CD-ROM drive, a flexible disk drive, and the like.

  The recording medium 28 is configured by a computer-readable recording medium 28 such as a CD-ROM, and stores a control program for causing the computer to function as the path search unit 2. The user installs the control program in the computer by causing the recording medium drive device 27 to read the recording medium 28.

  The I / F 29 is configured by a plurality of ports conforming to the USB standard, for example, and is connected to the range sensor 1, the direction sensor 3, the GPS sensor 8, and the motor control unit 11. A left motor drive unit 9 and a right motor drive unit 10 are connected to the motor control unit 11. An operation unit 7 is connected to the motor control unit 11.

In accordance with the instruction signal output from the route search unit 2, the motor control unit 11 includes the left motor drive unit 9 and the left motor drive unit 9 so that each of the left motor M1 and the right motor M2 rotates at the rotation speed specified by the route search unit 2. The right motor drive unit 10 is controlled. Left motor driving unit 9 and the right motor driver 10, thus controlling the motor control unit 11, is rotated at a rotational speed designated respective left motor M1 and the right motor M2.

  FIG. 3 is a block diagram showing a functional configuration of the mobile system shown in FIG. As shown in FIG. 3, the mobile system includes an acquisition unit 111 (an example of an acquisition unit), a map data generation unit 112 (an example of a map data generation unit), a specification unit 113 (an example of a specification unit), and a voting unit 114 (a voting unit). ), Safety factor calculation unit 115 (an example of safety factor calculation means), search unit 116 (an example of search means), movement control unit 117 (an example of movement control means), map data storage unit 121, and movement safety area A map data storage unit 122 is provided.

  The acquisition unit 111 to the movement control unit 117 are configured by the CPU 23 of the route search unit 2, and the map data storage unit 121 and the movement safety area map data storage unit 122 are configured by the RAM 24 of the route search unit 2 and the like.

  The acquisition unit 111 acquires range data indicating a distance and a direction to an object existing around the mobile system measured by the range sensor 1. Further, the acquisition unit 111 acquires position data in the global coordinate system of the mobile system measured by the GPS sensor 8. The acquisition unit 111 acquires azimuth data of the mobile system measured by the azimuth sensor 3. The acquisition unit 111 acquires position data indicating the goal position input by the user by operating the input device 21. Here, as the position data indicating the goal position, position data including a latitude component and a latitude component in the global coordinate system is employed.

  The map data generation unit 112 generates two-dimensional map data indicating the position of the object by plotting the range data acquired by the acquisition unit 111 in a two-dimensional coordinate space. Here, the map data generation unit 112 generates map data as follows, for example.

  First, it is a coordinate plane when the ground surface is viewed from above, and the direction in front of the range sensor 1 is set as the first coordinate axis, and the direction orthogonal to the first coordinate axis is set as the second coordinate axis. The coordinate plane thus set is set as a two-dimensional coordinate space. Next, a reference point indicating the position of the range sensor 1 is plotted in the set coordinate space. Next, the direction from the reference point of the range data to be plotted is determined from the direction data included in the range data, and the distance from the reference point of the range data to be plotted is determined from the distance data included in the range data . Next, the range data is plotted at the coordinate position in the coordinate space specified by the determined direction and distance. Such processing is executed for each range data output from the range sensor 1 to generate map data.

  In addition, the map data generation unit 112 causes the range sensor 1 to scan the laser beam a plurality of times with a certain time interval (scan cycle), and each time the range sensor 1 scans one map data. By generating, a plurality of map data is generated. The generated plurality of map data is stored in the map data storage unit 121.

  The specifying unit 113 extracts a point group indicating each object in each map data generated by the map data generating unit 112, and selects a point group satisfying a predetermined pedestrian condition from the extracted point group. It is specified as a point cloud indicating. Specifically, the specifying unit 113 specifies a point group indicating a pedestrian as follows. First, in each map data, a lump point group, that is, a point group existing continuously, is extracted as a point group indicating one object.

  Next, in each map data, the point group which the width | variety and a structure score satisfy | fill a predetermined pedestrian condition is specified as a candidate point group used as a pedestrian candidate. Next, candidate point groups indicating the same object are associated with each other between different map data, and the moving speed of the associated candidate point group is calculated. Next, a point group in which the calculated moving speed satisfies a predetermined pedestrian condition is specified as a point group indicating a pedestrian.

  Note that the moving speed of the candidate point group may be obtained as follows, for example. First, each map data is regarded as one frame constituting a moving image, and associated candidate point groups are arranged in time series. Next, the centroid position of each candidate point group arranged in time series is specified. Next, the movement distance between the frames at the specified center of gravity position is obtained. Next, the moving speed between the frames of each candidate point group is obtained by dividing the obtained moving distance by the scanning cycle by the range sensor 1, and the average value of the obtained moving speeds is calculated as the moving speed of the candidate point group. .

  The specifying unit 113 specifies a point group that does not satisfy the predetermined pedestrian condition as a point group indicating an obstacle.

  The voting unit 114 generates a voting space by dividing the two-dimensional coordinate space indicated by the map data into a grid, and indicates the pedestrian specified by the specifying unit 113 in each grid constituting the generated voting space. Vote for the range data that makes up the point cloud.

  Here, the voting unit 114 changes the voting value of the grid according to a predetermined two-dimensional distribution function around the grid in which the range data constituting the point cloud indicating the pedestrian is voted, thereby voting the surrounding grid. Set a value and vote on the surrounding grid with the set vote value. Here, for example, a two-dimensional Gaussian distribution function can be adopted as the two-dimensional distribution function.

  In addition, the voting unit 114 votes for the range data of the point cloud indicating the pedestrian by setting the vote value to a positive value (for example, “+1”), and for the range data constituting the point cloud indicating the obstacle. Vote with a negative voting value (eg, “−1”). Thereby, when calculating a safety factor, an obstacle and a pedestrian can be distinguished clearly.

  In addition, the voting unit 114 also applies to the range data constituting the point cloud indicating the obstacle in the same manner as the range data constituting the point group indicating the pedestrian, with the grid where the range data is voted as the center. The voting values of the surrounding grids are set by changing the voting values of the lattices according to a two-dimensional Gaussian distribution function.

  The safety factor calculation unit 115 calculates the safety factor of each grid so that the higher the voting result by the voting unit 114 is, the higher the safety factor is. Specifically, the safety factor calculation unit 115 calculates the safety factor of each grid by obtaining the sum of the vote values voted by the voting unit 114 for each grid, and generates moving safety area map data. The generated travel safety area map data is stored in the travel safety area map data storage unit 122.

  In addition, the safety factor calculation unit 115 is configured to set the two-dimensional coordinate space based on the position of the mobile system in the global coordinate system measured by the GPS sensor 8 and the orientation of the mobile system measured by the orientation sensor 3. By converting the first and second coordinate axes into the global coordinate system, the position in the global coordinate system of each range data plotted is obtained.

  In the voting space in which the safety factor is calculated, the search unit 116 determines a route in which the distance between the grid indicating the start position and the grid indicating the predetermined goal position is the shortest and the safety factor is the highest. A search is performed using an algorithm (for example, A * algorithm).

  As the start position, position data in the global coordinate system of the reference point plotted on the map data by the map data generation unit 112 can be adopted.

  The movement control unit 117 outputs an instruction signal to the motor control unit 11 to drive the left motor M1 and the right motor M2 so that the movement system moves according to the route searched by the route search unit 2. Here, the movement control unit 117 outputs an instruction signal to the motor control unit 11 so that the rotation speed of the left motor M1 is larger than the rotation speed of the right motor M2 when the movement system is turned rightward. On the other hand, when the movement control unit 117 turns the movement system in the left direction, the movement control unit 117 outputs an instruction signal to the motor control unit 11 so that the rotation speed of the right motor M2 is higher than the rotation speed of the left motor M1.

  Next, the operation of the mobile system according to the embodiment of the present invention will be described. FIG. 4 is a flowchart showing an operation when the mobile system according to the embodiment of the present invention searches for a route.

  First, in step S <b> 1, the acquisition unit 111 acquires position data indicating the current position of the mobile system measured by the GPS sensor 8, and acquires direction data indicating the direction of the mobile system measured by the direction sensor 3. .

  Next, the acquisition unit 111 acquires range data measured by the range sensor 1 (step S2). Here, as described above, the range sensor 1 scans the laser beam a plurality of times according to the scan period. As the scan period, it is preferable to adopt a value suitable for obtaining the moving speed of the pedestrian.

  Next, each time the range sensor 1 scans the laser beam once, the map data generation unit 112 generates a plurality of map data by plotting the measured range data in a two-dimensional coordinate space. (Step S3). In this case, in the generated map data, the first coordinate axis is set, for example, in the direction directly in front of the range sensor 1, and the second coordinate axis is set, for example, in a direction orthogonal to the first coordinate axis.

  Next, the specifying unit 113 uses the plurality of pieces of map data created in step S3 to extract a point cloud indicating each object existing around the movement system from each map data, and the extracted point cloud is predetermined. A point cloud indicating a pedestrian and a point cloud indicating an obstacle are specified depending on whether or not the pedestrian condition is satisfied (step S4).

  FIG. 5 is a table showing pedestrian conditions. As shown in FIG. 5, as the pedestrian condition, the point group width w (cm), the number of constituent points N (pieces), and the moving speed v (cm / sec) are employed. In the present embodiment, for example, 5 ≦ w ≦ 60 is adopted as the width w, 1 ≦ N ≦ 30 is adopted as the number of constituent points N, and v ≧ 12 is adopted as the moving speed v, for example.

  Therefore, in each map data, the specifying unit 113 first specifies a point group in which the width w and the number of constituent points N satisfy the conditions shown in FIG. 5 as a point group candidate indicating a pedestrian. Each map data is regarded as a moving image frame, and point cloud candidates indicating the same pedestrian in each frame are associated with each other and arranged in time series.

  Then, the movement distance between the frames of the centroid positions of the point group candidates arranged in time series is obtained, and the movement speed between the frames of the associated point group candidates is obtained by dividing the movement distance by the scanning period, and obtained. The average value of the moving speeds is calculated as the moving speed of the associated point group candidate. Then, a point group candidate whose calculated moving speed satisfies the condition shown in FIG. 5 is specified as a point group indicating a pedestrian.

  For example, the specifying unit 113 may associate the point group candidate in the next frame closest to a point group candidate in a certain frame as a point group candidate indicating the same object.

  Also, as a technique for extracting a point cloud indicating a pedestrian from map data, a technique developed by Arras et al. That obtains a geometric feature of a point cloud obtained by scanning the height of a person's ankle is known. . Therefore, the point cloud indicating the pedestrian may be specified using the method developed by the Arras et al., Or the method developed by the Arras et al. And the method for specifying the point cloud indicating the pedestrian described above may be combined. A point cloud indicating a pedestrian may be specified.

  The method developed by Arras et al. Is “Kai O. Arras, Oscar Martinez Mozos, Wolfman Burgard:“ Using Boosted Features for the Detection of People in 2D Range Data ”, IEEE International Conference on Robotics and Automation, pp. 3402. 3407. 2007. ”.

  6A is a schematic diagram illustrating an example of map data generated by the map data generation unit 112, and FIG. 6B is map data obtained by extracting a point cloud indicating a pedestrian from the map data illustrated in FIG. It is a schematic diagram which shows.

  As shown in FIG. 6A, it can be seen that a plurality of range data are plotted in a two-dimensional coordinate space. And as a result of performing the process which specifies the point group which shows the pedestrian mentioned above, as shown to FIG. 6 (B), three point groups shown by the dark black circle of the center part are specified as a point group which shows a pedestrian. I understand that. And it turns out that the point cloud plotted on the other ends other than that does not satisfy the pedestrian condition, and therefore is specified as an obstacle.

  Next, the voting unit 114 generates a voting space by dividing the two-dimensional coordinate space indicated by the map data into a lattice shape (step S5).

  Next, in each map data generated in step S3, the voting unit 114 votes on each grid in the voting space with a vote value of 1 for the range data constituting the point cloud indicating the pedestrian, and the obstacles. For the range data constituting the point cloud indicating voting, the vote value is set to −1 and voted to each grid in the voting space (step S6).

  FIG. 7A shows map data whose resolution is lowered to the resolution of the voting space, and FIG. 7B shows the voting space in which the range data is voted.

  In FIG. 7B, as the gray color becomes darker, the safety factor is higher, and black indicates that the safety factor is a negative value. In FIG. 7B, the size of each lattice is 10 (cm) square.

  Since the range data D1 indicating the pedestrian in FIG. 7A is located in the grid K1 in FIG. 7B, the voting value is set to 1 and voted on the grid K1. Therefore, the voting unit 114 applies a two-dimensional Gaussian distribution function centering on the lattice K1, changes the voting value 1 of the lattice K1 according to the applied two-dimensional Gaussian distribution function, and determines the voting values of the lattices around the lattice K1. And vote for the surrounding grid with the set vote value.

  Here, the reason why the two-dimensional Gaussian distribution function is applied is that the region of the pedestrian's torso is considered safe. In other words, since the range sensor 1 scans the laser beam on the horizontal plane about the height of the pedestrian below the pedestrian from the ground, the size of the point cloud indicating the pedestrian has a size around the pedestrian below the pedestrian. It will be. Therefore, by applying a two-dimensional Gaussian distribution function and making the lattice area to be voted wide, voting taking into account the size of the pedestrian's trunk can be performed. Therefore, it is preferable to adopt a value such that a lattice included in an area having a radius around the human torso (for example, 20 cm) from a central lattice is voted as the half width of the two-dimensional Gaussian distribution function.

  For example, in FIG. 7B, each lattice located in a circular region R1 having a radius of three lattices excluding the lattice K1 around the lattice K1 is voted. The voting value of each lattice in the region R1 is set to a value obtained by changing 1 which is the voting value of the lattice K1 according to a Gaussian function. Therefore, the voting value is lower as the distance from the grid K1 in the region R1 increases.

  In this way, the voting unit 114 votes each range data included in the plurality of map data created in step S3 to one voting space shown in FIG. 7B. In addition, for the range data constituting the point cloud indicating the obstacle, voting is performed with the vote value set to -1. And about the ranging data which comprises the point cloud which shows a pedestrian and an obstruction, it votes also to the surrounding grid | lattice according to a two-dimensional Gaussian distribution function.

  Next, the safety factor calculation unit 115 calculates the total value of the vote values voted by the voting unit 114 in each lattice, and calculates the calculated total value as the safety factor of each lattice (step S7). For example, in the grid K1 shown in FIG. 7B, the voting value set when voting the range data D1 shown in FIG. 7A and the range data D2 located around the range data D1 are included. The vote is made according to the vote value set when voting. Therefore, the total value of the voting values voted for the grid K1 when the range data D1 is voted is calculated as the safety factor of the grid K1.

  As a result, when a plurality of pedestrians pass through the same region, the safety factor increases each time. That is, the region through which more pedestrians pass becomes a region with a higher safety factor. An upper limit value may be provided for the safety factor value.

  Next, the safety factor calculation unit 115 uses the position data measured by the GPS sensor 8 in step S1 and the azimuth data measured by the azimuth sensor 3 to set the first and second voting spaces. The coordinate axes are converted into the global coordinate system (step S8).

  Specifically, the safety factor calculation unit 115 indicates that the position data measured by the GPS sensor 8 indicates a reference point RP (see FIG. 7B) corresponding to the current position of the mobile system, and the first The first and second coordinate axes set in the original moving safety area map data using the position data and the azimuth data so that the coordinate axis Z1 indicates the longitude direction and the second coordinate axis Z2 indicates the latitude direction. Is transformed to the global coordinate system. Accordingly, the first coordinate axis Z1 shown in FIG. 7B is oriented in the longitude direction, the second coordinate axis Z2 is oriented in the latitude direction, and each grid in the moving safety area map data is represented in the global coordinate system, that is, It is represented by a longitude component and a latitude component.

  In addition, since the range data D3 constituting the point cloud indicating the obstacle is located on the grid K2, the range data D3 is voted on the grid K2, but the range data D3 is the measurement data constituting the point cloud indicating the obstacle. Since it is area data, the vote value of the grid K2 is -1. Further, since the two-dimensional Gaussian distribution function is applied to the obstacle around the lattice K2, the obstacle is also voted for the lattice around the lattice K2. In addition, about the two-dimensional Gaussian distribution function applied about an obstruction, you may make a half value width smaller than the two-dimensional Gaussian distribution function applied about a pedestrian. Through the above processing, the travel safety area map data as shown in FIG. 7B is generated (step S9).

  Next, the search unit 116 sets a grid indicating the start position and a grid indicating the goal position in the moving safety area map data generated by the safety factor calculation unit 115, and a grid indicating the goal position from the grid indicating the start position. A safe and shortest route is searched for (step S10).

  FIG. 8 is a schematic diagram illustrating an example of a route searched by the search unit 116. Note that FIG. 8 shows a route searched for a black line. The search unit 116 calculates the total cost f (n) from the lattice SK indicating the start position to the lattice GK indicating the goal position, and searches for a route that minimizes the total cost f (n). The total cost f (n) is defined by equation (1).

However, h (n) is an estimated value of a cost from a certain lattice n to the lattice GK, and is defined by a linear distance D (n, goal) from the lattice n to the lattice GK. g (n) is the sum of the costs of each lattice from lattice SK to lattice n. The cost of each lattice is proportional to the linear distance D (i, i + 1) between adjacent lattices and inversely proportional to the safety factor α _{i + 1} of the lattice i + 1. Then, the search unit 116 searches from the lattice SK so that f (n) becomes small, and the route obtained when reaching the lattice GK becomes the optimum route.

  Specifically, the search unit 116 sets the lattice SK as the initial attention lattice, calculates the costs of the attention lattice and, for example, eight lattices adjacent to the attention lattice, and the calculated cost is minimized. The route is searched by repeating the process of setting the lattice as the next target lattice.

Here, assuming that the target lattice is CK and any one of the eight adjacent lattices is NK, the cost of the target lattice CK and the lattice NK is D (CK, NK) / α _NK + D (NK, GK). Then, the lattice with the lowest cost among the lattices NK is set as the next attention lattice CK. As D (CK, NK), for example, a linear distance connecting the centers of gravity of the target lattice CK and the lattice NK can be employed. As D (NK, GK), for example, a linear distance connecting the centers of gravity of the lattice NK and the lattice GK can be employed.

  In addition, among the eight lattices, the lattices that are the target of cost calculation in the past attention lattice CK out of the eight lattices are not subject to cost calculation for all the eight lattices adjacent to the attention lattice CK. Alternatively, it may be excluded from the cost calculation target.

  As a result, as shown in FIG. 8 as an optimum route, it is possible to search for a route that passes through a region with a high safety factor in consideration of the route distance. In addition, since an area through which more pedestrians have passed is selected as a route, an optimum route can be obtained even when there are a plurality of routes.

  Next, processing when the mobile system actually moves according to the searched route will be described. The mobile system always estimates its own position (x, y, θ) by the GPS sensor 8 and the direction sensor 3. However, x represents a latitude component, y represents a longitude component, and θ represents an orientation of the mobile system. First, pedestrian recognition by the range sensor 1 and generation of movement safety area map data are performed with the movement system stopped. When the number of grids in which the safety factor is recorded exceeds a certain number, a route to the destination is searched based on the generated moving safety area map data and the self position. These processes have been described above. Then, the movement control unit 117 moves the movement system so that the estimated self-position does not deviate from the searched route.

  A goal position may be set outside the generated travel safety area map data. In this case, the route search unit 2 sets the position closest to the goal position as the subgoal position in the generated movement safety area map data, searches for the route to the set subgoal position, and moves to the set subgoal position. Move the system.

  When the movement system reaches the subgoal position, the route search unit 2 again generates the movement safety area map data. If the set goal position exists in the generated movement safety area map data, the route search unit 2 returns to the goal position. And the mobile system may be moved to the goal position.

  On the other hand, if the set goal position does not exist in the generated movement safety area map data, the subgoal position may be set as described above and the movement system may be moved to the subgoal position. And the route search part 2 should just reach | attain the goal position aiming at a moving system by performing this process repeatedly.

  FIG. 9 is a flowchart illustrating processing when the mobile system moves along the route searched by the search unit 116. First, the movement control unit 117 outputs an instruction signal to the left motor driving unit 9 and the right motor driving unit 10 to start driving the left motor M1 and the right motor M2 (step S21).

  Next, the acquisition unit 111 acquires position data from the GPS sensor 8 and also acquires direction data from the direction sensor 3 (YES in step S22). Here, the acquisition unit 111 acquires position data and azimuth data at regular time intervals. On the other hand, when the position data and the direction data are not acquired by the acquisition unit 111 (NO in step S22), the process proceeds to step S25.

  Next, the movement control unit 117 determines whether or not it is necessary to change the traveling direction of the movement system (step S23). In this case, the movement control unit 117 specifies the traveling direction of the mobile system from the position data and the azimuth data acquired by the acquiring unit 111. If the specified traveling direction deviates from the direction of the searched route, the movement control unit 117 proceeds. An instruction signal for setting the rotational speeds of the left and right motors M1, M2 is output to the motor control unit 11 so that the direction faces the direction of the searched route (step S24).

  On the other hand, if the traveling direction of the mobile system does not deviate from the direction of the searched route (NO in step S23), the movement control unit 117 advances the process to step S25.

  Next, the movement control unit 117 uses the position data acquired by the acquisition unit 111 to determine whether or not the movement system has arrived at the goal position. If the movement system has not arrived at the goal position (in step S25). NO), the process is returned to step S22, and the processes of steps S22 to S25 are repeated. On the other hand, when it is determined that the movement system has arrived at the goal position (YES in step S25), the movement control unit 117 stops the left motor M1 and the right motor M2 (step S26), and ends the process.

  Next, in order to verify the effectiveness of the method of the present mobile system, a navigation experiment conducted in an outdoor environment will be described. In this navigation experiment, a point about 50 (m) ahead was set as the goal position, and an obstacle area that assumed a dangerous area was provided in the middle of the route. As a result of the navigation experiment, the mobile safety area map data of the mobile system could be generated by recognizing the pedestrian. FIG. 10 shows the travel safety area map data obtained by the navigation experiment, the searched route, and the travel route of the mobile system. Further, FIGS. 11A to 11D are views showing the state of the traveling system during traveling. 11A to 11D are images of the moving system taken at positions “1”, “2”, “3”, and “4” shown in FIG. 10, respectively.

  In FIG. 10, a slightly darker (light gray) line indicates a region with a high safety factor, lines dotted on both upper and lower sides indicate obstacle regions, a line L1 indicates a searched route, L2 indicates a travel route of the movement system, and a cross indicates a subgoal position.

  As shown in FIG. 10, it is possible to generate mobile safety area map data indicating a high safety area where the mobile robot can move, and to perform a route search based on the generated mobile safety area map data. Thus, the moving system can be run so as to avoid obstacles that are difficult to measure with the range sensor 1. In addition, the mobile system stopped at the subgoal position, and by repeating the enlargement and navigation of the mobile safety area map data, it was possible to finally reach the goal position at a remote location.

  Thus, in this movement system, by using pedestrian trajectory information, the movement system can be moved along a safe route according to the situation even in a dynamic environment. Further, as can be seen from the above navigation experiment, even when there are obstacles that are difficult to measure with the range sensor 1, the path in which the mobile system automatically avoids the obstacles by using the pedestrian trajectory information. Can be explored.

DESCRIPTION OF SYMBOLS 1 Range sensor 2 Path | route search part 3 Direction sensor 8 GPS sensor 9 Left motor drive part 10 Right motor drive part 11 Motor control part 111 Acquisition part 112 Map data generation part 113 Specification part 114 Voting part 115 Safety factor calculation part 116 Search part 117 movement control unit 121 map data storage unit 122 movement safety area map data storage unit

Claims (7)

An acquisition means for acquiring range data indicating a position to an object existing around;
Map data generation means for generating two-dimensional map data indicating the position of the object by plotting the range data acquired by the acquisition means in a two-dimensional coordinate space;
A means for extracting a point group indicating each object existing from the map data, and specifying a point group satisfying a predetermined pedestrian condition among the extracted point groups as a point group indicating a pedestrian;
A voting space is generated by dividing the two-dimensional coordinate space indicated by the map data into a lattice shape, and a point cloud indicating the pedestrian specified by the specifying means is formed on each lattice constituting the generated voting space. Voting means for voting each range data to be
The safety factor calculating means for calculating the safety factor of each lattice so that the safety factor becomes higher as the lattice has a higher voting result by the voting means;
In the voting space where the safety factor is calculated, a grid indicating a predetermined start position is set as a starting attention grid, and the cost of each grating from the starting position indicating the adjacent grid adjacent to the attention grid is added. and values combined, the sum of the linear distance between the grid showing the adjacent grid and the predetermined goal position is calculated as the total cost, the total cost calculated to search the minimum and ing path using a * algorithm searches Means and
The cost of each grid, the path search apparatus having a value proportional to the linear distance between adjacent contact grid, and inversely proportional to the safety factor of the adjacent grid.   The route search device according to claim 1, wherein the specifying unit employs a moving speed and a shape of a point group as the pedestrian condition.   3. The route search device according to claim 2, wherein the shape of the point group in the pedestrian condition includes the number of points constituting the point group and the width of the point group. The range data is measured by irradiating the laser beam in the horizontal direction so that the height from the ground is below the pedestrian's knee,
The voting means sets the voting values of the surrounding grids by changing the voting values of the grids according to a predetermined two-dimensional distribution function around the grids where the range data constituting the point cloud indicating the pedestrian is voted. The route search device according to any one of claims 1 to 3, wherein The specifying means specifies a point group that does not satisfy the pedestrian condition as a point group indicating an obstacle,
The voting means votes with the voting value set to a positive value for the range data of the point cloud indicating the pedestrian, and the negative value for the range data constituting the point group indicating the obstacle. 5. The route search device according to claim 4, wherein the voting is performed as follows. A GPS sensor and a direction sensor;
The safety factor calculation means converts the coordinate system of the voting space into a global coordinate system based on position data indicating its current position measured by the GPS sensor and direction data measured by the direction sensor. ,
The search means sets the grid indicating the current position measured by the GPS sensor in the voting space, and searches the route by setting the grid indicating the set current position as the start position. The route search device according to claim 1, wherein the route search device is characterized in that: A route search device according to any one of claims 1 to 6,
A tire portion;
Tire drive means for driving the tire portion,
The route search device drives the tire driving means to move along the route searched by the route search device.

Images