SYSTEM, METHOD AND WORK VEHICLE

Abstract

To obtain a continuous course with a small amount of calculation.SOLUTION: A route search part searches a series of vehicle positions and attitudes being a route of moving from an initial position of the vehicle to a target position as elements on the basis of a first constraint condition representing the position of an obstacle. A constraint condition generation part generates a second constraint condition on which a penalty value increases in accordance with a deviation distance from the route. The route search part searches a series vehicle positions, attitudes, speeds and steering angles being a course of moving the initial position of the vehicle to the target position as elements on the basis of the second constraint condition.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present disclosure relates to systems, methods and work vehicles. [Background technology]

[0002] At work sites such as quarrying sites and mines, automatic driving systems for work vehicles are sometimes introduced. The automatic driving system calculates a trajectory for the work vehicle to reach a destination position, and drives the work vehicle based on the calculated trajectory. Patent Document 1 discloses a technology relating to a method for generating a driving route for an unmanned vehicle. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2010-073080 A Summary of the Invention [Problem to be solved by the invention]

[0004] Trajectory calculation for a vehicle to travel automatically is an optimization problem. In addition, in the trajectory calculation, obstacles present at the work site are given as constraints. However, the mathematical model used to search for the trajectory is represented by multidimensional vectors such as the vehicle's position, attitude, steering angle, speed, and acceleration, and the position and shape of the obstacle are not necessarily simple, and the constraints may be highly nonlinear. Therefore, the calculation of the trajectory requires a large amount of calculation or convergence to a local solution occurs, making it difficult to calculate the vehicle's travel trajectory online. On the other hand, if the number of dimensions of the model is reduced to reduce the amount of calculation, the operation related to the reduced dimensions may become discontinuous in the obtained trajectory. An object of the present disclosure is to provide a trajectory search device and a trajectory search method capable of obtaining a continuous trajectory with a small amount of calculation. [Means for solving the problem]

[0005] A first aspect of the present invention is a system for searching a trajectory for an automatic driving of a vehicle, comprising a processor. The processor comprises: a route searching unit that searches for a sequence of elements including the position and attitude of the vehicle, which is a route for moving from an initial position of the vehicle to a target position, based on a first constraint condition that represents the position of an obstacle, a constraint condition generating unit that generates a second constraint condition in which a penalty value increases according to a deviation distance from the route, and a trajectory searching unit that searches for a sequence of elements including the position, attitude, speed, and steering angle of the vehicle, which is a trajectory for moving from the initial position of the vehicle to a target position, based on the second constraint condition.

[0006] A second aspect of the present invention is a method for searching for a trajectory for a vehicle to travel automatically. The method includes the following processes. A first process searches for a sequence having the position and attitude of a vehicle as elements, which is a path for moving from an initial position of the vehicle to a target position, based on a first constraint condition that represents the position of an obstacle. A second process generates a second constraint condition in which a penalty value increases according to the deviation distance from the path. A third process searches for a sequence having the position, attitude, speed, and steering angle of the vehicle as elements, which is a trajectory for moving from the initial position of the vehicle to a target position, based on the second constraint condition.

[0007] A third aspect of the present invention is a work vehicle comprising a vehicle body, a traveling device for traveling the vehicle body, and a processor. The processor comprises a route search unit that searches for a sequence of elements including the position and attitude of the vehicle, which is a route for moving from an initial position of the work vehicle to a target position, based on a first constraint condition representing the position of an obstacle, a constraint condition generation unit that generates a second constraint condition in which a penalty value increases according to the deviation distance from the route, a trajectory search unit that searches for a sequence of elements including the position, attitude, speed and steering angle of the work vehicle, which is a trajectory for moving from the initial position of the work vehicle to a target position, based on the second constraint condition, and a traveling control unit that controls the traveling device according to the trajectory. Effect of the Invention

[0008] According to at least one of the above aspects, the trajectory search device can obtain a continuous trajectory with a small amount of calculation. [Brief description of the drawings]

