MOBILE BODY CONTROL SYSTEM, CONTROL METHOD THEREOF, PROGRAM, AND MOBILE BODY

Abstract

To appropriately generate a running route with the position attitude of a vehicle being taken into consideration.SOLUTION: This moving object control system sets the present position of a moving object and the aiming position thereof, generates a first route from the present position to the aiming position so as to satisfy a predetermined boundary condition on an lw coordinate in which a straight line connecting the present position of the moving object to the aiming position is taken as an l-axis and an orthogonal straight line to the l-axis is taken as a w-axis, and converts the generated first route in an xy coordinate in which the travelling direction of the moving object is taken as an x-axis and an orthogonal axis to the x-axis is taken as a y-axis.SELECTED DRAWING: Figure 7

Description

[Technical field]

[0001] The present invention relates to a mobile object control system, a control method thereof, a program, and a mobile object. [Background technology]

[0002] In recent years, small moving bodies such as electric vehicles with a seating capacity of one or two people, called ultra-compact mobility (also called micromobility), and mobile robots that provide various services to people, have become known. Some of these moving bodies perform autonomous driving while periodically generating a driving route to a destination.

[0003] Patent Document 1 proposes a travel route generation device that sets a comfortable travel route that reduces the burden on the occupants. Specifically, the travel route generation device sets a travel route with the gradualest change in curvature per travel distance based on the angle between the traveling direction of the vehicle at the current position and the traveling direction of the vehicle at the target position, and the curvature of the travel route and the travel distance according to the steering angle of the vehicle. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2018-2082 A Summary of the Invention [Problem to be solved by the invention]

[0005] Micromobility includes, for example, a three-wheeled vehicle equipped with a front wheel and a tail wheel (driven wheel) that operates following the drive of the front wheel. In such a vehicle, when starting to travel or when determining whether to reach a destination, there is a possibility that the vehicle will turn on the spot according to the target value of its attitude angle. When turning on the spot occurs, the angle of the tail wheel becomes large, which adversely affects the riding comfort of the passengers. In addition, small micromobility requires the use of hardware resources to be minimized as much as possible. Therefore, it is desirable to reduce the amount of processing required when generating a travel route as much as possible and to efficiently use limited hardware resources.

[0006] The present invention has been made in consideration of the above problems, and has an object to generate a driving route that takes into account the position and attitude of a vehicle at low calculation cost. [Means for solving the problem]

[0007] According to the present invention, for example, a mobile object control system includes a setting means for setting a current position and a target position of a mobile object, and a setting means for setting a first path from the current position to the target position on an lw coordinate system in which a line connecting the current position and the target position of the mobile object is the l axis and a line perpendicular to the l axis is the w axis, so as to satisfy predetermined boundary conditions including a condition related to an attitude angle at the current position of the mobile object and a condition related to an attitude angle at the target position of the mobile object. , using polynomials The present invention is characterized in that the present invention comprises a route generation means for generating a first route, and a conversion means for converting the generated first route into an xy coordinate system in which the x-axis is the direction of travel of the moving body and the y-axis is an axis perpendicular to the x-axis, and the route generation means generates a curved route by setting the curvature of the route at at least one of the current position and the target position to 0 as the predetermined boundary condition. Effect of the Invention

[0008] According to the present invention, a driving route that takes into account the position and attitude of a vehicle can be generated at low calculation cost. [Brief description of the drawings]

[0009] [Figure 1]FIG. 1 is a block diagram showing an example of the hardware configuration of a moving body according to an embodiment; [Diagram 2] FIG. 1 is a block diagram showing a control configuration of a moving object according to an embodiment; [Diagram 3] FIG. 2 is a block diagram showing a functional configuration of a moving body according to an embodiment; [Figure 4] FIG. 1 illustrates an occupancy grid map according to an embodiment. [Diagram 5] FIG. 1 illustrates a method for generating an occupancy grid map according to an embodiment. [Figure 6] FIG. 1 illustrates a global route and a local route according to an embodiment. [Figure 7] FIG. 1 is a diagram showing route generation taking into account the position and orientation of a moving object according to an embodiment; [Figure 8] FIG. 1 is a diagram showing route generation taking into account the position and orientation of a moving object according to an embodiment; [Figure 9] FIG. 13 is a diagram showing a path (polynomial curve) generated in consideration of the position and orientation of a moving object according to an embodiment; [Figure 10] A flowchart showing a process for controlling the travel of a moving object according to an embodiment. [Figure 11] A flowchart showing a process for generating a global route according to an embodiment. [Figure 12] FIG. 1 is a diagram for explaining a procedure for determining an obstacle collision of a polynomial path according to an embodiment. [Figure 13] FIG. 1 is a diagram showing route generation at an intersection taking into account the position and orientation of a moving object according to an embodiment; [Figure 14] FIG. 1 is a diagram showing route generation at an intersection taking into account the position and orientation of a moving object according to an embodiment; [Figure 15] A flowchart showing a process for generating a global route at an intersection according to an embodiment. [Figure 16] FIG. 1 is a diagram illustrating route generation for lane change of a moving object according to an embodiment; DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] Hereinafter, the embodiments will be described in detail with reference to the accompanying drawings. Note that the following embodiments do not limit the invention according to the claims, and not all combinations of features described in the embodiments are essential to the invention. Two or more features among the multiple features described in the embodiments may be arbitrarily combined. In addition, the same reference numbers are used for the same or similar configurations, and duplicated descriptions are omitted.

[0011] <Mobile configuration> The configuration of a moving body 100 according to this embodiment will be described with reference to Fig. 1. Fig. 1(A) shows a side view of the moving body 100 according to this embodiment, and Fig. 1(B) shows the internal configuration of the moving body 100. In the figure, arrow X indicates the front-rear direction of the moving body 100, F indicates the front, and R indicates the rear. Arrows Y and Z indicate the width direction (left-right direction) and up-down direction of the moving body 100.

[0012] The moving body 100 is equipped with a battery 113, and is, for example, an ultra-compact mobility that moves mainly by the power of a motor. An ultra-compact mobility is a very small vehicle that is more compact than a general automobile and has a passenger capacity of about one or two people. The moving body 100 can run on roadways and sidewalks, as well as on the premises of various facilities and public open spaces. In this embodiment, a three-wheeled ultra-compact mobility is described as an example of the moving body 100, but this is not intended to limit the present invention, and it may be, for example, a four-wheeled vehicle or a saddle-type vehicle. In addition, the vehicle of the present invention is not limited to a vehicle, but may be a vehicle that carries luggage and runs alongside a person walking, or a vehicle that leads a person. Furthermore, the present invention is not limited to a four-wheeled or two-wheeled vehicle, and may also be applicable to a walking robot that is capable of autonomous movement.

