System, method, and work vehicle

Abstract

The route search section searches for a route, i.e., a series of the position and posture of the vehicle, for moving from an initial position of the vehicle to a target position, based on a first constraint condition indicating the position of an obstacle. The constraint condition generation section generates a second constraint condition in which a penalty value increases in accordance with a distance of deviation from the route. The track search section searches for a track, i.e., a series of the position, posture, speed, and steering angle of the vehicle, for moving from the initial position of the vehicle to the target position, based on the second constraint condition.

Description

Technical Field

[0001] This disclosure relates to systems, methods, and operating vehicles.

[0002] This application claims priority to Japanese Patent Application No. 2021-061381, filed on March 31, 2021, the contents of which are incorporated herein by reference. Background Technology

[0003] In work sites such as quarrying sites and mines, autonomous driving systems for work vehicles are sometimes implemented. These systems calculate a path for the work vehicle to reach its destination and then guide the vehicle along that path. Patent Document 1 discloses the technology involved in a method for generating the driving path of an unmanned vehicle.

[0004] Prior art literature

[0005] Patent documents

[0006] Patent Document 1: Japanese Patent Application Publication No. 2010-073080 Summary of the Invention

[0007] The problem that the invention aims to solve

[0008] The calculation of tracks for autonomous vehicle navigation is an optimization problem. Furthermore, obstacles present at the work site are given as constraints in the track calculation. However, the mathematical model used for track search is represented by multi-dimensional vectors containing the vehicle's position, attitude, steering angle, velocity, and acceleration. Additionally, the positions and shapes of obstacles may not be simple, increasing the nonlinearity of the constraints. Therefore, the computational cost increases, or convergence to local solutions may occur, making it difficult to calculate the vehicle's trajectory online. On the other hand, if the model's dimensionality is reduced to decrease computational cost, the actions involved in the reduced dimensions may be discontinuous in the resulting track.

[0009] The purpose of this disclosure is to provide an orbital search apparatus and method that can obtain continuous orbits with less computation.

[0010] Solution for solving the problem

[0011] The first aspect of the present invention is a system for searching a track for automatic vehicle movement, and includes a processor. The processor includes: a path search unit that searches for a path for moving the vehicle from an initial position to a target position, i.e., a series of paths including the vehicle's position and orientation, based on a first constraint condition indicating the position of obstacles; a constraint condition generation unit that generates a second constraint condition whose penalty value increases based on the deviation distance from the path; and a track search unit that searches for a track for moving the vehicle from its initial position to the target position, i.e., a series of paths including the vehicle's position, orientation, speed, and steering angle, based on the second constraint condition.

[0012] A second aspect of the invention is a method for searching a track for autonomous driving of a vehicle. This method includes the following processes: A first process, based on a first constraint representing the position of obstacles, searches for a path for moving the vehicle from its initial position to a target position, i.e., a series of paths including the vehicle's position and orientation. A second process generates a second constraint that increases a penalty value based on the deviation distance from the path. A third process, based on the second constraint, searches for a track for moving the vehicle from its initial position to the target position, i.e., a series of paths including the vehicle's position, orientation, speed, and steering angle.

[0013] A third aspect of the present invention is a work vehicle, comprising: a vehicle body; a travel device for moving the vehicle body; and a processor. The processor comprises: a path search unit that searches for a path for moving the work vehicle from its initial position to a target position, i.e., a series of paths including the vehicle's position and orientation, based on a first constraint condition indicating the position of an obstacle; a constraint condition generation unit that generates a second constraint condition whose penalty value increases based on the deviation distance from the path; a track search unit that searches for a track for moving the work vehicle from its initial position to the target position, i.e., a series of paths including the work vehicle's position, orientation, speed, and steering angle, based on the second constraint condition; and a travel control unit that controls the travel device according to the track.

[0014] Invention Effects

[0015] According to at least one of the above methods, the track search device can obtain continuous tracks with less computation. Attached Figure Description