[0009] [Figure 1] FIG. 1 is a side view of a work vehicle according to a first embodiment. [Diagram 2] 1 is a schematic block diagram showing a configuration of a control device according to a first embodiment. [Diagram 3] FIG. 4 is a diagram showing an example of map data according to the first embodiment. [Figure 4] FIG. 4 is a diagram illustrating an example of a tolerance series according to the first embodiment. [Diagram 5] 4 is a flowchart showing a method for generating a tolerance range series according to the first embodiment. [Figure 6] 4 is a diagram showing a relationship between a first rectangle and a second rectangle according to the first embodiment. FIG. [Figure 7] 4 is a flowchart showing an automatic driving process of the control device according to the first embodiment. [Figure 8] FIG. 4 is a diagram showing the relationship between the time step size and the position of the calculation result in the model according to the first embodiment. [Figure 9] FIG. 13 is a diagram showing the relationship between the time step size and the position of the calculation result in a model according to a comparative example. [Figure 10] FIG. 11 is a schematic diagram showing the configuration of an autonomous driving system according to another embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] First Embodiment <<Configuration of work vehicles>> Hereinafter, the embodiments will be described in detail with reference to the drawings. FIG. 1 is a side view of a work vehicle according to a first embodiment. The work vehicle 100 according to the first embodiment is a wheel loader. The work vehicle 100 travels by autonomous driving. In other words, the work vehicle 100 is an unmanned vehicle. The work vehicle 100 includes a vehicle body 110, a work implement 120, and a control device 140.

[0011] The vehicle body 110 includes a front vehicle body 111, a rear vehicle body 112, and a steering actuator 113. The front vehicle body 111 and the rear vehicle body 112 are attached so as to be rotatable around a steering axis extending in the up-down direction of the vehicle body 110. A pair of front wheels WF are provided on the lower part of the front vehicle body 111. A pair of rear wheels WR are provided on the lower part of the rear vehicle body 112.

[0012] The rear body 112 includes a power source 1121 and a power transmission device 1122. The power source 1121 is, for example, a diesel engine. The power transmission device 1122 changes the speed of the driving force input to the input shaft and outputs it from the output shaft. The output shaft of the power transmission device 1122 is connected to the front wheels WF via a front axle (not shown), and is connected to the rear wheels WR via a rear axle (not shown). The front axle supports the front body 111 so that it can run. The rear axle supports the rear body 112 so that it can run. The front axle and the rear axle are equipped with braking devices (not shown). The power transmission device 1122 is provided with a rotation sensor 1123. The rotation sensor 1123 measures the rotation speed of the output shaft of the power transmission device 1122. The control device 140 can identify the speed and acceleration of the work vehicle 100 based on the measurement data of the rotation sensor 1123. In addition, a detection device 1124 and a positioning device 1125 are provided on the upper part of the rear body 112. The detection device 1124 detects the three-dimensional position of an object present around the work vehicle 100. Examples of the detection device 1124 include LiDAR (Light Detection and Ranging), a stereo camera, and a UWB (Ultra Wide Band) ranging device. The detection device 1124 is provided so that the detection direction faces forward, for example. The positioning device 1125 measures the position and attitude (attitude angle) of the work vehicle 100 in global coordinates using a GNSS (Global Navigation Satellite System) or the like. The position is represented by, for example, latitude and longitude. The attitude is represented by, for example, the azimuth angle of the rear body 112 with the north as the reference orientation. Hereinafter, the position and attitude of the work vehicle 100 are also referred to as the pose of the work vehicle 100.

[0013] The steering actuator 113 is, for example, a hydraulic cylinder. A base end of the steering actuator 113 is attached to the rear vehicle body 112, and a tip end is attached to the front vehicle body 111. The steering actuator 113 determines the angle between the front vehicle body 111 and the rear vehicle body 112 by expanding and contracting with hydraulic oil. In other words, the steering angle of the front wheels WF is determined by the expansion and contraction of the steering actuator 113. The steering actuator 113 is provided with a stroke sensor 1131 that measures the stroke length of the steering actuator 113. The control device 140 can identify the steering angle of the work vehicle 100 based on the measurement data of the stroke sensor 1131.

[0014] The power transmission device 1122, the braking device and the steering actuator 113 constitute a traveling device for causing the vehicle body 110 to travel.