[0013] The moving body 100 is an electric autonomous vehicle equipped with a propulsion unit 112 and using a battery 113 as a main power source. The battery 113 is, for example, a secondary battery such as a lithium ion battery, and the moving body 100 is self-propelled by the propulsion unit 112 with the power supplied from the battery 113. The propulsion unit 112 is a tricycle equipped with a pair of left and right front wheels 120 and a tail wheel (driven wheel) 121. The propulsion unit 112 may be in another form, such as a four-wheeled vehicle. The moving body 100 is equipped with a seat 111 for one or two people.

[0014] The traveling unit 112 includes a steering mechanism 123. The steering mechanism 123 is a mechanism that changes the steering angle of the pair of front wheels 120 using motors 122a and 122b as drive sources. The traveling direction of the moving body 100 can be changed by changing the steering angle of the pair of front wheels 120. The tail wheel 121 does not have an individual drive source, but is a driven wheel that operates following the drive of the pair of front wheels 120. In addition, the tail wheel 121 is connected to the body of the moving body 100 via a rotating part. The rotating part rotates so that the direction of the tail wheel 121 changes separately from the rotation of the tail wheel 121. In this way, the moving body 100 according to this embodiment employs a differential two-wheel type mobility with a tail wheel, but is not limited to this.

[0015] The moving body 100 includes a detection unit 114 that recognizes a plane in front of the moving body 100. The detection unit 114 is an external sensor that monitors the front of the moving body 100, and in this embodiment, is an imaging device that captures an image in front of the moving body 100. In this embodiment, the detection unit 114 will be described as an example of a stereo camera having an optical system such as two lenses and respective image sensors. However, it is also possible to adopt a radar or a lidar (Light Detection and Ranging) instead of or in addition to the imaging device. Also, in this embodiment, an example in which the detection unit 114 is provided only in front of the moving body 100 will be described, but this is not intended to limit the present invention, and the detection unit 114 may be provided behind, on the left or right of the moving body 100.

[0016] The moving body 100 according to this embodiment captures an image of the area in front of the moving body 100 using the detection unit 114, and detects obstacles and topography (intersections) from the captured image. Furthermore, the moving body 100 can control its travel while generating an occupancy grid map in which the area around the moving body 100 is divided into a grid and obstacle information is accumulated in each grid (hereinafter also referred to as a grid). The occupancy grid map is generated when traveling on a sidewalk or in a facility, and is useful for planning a route to avoid obstacles. On the other hand, when traveling on a roadway, it is not necessary to generate an occupancy grid map because the road structure is recognized and a route is planned. However, even when traveling on a roadway, an occupancy grid map may be generated by regarding lane boundaries, parked vehicles, etc. as obstacles, and may be used for route planning including lane changes to avoid these obstacles. That is, in the present invention in which a global route is generated by a polynomial curve, it is not necessary to generate an occupancy grid map, but it is desirable to generate an occupancy grid map in a traveling scene in which an obstacle may exist. Details of the occupancy grid map will be described later.

[0017] <Control configuration of moving object> FIG. 2 is a block diagram of a control system of the moving body 100 according to this embodiment. Here, the configuration necessary for implementing the present invention will be mainly described. Therefore, in addition to the configuration described below, other configurations may be included. In this embodiment, each unit described below is described as being included in the moving body 100, but this is not intended to limit the present invention, and the moving body control system may be realized as a moving body control system including multiple devices. For example, some functions of the control unit 130 may be realized by a server device connected so as to be able to communicate, or the detection unit 114 and the GNSS sensor 134 may be provided as external devices. The moving body 100 includes a control unit (ECU) 130. The control unit 130 includes a processor represented by a CPU, a storage device such as a semiconductor memory, an interface with an external device, and the like. The storage device stores a program executed by the processor, data used by the processor for processing, and the like. A plurality of sets of the processor, storage device, and interface may be provided according to the functions of the moving body 100 and configured to be able to communicate with each other.

[0018] The control unit 130 acquires the detection results of the detection unit 114, input information from the operation panel 131, voice information input from the voice input device 133, position information from the GNSS sensor 134, and information received via the communication unit 136, and executes corresponding processing. The control unit 130 controls the motors 122a and 122b (controls the driving of the driving unit 112), controls the display of the operation panel 131, notifies the occupants of the moving object 100 by voice from the speaker 132, and outputs information.

[0019] The voice input device 133 collects the voice of the passenger of the moving body 100. The control unit 130 can recognize the input voice and execute a corresponding process. The GNSS (Global Navigation Satellite system) sensor 134 receives a GNSS signal to detect the current position of the moving body 100. The storage device 135 is a storage device that stores images captured by the detection unit 114, obstacle (target) information, previously generated routes, and an occupancy grid map. The storage device 135 may also store programs executed by the processor, data used by the processor for processing, and the like. The storage device 135 may store various parameters (e.g., learned parameters and hyperparameters of a deep neural network) of a machine learning model for voice recognition or image recognition executed by the control unit 130.

[0020] The communication unit 136 communicates with a communication device 140, which is an external device, via wireless communication such as Wi-Fi or fifth generation mobile communication. The communication device 140 is, for example, a smartphone, but is not limited to this, and may be an earphone-type communication terminal, a personal computer, a tablet terminal, a game console, etc. The communication device 140 connects to a network via wireless communication such as Wi-Fi or fifth generation mobile communication.

[0021] A user who owns the communication device 140 can give instructions to the moving body 100 via the communication device 140. The instructions include, for example, an instruction to call the moving body 100 to a position desired by the user and join the moving body 100. Upon receiving the instruction, the moving body 100 sets a target position based on the position information included in the instruction. In addition to such instructions, the moving body 100 can set a target position from an image captured by the detection unit 114, or based on an instruction from a user riding on the moving body 100 via the operation panel 131. When setting a target position from a captured image, for example, a person who is raising his hand toward the moving body 100 is detected in the captured image, and the position of the detected person is estimated and set as the target position.