[0016] Figure 1 This is a side view of the work vehicle according to the first embodiment.

[0017] Figure 2 This is a schematic block diagram showing the structure of the control device according to the first embodiment.

[0018] Figure 3 This is a diagram illustrating an example of the mapping data in the first embodiment.

[0019] Figure 4 This is a diagram illustrating a series of examples of the permissible ranges of the first embodiment.

[0020] Figure 5 This is a flowchart illustrating a method for generating a series of permissible ranges according to the first embodiment.

[0021] Figure 6 This is a diagram showing the relationship between the first rectangle and the second rectangle in the first embodiment.

[0022] Figure 7 This is a flowchart illustrating the automatic driving process of the control device according to the first embodiment.

[0023] Figure 8 This is a graph showing the relationship between the width of the time scale and the position of the calculation result in the model of the first embodiment.

[0024] Figure 9 This is a graph showing the relationship between the width of the time scale and the position of the calculation results in the comparative example model.

[0025] Figure 10 This is a schematic diagram illustrating the structure of an autonomous driving system according to other embodiments. Detailed Implementation

[0026] <First Implementation Method>

[0027] Structure of Operating Vehicles

[0028] Hereinafter, the embodiments will be described in detail with reference to the accompanying drawings.

[0029] Figure 1 This is a side view of the work vehicle according to the first embodiment.

[0030] The work vehicle 100 in the first embodiment is a wheeled loader. The work vehicle 100 operates through automatic driving. That is, the work vehicle 100 is an unmanned vehicle. The work vehicle 100 includes a body 110, a working device 120, and a control device 140.

[0031] The vehicle body 110 includes a front body 111, a rear body 112, and a steering actuator 113. The front body 111 and the rear body 112 are mounted so as to be rotatable about a steering axis extending vertically along the vehicle body 110. A pair of front wheels WF are provided on the lower part of the front body 111. A pair of rear wheels WR are provided on the lower part of the rear body 112.

[0032] The rear vehicle 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 transmits the driving force input to the input shaft at a higher speed and outputs it from the output shaft. The output shaft of the power transmission device 1122 is connected to the front wheel WF via a front axle (not shown) and to the rear wheel WR via a rear axle (not shown). The front axle supports the front vehicle body 111 for movement. The rear axle supports the rear vehicle body 112 for movement. Braking devices (not shown) are provided on the front and rear axles. A rotation sensor 1123 is provided on the power transmission device 1122. The rotation sensor 1123 measures the rotational speed of the output shaft of the power transmission device 1122. The control device 140 can determine 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 vehicle body 112. The detection device 1124 detects the three-dimensional positions of objects existing around the work vehicle 100. Examples of the detection device 1124 include LiDAR (Light Detection and Ranging), stereo cameras, and UWB (Ultra Wide Band) ranging devices. The detection device 1124 is, for example, positioned with the detection direction facing forward. The positioning device 1125 measures the position and attitude (attitude angle) of the work vehicle 100 in global coordinates using GNSS (Global Navigation Satellite System) or similar methods. The position is represented, for example, by latitude and longitude. The attitude is represented, for example, by the azimuth angle of the rear vehicle body 112 with north as the reference orientation. Hereinafter, the position and attitude of the work vehicle 100 will also be referred to as the attitude of the work vehicle 100.

[0033] The steering actuator 113 is, for example, a hydraulic cylinder. The base of the steering actuator 113 is mounted to the rear vehicle body 112, and the front end is mounted to the front vehicle body 111. The steering actuator 113 extends and retracts via hydraulic fluid, thereby defining the angle between the front vehicle body 111 and the rear vehicle body 112. That is, the extension and retraction of the steering actuator 113 defines the steering angle of the front wheel (WF). A stroke sensor 1131 is provided on the steering actuator 113 to measure the stroke length of the steering actuator 113. The control device 140 can determine the steering angle of the work vehicle 100 based on the measurement data from the stroke sensor 1131.