[0015] The work machine 120 is used for excavating and transporting work objects such as earth and sand. The work machine 120 is provided at the front of the vehicle body 110. The control device 140 causes the work vehicle 100 to travel based on the detection data from the detection device 1124 .

[0016] Configuration of the control device 140 FIG. 2 is a schematic block diagram showing the configuration of the control device 140 according to the first embodiment. The control device 140 is a computer that includes a processor 210, a main memory 230, a storage 250, and an interface 270.

[0017] Storage 250 is a non-transitory tangible storage medium. Examples of storage 250 include a magnetic disk, an optical disk, a magneto-optical disk, and a semiconductor memory. Storage 250 may be an internal medium directly connected to the bus of control device 140, or may be an external medium connected to control device 140 via interface 270 or a communication line. Storage 250 stores a program for controlling work vehicle 100.

[0018] The program may be for realizing a part of the functions to be performed by the control device 140. For example, the program may be for realizing the functions by combining with other programs already stored in the storage 250 or by combining with other programs implemented in other devices. In other embodiments, the control device 140 may include a custom LSI (Large Scale Integrated Circuit) such as a PLD (Programmable Logic Device) in addition to or instead of the above configuration. Examples of PLDs include PAL (Programmable Array Logic), GAL (Generic Array Logic), CPLD (Complex Programmable Logic Device), and FPGA (Field Programmable Gate Array). In this case, some or all of the functions realized by the processor may be realized by the integrated circuit.

[0019] <Software Configuration> The processor 210 executes a program to provide an acquisition unit 211, a map generation unit 213, a goal determination unit 212, a route search unit 214, a constraint condition generation unit 215, a trajectory search unit 216, and a driving control unit 217. The control device 140 is an example of a trajectory search device.

[0020] The acquisition unit 211 acquires measurement data from the stroke sensor 1131, the rotation sensor 1123, the detection device 1124, and the positioning device 1125. As a result, the acquisition unit 211 identifies the position, attitude angle, steering angle, speed, and acceleration of the work vehicle 100, as well as the positions of obstacles present around the work vehicle 100.

[0021] The map generation unit 213 generates map data showing the surroundings of the work vehicle 100 using the measurement data of the detection device 1124 and the measurement data of the positioning device 1125 acquired by the acquisition unit 211. The map generation unit 213 generates the map data by, for example, SLAM (Simultaneous Localization and Mapping) technology. The map data is represented by, for example, an occupied grid map related to a global coordinate system. FIG. 3 is a diagram showing an example of map data according to the first embodiment. In addition, each grid of the map data is assigned a score representing the distance from an obstacle. The score of a grid showing a position where an obstacle exists is zero, and the farther the distance from the obstacle, the higher the score. The score is represented by, for example, the Manhattan distance from the nearest obstacle.

[0022] Since the detection device 1124 is fixed to the rear vehicle body 112, the map generation unit 213 can identify the position of the obstacle in the global coordinate system by translating the position of the obstacle indicated by the measurement data of the detection device 1124 based on the position indicated by the measurement data of the positioning device 1125 and rotating it based on the attitude angle. The map data generated by the map generation unit 213 is recorded in the main memory 230.

[0023] The target determination unit 212 determines a target pose (target position) of the work vehicle 100. For example, when no soil is held on the work machine 120, the target determination unit 212 determines the position of a pre-specified loading site and the attitude angle relative to the loading site as the target pose. Also, when soil is held on the work machine 120, the target determination unit 212 determines the position of a pre-specified dumping site and the attitude angle relative to the dumping site as the target pose. At this time, for example, the target determination unit 212 may correct the positions of the loading site and the dumping site based on map data.