[0022] <Functional configuration of mobile object> Next, the functional configuration of the moving body 100 according to this embodiment will be described with reference to Fig. 3. The functional configuration described here is realized in the control unit 130, for example, by the CPU reading out a program stored in a memory such as a ROM into a RAM and executing it. Note that the functional configuration described below describes only the functions necessary for explaining the present invention, and does not describe all of the functional configurations actually included in the moving body 100. In other words, the functional configuration of the moving body 100 according to the present invention is not limited to the functional configuration described below.

[0023] The user instruction acquisition unit 301 has a function of receiving instructions from a user, and can receive user instructions via the operation panel 131, user instructions from an external device such as the communication device 140 via the communication unit 136, and user spoken instructions via the voice input device 133. As described above, the user instructions include an instruction to set a target position (also referred to as a destination) of the moving body 100 and an instruction related to driving control of the moving body 100.

[0024] The image information processing unit 302 processes the captured image acquired by the detection unit 114. Specifically, the image information processing unit 302 creates a depth image from the stereo image acquired by the detection unit 114 and converts it into a 3D point cloud. The 3D point cloud image data is used to detect obstacles and targets that impede the travel of the mobile body 100. The image information processing unit 302 may also include a machine learning model that processes image information and execute a learning stage process and an inference stage process of the machine learning model. The machine learning model of the image information processing unit 302 can perform a process of recognizing a three-dimensional object or the like included in the image information by performing a calculation of a deep learning algorithm using a deep neural network (DNN), for example.

[0025] The grid map generating unit 303 creates a grid map of a predetermined size (for example, each cell is 10cmX10cm in an area of ​​20mX20m) based on the image data of the 3D point cloud. This is aimed at reducing the amount of data of the 3D point cloud, which is large and difficult to process in real time. The grid map includes, for example, a grid map showing the difference between the maximum height and the minimum height of the point cloud in the grid (expressing whether the cell is a step) and a grid map showing the maximum height of the point cloud in the grid from the reference point (expressing the topographical shape of the cell). Furthermore, the grid map generating unit 303 removes spike noise and white noise contained in the generated grid map, detects obstacles with a height equal to or higher than a predetermined height, and generates an occupancy grid map showing whether or not there is a solid object that serves as an obstacle for each grid.

[0026] The route generating unit 304 generates a travel route of the moving body 100 to the target position set by the user instruction acquiring unit 301. Specifically, the route generating unit 304 generates a route using an occupied grid map generated by the grid map generating unit 303 from the captured image of the detection unit 114 without requiring obstacle information of a high-precision map. Note that since the detection unit 114 is a stereo camera that captures an image of the area in front of the moving body 100, it cannot recognize obstacles or terrain in other directions. Therefore, it is desirable for the moving body 100 to store the detected obstacle information for a predetermined period of time in order to avoid collision with an obstacle outside the viewing angle or getting stuck in a dead end. This allows the moving body 100 to generate a route taking into account both obstacles detected in the past and obstacles detected in real time.

[0027] In addition, the path generating unit 304 periodically generates a global path using the occupancy grid map, and further periodically generates a local path so as to follow the global path. That is, the target position of the local path is determined by the global path. In addition, as the generation period of each path, in this embodiment, the generation period of the global path is 100 ms, and the generation period of the local path is 50 ms, but this does not limit the present invention. As an algorithm for generating a global path, various algorithms such as RRT (Rapid-exploring Random Tree), PRM (Probabilistic Road Map), and A* are known. In addition, since a differential two-wheel type mobility with a tail wheel is adopted as the moving body 100, the path generating unit 304 generates a local path taking into account the tail wheel 121, which is a driven wheel. In addition, according to this embodiment, when generating a global path, the path generating unit 304 takes into account the attitude angle at the current position of the moving body 100 and the attitude angle at the target position of the path. By taking into consideration the position and attitude angle of the moving body 100 in this manner, it is possible to prevent turning at the current position or the target position.

[0028] The traveling control unit 305 controls the traveling of the moving body 100 according to the local route. Specifically, the traveling control unit 305 controls the traveling unit 112 according to the local route to control the speed and angular velocity of the moving body 100. Furthermore, the traveling control unit 305 controls the traveling in response to various operations of the driver. When a deviation occurs in the driving plan of the local route due to the operation of the driver, the traveling control unit 305 may again acquire a new local route generated by the route generation unit 304 and control the traveling, or may control the speed and angular velocity of the moving body 100 so as to eliminate the deviation from the local route in use.

[0029] <Occupancy grid map> FIG. 4 shows an occupancy grid map 400 including obstacle information according to this embodiment. The mobile body 100 according to this embodiment travels without relying on obstacle information from a high-precision map, so all obstacle information is acquired from the recognition results of the detection unit 114. At this time, it is necessary to store the obstacle information in order to avoid collision with an obstacle outside the viewing angle or getting stuck in a dead end. Therefore, in this embodiment, as a method for storing obstacle information, an occupancy grid map is used from the viewpoints of reducing the amount of information of the three-dimensional point cloud of the stereo image and ease of handling in route planning. Here, a map including obstacle information will be described, but it may include other target information, intersection information, etc. instead of or in addition to the obstacle information.

[0030] The grid map generating unit 303 according to this embodiment divides the surrounding area of ​​the moving body 100 into a grid, and generates an occupancy grid map including information indicating the presence or absence of an obstacle for each grid (divided area). Note that, here, an example in which a predetermined area is divided into a grid will be described, but instead of dividing into a grid, it is also possible to divide into other shapes and create an occupancy map indicating the presence or absence of an obstacle for each divided area. Also, in the present invention, since a smooth curved route that does not depend on the divided areas is generated, control for dividing into a plurality of areas is not essential. The occupancy grid map 400 is a surrounding area of, for example, a 40mX40m or 20mx20m area around the moving body 100, and the area is divided into grids (grids) of 20cmX20cm or 10cmx10cm, and is dynamically set according to the movement of the moving body 100. In other words, the occupancy grid map 400 is a real-time changing area that is shifted so that the moving body 100 is always at the center according to the movement of the moving body 100. The size of the area can be set arbitrarily depending on the hardware resources of the moving object 100 .