[0034] The power transmission device 1122, the braking device, and the steering actuator 113 are driving devices for moving the vehicle body 110.

[0035] The working device 120 is used for excavating and transporting objects such as sand and soil. The working device 120 is located at the front of the vehicle body 110.

[0036] The control device 140 drives the work vehicle 100 based on the detection data from the detection device 1124.

[0037] Structure of Control Device 140

[0038] Figure 2 This is a schematic block diagram showing the structure of the control device 140 according to the first embodiment.

[0039] The control device 140 is a computer equipped with a processor 210, a main memory 230, a storage device 250, and an interface 270.

[0040] Storage 250 is a non-temporary tangible storage medium. Examples of storage 250 include magnetic disks, optical disks, optical discs, semiconductor memories, etc. Storage 250 can be an internal medium directly connected to the bus of control device 140, or an external medium connected to control device 140 via interface 270 or a communication line. Storage 250 stores programs used to control the work vehicle 100.

[0041] The program can be a part of the program used to implement the functions of the control device 140. For example, the program can function by combining with other programs already stored in the memory 250 or with other programs installed on other devices. It should be noted that in other embodiments, the control device 140 may also have a custom LSI (Large Scale Integrated Circuit) such as a PLD (Programmable Logic Device) in addition to or replacing the above-described structure. 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, part or all of the functions implemented by the processor can also be implemented by the integrated circuit.

[0042] Software Structure

[0043] The processor 210, by executing a program, includes an acquisition unit 211, a mapping generation unit 213, a target determination unit 212, a path search unit 214, a constraint condition generation unit 215, a track search unit 216, and a travel control unit 217. The control device 140 is an example of a track search device.

[0044] 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. From this, the acquisition unit 211 determines 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.

[0045] The mapping generation unit 213 uses the measurement data from the detection device 1124 and the measurement data from the positioning device 1125 acquired by the acquisition unit 211 to generate mapping data representing the surroundings of the work vehicle 100. The mapping generation unit 213 generates the mapping data, for example, using SLAM (Simultaneous Localization and Mapping) technology. The mapping data is represented, for example, by an occupied grid map involving a global coordinate system. Figure 3 This is a diagram illustrating an example of the mapping data in the first embodiment. Additionally, each cell in the mapping data is labeled with a score representing the distance to an obstacle. The cell indicating the location of an obstacle has a score of zero, and the score increases with distance from the obstacle. The score is represented, for example, by the Manhattan distance to the nearest obstacle.

[0046] The detection device 1124 is fixed to the rear vehicle body 112. Therefore, the mapping generation unit 213 moves the position of the obstacle represented by the measurement data of the positioning device 1125 in parallel based on the position represented by the measurement data of the positioning device 1125, and rotates it based on the attitude angle, thereby determining the position of the obstacle in the global coordinate system. The mapping data generated by the mapping generation unit 213 is recorded in the main memory 230.

[0047] The target determination unit 212 determines the target attitude (target position) of the working vehicle 100. For example, if the working device 120 is not holding sand, the target determination unit 212 determines the pre-specified loading site position and the attitude angle relative to the loading site as the target attitude. Conversely, if the working device 120 is holding sand, the target determination unit 212 determines the pre-specified unloading site position and the attitude angle relative to the unloading site as the target attitude. In this case, the target determination unit 212 can also, for example, correct the positions of the loading site and the unloading site based on mapping data.

[0048] The path search unit 214 searches for a path connecting the current attitude (initial position) of the work vehicle 100 obtained by the acquisition unit 211 to the target attitude determined by the target determination unit 212. A "path" is represented by a sequence of attitudes. That is, the path found by the path search unit 214 is a series of positions and attitude angles of the work vehicle 100. The path search unit 214 searches for the path using a complete search algorithm (first search algorithm) such as dynamic programming. Examples of dynamic programming algorithms include Dijkstra's method and Hybrid A*. When using Hybrid A*, the heuristic cost can be obtained using the Reeds Shepp method or the Laplace Potential method. The Reeds Shepp method is a method for finding the shortest path of a nonholonomic system that ignores the continuity of obstacles or curvature. The Laplace Potential method is a method for finding the shortest path that ignores nonholonomic constraints.