[0024] The route search unit 214 searches for a route connecting the current pose (initial position) of the work vehicle 100 acquired by the acquisition unit 211 to the target pose determined by the target determination unit 212. A "route" is represented by an ordered set of poses. In other words, the route searched for by the route search unit 214 is a sequence of the positions and attitude angles of the work vehicle 100. The route search unit 214 searches for a route using a search algorithm (first search algorithm) having completeness such as dynamic programming. Examples of algorithms related to dynamic programming include Dijkstra's algorithm and Hybrid A*. When Hybrid A* is used, the heuristic cost can be obtained by the Reeds Shepp method or the Laplace Potential method. The Reeds Shepp method is a method for obtaining the shortest route in a nonholonomic system when the continuity of obstacles and curvature is ignored. The Laplace Potential method is a method for obtaining the shortest route when nonholonomic constraints are ignored. The constraint conditions (first constraint conditions) for the search by the route search unit 214 are that the vehicle does not come into contact with an obstacle represented by the map data generated by the map generation unit 213, that the pose at the end point matches the target pose, and that the steering angle is within a predetermined movable range. The evaluation function for the search by the route search unit 214 is a function whose value decreases as the travel distance becomes shorter. The model used for the search by the route search unit 214 is expressed by equation (1).

[0025]

number

[0026] In formula (1), x[k] indicates the x coordinate (e.g., latitude) at time k. y[k] indicates the y coordinate (e.g., longitude) at time k. φ[k] indicates the attitude angle at time k. δ[k] indicates the steering angle at time k. v[k] indicates the speed at time k. k+1 indicates the time unit time after time k. Δt indicates the time step size. L indicates the length from the articulation center to the front wheel WF. In other words, the mathematical model used by the path search unit 214 is a model of three elements, the x coordinate, y coordinate, and attitude angle of the work vehicle 100, and does not include steering angle and speed as elements. While a search algorithm related to dynamic programming has completeness, the amount of calculation increases exponentially with the number of dimensions of the model and input. Therefore, the path search unit 214 according to the first embodiment can reduce the amount of calculation by reducing the number of elements of the model.

[0027] In addition, in equation (1), L / tan(δ[k]) represents the turning radius of the work vehicle 100. That is, in equation (1), the increment of the x coordinate is expressed as the difference between the sine of the attitude angle φ[k] and the sine of the attitude angle φ[k+1] multiplied by the turning radius of the work vehicle 100 at time k. Similarly, in equation (1), the increment of the y coordinate is expressed as the difference between the cosine of the attitude angle φ[k] and the cosine of the attitude angle φ[k+1] multiplied by the turning radius of the work vehicle 100 at time k.

[0028] The route search unit 214 acquires a sequence of poses of the work vehicle 100 as a route by searching based on the model shown in formula (1). Note that although the model shown in formula (1) uses time as an index, the constraint condition generation unit 215 does not treat the route index as representing time. For example, a pose x[i] on the route does not indicate a pose at time i, but simply indicates the i-th pose. Meanwhile, the time series of steering angle and speed obtained by the route search unit 214 is used as the initial value of the trajectory search unit 216 described later. Note that the model used by the route search unit 214 is a pose model and does not include steering angle and speed as elements, so the searched series of steering angle and acceleration lack continuity. In other words, the route searched by the route search unit 214 may have discontinuous curvature or may include unnecessary switchbacks.

[0029] The constraint condition generating unit 215 generates a constraint condition (second constraint condition) to be used in the search by the trajectory searching unit 216 based on the route obtained by the route searching unit 214. The constraint condition to be used in the search by the trajectory searching unit 216 is that the position of the work vehicle 100 is located within an allowable range series including the route obtained by the route searching unit 214. The constraint condition is expressed by a penalty function in which a penalty value increases according to the deviation distance from the allowable range series, and the penalty value becomes zero when the work vehicle 100 is located within the allowable range series. FIG. 4 is a diagram showing an example of an allowable range series according to the first embodiment. The allowable range series is a series of allowable ranges represented by a rectangle. The allowable range is represented by the coordinates of the four vertices of the rectangle. The constraint condition generating unit 215 determines the allowable range series based on the map data generated by the map generating unit 213 and the route generated by the route searching unit 214. By converting the route into an allowable range series by the constraint condition generating unit 215, discontinuities in the curvature of the route and unnecessary switchbacks can be ignored.