[0031] Furthermore, in the occupancy grid map 400, the presence / absence information of obstacles detected from the image captured by the detection unit 114 is defined for each grid. For example, the presence / absence information is defined as "0" for a passable area and "1" for a non-passable area (i.e., the presence of an obstacle). In FIG. 4, 401 indicates a grid in which an obstacle exists. An obstacle-existing area indicates an area that the mobile body 100 cannot pass through, and is formed of, for example, a solid object of 5 cm or more. Therefore, the mobile body 100 generates a route so as to avoid these obstacles 401.

[0032] <Accumulation of obstacle information> FIG. 5 explains the accumulation of obstacle information in the occupied grid map according to this embodiment. 500 indicates a local map that moves in accordance with the movement of the moving body 100. The local map 500 is shifted according to the movement of the moving body 100 in the x-axis direction and the y-axis direction on the grid map. The local map 500 shows how a dotted area 501 is deleted and a solid area 502 is added according to the amount of movement Δx of the moving body 100 in the x-axis direction, for example. The area to be deleted is an area opposite to the moving direction of the moving body 100, and the area to be added is an area in the moving direction. Similarly, areas are deleted and added in the y-axis direction according to the movement of the moving body 100. In addition, the local map 500 accumulates obstacle information detected in the past. Note that, when an obstacle exists in a grid included in the deletion area, the obstacle information is deleted from the local map 500, but it is desirable to hold the obstacle information separately from the local map 500 for a certain period of time. Such information is useful, for example, when the moving body 100 changes its course and the deleted region is again included in the local map 500, and can improve the accuracy with which the moving body 100 avoids obstacles.

[0033] 510 indicates an obstacle detection map showing detection information of an obstacle present in front of the moving body 100 from an image captured by the detection unit 114 of the moving body 100. The obstacle detection map 510 indicates real-time information and is periodically generated according to the captured image acquired from the detection unit 114. Since moving obstacles such as people and vehicles are also expected, it is desirable to update the obstacles detected in the past within the viewing angle 511 of the detection unit 114, which is the forward region of the moving body 100, by the obstacle detection map 510 generated periodically, rather than fixing and storing them. This makes it possible to recognize moving obstacles and prevent the generation of a route that avoids them more than necessary. On the other hand, for the rear region of the moving body 100 (strictly speaking, outside the viewing angle of the detection unit 114), obstacles detected in the past are stored as shown in the local map 500. This makes it possible to easily generate a route that avoids collision with the obstacle that has passed, for example, when an obstacle is detected in the forward region and a detour is generated.

[0034] Reference numeral 520 denotes an occupancy grid map generated by adding together the local map 500 and the obstacle detection map 510. In this manner, the occupancy grid map 520 is generated as a grid map that combines the local map and obstacle detection information that change in real time with obstacle information that has been detected and accumulated in the past.

[0035] <Route Generation> 6 shows a travel path generated by the moving body 100 according to this embodiment. The path generating unit 304 according to this embodiment periodically generates a global path 602 using an occupancy grid map according to a set target position 601, and further periodically generates a local path 603 so as to follow the global path.

[0036] The target position 601 is set based on various instructions. For example, the instructions include instructions from a passenger riding on the moving body 100 and instructions from a user outside the moving body 100. The instructions from the passenger are given via the operation panel 131 or the voice input device 133. The instructions given via the operation panel 131 may be a method of specifying a specific grid of a lattice map displayed on the operation panel 131. In this case, the size of each grid may be set large so that it is possible to select from a wider range of maps. The instructions given via the voice input device 133 may be instructions using surrounding landmarks as a landmark. The landmarks may include passersby, signs, signs, outdoor facilities such as vending machines, building components such as windows and entrances, roads, vehicles, motorcycles, etc., included in the speech information. When the instructions given via the voice input device 133 are received, the route generation unit 304 detects the specified landmark from the captured image acquired by the detection unit 114 and sets it as the target position.

[0037] Machine learning models are used for these voice and image recognition. Machine learning models, for example, perform calculations of deep learning algorithms using deep neural networks (DNNs) to recognize place names, landmark names such as buildings, store names, and landmark names contained in spoken information and image information. The voice recognition DNN becomes trained by performing the learning stage processing, and by inputting new spoken information into the trained DNN, recognition processing (inference stage processing) for the new spoken information can be performed. In addition, the image recognition DNN can recognize passersby, signs, road signs, outdoor facilities such as vending machines, building components such as windows and entrances, roads, vehicles, and motorcycles contained in images.

[0038] In addition, instructions from a user outside the moving body 100 can be sent via the user's own communication device 140 to the moving body 100 via the communication unit 136, or the user can call the moving body 100 by, for example, raising a hand toward the moving body 100 as shown in Fig. 6. Instructions from the communication device 140 are given by operation input or voice input, similar to the instructions from the passengers.

[0039] When the target position 601 is set, the path generating unit 304 generates a global path 602 using the generated occupancy grid map. As a method of generating a global path, a path (first path) is generated at low calculation cost using a polynomial (parameter) described later, and if the generated path does not collide with a detected obstacle, the path is adopted as the global path. In the method of generating a path using a polynomial, in addition to being able to generate a path at low calculation cost, a path is generated that takes into account the attitude of the moving body 100 at the current position (own vehicle position) and the target position, and turning control for attitude correction can be reduced as much as possible. On the other hand, when the generated path collides with an obstacle, various search algorithms such as RRT, PRM, and A* are known as a route generation that avoids the obstacle, but any method may be used. That is, according to this embodiment, a path is first generated simply using a polynomial described later, and a route generation that avoids the obstacle is performed near the obstacle. For example, it may be possible to first determine whether an obstacle is detected in the vicinity of the vehicle, generate a path using a polynomial if there is no obstacle, and generate a global route using a search algorithm that avoids the obstacle if there is an obstacle. In other words, the path generation method may be switched depending on the presence or absence of an obstacle.

[0040] When the global path is generated, the path generating unit 304 generates a local path 603 so as to follow the generated global path 602. There are various methods for planning a local path, such as Dynamic Window Approach (DWA), Model Predictive Control (MPC), clothoid tentacles, and Proportional-Integral-Differential (PID) control. The global path 602 shown in FIG. 6 is a path that depends on the grid of an occupancy lattice map generated to avoid obstacles, and does not take into account the position and attitude of the moving body 100. The local path generated based on such a path has a smooth curve, which can improve the riding comfort of the passengers. On the other hand, since the position and attitude are not taken into account, a sharp turn may occur the next time the vehicle departs, depending on the angle of the tail wheel at the time of stopping. On the other hand, in path generation using a polynomial, a global path is generated by taking into account the attitude angle of the current position of the moving body 100 and the attitude angle of the target position. In this case, the global path itself is generated as a continuous curve, so that the path is formed by a smooth curve.