[0049] The constraints (first constraints) of the search performed by the path search unit 214 are: not contacting the obstacle represented by the mapping data generated by the mapping generation unit 213, the attitude at the endpoint being consistent with the target attitude, and the steering angle being within the specified movable range. The evaluation function of the search performed by the path search unit 214 is a function whose value decreases as the travel distance decreases. The model for the search performed by the path search unit 214 is represented by equation (1).

[0050] [Formula 1]

[0051]

[0052] In equation (1), x[k] represents the x-coordinate (e.g., latitude) at time k. y[k] represents the y-coordinate (e.g., longitude) at time k. φ[k] represents the attitude angle at time k. δ[k] represents the steering angle at time k. v[k] represents the speed at time k. k+1 represents the time after a unit of time at time k. Δt represents the width of the time scale. L represents the length from the articulation center to the front wheel WF. That is, the mathematical model used by the path search unit 214 is a model of the three elements of the working vehicle 100: x-coordinate, y-coordinate, and attitude angle, excluding steering angle and speed.

[0053] The search algorithm involved in dynamic programming has completeness, but on the other hand, the computational cost increases exponentially with respect to the dimensionality of the model and the input. Therefore, the path search unit 214 in the first embodiment can suppress the computational cost by reducing the number of elements in the model.

[0054] It should be noted that in equation (1), L / tan(δ[k]) represents the turning radius of the working vehicle 100. That is, in equation (1), the increment of the x-coordinate is the value obtained by multiplying the difference between the sine of the attitude angle φ[k] and the sine of the attitude angle φ[k+1] by the turning radius of the working vehicle 100 at time k. Similarly, in equation (1), the increment of the y-coordinate is the value obtained by multiplying the difference between the cosine of the attitude angle φ[k] and the cosine of the attitude angle φ[k+1] by the turning radius of the working vehicle 100 at time k.

[0055] The path search unit 214 obtains a series of attitudes of the work vehicle 100 as a path by searching based on the model shown in Equation (1). It should be noted that the model shown in Equation (1) uses time as an index, but in the constraint generation unit 215, the path index is not treated as an indicator of time. For example, the attitude x[i] in the path does not represent the attitude at time i, but only the i-th attitude. On the other hand, the time series of steering angle and speed obtained by the path search unit 214 is used as the initial value of the track search unit 216 described later. It should be noted that the model used by the path search unit 214 is an attitude model, which does not include steering angle and speed in the elements, so the series of steering angle and acceleration searched lacks continuity. That is, the path searched by the path search unit 214 sometimes has discontinuous curvature or contains unnecessary turns.

[0056] The constraint generation unit 215 generates constraints (second constraints) for the search performed using the track search unit 216 based on the path obtained by the path search unit 214. The constraint used in the search performed by the track search unit 216 is that the position of the work vehicle 100 is within a permissible range series encompassing the path obtained by the path search unit 214. The constraint is represented by a penalty function, which increases the penalty value based on the deviation distance from the permissible range series, and the penalty value becomes zero when the work vehicle 100 is within the permissible range series. Figure 4 This diagram illustrates an example of the permissible range series according to the first embodiment. The permissible range series is a series of permissible ranges represented by rectangles. The permissible range is represented by the coordinates of the four vertices of the rectangles. The constraint condition generation unit 215 determines the permissible range series based on the mapping data generated by the mapping generation unit 213 and the path generated by the path search unit 214. By converting the path into a permissible range series by the constraint condition generation unit 215, discontinuities in the curvature of the path and unnecessary backtracking can be ignored.

[0057] The constraint generation unit 215 generates a series of permissible ranges according to the following steps. Figure 5 This is a flowchart illustrating a method for generating a series of permissible ranges according to the first embodiment.