[0030] The constraint condition generating unit 215 generates an acceptable range sequence in the following procedure: Fig. 5 is a flowchart showing a method for generating an acceptable range sequence according to the first embodiment. The constraint condition generating unit 215 selects poses one by one from the sequence of poses related to the route (step S51), and executes the following processing of steps S52 to S56 for the selected pose. Based on the pose selected in step S51, the constraint condition generating unit 215 places a first rectangle R1 representing the outline of the work vehicle 100 on the map data recorded in the main memory 230 (step S52). The constraint condition generating unit 215 calculates the distance (Euclidean distance) from each point to the nearest obstacle based on the scores of multiple lattices that overlap with multiple points set on the four sides of the first rectangle R1 in the map data (step S53). FIG. 6 is a diagram showing the relationship between the first rectangle R1 and the second rectangle R2 according to the first embodiment. When a first rectangle R1 is composed of two long sides E1 and E2 and two short sides E3 and E4, for example, four vertices p1, p3, p6, and p8 of the first rectangle, two points p4 and p5 that divide the long side E1 into thirds, two points p9 and p10 that divide the long side E2 into thirds, a point p2 that divides the short side E3 in half, and a point p7 that divides the short side E4 in half are set. Since the score according to the first embodiment represents the Manhattan distance from the obstacle, the distance d to the obstacle can be calculated using the score n according to the following formula (2).

[0031]

number

[0032] The constraint condition generating unit 215 identifies the shortest one of the distances d found in step S53 for each of the four sides of the first rectangle based on the map data (step S54). The constraint condition generating unit 215 uses the distances d found in step S54 to find the positions of the four vertices P1, P2, P3, and P4 of the second rectangle R2 that represent the allowable range by the following formula (3) (step S55).

[0033]

number

[0034] In other words, the allowable range is represented by a second rectangle R2 that includes the center point p0 of a first rectangle R1 representing the work vehicle 100.

[0035] The constraint generating unit 215 determines the position and azimuth angle of the allowable range in the global coordinate system by arranging a rectangle of the allowable range calculated based on the pose selected in step S51 (step S56). The constraint generating unit 215 can obtain an allowable range series by executing the above-mentioned processes from step S52 to step S56 for all poses on the path.

[0036] The trajectory searching unit 216 shown in FIG. 2 searches for a trajectory connecting the current pose of the work vehicle 100 acquired by the acquisition unit 211 to the target pose determined by the target determination unit 212. A "trajectory" is represented by a series of poses with time as a parameter. In other words, the trajectory searching unit 216 obtains, as a trajectory, the pose of the work vehicle 100 for each time and a series of steering angles and speeds for realizing the pose for each time. The trajectory searching unit 216 searches for a trajectory by a search algorithm (second search algorithm) using a quadratic approximation of an evaluation function. Examples of search algorithms using a quadratic approximation of an evaluation function include differential dynamic programming, iLQR (iterative Linear Quadratic Regulator), and SQP (Sequential Quadratic Programming). The search by the trajectory searching unit 216 is performed so as to minimize the evaluation value obtained by the sum of an evaluation function whose value decreases as the travel time becomes shorter and a penalty function generated by the constraint condition generating unit 215. In the search by the trajectory searching unit 216, the time step width Δt shown in formula (1) is also targeted for optimization.

[0037] In the search algorithm using the quadratic approximation of the evaluation function, the increase in the amount of calculation with respect to the number of dimensions is more gradual than in the search algorithm related to dynamic programming. Therefore, even if the steering angle, speed, and time step size are added to the optimization targets, the amount of calculation does not explode. On the other hand, the search algorithm using the quadratic approximation of the evaluation function does not have completeness, for example, convergence to a local solution may occur if the constraint conditions are highly nonlinear. Therefore, by using constraint conditions that can be expressed by linear functions generated by the constraint condition generation unit 215, the problem can be simplified and convergence to a local solution can be prevented.

[0038] The driving control unit 217 controls the steering actuator 113, the power source 1121, the power transmission device 1122, and the braking device in accordance with the trajectory found by the trajectory searching unit 216. That is, the driving control unit 217 reads out the steering angle and speed related to the current time from the series related to the trajectory, controls the steering actuator 113 to realize the steering angle, and controls the power source 1121, the power transmission device 1122, and the braking device to realize the speed.