[0041] <Route generation using polynomials> Next, a method for generating a route using a polynomial according to this embodiment will be described with reference to Figs. 7 and 8. Here, an example of generating a curved route using a polynomial (parameter) on a sidewalk, public open space, etc. will be described. Figs. 7 and 8 show a curved route generated using a polynomial. In addition to the algorithm for generating a polynomial curve shown below, a route may be generated that connects a straight line to at least one of the front and rear of a polynomial curve.

[0042] Reference numeral 700 indicates route generation using a polynomial. Reference numeral 701 indicates the route of the generated curve (polynomial curve). Reference numeral 702 indicates the current position of the moving body 100. Reference numeral 703 indicates the target position of the moving body 100. The target position 703 differs from the set final target position 601 and indicates the closest point to the moving body 100 among multiple intermediate points obtained by dividing the route to the target position 601. Reference numeral 705 indicates the traveling direction (attitude angle) of the moving body 100 at the current position 702. Reference numeral 706 indicates the direction (attitude angle) of the moving body 100 at the target position 703. Reference numeral 704 indicates the intersection point between the straight line of the traveling direction 705 of the current position and the straight line of the direction 706 at the target position.

[0043] Here, the moving direction 705 of the moving body 100 is the x-axis, and the axis perpendicular to the x-axis is the y-axis (xy coordinates). Also, the line connecting the current position 702 and the target position 703 of the moving body 100 is the l-axis, and the line perpendicular to the l-axis is the w-axis (lw coordinates). According to this embodiment, the path generating unit 304 converts the lw coordinates into xy coordinates based on a predetermined boundary condition 710, and generates a path from the current position 702 to the target position 703 using the following formula (1). Note that R indicates a rotation matrix.

[0044]

number

[0045] The boundary conditions refer to conditions at a current position 702 and a target position 703 of the moving body 100. As shown in Fig. 7, the predetermined boundary conditions 710 are set, for example, as follows. w(0)=0, w(L)=0, dw(0) / dl=-tanφ, dw(L) / dl=tan(θmax-φ) L indicates the distance (length) from the current position 702 to the target position 703. φ indicates the angle between the x-axis and the l-axis. θmax indicates the angle between the x-axis and the line in the direction 706 at the intersection 704.

[0046] Here, since it cannot be expressed as w=w(l), we use parameters as follows: l(t)=a0+a1t+a2t 2 +a3t 3 w(t) = b0 + b1t + b2t 2 +b3t 3 If the above-mentioned predetermined boundary condition 710 is rewritten in parametric notation for t: 0→1, it can be expressed as the following mathematical expression (2).

[0047]

number

[0048] From the above formula (2), a0 to a3 and b0 to b3 are obtained. The curvature can be changed by adjusting the parameters k0 and k1. It is possible to obtain optimal parameters k0 and k1, but since optimization affects the generation of a global route at low calculation cost in this embodiment, it is preferable to adopt a method for obtaining an approximate solution. For example, in the approximate solution method for parameters k0 and k1, it is possible to analytically solve the problem by providing a condition that the curvature between the current position and the target position is 0.

[0049] 800 in FIG. 8 indicates a curved path 801 generated when the current position 802 of the moving body 100 is changed on a sidewalk or public open space similar to that in FIG. 7. Similarly, 803 indicates the target position of the moving body 100. 805 indicates the moving direction (attitude angle) of the moving body 100 at the current position 802. 806 indicates the direction (attitude angle) of the moving body 100 at the target position 803. 804 indicates the intersection point between the straight line of the moving direction 805 of the current position and the straight line of the direction 806 at the target position. As described with reference to FIG. 7, the path generating unit 304 generates a curved path from the current position 802 to the target position 803 using the above formula (1) and the boundary condition 810. It can be seen that the path 801 is generated as a polynomial curve including a plurality of curves. In this way, in the path generation by the polynomial curve in this embodiment, curves of various shapes can be generated according to the attitude at the current position and the attitude at the target position. Furthermore, by generating a route that connects straight lines before and after the curve, it is possible to generate a route to the final target position.

[0050] FIG. 9 shows variations of polynomial curves generated by the path generating unit 304 according to this embodiment. Reference numeral 900 shows a polynomial curve when the moving body 100 turns left. Reference numeral 910 shows a polynomial curve when the moving body 100 turns right. Reference numeral 920 shows a polynomial curve when the moving body 100 changes lanes. Reference numeral 930 shows a polynomial curve when the moving body 100 makes a U-turn. In each graph shown in FIG. 9, the horizontal axis is x, the vertical axis is y, and (0,0) is the current position of the moving body 100. Also, each graph shows curves at multiple target positions.

[0051] <Basic control of moving objects> 10 is a flowchart showing the basic control of the moving body 100 according to this embodiment. The process described below is realized in the control unit 130, for example, by the CPU reading out a program stored in a memory such as a ROM into the RAM and executing the program.

[0052] In S101, the control unit 130 sets a target position of the moving body 100 based on a user instruction received by the user instruction acquisition unit 301. The user instruction can be received by various methods as described above. Next, in S102, the control unit 130 captures an image of the area in front of the moving body 100 by the detection unit 114, and acquires the captured image. The acquired captured image is processed by the image information processing unit 302, and a depth image is created and converted into a three-dimensional point cloud. In S103, the control unit 130 detects an obstacle, for example, a solid object of 5 cm or more, from the three-dimensional point cloud image. In S104, the control unit 130 generates an occupancy grid map of a predetermined area centered on the moving body 100 according to the detected obstacle and the position information of the moving body 100.

[0053] Next, in S105, the control unit 130 generates a travel path for the moving body 100 by the path generation unit 304. As described above, the path generation unit 304 generates a global path using a polynomial curve, judges whether the generated path collides with an obstacle, and generates a global path by a search algorithm using an occupancy grid map if a collision occurs. In addition, the path generation unit 304 generates a local path according to the generated global path. Next, in S106, the control unit 130 determines the speed and angular velocity of the moving body 100 according to the generated local path, and controls the traveling. After that, in S107, the control unit 130 judges whether the moving body 100 has reached the target position based on the position information from the GNSS sensor 134, and if the target position has not been reached, the process returns to S102, and the process of generating a path while updating the occupancy grid map and controlling the traveling is repeated. On the other hand, if the moving body 100 has reached the target position, the process of this flowchart ends.