[0058] The constraint generation unit 215 selects a posture one by one from the series of postures involved in the path (step S51), and performs the following steps S52 to S56 for the selected posture. Based on the posture selected in step S51, the constraint generation unit 215 configures a first rectangle R1 representing the shape of the work vehicle 100 on the mapping data recorded in the main memory 230 (step S52). Based on the scores of multiple grids in the mapping data that overlap with multiple points set on the four sides of the first rectangle R1, the constraint generation unit 215 calculates the distance (Euclidean distance) from each point to the nearest obstacle (step S53). Figure 6 This is a diagram illustrating the relationship between the first rectangle R1 and the second rectangle R2 in the first embodiment. In the case where the first rectangle R1 is composed of two long sides E1 and E2 and two short sides E3 and E4, for example, the four vertices of the first rectangle are set as p1, p3, p6, p8; the two points that divide the long side E1 into three equal parts are p4 and p5; the two points that divide the long side E2 into three equal parts are p9 and p10; the point that divides the short side E3 in half is p2; and the point that divides the short side E4 in half is p7. The score in the first embodiment represents the Manhattan distance from the obstacle; therefore, the distance d from the obstacle is calculated using the score n via the following equation (2).

[0059] [Equation 2]

[0060]

[0061] Based on the mapping data, the constraint generation unit 215 determines the shortest distance among the distances d calculated in step S53 for each of the four sides of the first rectangle (step S54). Using the distances d calculated in step S54, the constraint generation unit 215 calculates the positions of the four vertices P1, P2, P3, and P4 of the second rectangle R2, which represents the allowable range, using the following equation (3) (step S55).

[0062] [Formula 3]

[0063]

[0064] That is, the permissible range is represented by a second rectangle R2 containing the center point p0 of the first rectangle R1 representing the working vehicle 100.

[0065] The constraint generation unit 215 determines the position and azimuth of the allowable range in the global coordinate system by configuring a rectangle of the allowable range calculated based on the attitude selected in step S51 (step S56).

[0066] The constraint generation unit 215 can obtain a series of allowable ranges by performing the processing from steps S52 to S56 above for all the poses of the path.

[0067] Figure 2 The track search unit 216 shown searches for a track connecting the current attitude of the work vehicle 100 obtained by the acquisition unit 211 to the target attitude determined by the target determination unit 212. The "track" is represented by a series of attitudes with time as a parameter. That is, the track search unit 216 calculates a series of attitudes for achieving the attitude of the work vehicle 100 at each moment, as well as the steering angle and speed of that attitude at each moment, as the track. The track search unit 216 searches for the track using a search algorithm (second search algorithm) that uses a quadratic approximation of the evaluation function. Examples of search algorithms using a quadratic approximation of the evaluation function include differential dynamic programming, iLQR (iterative Linear Quadratic Regulator), and SQP (Sequential Quadratic Programming).

[0068] The search performed using the track search unit 216 is executed in a manner that minimizes the sum of the evaluation function obtained by the shortest travel time and the penalty function generated by the constraint generation unit 215. In the search performed using the track search unit 216, the time scale width Δt shown in equation (1) is also optimized.

[0069] The search algorithm using a quadratic approximation with an evaluation function has a slower computational cost relative to the increase in dimensionality compared to search algorithms involving dynamic programming. Therefore, even when factors such as the turning angle, velocity, and time scale width are added to the optimization object, there is no explosive increase in computational cost. On the other hand, the search algorithm using a quadratic approximation with an evaluation function lacks the integrity that sometimes leads to convergence to local solutions if the constraints are highly nonlinear. Therefore, by using constraints that can be represented by linear functions generated by the constraint generation unit 215, the problem can be simplified, and convergence to local solutions can be prevented.