[0039] Operation of the control device 140 FIG. 7 is a flowchart showing the automatic driving process of the control device 140 according to the first embodiment. The target determination unit 212 of the control device 140 determines a target pose based on the state of the work vehicle 100 (step S1). The acquisition unit 211 acquires measurement data from the stroke sensor 1131, the rotation sensor 1123, the detection device 1124, and the positioning device 1125 (step S2). The traveling control unit 217 determines whether or not the error between the target pose determined in step S1 and the current pose is equal to or smaller than a predetermined threshold value based on the measurement data from the positioning device 1125 acquired in step S2 (step S3).

[0040] If the error from the target pose is greater than the threshold value (step S3: NO), the map data recorded in the main memory 230 is updated based on the measurement data of the detection device 1124 acquired in step S2 (step S4). The route search unit 214 searches for a route connecting the current pose of the work vehicle 100 to the target pose determined in step S1 (step S5).

[0041] Next, the constraint condition generating unit 215 executes the allowable range series generation process shown in Fig. 5 based on the route searched in step S5 (step S6). The constraint condition generating unit 215 generates a penalty function based on the generated allowable range series (step S7). The trajectory searching unit 216 searches for a trajectory connecting the current pose of the work vehicle 100 to the target pose using the penalty function generated in step S7 (step S8). At this time, the trajectory searching unit 216 uses the series of pose, steering angle, and speed obtained in the route search in step S5 as initial values ​​for the search.

[0042] The traveling control unit 217 controls the steering actuator 113, the power source 1121, the power transmission device 1122, and the braking device in accordance with the trajectory obtained in step S8 (step S9). Then, the control device 140 returns the process to step S2.

[0043] If the error from the target pose is greater than the threshold value in step S3 (step S3: NO), the control device 140 ends the automatic traveling process. After executing the automatic excavation process or the automatic soil release process, the control device 140 executes the automatic traveling process again.

[0044] Actions and Effects The control device 140 according to the first embodiment searches for a path for moving from the initial position of the work vehicle 100 to a target position based on a first constraint condition that represents the position of an obstacle, and searches for a trajectory based on a second constraint condition (penalty function) in which a penalty value increases according to the deviation distance from the path. In this way, the control device 140 searches for a trajectory in two stages by changing the constraint condition. By setting the constraint condition used in the trajectory search to a constraint condition that corresponds to the deviation distance from the path, the nonlinearity of the constraint condition related to the trajectory search can be weakened, and an increase in the amount of calculations can be suppressed. In other words, the control device 140 can obtain a continuous trajectory with a small amount of calculations.

[0045] Moreover, the control device 140 according to the first embodiment searches for a route using a complete search algorithm, and searches for a trajectory using a second search algorithm in which the increase in the amount of calculation relative to the number of elements is gradual. This allows the control device 140 to obtain a route and a trajectory while suppressing the amount of calculation and without converging to a local solution.

[0046] Furthermore, the penalty function used in searching for a trajectory according to the first embodiment is a function in which the penalty value increases according to the deviation distance from a second rectangle that includes each position on the route and extends in the width direction of the work vehicle 100, and the penalty value becomes constant when the work vehicle 100 is located within the second rectangle. In this way, the control device 140 can obtain a highly linear penalty function.

[0047] Moreover, in the first embodiment, a route and trajectory are searched for using a model shown in equation (1). The model shown in equation (1) expresses the position (x[k+1], y[k+1]) of the work vehicle 100 after a unit time based on the current position (x[k], y[k]) of the work vehicle 100, the turning radius calculated from the steering angle δ[k] of the work vehicle 100, the current attitude angle φ[k] of the work vehicle 100, and the attitude angle φ[k+1] of the work vehicle 100 after a unit time.

[0048] Fig. 8 is a diagram showing the relationship between the time step size and the position of the calculation result in the model according to the first embodiment, and Fig. 9 is a diagram showing the relationship between the time step size and the position of the calculation result in the model according to the comparative example. As a vehicle model, the following equation (4) may be used.

[0049]

number