[0054] <Procedure for generating global routes> 11 is a flowchart showing a procedure for generating a global route performed in the route generation in S105 according to the present embodiment. The process described below is realized in the control unit 130, for example, by the CPU reading a program stored in a memory such as a ROM into the RAM and executing it.

[0055] S201 to S207 show repeated processing at 10 Hz. In S202, the control unit 130 acquires the current position, the target position, and the attitude of the moving body 100 at each position. The target position acquired here may be the target position acquired in S101, or may be the nearest position when the target position is subdivided. In addition, the control unit 130 acquires, as the current position, information on the current vehicle position and its attitude in the previously generated occupancy grid map. Information on the attitude is acquired from a group of sensors such as the detection unit 114.

[0056] Next, in S203, the control unit 130 generates a polynomial path using the information acquired in S202 and the above-mentioned formula (1). Furthermore, in S204, the control unit 130 maps the polynomial path generated in S203 onto the occupancy lattice map generated in S104, and judges whether or not the path collides with an obstacle. A detailed judgment method will be described later with reference to FIG. 12. If it is judged that the polynomial path collides with an obstacle, the process proceeds to S205, where the control unit 130 generates a global route by a search algorithm using the occupancy lattice map instead of the polynomial path generated in S203, and proceeds to S206. On the other hand, if it is judged that the polynomial path does not collide with an obstacle, the process proceeds to S206.

[0057] In S206, the control unit 130 outputs the polynomial path generated in S203, or if a global path was generated by the search algorithm in S205, the path as the global path. Thereafter, the control unit 130 generates local paths based on the output global path, and performs the driving control in S106.

[0058] <Obstacle collision detection> Fig. 12 is a diagram showing a method of determining whether a polynomial path has collided with an obstacle according to this embodiment. 1201 shows a detected obstacle 1210 and a generated polynomial path 1211 superimposed on a generated occupancy grid map. 1210 shows a detected obstacle mapped on the occupancy grid map. 1211 shows the generated polynomial path. 1212 shows the current position and its orientation. 1213 shows a target position and its orientation.

[0059] 1202 shows only the obstacle map extracted from 1201. 1203 shows a Minkowski distance map (first map) when the vehicle width margin is taken into account from the obstacle map 1202. The Minkowski distance map 1203 is generated using a filter such as a uniform filter or a Gaussian filter. Region 1214 is the obstacle region taking into account the vehicle width margin.

[0060] On the other hand, 1204 indicates the generated polynomial path 1211 extracted from 1201. 1205 indicates the state in which the polynomial path 1211 is mapped onto the occupancy lattice map (second map). 1215 indicates the area through which the polynomial path 1211 passes on the occupancy lattice map.

[0061] According to this embodiment, a cost map 1206 (third map) is generated by the Hadamard product of a minkowski distance map 1203 indicating an obstacle area taking into account the vehicle width margin and a polynomial path map 1205. That is, in the cost map 1206, a position where an area 1214 indicating an obstacle taking into account the vehicle width and an area 1215 through which the polynomial path passes overlap is acquired. 1216 indicates the overlapping position, and if such an area exists, it is determined that the generated polynomial path will collide with the obstacle. On the other hand, if the area shown in 1216 does not exist, it is determined that the generated polynomial path will not collide with the obstacle.

[0062] Thus, according to this embodiment, a polynomial path that can be generated at low calculation cost is first generated on a sidewalk or in a public open space, and then it is determined whether the polynomial path collides with an obstacle. If a collision occurs, a global route is generated by a search algorithm using an occupancy grid map. On the other hand, if the polynomial path does not collide with an obstacle, the polynomial path is used as a global route, making it possible to generate a path at low calculation cost. Furthermore, when generating a polynomial path, a route is generated that takes into account the attitude of the mobile body 100 at the current position and the target position, making it possible to avoid driving control that causes discomfort to passengers, such as sharp turns.

[0063] <Second embodiment> The second embodiment of the present invention will be described below. In this embodiment, unlike the above embodiment, a method of using a polynomial path on a roadway will be described. In this embodiment, route generation at an intersection will be described as an example of a case in which a polynomial path is used on a roadway. Here, control will be described on the assumption that no obstacle exists at the intersection. Therefore, in this embodiment, S103 and S104 may be omitted from FIG. 10 showing the basic control of the moving body 100. It should be noted that this is not intended to limit the present invention, and as described in the above first embodiment, the presence or absence of an obstacle may be detected at an intersection as well, and it may be determined whether or not the generated polynomial path will collide with the obstacle.

[0064] <Route generation at intersections using polynomials> Next, a method for generating a route using a polynomial according to this embodiment will be described with reference to Fig. 13 and Fig. 14. Here, an example of generating a route of a polynomial curve at an intersection or the like will be described. The generation method described below can be applied when generating a curved route on a roadway not only at an intersection but also in a scene where the traveling direction of the moving body 100 changes due to a curve, a lane change, or the like. Also, Fig. 13 corresponds to Fig. 7 of the first embodiment, and Fig. 14 corresponds to Fig. 8.

[0065] Reference numeral 1300 indicates route generation at an intersection. Reference numeral 1301 indicates the route of the generated polynomial curve. Reference numeral 1302 indicates the current position of the moving body 100. Reference numeral 1303 indicates the target position of the moving body 100 (here, the exit of the intersection is set). The target position 1303 differs from the set final target position 601, and indicates the point closest to the moving body 100 among multiple intermediate points obtained by dividing the route to the target position 601. For example, when entering an intersection, the exit of the intersection is set as the target position. Reference numeral 1305 indicates the traveling direction (attitude angle) of the moving body 100 at the current position 1302. Reference numeral 1306 indicates the direction (attitude angle) of the moving body 100 at the target position 1303. Reference numeral 1304 indicates the intersection point between the straight line of the traveling direction 1305 of the current position and the straight line of the direction 1306 at the target position.