[0070] The driving control unit 217 controls the steering actuator 113, power source 1121, power transmission device 1122, and braking device according to the track obtained by the track search unit 216. That is, the driving control unit 217 reads the steering angle and speed involved at the current moment from the series related to the track, controls the steering actuator 113 to achieve the steering angle, and controls the power source 1121, power transmission device 1122, and braking device to achieve the speed.

[0071] Operation of Control Device 140

[0072] Figure 7 This is a flowchart illustrating the automatic driving process of the control device 140 according to the first embodiment.

[0073] The target determination unit 212 of the control device 140 determines the target attitude based on the state of the work vehicle 100 (step S1). The acquisition unit 211 acquires the measurement data of the stroke sensor 1131, the rotation sensor 1123, the detection device 1124, and the positioning device 1125 (step S2). Based on the measurement data of the positioning device 1125 acquired in step S2, the travel control unit 217 determines whether the error between the target attitude determined in step S1 and the current attitude is below a predetermined threshold (step S3).

[0074] If the error with respect to the target posture is greater than the threshold (step S3: No), the mapping data recorded in the main memory 230 is updated based on the measurement data of the detection device 1124 obtained in step S2 (step S4). The path search unit 214 searches for a path from the current posture of the work vehicle 100 to the target posture determined in step S1 (step S5).

[0075] Next, the constraint generation unit 215 executes the process based on the path searched in step S5. Figure 5 The permissible range series generation process is shown in step S6. The constraint generation unit 215 generates a penalty function based on the generated permissible range series (step S7). The track search unit 216 uses the penalty function generated in step S7 to search for a track connecting the current attitude of the work vehicle 100 to the target attitude (step S8). At this time, the track search unit 216 uses the series of attitude, steering angle, and speed obtained in the path search in step S5 as the initial values ​​for the search.

[0076] The driving control unit 217 controls the steering actuator 113, power source 1121, power transmission device 1122, and braking device according to the trajectory determined in step S8 (step S9). Then, the control device 140 returns the process to step S2.

[0077] If the error with the target posture is greater than the threshold in step S3 (step S3: No), the control device 140 terminates the automatic driving process. After performing automatic excavation and automatic soil unloading, the control device 140 performs the automatic driving process again.

[0078] Function / Effect

[0079] The control device 140 of the first embodiment searches for a path for moving from the initial position of the work vehicle 100 to the target position based on a first constraint indicating the position of an obstacle, and searches for a track based on a second constraint (penalty function) where the penalty value increases according to the deviation distance from the path. In this way, the control device 140 searches for the track in two stages by changing the constraints. By setting the constraints used for track searching to correspond to the deviation distance from the path, the nonlinearity of the constraints involved in track searching can be reduced, thus suppressing the increase in computational load. That is, the control device 140 can obtain a continuous track with less computation.

[0080] Furthermore, the control device 140 of the first embodiment uses a search algorithm with completeness to search for paths and a second search algorithm with a slower increase in computational cost relative to the number of features to search for trajectories. Thus, the control device 140 can obtain paths and trajectories while suppressing computational cost and avoiding convergence to local solutions.

[0081] Furthermore, the penalty function for track searching in the first embodiment is a function in which the penalty value increases based on the deviation distance from the second rectangle that includes the positions involved in the path and extends in the width direction of the work vehicle 100, and the penalty value remains constant when the work vehicle 100 is located within the second rectangle. In this way, the control device 140 can obtain a penalty function with high linearity.

[0082] In addition, in the first embodiment, the model shown in Equation (1) is used to search for the path and track. The model shown in Equation (1) represents 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 based on the turning angle δ[k] of the work vehicle 100, the current posture angle φ[k] of the work vehicle 100, and the posture angle φ[k+1] of the work vehicle 100 after a unit time.

[0083] Figure 8 This is a graph showing the relationship between the width of the time scale and the position of the calculation result in the model of the first embodiment. Figure 9 This is a graph showing the relationship between the width of the time scale and the position of the calculation results in the comparative example model.

[0084] As a model of a vehicle, the model shown in Equation (4) is sometimes used.

[0085] [Formula 4]