[0050] However, in the model shown in formula (4), the increment of the x coordinate and the increment of the y coordinate are approximated to a linear movement, so that when the value of the time step size Δt changes, an error occurs in the calculation result of the position as shown in FIG. 9. In contrast, in the model shown in formula (1), the increment of the x coordinate and the increment of the y coordinate are approximated to a circular arc by the turning radius and the attitude angle, so that even if the value of the time step size Δt changes as shown in FIG. 8, no error occurs in the calculation result of the position. Therefore, by using the model shown in formula (1), the control device 140 according to the first embodiment can ignore the influence of the time step, so that the path generated by the path search unit 214 can be easily converted into a trajectory. As a result, the control device 140 can start a trajectory search using the trajectory that does not come into contact with the obstacle and reaches the target state as an initial value, which is obtained by the path search unit 214. By setting the trajectory that reaches the target state as an initial value, the path search unit 214 can calculate an appropriate trajectory without converging to a local solution.

[0051] Other Embodiments Although one embodiment has been described in detail above with reference to the drawings, the specific configuration is not limited to the above, and various design changes are possible. That is, in other embodiments, the order of the above-mentioned processes may be changed as appropriate. Also, some of the processes may be executed in parallel. The control device 140 according to the embodiment described above may be configured by a single computer, or the configuration of the control device 140 may be divided and arranged among multiple computers, and the multiple computers may function as the control device 140 by cooperating with each other. In this case, some of the computers constituting the control device 140 may be mounted inside the work vehicle 100, and other computers may be provided outside the work vehicle 100.

[0052] Fig. 10 is a schematic diagram showing the configuration of an autonomous driving system 2 according to another embodiment. The work vehicle 100 according to the first embodiment travels autonomously under the control of a control device 140 provided inside. In contrast, in the autonomous driving system 2 shown in Fig. 9, a control server 800 provided remotely from the work vehicle 100 controls the work vehicle 100. The control server 800 and the control device 140 are connected via wireless communication.

[0053] 2 are provided in the control server 800. The acquisition unit 211 acquires measurement data from each measuring device via wireless communication. The travel control unit 217 transmits control commands to the control device 140 via wireless communication.

[0054] In addition, the control device 140 according to the first embodiment uses dynamic programming to search for a route, but is not limited to this. For example, in other embodiments, a route may be generated geometrically by combining curves such as straight lines, circular arcs, Bezier curves, clothoid curves, and B-spline curves.

[0055] In addition, the control device 140 according to the first embodiment uses a search algorithm that uses a quadratic approximation of an evaluation function to search for a trajectory, but is not limited to this. For example, in other embodiments, the control device 140 may obtain a trajectory by reformulating the problem as a mathematical programming problem.

[0056] Furthermore, the control device 140 according to the first embodiment sets the movable range represented by the second rectangle as a constraint condition for searching the trajectory, but is not limited to this. For example, the constraint condition according to other embodiments may be represented by a line segment that passes through a point on the route and extends only in the width direction of the work vehicle 100.

[0057] Furthermore, the control device 140 according to the first embodiment determines the second rectangle R2 based on equation (3), but is not limited to this. For example, the control device 140 according to other embodiments may determine the second rectangle R2 in the procedure shown below. The control device 140 may determine the positions of the four vertices R1, R2, R3, and R4 of the second rectangle R2 using the following equation (5) using the azimuth angle α in the direction in which the obstacle closest to the work vehicle 100 exists and the distance d to the obstacle. In equation (5), L indicates the length from the articulation center to the front wheel WF, and l indicates the tread width.

[0058]

number

[0059] Further, the work vehicle 100 according to the first embodiment is a wheel loader that is an articulated vehicle, but is not limited thereto. For example, the vehicle according to other embodiments may be another vehicle such as a dump truck, and may not be an articulated vehicle. In the case of a vehicle that is not an articulated vehicle, for example, the steering angle is the angle between the steered wheels and the vehicle body. Further, the work vehicle 100 according to other embodiments may be a tracked vehicle, or may be a vehicle having wheels that cannot be turned, such as a skid steer loader. In this case, for example, the turning radius can be calculated from the speed difference between the left and right tracks or wheels. [Explanation of symbols]