[0066] Here, the moving direction 1305 of the moving body 100 is the x-axis, and the axis perpendicular to the x-axis is the y-axis (xy coordinates). The line connecting the current position 1302 and the target position 1303 of the moving body 100 is the l-axis, and the line perpendicular to the l-axis is the w-axis (lw coordinates). According to this embodiment, the path generating unit 304 converts the lw coordinates into xy coordinates based on a predetermined boundary condition 1310, and generates a path from the current position 1302 to the target position 1303 using the above formula (1). Details have already been described in FIG. 7 and will be omitted.

[0067] Reference numeral 1400 in FIG. 14 indicates a curved path 1401 generated when a current position 1402 of the moving body 100 is changed at an intersection similar to that in FIG. 13. Similarly, reference numeral 1403 indicates a target position of the moving body 100. Reference numeral 1405 indicates the traveling direction (attitude angle) of the moving body 100 at the current position 1402. Reference numeral 1406 indicates the direction (attitude angle) of the moving body 100 at the target position 1403. Reference numeral 1404 indicates an intersection point between a straight line of the traveling direction 1405 of the current position and a straight line of the direction 1406 at the target position. As described with reference to FIG. 7, the path generating unit 304 generates a curved path from the current position 1402 to the target position 1403 using the above formula (1) and the boundary condition 810.

[0068] <Procedure for generating global routes> 15 is a flowchart showing a procedure for generating a global route performed in the route generation in S105 according to the present embodiment. The process described below is realized in the control unit 130, for example, by the CPU reading a program stored in a memory such as a ROM into the RAM and executing it.

[0069] S301 to S306 show repeated processing at 10 Hz. In S302, the control unit 130 acquires recognition information, the current position, and the attitude of the moving body 100 at the position. Here, the recognition information indicates recognition information of the road structure. The recognition information of the road structure recognizes white lines and the like from an image captured by the detection unit 114, and recognizes the driving lanes, intersections, crosswalks, and the like of the road. The recognition information is output by a machine learning model that processes image information (captured image). The machine learning model performs a process of recognizing the road shape included in the image information by performing calculations of a deep learning algorithm using, for example, a deep neural network (DNN). The recognition information includes various lines and lane information of the road, the lane in which the vehicle is located (Ego lane), various intersections (intersections, such as intersections), entrances to various roads (Road entrances), and the like. Note that, since it is assumed here that a path is generated at an intersection, the acquired recognition information includes at least information on the intersection. In addition, the control unit 130 acquires sensor information of the GNSS sensor 134 as the current position. Information about the posture is obtained from a group of sensors such as the detection unit 114 .

[0070] Next, in S203, the control unit 130 determines a target lane and a target position for exiting the intersection based on the recognition information acquired in S202. For example, when there are multiple lanes that serve as exits from the intersection, the control unit 130 determines which of the lanes to generate a route to. Note that the traveling direction at the intersection is based on a direction instruction from the occupant, and is information regarding steering by the occupant.

[0071] After determining the target lane and the target position, in S304, the control unit 130 generates a polynomial path using the information acquired in S302 and S303 and the above formula (1). Then, in S305, the control unit 130 outputs the polynomial path generated in S304 as a global path. Then, the control unit 130 generates a local path based on the output global path, and performs driving control in S106.

[0072] In this way, according to the present embodiment, a polynomial path is generated that takes into account the attitude between the current position and the target position in a driving scene where it is not necessary to detect the presence or absence of an obstacle. This makes it possible to avoid driving control that causes discomfort to passengers, such as sharp turns, even when route generation is performed using road recognition information.

[0073] <Lane change> 16 shows a polynomial path generated when a moving object 100 traveling on a roadway changes lanes. Reference numeral 1601 indicates the current position and direction (attitude) of the moving object 100. Reference numeral 1602 indicates a target position and direction (attitude). Reference numeral 1603 indicates a polynomial path generated taking into consideration the current position 1601 and the target position 1602. As shown in reference numeral 1603, by taking into consideration the attitudes at the current position 1601 and the target position 1602, a route with a smooth curve can be generated, and it can be seen that the polynomial curve of the present invention is also useful for lane changes.

[0074] <Summary of the embodiment> 1. The mobile object control system (e.g., 100) of the above embodiment includes: A setting means (301, S101, S201) for setting a current position and a target position of a moving object; a route generating means (FIG. 2, 304, S105) for generating a first route from the current position of the moving object to the target position on an lw coordinate system in which a line connecting the current position of the moving object and the target position is the l axis and a line perpendicular to the l axis is the w axis so as to satisfy a predetermined boundary condition; The present invention is characterized by comprising:

[0075] According to this embodiment, a driving route can be generated in an optimal manner, taking into account the position and attitude of the vehicle.

[0076] 2. In the mobile object control system of the above embodiment, the route generation means sets the curvature of the route at at least one of the current position and the target position to 0 as the predetermined boundary condition, and generates a curved route (FIG. 7).

[0077] According to this embodiment, the curvature of the current position and the target position can be set to 0, and the polynomial can be solved analytically, thereby achieving lower calculation costs.

[0078] 3. The mobile object control system of the above embodiment is An acquisition means (114) for acquiring an image of the periphery of the moving object; A detection means (302) for detecting an obstacle included in the captured image; a map generating means (303) for dividing an area around the moving object into a plurality of areas and generating an occupancy map for each of the areas indicating the occupancy of the obstacles detected by the detecting means; and determining whether the converted first route collides with an obstacle included in the occupied map (S204). When the determining means determines that the first route will collide with an obstacle included in the occupation map, the route generating means generates a second route that avoids the obstacle using a search algorithm (S205).

[0079] According to this embodiment, a polynomial path is first generated with low computational cost, and if that path collides with an obstacle, a path that avoids the obstacle can be generated using a search algorithm, making it possible to avoid the obstacle while keeping computational costs as low as possible.

[0080] 4. In the mobile object control system of the above embodiment, The determination means is A means (1203) for generating a first map by converting the area of ​​the obstacle detected by the detection means into an area taking into account the vehicle width; A means (1205) for generating a second map by mapping the first route onto the occupancy map; a means (1206) for generating a third map indicating whether the first path will collide with an obstacle detected by the detection means by using a Hadamard product of the first map and the second map; Equipped with.

[0081] According to this embodiment, it is possible to simply and safely determine whether a polynomial path generated at low calculation cost hits an obstacle.