[0086]

[0087] However, the model shown in equation (4) approximates the increments of the x-coordinate and y-coordinate as linear movements. Therefore, if the value of the time scale width Δt changes, then... Figure 9 As shown, the position calculation results produce errors. In contrast, the model shown in equation (1) approximates the increments of the x-coordinate and y-coordinate as circular arcs by using the radius of rotation and attitude angle, thus even if... Figure 8 As shown, the change in the value of the time scale width Δt does not cause errors in the position calculation result. Therefore, by using the model shown in Equation (1), the control device 140 of the first embodiment can ignore the influence of the time scale, and thus can easily convert the path generated by the path search unit 214 into a track. As a result, the control device 140 can start track search with the track reaching the target state as the initial value without contacting the obstacles obtained by the path search unit 214. By setting the track reaching the target state as the initial value, the path search unit 214 can calculate the appropriate track without converging to a local solution.

[0088] <Other Implementation Methods>

[0089] The above description of one embodiment, with reference to the accompanying drawings, is detailed. However, the specific structure is not limited to the above-described manner, and various design changes are possible. That is, in other embodiments, the order of the above-described processes can be appropriately altered. Furthermore, some processes can be executed in parallel.

[0090] The control device 140 described in the above embodiments can be a device consisting of a single computer, or it can be a device in which the structure of the control device 140 is separately arranged in multiple computers, and the multiple computers cooperate with each other to perform the function of the control device 140. In this case, the computer constituting part of the control device 140 can also be installed inside the work vehicle 100, and other computers can also be installed outside the work vehicle 100.

[0091] Figure 10 This is a schematic diagram showing the structure of an autonomous driving system 2 according to another embodiment. The work vehicle 100 of the first embodiment drives autonomously under the control of an internal control device 140. In contrast, Figure 9 The automated driving system 2 shown controls the work vehicle 100 via a remotely configured control server 800. The control server 800 and the control device 140 are connected wirelessly.

[0092] In this case, Figure 2 Each processing unit shown is located in the control server 800. The acquisition unit 211 acquires the measurement data of each measuring device via wireless communication. In addition, the driving control unit 217 sends control commands to the control device 140 via wireless communication.

[0093] Furthermore, while the control device 140 in the first embodiment uses dynamic programming for path searching, it is not limited to this method. For example, in other embodiments, paths can be geometrically generated by combining curves such as straight lines, arcs, Bézier curves, spiral curves, and B-spline curves.

[0094] Furthermore, the control device 140 in the first embodiment uses a search algorithm that employs a quadratic approximation using an evaluation function in the search for the orbit, but is not limited to this. For example, in other embodiments, the control device 140 may also determine the orbit by re-formulating it as a mathematical programming problem.

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

[0096] Furthermore, the control device 140 of the first embodiment calculates the second rectangle R2 based on equation (3), but is not limited thereto. For example, the control device 140 of other embodiments may also calculate the second rectangle R2 according to the steps shown below. The control device 140 may also use the azimuth angle α of the direction in which the nearest obstacle to the work vehicle 100 exists and the distance d to the obstacle to calculate the positions of the four vertices R1, R2, R3, and R4 of the second rectangle R2 using the following equation (5). In equation (5), L represents the length from the hinge center to the front wheel WF, and l represents the tread width.

[0097] [Formula 5]

[0098]

[0099] Furthermore, while the work vehicle 100 in the first embodiment is a wheeled loader that is an articulated vehicle, it is not limited to this. For example, the vehicle in other embodiments may be a dump truck or other vehicles, or it may not be an articulated vehicle. In the case of a non-articulated vehicle, for example, the steering angle refers to the angle between the steered wheel and the vehicle body. Additionally, the work vehicle 100 in other embodiments may be a tracked vehicle or a skid steer loader or other vehicle with wheels that cannot be steered. In this case, for example, the turning radius can be determined based on the speed difference between the left and right tracks or wheels.

[0100] Industrial availability