[0060] 100...work vehicle 110...body 111...front body 112...rear body 1121...power source 1122...power transmission device 1123...rotation sensor 1124...detection device 1125...positioning device 113...steering actuator 1131...stroke sensor 120...work machine 140...control device 210...processor 211...acquisition unit 212...target determination unit 213...map generation unit 214...route search unit 215...constraint condition generation unit 216...trajectory search unit 217...travel control unit 230...main memory 250...storage 270...interface WF...front wheel WR...rear wheel 800...control server 2...automatic driving system

Claims

1. A system for searching for a trajectory for an automatic driving of a vehicle, comprising: A processor is provided. The processor, a route search unit that searches for a sequence of elements including a position and an attitude of the vehicle, which is a route for moving from an initial position of the vehicle to a target position, based on a first constraint condition including that the vehicle does not come into contact with an obstacle included in map data representing the surroundings of the vehicle; a constraint condition generating unit that generates a second constraint condition in which a penalty value increases according to a deviation distance from an area that includes the positions of each element of the route and extends in a width direction of the vehicle, and the penalty value becomes constant when the vehicle is located within the area; a trajectory search unit that searches for a sequence of elements including a position, an attitude, a speed, and a steering angle of the vehicle, which is a trajectory for moving from an initial position of the vehicle to a target position, based on the second constraint condition; A system comprising:

2. The route search unit searches for the route using a first search algorithm having completeness; The trajectory search unit searches for the trajectory using a second search algorithm in which the increase in the amount of calculation with respect to the number of elements is more gradual than that of the first search algorithm. The system of claim 1 .

3. The constraint condition generating unit generates the second constraint condition based on the position of the obstacle and the position and the orientation of each element of the path. The system of claim 1 .

4. The route search unit searches for the route based on a model representing the vehicle; The model represents a position of the vehicle after a unit time based on a current position of the vehicle, a turning radius calculated from a steering angle of the vehicle, a current attitude angle of the vehicle, and an attitude angle of the vehicle after a unit time. A system according to any one of claims 1 to 3.

5. A method for searching for a trajectory for automatic driving of a vehicle, comprising: searching for a sequence of elements including the position and the attitude of the vehicle, which is a route for moving the vehicle from an initial position to a target position, based on a first constraint condition including that the vehicle does not come into contact with an obstacle included in map data representing the surroundings of the vehicle; generating a second constraint condition in which a penalty value increases according to a deviation distance from an area that includes the positions of each element of the route and extends in a width direction of the vehicle, and the penalty value becomes constant when the vehicle is located within the area; searching for a sequence of elements including a position, an attitude, a speed, and a steering angle of the vehicle, the sequence being a trajectory for moving the vehicle from an initial position to a target position, based on the second constraint condition; A method for providing the above.

6. The step of searching for a route includes searching for the route using a first search algorithm having completeness; The step of searching for the trajectory searches for the trajectory using a second search algorithm in which the increase in the amount of calculation with respect to the number of elements is more gradual than that of the first search algorithm. The method according to claim 5.

7. The step of generating the second constraint condition includes generating the second constraint condition based on a position of the obstacle and the positions and the orientations of each element of the path. The method according to claim 5.

8. The step of searching for a route includes searching for the route based on a model representing the vehicle; The model represents a position of the vehicle after a unit time based on a current position of the vehicle, a turning radius calculated from a steering angle of the vehicle, a current attitude angle of the vehicle, and an attitude angle of the vehicle after a unit time. A method according to any one of claims 5 to 7.

9. The car body and A traveling device for traveling the vehicle body; A work vehicle comprising: The processor, a route search unit that searches for a sequence having elements of the position and attitude of the work vehicle, which is a route for moving from an initial position of the work vehicle to a target position, based on a first constraint condition including that the work vehicle does not come into contact with an obstacle included in map data representing the surroundings of the work vehicle; a constraint condition generating unit that generates a second constraint condition in which a penalty value increases according to a deviation distance from an area that includes the positions of each element of the route and extends in a width direction of the work vehicle, and the penalty value becomes constant when the work vehicle is located within the area; a trajectory search unit that searches for a series of elements including a position, an attitude, a speed, and a steering angle of the work vehicle, which is a trajectory for moving the work vehicle from an initial position to a target position, based on the second constraint condition; A travel control unit that controls the travel device according to the trajectory; A work vehicle equipped with:

Images