[0082] 5. In the mobile object control system of the above embodiment, An acquisition means (114) for acquiring an image of the periphery of the moving object; A recognition means (302) for recognizing a road shape included in the captured image, The setting means sets the current position and the target position based on information about the captured image and the recognition information by the recognition means (S302).

[0083] According to this embodiment, even on roadways where an occupancy grid map is not generated, a path that takes into account the vehicle position and orientation can be generated using a polynomial path.

[0084] 6. In the mobile object control system of the above embodiment, The route generation means generates a route at an intersection, The setting means sets a lane serving as an exit from the intersection as the target position when the moving body enters the intersection (S302, FIG. 13, FIG. 14).

[0085] According to this embodiment, a path can be generated at an intersection by using a polynomial path, taking into account the position and attitude of the vehicle.

[0086] 7. In the mobile object control system of the above embodiment, An acquisition means (114) for acquiring an image of the periphery of the moving object; A detection means (302) for detecting an obstacle included in the captured image; a map generating means (303) for dividing an area around the moving object into a plurality of areas and generating an occupancy map for each of the areas indicating the occupancy of the obstacles detected by the detecting means; Further equipped with The path generating means prioritizes generating the first path in a vicinity where the obstacle does not exist, and prioritizes generating a second path that avoids the obstacle in a vicinity where the obstacle exists.

[0087] According to this embodiment, it is possible to generate a path that takes into account the position and attitude of the vehicle depending on the presence or absence of an obstacle, while accurately avoiding the obstacle. [Explanation of symbols]

[0088] 100...mobile body, 111...seat, 112...driving unit, 113...battery, 114...detection unit, 120...front wheel, 121...tail wheel, 122a, 122b...motor, 123...steering mechanism, 130...control unit, 131...operation panel, 132...speaker, 133...voice input device, 134...GNSS sensor, 135...storage device, 136...communication unit, 140...communication device, 301...user instruction acquisition unit, 302...image information processing unit, 303...grid map generation unit, 304...route generation unit, 305...driving control unit

Claims

1. A mobile object control system, A setting means for setting a current position and a target position of a moving body; a path generating means for generating, by using a polynomial, a first path from the current position to the target position on an lw coordinate system in which a straight line connecting the current position and the target position of the moving body is defined as an l axis and a straight line perpendicular to the l axis is defined as a w axis, so as to satisfy predetermined boundary conditions including a condition related to an attitude angle at the current position of the moving body and a condition related to an attitude angle at the target position of the moving body; a conversion means for converting the generated first route into an x-y coordinate system in which the moving direction of the moving object is the x-axis and an axis perpendicular to the x-axis is the y-axis; Equipped with The vehicle control system according to claim 1, wherein the path generating means generates a curved path by setting a curvature of the path at at least one of the current position and the target position to 0 as the predetermined boundary condition.

2. An acquisition means for acquiring a captured image of the periphery of a moving object; A detection means for detecting an obstacle included in the captured image; a map generating means for dividing an area around the moving object into a plurality of areas and generating an occupancy map showing the occupancy of the obstacles detected by the detecting means for each of the areas; and a determination means for determining whether the converted first route collides with an obstacle included in the occupation map, 2. The mobile object control system according to claim 1, wherein when the determination means determines that the first route will collide with an obstacle included in the occupancy map, the route generation means generates a second route that avoids the obstacle using a search algorithm.

3. The determination means is a means for generating a first map by converting an area of ​​the obstacle detected by the detection means into an area taking into consideration a vehicle width; means for generating a second map by mapping the first route onto the occupancy map; a means for generating a third map indicating whether or not the first path will collide with an obstacle detected by the detection means by using a Hadamard product of the first map and the second map; 3. The mobile object control system according to claim 2, further comprising:

4. An acquisition means for acquiring a captured image of the periphery of a moving object; A recognition means for recognizing a road shape included in the captured image, 2. The mobile object control system according to claim 1, wherein the setting means sets the current position and the target position based on information about the captured image and information recognized by the recognition means.

5. The route generation means generates a route at an intersection, 5. The mobile object control system according to claim 4, wherein the setting means sets a lane that is an exit of the intersection as the target position when the mobile object enters the intersection.

6. An acquisition means for acquiring a captured image of the periphery of a moving object; A detection means for detecting an obstacle included in the captured image; a map generating means for dividing an area around the moving object into a plurality of areas and generating an occupancy map showing the occupancy of the obstacles detected by the detecting means for each of the areas; Further equipped with 2. The mobile object control system according to claim 1, wherein the route generating means prioritizes generating the first route in a vicinity where the obstacle does not exist, and prioritizes generating a second route that avoids the obstacle in a vicinity where the obstacle exists.

7. A control method for a mobile object control system, comprising: A setting step of setting a current position and a target position of the moving body; a path generating step of generating, using a polynomial, a first path from the current position to the target position on an lw coordinate system in which a line connecting the current position and the target position of the moving body is defined as an l axis and a line perpendicular to the l axis is defined as a w axis, so as to satisfy predetermined boundary conditions including a condition related to an attitude angle at the current position of the moving body and a condition related to an attitude angle at the target position of the moving body; a conversion step of converting the generated first route into an xy coordinate system in which the moving direction of the moving object is the x-axis and an axis perpendicular to the x-axis is the y-axis; Including, A control method for a mobile object control system, wherein the path generation step generates a curved path by setting the curvature of the path at at least one of the current position and the target position to 0 as the predetermined boundary condition.

8. A program for causing a computer to function as each of the means of the mobile object control system according to any one of claims 1 to 6.

9. A mobile object, A setting means for setting a current position and a target position of a moving body; a path generating means for generating, by using a polynomial, a first path from the current position to the target position on an lw coordinate system in which a straight line connecting the current position and the target position of the moving body is defined as an l axis and a straight line perpendicular to the l axis is defined as a w axis, so as to satisfy predetermined boundary conditions including a condition related to an attitude angle at the current position of the moving body and a condition related to an attitude angle at the target position of the moving body; a conversion means for converting the generated first route into an x-y coordinate system in which the moving direction of the moving object is the x-axis and an axis perpendicular to the x-axis is the y-axis; Equipped with The moving body, wherein the path generating means generates a curved path by setting a curvature of the path at at least one of the current position and the target position to 0 as the predetermined boundary condition.

Images