[0101] According to at least one of the above methods, the track search device can obtain continuous tracks with less computation.

[0102] Explanation of reference numerals in the attached figures

[0103] 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…Working device; 140…Control device; 210…Processor; 211…Acquisition unit; 212…Target determination unit; 213…Mapping generation unit; 214…Path search unit; 215…Constraint condition generation unit; 216…Track search unit; 217…Driving control unit; 230…Main memory; 250…Storage device; 270…Interface; WF…Front wheel; WR…Rear wheel; 800…Control server; 2…Autopilot system.

Claims

1. A system for searching a track for automated vehicle navigation. in, The system has a processor. The processor has: The path search unit searches for a path for moving from the vehicle's initial position to a target position based on a first constraint indicating the position of an obstacle. This path is a series of paths with the vehicle's position and orientation as elements. The constraint generation unit generates a second constraint condition, which is the following condition: the penalty value increases based on the deviation distance from the location of each element involving the path and the area extending in the width direction of the vehicle, and the penalty value is constant when the vehicle is located within the area; as well as The track search unit searches for a track for moving the vehicle from its initial position to a target position based on the second constraint. This track is a series of elements including the vehicle's position, attitude, speed, and steering angle.

2. The system according to claim 1, wherein, The path search unit uses a first search algorithm with completeness to search the path. The orbit search unit uses a second search algorithm, which has a slower computational load relative to the number of features compared to the first search algorithm, to search for the orbit.

3. The system according to claim 1, wherein, The constraint generation unit generates the second constraint based on the position of the obstacle and the position and posture involved in each element of the path.

4. The system according to claim 1, wherein, The path search unit searches for the path based on a model representing the vehicle. The model represents the vehicle's position after a unit of time based on the vehicle's current position, the turning radius calculated according to the vehicle's steering angle, the vehicle's current posture angle, and the vehicle's posture angle after a unit of time.

5. A method for searching a track, wherein the track is for the automatic driving of a vehicle, The method includes: The steps of searching for a path to move from the vehicle's initial position to a target position based on a first constraint representing the position of an obstacle, wherein the path is a series of elements including the vehicle's position and orientation; The step of generating a second constraint, wherein the second constraint is that the penalty value increases based on the deviation distance from the location of each element containing the path and extending in the width direction of the vehicle, and the penalty value is constant when the vehicle is located within the area; and Based on the second constraint, the step of searching for a trajectory for moving the vehicle from its initial position to a target position, the trajectory being a series of elements including the vehicle's position, attitude, speed, and steering angle.

6. The method according to claim 5, wherein, In the step of searching the path, a first search algorithm with completeness is used to search the path. In the step of searching the orbit, a second search algorithm is used to search the orbit, which has a slower computational cost relative to the number of features compared to the first search algorithm.

7. The method according to claim 5, wherein, The step of generating the second constraint is based on the position of the obstacle and the position and posture of each element of the path.

8. The method according to claim 5, wherein, In the step of searching the path, the path is searched based on the model representing the vehicle. The model represents the vehicle's position after a unit of time based on the vehicle's current position, the turning radius calculated according to the vehicle's steering angle, the vehicle's current posture angle, and the vehicle's posture angle after a unit of time.

9. A working vehicle, comprising: Body; A driving device for moving the vehicle body; and processor, in, The processor has: The path search unit searches for a path for moving from the initial position of the work vehicle to the target position based on a first constraint indicating the position of the obstacle. This path is a series of paths with the position and orientation of the work vehicle as elements. The constraint generation unit generates a second constraint condition, which is as follows: the penalty value increases based on the deviation distance from the location of each element involving the path and the area extending in the width direction of the work vehicle, and the penalty value is constant when the work vehicle is located in the area. The track search unit searches for a track for moving from the initial position of the work vehicle to the target position based on the second constraint condition. The track is a series of elements including the position, attitude, speed and steering angle of the work vehicle. as well as The travel control unit controls the travel device according to the track.

Images