JP7100358B2 - Automatic driving control device, automatic driving control method and program for automatic driving control - Google Patents
Automatic driving control device, automatic driving control method and program for automatic driving control Download PDFInfo
- Publication number
- JP7100358B2 JP7100358B2 JP2018214670A JP2018214670A JP7100358B2 JP 7100358 B2 JP7100358 B2 JP 7100358B2 JP 2018214670 A JP2018214670 A JP 2018214670A JP 2018214670 A JP2018214670 A JP 2018214670A JP 7100358 B2 JP7100358 B2 JP 7100358B2
- Authority
- JP
- Japan
- Prior art keywords
- deviation
- curvature
- temporary
- position deviation
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 34
- 238000004364 calculation method Methods 0.000 claims description 123
- 238000001514 detection method Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 17
- 230000014509 gene expression Effects 0.000 claims description 13
- 238000012545 processing Methods 0.000 description 14
- 238000013500 data storage Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000005314 correlation function Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000007562 laser obscuration time method Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 210000002784 stomach Anatomy 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/10—Path keeping
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
Landscapes
- Engineering & Computer Science (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
- Guiding Agricultural Machines (AREA)
Description
本発明は、自動走行制御装置、自動走行制御方法および自動走行制御用プログラムに関し、特に、あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置に用いて好適なものである。 The present invention relates to an automatic driving control device, an automatic driving control method, and an automatic driving control program, and is particularly suitable for use in an automatic driving control device that automatically drives a vehicle along a preset target route. It is a thing.
従来、車両の現在位置および方位を検出する機能を作業車両に搭載し、逐次検出される現在位置および方位に基づいて、圃場内にあらかじめ設定した目標経路に沿って作業車両を自動的に走行させるようにした自動走行制御装置が知られている(例えば、特許文献1~3参照)。 Conventionally, a function to detect the current position and orientation of the vehicle is installed in the work vehicle, and the work vehicle is automatically driven along a preset target route in the field based on the sequentially detected current position and orientation. Such an automatic traveling control device is known (see, for example, Patent Documents 1 to 3).
特許文献1に記載の自動走行制御装置では、車両の現在位置の目標経路に対するX軸上の位置偏差Xerrおよび目標経路の方向と車両の進行方向との角度偏差θerrを検出して、その両方またはその何れか一方を零に収束させるような操舵を行わせるようにしている。具体的には、フィードバック制御理論の1つである公知の最適レギュレータを利用して、位置偏差Xerrおよび角度偏差θerrを状態量とし、舵角δを制御量として、その位置偏差Xerrおよび角度偏差θerrを零に収束させるように車両の舵角δを制御する。 The automatic travel control device described in Patent Document 1 detects a position deviation X err on the X-axis with respect to the target path of the current position of the vehicle and an angle deviation θ err between the direction of the target path and the traveling direction of the vehicle, and detects the err. Steering is performed so that both or one of them converges to zero. Specifically, using a known optimum regulator, which is one of the feedback control theories, the position deviation X err and the angle deviation θ err are set as the state quantities, and the steering angle δ is used as the control amount, and the position deviation X err and the angle deviation X err and The steering angle δ of the vehicle is controlled so that the angle deviation θ err converges to zero.
特許文献2に記載の自動走行制御装置では、目標経路に対する自車位置との間の位置偏差に基づいて、PID制御演算によって偏差解消の第1操舵値を出力するとともに、位置偏差と、位置偏差が大きくなるに従って減少傾向を示す重み係数を用いて調整された方位偏差とに基づいて、PID制御演算によって偏差解消の第2操舵値を出力する。そして、第1操舵値と第2操舵値とに基づいて、目標経路に沿って走行するための目標操舵値を出力するようにしている。 In the automatic traveling control device described in Patent Document 2, the first steering value for eliminating the deviation is output by the PID control calculation based on the position deviation from the own vehicle position with respect to the target route, and the position deviation and the position deviation are output. The second steering value for eliminating the deviation is output by the PID control calculation based on the orientation deviation adjusted by using the weighting coefficient indicating the decreasing tendency as the value increases. Then, based on the first steering value and the second steering value, the target steering value for traveling along the target route is output.
この特許文献2に記載の制御方法によれば、方位偏差を解消するための第2操舵値を求める際に、位置偏差も考慮される。ここで、位置偏差が大きい場合は、方位偏差をある程度無視して、位置偏差の解消に重きをおいた目標操舵値が出力される。このため、車体がスリップや土塊の乗り上げなどによって大きな位置偏差を生じた場合に、その位置偏差を迅速に解消することができるようになされている。 According to the control method described in Patent Document 2, the position deviation is also taken into consideration when obtaining the second steering value for eliminating the directional deviation. Here, when the position deviation is large, the target steering value that emphasizes the elimination of the position deviation is output by ignoring the directional deviation to some extent. Therefore, when a large positional deviation occurs due to slipping or riding on a mass of soil on the vehicle body, the positional deviation can be quickly eliminated.
特許文献3に記載の自動走行制御装置では、ティーチング経路に対して平行な目標経路を生成し、当該目標経路上を自律的に走行する田植機において、オペレータにより自律運転が指示された場合、次の目標経路へ向けて自動的に旋回し、かつ、旋回動作の終了後に引き続き、次の目標経路上を自律的に走行するように成されている。この特許文献3に記載された制御方法によれば、農用作業車を圃場端で折り返しさせた後に、折り返し前の走行経路と平行に自律直進走行するような往復走行を行う場合に、次行程への遷移時に農用作業車の植付け開始位置の位置決めを容易に行うことが可能となる。 In the automatic driving control device described in Patent Document 3, when a target route parallel to the teaching route is generated and the rice transplanter autonomously travels on the target route is instructed to operate autonomously, the following It is configured to automatically turn toward the target route of the vehicle and to autonomously travel on the next target route after the turning operation is completed. According to the control method described in Patent Document 3, when the agricultural work vehicle is turned back at the field edge and then reciprocated so as to run autonomously in a straight line in parallel with the running path before turning back, the process proceeds to the next step. It becomes possible to easily position the planting start position of the agricultural work vehicle at the time of transition.
農用の作業車両の場合、特許文献3に記載されているように、圃場を往復走行しながら作業をするため、圃場端で180度旋回するような目標経路が設定される。この旋回時においては、作業車両の現在位置と目標経路との位置偏差、および、作業車両の現在方位と目標経路との方位偏差の何れもが大きな状態となる。そのため、直線状の目標経路に沿って直進の自動走行をしている場合に比べて、位置偏差および方位偏差を解消させるまでに要する時間が長くなる。 In the case of a work vehicle for agriculture, as described in Patent Document 3, since the work is performed while traveling back and forth in the field, a target route that turns 180 degrees at the edge of the field is set. At the time of this turn, both the position deviation between the current position of the work vehicle and the target path and the directional deviation between the current direction of the work vehicle and the target path are in a large state. Therefore, the time required to eliminate the position deviation and the azimuth deviation becomes longer than in the case of the automatic traveling straight along the linear target route.
特許文献2には、位置偏差が大きい場合に、方位偏差をある程度無視して、位置偏差の解消に重きをおいた目標操舵値を算出することが記載されている。よって、この特許文献2に記載された制御方法を、位置偏差が大きくなる旋回時に適用すれば、その位置偏差を迅速に解消することは可能と考えられる。しかしながら、このとき方位偏差の解消は軽視されているので、位置偏差および方位偏差の両方を迅速に解消することはできない。 Patent Document 2 describes that when the position deviation is large, the target steering value is calculated with an emphasis on eliminating the position deviation by ignoring the directional deviation to some extent. Therefore, if the control method described in Patent Document 2 is applied at the time of turning when the position deviation becomes large, it is considered possible to quickly eliminate the position deviation. However, since the elimination of the directional deviation is neglected at this time, both the positional deviation and the directional deviation cannot be eliminated quickly.
本発明は、このような問題を解決するために成されたものであり、あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置において、車両を往復走行させる場合における旋回時のように、目標経路に対する車両の位置偏差および方位偏差の何れもが大きな状態となる場合であっても、位置偏差および方位偏差の両方を迅速に解消することができるようにすることを目的とする。 The present invention has been made to solve such a problem, and is a case where the vehicle is reciprocated in an automatic traveling control device that automatically travels the vehicle along a preset target route. To be able to quickly eliminate both the position deviation and the orientation deviation even when both the position deviation and the orientation deviation of the vehicle with respect to the target route become large, such as when turning in. With the goal.
上記した課題を解決するために、本発明は、車両が旋回する際に走行の制御値として用いる経路の曲率として、目標経路に対する自車方位の現在の方位偏差および目標経路に対する自車位置の現在の位置偏差を解消するための目標曲率を算出する位置・方位制御部を備え、当該位置・方位制御部により算出された目標曲率に対して実際の曲率が一致するように車両の走行を制御するようにしている。ここで、位置・方位制御部は、仮位置偏差取得部と目標曲率設定部とを備える。仮位置偏差取得部は、所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って方位偏差が解消されるまで車両を走行させたと仮定した場合に生じる、目標経路または車両の現在位置の何れかからの位置偏差である仮位置偏差を取得する。目標曲率設定部は、仮位置偏差と現在の位置偏差とを比較しながら、仮位置偏差を解消するために必要な目標曲率を設定する。 In order to solve the above-mentioned problems, the present invention presents the current azimuth deviation of the vehicle azimuth with respect to the target route and the current position of the vehicle with respect to the target route as the curvature of the route used as the travel control value when the vehicle turns. It is equipped with a position / orientation control unit that calculates the target curvature to eliminate the position deviation of the vehicle, and controls the running of the vehicle so that the actual curvature matches the target curvature calculated by the position / orientation control unit. I am doing it. Here, the position / orientation control unit includes a temporary position deviation acquisition unit and a target curvature setting unit. The tentative position deviation acquisition unit follows a relational expression including a non-linear element consisting of a triangular function according to a curvature gradient, which is an amount of curvature that changes per predetermined mileage, which is acquired from the orientation deviation and the curvature by a predetermined relational expression. The temporary position deviation, which is the position deviation from either the target route or the current position of the vehicle, which occurs when the vehicle is assumed to have been driven until the orientation deviation is eliminated, is acquired. The target curvature setting unit sets the target curvature required to eliminate the temporary position deviation while comparing the temporary position deviation with the current position deviation.
例えば車両が往復走行する場合における旋回時のように、目標経路に対する車両の位置偏差および方位偏差の何れもが大きな状態では、目標経路に対する車両の偏差が比較的小さくて目標経路と車両との位置関係を線形近似可能な状態とは異なるので、一般的な線形制御を適用すると、位置偏差および方位偏差の両方を解消するまでに多くの時間を要する。これに対し、本発明によれば、曲率が所定走行距離当たりに変化する量である曲率勾配に従って方位偏差が解消されるまで車両を走行させたと仮定した場合に生じる仮位置偏差が取得されて、当該仮位置偏差に基づいて、現在の位置偏差を解消するために必要な目標曲率の設定が行われる。そして、この仮位置偏差を取得する際に、三角関数からなる非線形要素が含まれる関係式に従って方位偏差が解消されるような仮位置偏差が取得されるので、目標経路に対する車両の偏差が大きくて非線形性が無視できない状態となる場合であっても、位置偏差および方位偏差の両方を迅速に解消することが可能な走行制御を行うことができるようになる。 For example, when both the position deviation and the directional deviation of the vehicle with respect to the target route are large, such as when the vehicle makes a round trip, the deviation of the vehicle with respect to the target route is relatively small and the position between the target route and the vehicle is large. Since the relationship is different from the linear approximation possible state, it takes a lot of time to eliminate both the positional deviation and the directional deviation when general linear control is applied. On the other hand, according to the present invention, the temporary position deviation that occurs when the vehicle is driven until the directional deviation is eliminated according to the curvature gradient, which is the amount that the curvature changes per predetermined mileage, is acquired. Based on the temporary position deviation, the target curvature required to eliminate the current position deviation is set. Then, when acquiring this temporary position deviation, the temporary position deviation is acquired so that the orientation deviation is eliminated according to the relational expression including the non-linear element consisting of trigonometric functions, so that the deviation of the vehicle with respect to the target route is large. Even when the non-linearity becomes non-negligible, it becomes possible to perform traveling control capable of quickly eliminating both the position deviation and the orientation deviation.
以下、本発明の一実施形態を図面に基づいて説明する。図1は、本実施形態による自動走行制御装置100の機能構成例を示す図である。図1に示すように、本実施形態の自動走行制御装置100は、その機能構成として、目標経路取得部11、偏差検出部12、位置・方位制御部13および走行制御部14を備えている。位置・方位制御部13は、その具体的な機能構成として、目標曲率設定部13A、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cを備えている。また、本実施形態の自動走行制御装置100には、経路データ記憶部200、位置・方位センサ300および操舵装置400が接続されている。なお、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cは、特許請求の範囲の仮位置偏差取得部に相当する。
Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a diagram showing a functional configuration example of the automatic
上記各機能ブロック11~14は、ハードウェア、DSP(Digital Signal Processor)、ソフトウェアの何れによっても構成することが可能である。例えばソフトウェアによって構成する場合、上記各機能ブロック11~14は、実際にはコンピュータのCPU、RAM、ROMなどを備えて構成され、RAMやROM、ハードディスクまたは半導体メモリ等の記録媒体に記憶されたプログラムが動作することによって実現される。
Each of the above
経路データ記憶部200は、作業車両を自動走行させる際に目標経路として設定される走行経路を示す経路データを記憶する。走行経路の設定は、公知の技術を用いてあらかじめ行われ、その結果得られた経路データが経路データ記憶部200に記憶される。本実施形態では、一例として、作業車両が圃場の一辺と平行に直進し、直進の終点である圃場端において180度旋回をした後、旋回前の走行経路と平行に直進するような往復走行を繰り返す走行経路が設定されているものとする。この場合、経路データは、圃場の一端付近から他端付近までの往路を表す直線リンク(以下、往路直線リンクという)と、圃場の他端付近から一端付近までの復路を表す直線リンク(以下、復路直線リンクという)と、往路直線リンクの終端と復路直線リンクの始端とを結ぶ直線および復路直線リンクの終端と往路直線リンクの始端とを結ぶ直線で、旋回時の目標経路となる直線リンク(以下、旋回直線リンクという)との組み合わせにより構成される。
The route
なお、経路データ記憶部200に記憶される経路データは、上記一例として説明した往復直線リンクと旋回直線リンクとの組み合わせのように、全てのリンクが連続していることを必須とするものではない。すなわち、以下に説明する本実施形態による自動走行制御は、目標とする直線があれば実行することが可能である。例えば、旋回直線リンクが定義されない経路データも本実施形態による自動走行制御の対象となる。
It should be noted that the route data stored in the route
位置・方位センサ300は、作業車両の現在位置および現在方位を検出するセンサである。位置・方位センサ300は、例えば、GNSS(Global Navigation Satellite System:全球測位衛星システム)により構成される。一例として、位置・方位センサ300は、GPS(Global Positioning System)受信機であり、複数のGPS衛星から送られてくる電波をGPSアンテナで受信して、3次元測位処理あるいは2次元測位処理を行って作業車両の絶対位置および絶対方位を計算する。
The position /
また、位置・方位センサ300は、ジャイロセンサ、加速度センサや磁気方位センサを用いた自立航法センサであってもよい。自立航法センサは、所定走行距離毎に1個のパルスを出力して作業車両の移動距離を検出する距離センサと、作業車両の回転角度(移動方位)を検出する振動ジャイロ等の角速度センサ(相対方位センサ)とを含む。自立航法センサは、これらの距離センサおよび角速度センサによって車両の相対位置および相対方位を検出する。なお、距離センサに代えて加速度センサを用いてもよい。
Further, the position /
なお、ここに示した位置・方位センサ300は一例に過ぎず、これに限定されるものではない。作業車両の現在位置および現在方位を検出可能なセンサであれば、何れを用いてもよい。
The position /
操舵装置400は、後述するようにして自動走行制御装置100により算出された目標曲率に基づいて、作業車両の操舵を制御する装置である。
The steering device 400 is a device that controls the steering of the work vehicle based on the target curvature calculated by the automatic
自動走行制御装置100は、圃場内にあらかじめ設定した目標経路に沿って作業車両を自動的に走行させるための制御を行う装置である。以下、この自動走行制御装置100の機能構成について説明する。
The automatic
目標経路取得部11は、経路データ記憶部200に記憶されている経路データに基づいて、目標経路を取得する。上述のように、本実施形態では、作業車両が圃場内を往復走行する走行経路が設定されており、その走行経路を表す経路データが経路データ記憶部200に記憶されている。目標経路取得部11は、直進走行しているときの走行制御に必要な目標経路(往路直線リンク、復路直線リンクの何れか)や、旋回時の走行制御に必要な目標経路(旋回直線リンク、往路直線リンク、復路直線リンクの何れか)を適宜取得する。
The target
偏差検出部12は、目標経路取得部11により取得された目標経路を入力するとともに、位置・方位センサ300により検出された作業車両の現在位置および現在方位を入力する。位置・方位センサ300としてGPS受信機を用いている場合、GPS受信機からは作業車両の現在の絶対位置および絶対方位を示す信号が出力される。偏差検出部12は、このGPS受信機から出力される絶対位置および絶対方位をもとに、経路データ記憶部200に経路データとして記憶されている走行経路の座標系上における作業車両の現在位置および現在方位を検出する。
The
また、位置・方位センサ300として自立航法センサを用いている場合、自立航法センサからは作業車両の現在の相対位置および相対方位を示す信号が出力される。偏差検出部12は、この自立航法センサから出力される相対位置および相対方位から絶対位置および絶対方位を計算し、当該計算した絶対位置および絶対方位をもとに、経路データ記憶部200に経路データとして記憶されている走行経路の座標系上における作業車両の現在位置および現在方位を検出する。
When a self-contained navigation sensor is used as the position /
偏差検出部12は、作業車両に搭載された位置・方位センサ300からの出力に基づいて上述のようにして検出した作業車両の現在位置および現在方位と、目標経路取得部11により取得された目標経路とに基づいて、目標経路に対する自車方位の現在の方位偏差および目標経路に対する自車位置の現在の位置偏差を検出する。
The
図2は、偏差検出部12が検出する方位偏差および位置偏差を模式的に示す図である。図2は、例えば、往路直線リンクまたは復路直線リンクを目標経路TPとして取得し、作業車両WVを目標経路TPに沿って走行させるように制御する際の状況を示している。図2に示すように、作業車両WV上の所定の一点O(図2では一例として、車体の前後方向の中心線CLと、左右の後輪間を結ぶ直線との交点としている)を基点とし、車体の前後方向の中心線CLと、作業車両WV上の所定の一点Oを通り目標経路TPに平行な線PLとが成す角度が方位偏差θである。また、目標経路TPに平行な線PLと目標経路TPとの距離が位置偏差pである。
FIG. 2 is a diagram schematically showing the orientation deviation and the position deviation detected by the
なお、以下の説明において、位置偏差pは、目標経路TPの進行方向(図の矢印の向き)を基準として、作業車両WVが目標経路TPの右側にいるときに正の値、目標経路TPの左側にいるときに負の値をとるものとする。また、方位偏差θは、目標経路TPに対して走行車両WVが右方向に向かって走行している場合に正の値、左方向に向かって走行している場合に負の値をとるものとする。さらに、後述する曲率φは作業車両WVが右に旋回しているときの曲率が正の値をとり、曲率勾配λは右方向に操舵するときに正の値をとるものとする。ここで曲率φは、作業車両WVの操舵角と一定の関係を持つパラメータである。また、曲率勾配λは、作業車両WVの操舵速度および走行速度と一定の関係を持つパラメータである。 In the following description, the position deviation p is a positive value when the work vehicle WV is on the right side of the target route TP with respect to the traveling direction of the target route TP (direction of the arrow in the figure), and the target route TP. It shall take a negative value when it is on the left side. Further, the directional deviation θ is a positive value when the traveling vehicle WV is traveling to the right with respect to the target route TP, and a negative value when the traveling vehicle WV is traveling to the left. do. Further, it is assumed that the curvature φ described later takes a positive value when the work vehicle WV is turning to the right, and the curvature gradient λ takes a positive value when steering to the right. Here, the curvature φ is a parameter having a certain relationship with the steering angle of the work vehicle WV. Further, the curvature gradient λ is a parameter having a constant relationship with the steering speed and the traveling speed of the work vehicle WV.
なお、ここでは一例として、往路直線リンクまたは復路直線リンクを目標経路TPとする場合における方位偏差θおよび位置偏差pを示しているが、往路直線リンクや復路直線リンクの終端付近で旋回直線リンクを目標経路として走行制御する場合や、旋回直線リンクに沿って走行した後に復路直線リンクや往路直線リンクを目標経路として走行制御する場合にも、本実施形態の走行制御を適用可能であることは言うまでもない。むしろ、このように方位偏差θおよび位置偏差pが大きくなる旋回走行時において、以下に詳述する本実施形態の走行制御は特に有効である。 Here, as an example, the azimuth deviation θ and the position deviation p when the outward straight line link or the return straight link is the target route TP, but the turning straight link is formed near the end of the outward straight link or the return straight link. Needless to say, the travel control of the present embodiment can be applied to the travel control as the target route, or the travel control using the return linear link or the outward linear link as the target route after traveling along the turning straight link. stomach. Rather, the travel control of the present embodiment, which will be described in detail below, is particularly effective during turning travel in which the directional deviation θ and the position deviation p are large.
位置・方位制御部13は、作業車両WVが旋回する際に走行の制御値として用いる経路の曲率として、偏差検出部12により検出された方位偏差θおよび位置偏差pを解消するための目標曲率φtを算出する。なお、この位置・方位制御部13の動作の詳細は後述する。
The position /
走行制御部14は、位置・方位制御部13により算出された目標曲率φtに対して実際の曲率φが一致するように作業車両WVの走行を制御する。本実施形態では、走行制御部14は、位置・方位制御部13により算出された目標曲率φtをもとに操舵角を決定し、当該決定した操舵角に基づいて作業車両WVの走行を制御する。すなわち、走行制御部14は、位置・方位制御部13により算出された目標曲率φtを所定の相関関数に基づいて操舵角に変換し、これによって得られた操舵角情報を操舵装置400に出力する。すなわち、操舵装置400は、走行制御部14から取得した操舵角情報に基づいてモータ電圧を決定し、作業車両WVの操舵を制御する。
The
以下に、位置・方位制御部13の動作の詳細を説明する。位置・方位制御部13は、目標曲率設定部13A、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cの動作によって、目標曲率φtを算出する。すなわち、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cによって、現在の方位偏差θを解消するために必要な仮曲率勾配λ’を算出しつつ、当該仮曲率勾配λ’に従って作業車両WVが走行したと仮定した場合に目標経路TPとの間に生じる仮位置偏差p’を算出する。そして、その仮位置偏差p’を解消するために必要な目標曲率φtを目標曲率設定部13Aによって設定する。
The details of the operation of the position /
目標曲率設定部13Aは、偏差検出部12により検出された現在の方位偏差θおよび現在の位置偏差pを仮位置偏差算出部13Bに供給するとともに、暫定目標曲率φt’を設定して仮位置偏差算出部13Bに供給し、仮位置偏差p’を算出するよう仮位置偏差算出部13Bに指示をした後、仮位置偏差算出部13Bが算出した仮位置偏差p’を受け取る。
The target
目標曲率設定部13Aは、仮位置偏差算出部13Bから受け取った仮位置偏差p’が所定の条件を満たすか否かを判定する。そして、所定の条件を満たす場合は、仮位置偏差算出部13Bが仮位置偏差p’を算出する際に使用した暫定目標曲率φt’を目標曲率φtとして走行制御部14に対して出力する。一方、所定の条件を満たしていない場合は、新たに暫定目標曲率φt’を設定して仮位置偏差p’を再算出するよう仮位置偏差算出部13Bに指示をする。
The target
仮位置偏差算出部13Bは、目標曲率設定部13Aから与えられた暫定目標曲率φt’、現在の方位偏差θおよび現在の位置偏差pを受け取ると、仮曲率勾配算出部13Cに暫定目標曲率φt’を与えるとともに、現在の方位偏差θを仮方位偏差θ’として与え、仮曲率勾配λ’の算出を指示する。仮位置偏差算出部13Bは、目標曲率設定部13Aから与えられた暫定目標曲率φt’、仮方位偏差θ’と現在の位置偏差pを初期条件として、仮曲率勾配算出部13Cにより算出された仮曲率勾配λ’とに基づいて、仮方位偏差θ’が解消するまで作業車両WVを走行させたと仮定した場合に生じる目標経路TP(特許請求の範囲の所定位置に相当)との位置偏差である仮位置偏差p’の算出を行う。すなわち、作業車両の方位偏差θ及び位置偏差pを算出する三角関数からなる非線形要素が含まれる演算(後述する(式2)(式3)(式4)に対応する演算)処理と、当該処理により算出される仮曲率φ’および仮方位偏差θ’を仮曲率勾配算出部13Cに供給して仮曲率勾配λ’を算出する処理とを含むステップ処理を、仮方位偏差θ’が解消されるまで順次繰り返す。そして、この繰り返しによって算出した仮位置偏差p’を目標曲率設定部13Aに出力する。
When the temporary position
仮曲率勾配算出部13Cは、与えられた仮曲率φ’および仮方位偏差θ’をゼロに収束させるような仮曲率勾配λ’を算出する関係式(後述する(式1))に基づいて、仮位置偏差算出部13Bから指示された仮曲率φ’および仮方位偏差θ’を用いて仮曲率勾配λ’を算出し、これを仮位置偏差算出部13Bに出力する。
The tentative curvature
以下に、目標曲率設定部13A、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cの処理内容をより詳細に説明する。
Hereinafter, the processing contents of the target
まず、目標曲率設定部13Aは、偏差検出部12により検出された現在の方位偏差θおよび現在の位置偏差pを仮位置偏差算出部13Bに供給するとともに、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cの演算処理に用いるための暫定的な目標曲率φt’(暫定目標曲率φt’)を設定して仮位置偏差算出部13Bに供給し、仮位置偏差p’を算出するよう仮位置偏差算出部13Bに指示をする。仮位置偏差算出部13Bが暫定目標曲率φt’を用いて算出した位置偏差pの値を仮位置偏差p’と呼ぶ。
First, the target
仮位置偏差算出部13Bは、目標曲率設定部13Aから供給された現在の方位偏差θ、現在の位置偏差pおよび暫定目標曲率φt’を、仮位置偏差p’を算出する際の値として用いる。また、仮位置偏差算出部13Bは、目標曲率設定部13Aから供給された暫定目標曲率φt’および現在の方位偏差θを、仮曲率勾配算出部13Cが仮曲率勾配λ’を算出する際に使用する仮曲率φ’および仮方位偏差θ’の値として仮曲率勾配算出部13Cに供給する。
The temporary position
仮曲率勾配算出部13Cは、作業車両WVが旋回走行しているときの作業車両WVの旋回する曲率φと方位偏差θから、最終的に方位偏差θを解消するための制御値として、仮曲率φ’が単位走行距離(例えば1m)当たりに変化する量である仮曲率勾配λ’を算出する。
The tentative curvature
すなわち、仮曲率勾配算出部13Cは、仮方位偏差θ’および仮曲率φ’を変数として与えた上で、仮方位偏差θ’および仮曲率φ’がゼロに収束するために必要となる仮曲率勾配λ’を、例えば次の(式1)に示す所定の関数Λを用いた計算によって算出する。
λ’=Λ(θ’,φ’)・・・(式1)
この(式1)は、仮方位偏差θ’および仮曲率φ’がゼロに収束する機能を持った式であり、その一例として、λ’=-Aθ’-Bφ’を用いることが可能である(A,Bはシミュレーションまたは実機の動作から適宜設定されるパラメータである)。なお、(式1)が、特許請求の範囲の所定の関係式に相当する。
That is, the tentative curvature
λ'= Λ (θ', φ') ... (Equation 1)
This (Equation 1) is an equation having a function of converging the tentative orientation deviation θ'and the tentative curvature φ'to zero, and as an example thereof, λ'=-Aθ'-Bφ' can be used. (A and B are parameters that are appropriately set from the simulation or the operation of the actual machine). In addition, (Equation 1) corresponds to a predetermined relational expression in the claims.
具体的には、仮位置偏差取得部13Bから曲率φの値および方位偏差θの値としての仮曲率φ’および仮方位偏差θ’が与えられる都度、(式1)に基づいて仮曲率勾配λ’を算出し、仮位置偏差取得部13Bに返す。なお、仮位置偏差取得部13Bから仮曲率勾配算出部13Cに与えられる仮曲率φ’および仮方位偏差θ’の初期値は、目標曲率設定部13Aから供給された暫定目標曲率φt’および現在の方位偏差θの値である。
Specifically, each time the temporary position
仮方位偏差θ’および仮曲率φ’がゼロに収束するとは、両方の値が完全にゼロに等しくなることを必須とするものではなく、それぞれが所定値未満となることを含む。あるいは、仮方位偏差θ’および仮曲率φ’から所定の算出式によって算出される評価値が所定値未満となることを含む。本明細書では、このように仮方位偏差θ’および仮曲率φ’が所定の収束条件(以下、方位偏差収束条件という)を満たした状態を、仮方位偏差θ’が解消された状態と表現する。 The convergence of the pseudo-direction deviation θ'and the pseudo-curvature φ'does not require that both values be completely equal to zero, but includes that each is less than a predetermined value. Alternatively, it includes that the evaluation value calculated by the predetermined calculation formula from the temporary orientation deviation θ'and the temporary curvature φ'is less than the predetermined value. In the present specification, the state in which the tentative direction deviation θ'and the tentative curvature φ'satisfy a predetermined convergence condition (hereinafter referred to as the directional deviation convergence condition) is expressed as a state in which the tentative direction deviation θ'is eliminated. do.
なお、仮方位偏差θ’および仮曲率φ’の値が与えられれば、これらをゼロに収束するために必要となる仮曲率勾配λ’の値は(式1)によって一義的に定まる。つまり、仮方位偏差θ’および仮曲率φ’の値が与えられれば、(式1)により一の仮曲率勾配λ’を得ることができる。そこで、仮曲率勾配λ’の値を仮方位偏差θ’および仮曲率φ’の値と紐づけした値の情報として記憶媒体に記憶しておき、仮曲率勾配算出部13Cは、与えられた仮方位偏差θ’および仮曲率φ’の値をもとに仮曲率勾配λ’の値を記憶媒体から読み出して利用するようにしてもよい。このように記憶媒体にデータをあらかじめ記憶しておいて、それを読み出して利用する構成とすることにより、処理時間を短縮することができる。なお、記憶媒体に記憶されるデータは離散値となるので、それを補間するための計算を行うようにしても良い。
If the values of the pseudo-direction deviation θ'and the pseudo-curvature φ'are given, the value of the pseudo-curvature gradient λ'required to converge these to zero is uniquely determined by (Equation 1). That is, if the values of the pseudo-direction deviation θ'and the pseudo-curvature φ'are given, one pseudo-curvature gradient λ'can be obtained by (Equation 1). Therefore, the value of the tentative curvature gradient λ'is stored in the storage medium as information of the value associated with the value of the tentative curvature deviation θ'and the tentative curvature φ', and the tentative curvature
この仮曲率勾配λ’の算出の際に、仮曲率勾配算出部13Cは、仮方位偏差θ’を解消することのみを考慮し、位置偏差を解消することは考慮しない。そのため、仮に、仮曲率勾配算出部13Cにより算出される仮曲率勾配λ’に従って作業車両WVを走行させた場合、図3に示すように、仮方位偏差θ’が解消する時点(作業車両WVの中心線CLが目標経路TPに平行になる時点)で、作業車両WVの位置と目標経路TPとの間に位置偏差(仮位置偏差p’)に定常偏差が生じ得る。なお、作業車両WVが圃場を往復走行するときの実際の旋回は、図3に示す状況よりも大きな曲率φで急旋回するが、ここでは説明の便宜上、実際よりも緩やかな曲率φで旋回する様子を示している。以下に説明する他の図面も同様である。
When calculating the pseudocurvature gradient λ', the pseudocurvature
このように、実際の現在位置から、仮曲率勾配算出部13Cにより算出された仮曲率勾配λ’に従って仮方位偏差θ’が解消されるまで作業車両WVを走行させたと仮定した場合、仮位置偏差p’がゼロになるとは限らない。
In this way, assuming that the work vehicle WV is driven from the actual current position until the tentative direction deviation θ'is eliminated according to the tentative curvature gradient λ'calculated by the tentative curvature
そこで、仮位置偏差算出部13Bは、仮曲率勾配算出部13Cにより算出された仮曲率勾配λ’に従って仮方位偏差θ’が解消されるまで作業車両WVを走行させたと仮定した場合に生じる目標経路TPとの仮位置偏差p’を求める。この仮位置偏差p’を求める際に、仮位置偏差算出部13Bは、仮方位偏差θ’の三角関数値を用いた演算を行う。
Therefore, it is assumed that the temporary position
すなわち、仮位置偏差算出部13Bは、仮曲率勾配λ’と仮曲率φ’、仮曲率φ’と仮方位偏差θ’および仮方位偏差θ’と仮位置偏差p’との関係を示した次の(式2)(式3)(式4)に従って、(式1)により算出された仮曲率勾配λ’に従って仮方位偏差θ’が解消されるまで作業車両WVを走行させたと仮定した場合に生じる目標経路TPとの仮位置偏差p’を算出する。ここで、Lは走行距離を表す。なお、(式2)(式3)(式4)が、特許請求の範囲の三角関数からなる非線形要素が含まれる関係式に相当する。
That is, the temporary position
具体的には、仮位置偏差算出部13Bは、目標曲率設定部13Aから供給された暫定目標曲率φt’、現在の方位偏差θおよび現在の位置偏差pを初期値とし、方位偏差θが解消したと判断されるまで、都度、仮曲率勾配算出部13Cから仮曲率勾配λ’の値を取得しつつ(式2)(式3)(式4)の演算を繰り返す。そして方位偏差θが解消されたと判断されたときの位置偏差pの値を仮位置偏差p’として目標曲率設定部13Aに供給する。
Specifically, the temporary position
より具体的には、仮位置偏差算出部13Bは、(式2)(式3)(式4)を微小区間ごとに繰り返し演算するものであり、目標曲率設定部13Aから供給された暫定目標曲率φt’、現在の方位偏差θおよび現在の位置偏差pを初期値とし、それにより得られた曲率φの値(以下、仮曲率φ’と呼ぶ)、方位偏差の値(以下、仮方位偏差θ’と呼ぶ)及び仮方位偏差p’を再び(式2)(式3)(式4)に投入するとともに、仮曲率φ’、仮方位偏差θ’を仮曲率勾配算出部13Cに与える。そして得られた仮曲率勾配λ’、仮曲率φ’および仮方位偏差θ’をこれらの式に繰り返し投入する演算を、仮方位偏差θ’が解消したと判断されるまで繰り返す。仮位置偏差算出部13Bは、仮方位偏差θ’が解消したと判断すると、その時の仮位置偏差p’を目標曲率設定部13Aに送る。
More specifically, the temporary position
なお、仮曲率勾配算出部13Cおよび仮位置偏差算出部13Bは、次の(式5)に示すように、曲率φについて設定された下限値および上限値の範囲内で、仮方位偏差θ’を解消するための仮曲率勾配λ’を算出するとともに、仮位置偏差p’を求める。ここで、φminは曲率として採り得る値の最小値(左方向に最大限操舵した状態に相当する)、φmaxは曲率として採り得る値の最大値(右方向に最大限操舵した状態に相当する)である。φminの絶対値とφmaxの絶対値は同じであってもよいし、異なっていてもよい。
φmin≦φ’(L)≦φmax ・・・(式5)
As shown in the following (Equation 5), the temporary curvature
φ min ≤ φ'(L) ≤ φ max ... (Equation 5)
目標曲率設定部13Aは、仮位置偏差算出部13Bにより算出された仮位置偏差p’を受けて、当該仮位置偏差p’の値に応じて暫定目標曲率φt’を変更し、変更後の暫定目標曲率φt’を仮位置偏差算出部13Bに供給することにより、仮位置偏差p’を再算出するように指示する。目標曲率設定部13Aは、この処理を繰り返し行うことにより、都度算出される仮位置偏差p’に基づいて、現在の位置偏差pを解消させるための制御値として目標曲率φtを設定する。本実施形態では、仮位置偏差算出部13Bにより算出される仮位置偏差p’を微小化させるような目標曲率φtを設定する。
The target
すなわち、目標曲率設定部13Aは、まず、暫定目標曲率φt’を設定して仮位置偏差算出部13Bに供給し、その応答として、仮曲率勾配算出部13Cにより算出される仮曲率勾配λ’に従って仮方位偏差θ’が解消されるまで作業車両WVを走行させたと仮定した場合に生じる目標経路TPとの仮位置偏差p’を仮位置偏差算出部13Bから得る。
That is, the target
ここで、この仮位置偏差p’の絶対値が大きくて所定の収束条件(以下、位置偏差収束条件という)を満たしておらず、仮位置偏差p’が解消されていないと判断される場合は、目標曲率設定部13Aは、暫定目標曲率φt’の値を変えて仮位置偏差算出部13Bに供給し、仮位置偏差p’の算出を再度行わせることにより、仮位置偏差算出部13Bから仮位置偏差p’を再度取得する。
Here, when it is determined that the absolute value of the temporary position deviation p'is large and does not satisfy a predetermined convergence condition (hereinafter referred to as the position deviation convergence condition), and the temporary position deviation p'is not resolved. , The target
それでも、再度取得した仮位置偏差p’が位置偏差収束条件を満たしていない場合は、目標曲率設定部13Aは、暫定目標曲率φt’の値を再度変えて仮位置偏差算出部13Bに供給し、仮位置偏差p’の計算を再実行させることにより、仮位置偏差算出部13Bから仮位置偏差p’を再度取得する。以降、再度取得した仮位置偏差p’が位置偏差収束条件を満たすまでこの動作を繰り返し、位置偏差収束条件を満たしたときに用いた暫定目標曲率φt’の値を、最終的に求める目標曲率φtとして走行制御部14に出力する。
Even so, if the re-acquired temporary position deviation p'does not satisfy the position deviation convergence condition, the target
仮位置偏差p’が位置偏差収束条件を満たしていない場合に暫定目標曲率φt’を再設定する方法としては、任意の方法を用いることが可能である。例えば、暫定目標曲率φt’の値をランダムに変えるようにしてもよいし、公知の技術に従って処理のルールに基づき値を変えるようにしてもよい。本実施形態では、公知の二分法によって暫定目標曲率φt’の値を再設定する。すなわち、目標曲率設定部13Aは、仮位置偏差算出部13Bにより仮位置偏差p’が求められる毎に、仮位置偏差算出部13Bに指示した暫定目標曲率φt’の値を、目標経路TPに対する仮位置偏差p’の方向(仮位置偏差p’が正の値か負の値か)に応じて上端値または下端値の何れかに設定した上で、当該上端値または下端値を設定した範囲内で新たに暫定目標曲率φt’を設定し、仮位置偏差p’を再算出するよう仮位置偏差算出部13Bに指示する。そして、このような処理を繰り返し行うことにより、仮位置偏差p’を微小化させるような目標曲率φtを設定する。
Any method can be used as a method for resetting the provisional target curvature φ t'when the temporary position deviation p'does not satisfy the position deviation convergence condition. For example, the value of the provisional target curvature φ t'may be randomly changed, or the value may be changed based on the processing rules according to a known technique. In this embodiment, the value of the provisional target curvature φ t'is reset by a known dichotomy. That is, each time the temporary position deviation p'is obtained by the temporary position
上述したように、目標曲率設定部13Aは、仮位置偏差p’を微小化させることを位置偏差収束条件として用いている。仮位置偏差p’が微小化するとは、例えば、仮位置偏差p’の絶対値が所定値未満になることをいう。なお、二分法によって暫定目標曲率φt’の値を再設定する場合は、仮位置偏差p’の算出を繰り返すことによって、仮位置偏差p’が徐々に小さくなっていくことがある程度保証される。そのため、仮位置偏差p’が実際に所定値未満となったか否かを判定することに代えて、目標曲率設定部13Aの動作の繰り返し回数が所定値に達したことを検出することをもって、仮位置偏差p’を微小化させるための位置偏差収束条件を満たしたとみなすことが可能である。
As described above, the target
図4および図5は、位置・方位制御部13の動作例を示すフローチャートである。図5に示すフローチャートは、目標曲率設定部13Aの動作例を示すものであり、例えば、作業車両WVの自動走行制御を開始した後、所定時間毎または所定走行距離毎に繰り返し実行される。また、図4に示すフローチャートは、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cの動作例を示すものであり、図5のステップS13において呼び出されて実行されるものである。
4 and 5 are flowcharts showing an operation example of the position /
図4において、仮位置偏差算出部13Bは、まず、仮曲率勾配λ’および仮位置偏差p’を算出する際の初期値を設定する(ステップS1)。すなわち、仮位置偏差算出部13Bは、仮位置偏差p’の初期値を現在の位置偏差pに設定し、仮方位偏差θ’の初期値を現在の方位偏差θの値に設定し、仮曲率φ’の初期値を暫定目標曲率φt’の値に設定する。これらの初期値は何れも、目標曲率設定部13Aから与えられる。仮位置偏差算出部13Bは、設定した仮方位偏差θ’および仮曲率φ’を仮曲率勾配算出部13Cに供給する。
In FIG. 4, the temporary position
次に、仮曲率勾配算出部13Cは、仮位置偏差算出部13Bから供給された仮方位偏差θ’および仮曲率φ’を用いて、上述した(式5)に示す制約条件のもと、(式1)に従って仮曲率勾配λ’を算出する(ステップS2)。次いで、仮位置偏差算出部13Bは、ステップS2で仮曲率勾配算出部13Cが算出した仮曲率勾配λ’に従って作業車両WVが所定走行距離ΔLだけ移動したと仮定した場合の仮位置偏差p’、仮方位偏差θ’および仮曲率φ’を、上述の(式2)~(式4)に従って算出する(ステップS3)。
Next, the temporary curvature
例えば、仮位置偏差算出部13Bは、公知のオイラー法に従って、仮位置偏差p’、仮方位偏差θ’および仮曲率φ’を算出する。すなわち、新たな仮位置偏差p’は、現在の仮位置偏差p’の値に(sinθ’)・ΔLの値を加えた値として算出する。新たな仮方位偏差θ’は、現在の仮方位偏差θ’の値にφ’・ΔLの値を加えた値として算出する。新たな仮曲率φ’は、現在の仮曲率φ’の値にλ’・ΔLの値を加えた値として算出する。なお、ここに示したオイラー法は演算の一例であって、これに限定されるものではない。例えば、より精度の高いルンゲクッタ法など用いて算出することも可能である。
For example, the temporary position
そして、仮位置偏差算出部13Bは、ステップS3で算出した仮方位偏差θ’および仮曲率φ’が所定の方位偏差収束条件を満たしたか否かを判定する(ステップS4)。ここで、所定の方位偏差収束条件を満たしていないと判定された場合、処理はステップS2に戻る。一方、所定の方位偏差収束条件を満たすと判定された場合、仮位置偏差算出部13Bは、そのときステップS3で算出された仮位置偏差p’を目標曲率設定部13Aに出力する。これにより、図5のステップS13における1回の処理が終了する。
Then, the temporary position
図5において、目標曲率設定部13Aは、まず、二分法の初期値を設定する(ステップS11)。すなわち、目標曲率設定部13Aは、曲率を二分する範囲を定める上端値φHおよび下端値φLの初期値として、上端値φHの初期値をφmax、下端値φLの初期値をφminに設定するとともに、暫定目標曲率φt’の初期値をφmaxとφminの平均値(左右の最大曲率が等しい通常の車両であればφmin=-φmaxとなるため、その場合の初期値は0)に設定する。また、目標曲率設定部13Aは、仮位置偏差p’を微小化させるための位置偏差収束条件を判定するために用いる処理回数のカウント値iの初期値を0に設定する(ステップS12)。
In FIG. 5, the target
次に、目標曲率設定部13Aは、現在設定されている暫定目標曲率φt’の値を図4に示す計算の初期値として設定し、図4に示すフローチャートの処理を仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cに実行させることにより、仮位置偏差算出部13Bから仮位置偏差p’を得る(ステップS13)。そして、これにより算出された仮位置偏差p’が正の値か否かを判定する(ステップS14)。仮位置偏差p’が正の値である場合は、作業車両WVが目標経路TPの右側で仮方位偏差θ’が解消された場合に相当する。一方、仮位置偏差p’が負の値である場合は、作業車両WVが目標経路TPの左側で仮方位偏差θ’が解消された場合に相当する。
Next, the target
仮位置偏差p’が正の値であると判定された場合、目標曲率設定部13Aは、曲率を二分する範囲の上端値φHに現在の暫定目標曲率φt’を設定した上で(ステップS15)、当該上端値φH(=φt’)と下端値φLとの平均値を新たに暫定目標曲率φt’として設定する(ステップS17)。図6(a)は、この場合の設定内容を模式的に示す図である。なお、図6の模式図は、図5の処理を開始してから1回目(i=0)の処理におけるステップS17の処理内容を示しており、「現φt’」と書かれた現在の暫定目標曲率φt’の値が、ステップS11で初期値として設定されたφmaxとφminの平均値の場合(図6の場合は“0”)を示している。
When it is determined that the temporary position deviation p'is a positive value, the target
図6(a)に示すように、現在位置pが負の値(目標経路TPの左側)で仮位置偏差p’が正の値(目標経路TPの右側)である場合は、現在の暫定目標曲率φt’の絶対値が小さ過ぎて、仮方位偏差θ’が解消されるまでに作業車両WVが目標経路TPを超えてしまう状態に相当する。よって、この場合に目標曲率設定部13Aは、現在の暫定目標曲率φt’(これを上端値φHに設定する)と、暫定目標曲率φt’の値として設定可能な下端値φL(1回目の処理の場合は下限値φmin)との平均値を新たに暫定目標曲率φt’として設定することにより、作業車両WVがより急峻なカーブを描いて旋回するように設定を変える。
As shown in FIG. 6A, when the current position p is a negative value (left side of the target path TP) and the temporary position deviation p'is a positive value (right side of the target path TP), the current provisional target. This corresponds to a state in which the absolute value of the curvature φ t'is too small and the work vehicle WV exceeds the target path TP by the time the provisional orientation deviation θ'is eliminated. Therefore, in this case, the target
一方、仮位置偏差p’が正の値ではない(負の値である)と判定された場合、目標曲率設定部13Aは、曲率を二分する範囲の下端値φLに現在の暫定目標曲率φt’を設定した上で(ステップS16)、当該下端値φL(=φt’)と上端値φHとの平均値を新たに暫定目標曲率φt’として設定する(ステップS17)。図6(b)は、この場合の設定内容を模式的に示す図である。
On the other hand, when it is determined that the temporary position deviation p'is not a positive value (a negative value), the target
図6(b)に示すように、現在位置pが負の値(目標経路TPの左側)で仮位置偏差p’も負の値(目標経路TPの左側)である場合は、現在の暫定目標曲率φt’の絶対値が大き過ぎて、仮方位偏差θ’が解消される位置において作業車両WVが目標経路TPまで至らない状態に相当する。よって、この場合に目標曲率設定部13Aは、現在の暫定目標曲率φt’(これを下端値φLに設定する)と、暫定目標曲率φt’の値として設定可能な上端値φH(1回目の処理の場合は上限値φmax)との平均値を新たに暫定目標曲率φt’として設定することにより、作業車両WVがより緩やかなカーブを描いて旋回するように設定を変える。
As shown in FIG. 6B, when the current position p is a negative value (left side of the target path TP) and the temporary position deviation p'is also a negative value (left side of the target path TP), the current provisional target. This corresponds to a state in which the work vehicle WV does not reach the target path TP at a position where the absolute value of the curvature φ t'is too large and the temporary orientation deviation θ'is eliminated. Therefore, in this case, the target
その後、目標曲率設定部13Aは、処理回数のカウント値iをインクリメントし(ステップS18)、カウント値iが所定値Nに達したか否か、すなわち、位置偏差収束条件を満たしたか否かを判定する(ステップS19)。ここで、処理回数iが所定値Nにまだ達していない場合、処理はステップS13に戻る。一方、処理回数iが所定値Nに達した場合、目標曲率設定部13Aは、現在設定されている暫定目標曲率φt’を、最終的に求める目標曲率φtに代入する(ステップS20)。目標曲率設定部13Aは目標曲率φtを走行制御部14に出力する(ステップS21)。これにより、目標曲率設定部13Aによる1回の処理が終了する。
After that, the target
以上詳しく説明したように、本実施形態では、仮曲率φ’および仮方位偏差θ’に基づいて、仮方位偏差θ’を解消するための制御値として仮曲率勾配λ’を算出するとともに、当該算出した仮曲率勾配λ’に従って仮方位偏差θ’が解消されるまで作業車両WVを走行させたと仮定した場合に生じる仮位置偏差p’を、仮方位偏差θ’の三角関数からなる非線形要素が含まれる演算(式2~式4に示す演算)によって求める。また、このようにして算出された仮位置偏差p’を受けて暫定目標曲率φt’を変更し、変更後の暫定目標曲率φt’を用いて仮位置偏差p’を再算出するという処理を繰り返し行うことにより、仮位置偏差p’を微小化させるような目標曲率φtを算出する。 As described in detail above, in the present embodiment, the pseudo-curvature gradient λ'is calculated as a control value for eliminating the pseudo-orientation deviation θ'based on the pseudo-curvature φ'and the pseudo-azimuth deviation θ'. A non-linear element consisting of a triangular function of the tentative direction deviation θ'is the temporary position deviation p'that occurs when the work vehicle WV is run until the tentative direction deviation θ'is eliminated according to the calculated tentative curvature gradient λ'. It is obtained by the included operations (operations shown in Equations 2 to 4). Further, the process of changing the provisional target curvature φ t'in response to the provisional position deviation p'calculated in this way and recalculating the provisional position deviation p'using the changed provisional target curvature φ t ' . By repeating the above steps, the target curvature φ t that minimizes the temporary position deviation p'is calculated.
このように構成した本実施形態によれば、現在の方位偏差θを解消するために必要な仮曲率勾配λ’の算出を行いつつ、それによって生じる仮位置偏差p’が算出されて、当該仮位置偏差p’を微小化するために必要な目標曲率φtの算出が行われる。そして、この仮位置偏差p’を算出する際に、方位偏差の三角関数値と、最大舵角に起因する曲率の飽和という非線形要素を用いた演算によって仮位置偏差p’が求められるので、目標経路TPに対する作業車両WVの偏差が大きくて非線形性が無視できない状態となる場合であっても、位置偏差pおよび方位偏差θの両方を迅速に解消することが可能な走行制御を行うことができるようになる。 According to the present embodiment configured in this way, while calculating the pseudocurvature gradient λ'necessary for eliminating the current azimuth deviation θ, the tentative position deviation p'caused by the calculation is calculated, and the tentative position deviation p'is calculated. The target curvature φ t required to minimize the position deviation p'is calculated. Then, when calculating this temporary position deviation p', the temporary position deviation p'is obtained by the calculation using the trigonometric function value of the orientation deviation and the non-linear element of the saturation of the curvature caused by the maximum steering angle. Even when the deviation of the work vehicle WV with respect to the route TP is large and the non-linearity cannot be ignored, it is possible to perform traveling control capable of quickly eliminating both the position deviation p and the orientation deviation θ. Will be.
なお、上記実施形態は、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその要旨、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。 It should be noted that all of the above embodiments are merely examples of embodiment of the present invention, and the technical scope of the present invention should not be construed in a limited manner. That is, the present invention can be implemented in various forms without departing from its gist or its main features.
例えば、上記実施形態では、仮方位偏差θ’が解消する時点で作業車両WVの位置と目標経路TPとの間に生じる位置偏差を仮位置偏差p’とし、これが微小化する(ゼロに近づく)ように制御する例について説明したが、本発明はこれに限定されない。例えば、現在位置から目標経路TPの方向に対する作業車両WVの移動量を仮位置偏差p’とし、これと現在位置pとの和が微小化する(ゼロに近づく)ように制御するようにしてもよい。このようにすれば、あらゆる条件の走行シミュレーションを事前に行って、その結果(仮位置偏差p’)をテーブル情報として保持することも可能であるという点で好ましい。 For example, in the above embodiment, the position deviation generated between the position of the work vehicle WV and the target path TP at the time when the temporary orientation deviation θ'is eliminated is defined as the temporary position deviation p', which is miniaturized (approaches zero). Although the example of controlling in this manner has been described, the present invention is not limited thereto. For example, the amount of movement of the work vehicle WV from the current position to the direction of the target route TP is set as a temporary position deviation p', and the sum of this and the current position p is controlled to be miniaturized (approaching zero). good. By doing so, it is possible to perform a running simulation under all conditions in advance and retain the result (temporary position deviation p') as table information, which is preferable.
テーブル情報を用いた構成とする場合は、図7に示すように、仮位置偏差算出部13Bおよび仮曲率勾配算出部13Cに代えて、テーブル情報を記憶した仮位置偏差取得部13Dを備えるようにする。この場合の仮位置偏差取得部13Dは、偏差検出部12により検出された現在の方位偏差θおよび現在の位置偏差pと、目標曲率設定部13Aが設定する暫定目標曲率φt’とを入力として、テーブル情報を参照することによって仮位置偏差p’を取得するように構成する。
When the configuration uses table information, as shown in FIG. 7, instead of the temporary position
また、この構成の場合に目標曲率設定部13Aは、暫定目標曲率φt’を設定して仮位置偏差p’を取得するよう仮位置偏差取得部13Dに指示した後、仮位置偏差取得部13Dが取得した仮位置偏差p’を受け取る。そして、受け取った仮位置偏差p’が所定の条件(位置偏差収束条件)を満たしていない場合は、新たに暫定目標曲率φt’を設定して仮位置偏差p’を再取得するよう仮位置偏差取得部13Dに指示する。一方、受け取った仮位置偏差p’が所定の条件を満たす場合は、仮位置偏差取得部13Dが仮位置偏差p’を取得する際に使用した暫定目標曲率φt’を目標曲率φtとして走行制御部14に対して出力する。
Further, in the case of this configuration, the target
また、上記実施形態では、(式2)~(式4)のように、作業車両WVの走行距離Lを用いて仮曲率勾配λ’や仮位置偏差p’を算出する例について説明したが、本発明はこれに限定されない。例えば、作業車両WVの走行距離Lに代えて走行時間tを用いるようにしてもよい。 Further, in the above embodiment, an example of calculating the temporary curvature gradient λ'and the temporary position deviation p'using the mileage L of the work vehicle WV has been described as described in (Equation 2) to (Equation 4). The present invention is not limited to this. For example, the traveling time t may be used instead of the traveling distance L of the work vehicle WV.
また、上記実施形態では、位置・方位制御部13が仮曲率勾配λ’および仮位置偏差p’を算出する際に、(式5)に示すような曲率の制約条件を用いる例について説明したが、これに加えて、以下に示す曲率勾配の制約条件を用いるようにしてもよい。
λmin≦λ’(L)≦λmax
このようにすれば、仮位置偏差p’を算出する際に、仮方位偏差θ’の三角関数値、最大舵角に起因する曲率の飽和に加えて、車速および最大操舵速度(操舵モータの速度)に起因する曲率勾配の飽和という非線形要素も考慮した演算によって仮位置偏差p’を求めることができる。
Further, in the above embodiment, an example in which the position /
λ min ≤ λ'(L) ≤ λ max
In this way, when calculating the temporary position deviation p', in addition to the trigonometric function value of the temporary position deviation θ'and the saturation of the curvature due to the maximum steering angle, the vehicle speed and the maximum steering speed (speed of the steering motor). ) Saturation of the curvature gradient, which is a non-linear element, can be taken into consideration to obtain the temporary position deviation p'.
また、上記実施形態では、走行制御部14が作業車両WVの操舵角を制御する例について説明したが、本発明はこれに限定されない。例えば、車輪ではなくクローラを備えるような作業車両WVの場合は、目標曲率φtに対して実際の曲率φが一致するように左右のクローラの回転量の差を制御するようにしてもよい。
Further, in the above embodiment, an example in which the traveling
11 目標経路取得部
12 偏差検出部
13 位置・方位制御部
13A 目標曲率設定部
13B 仮位置偏差算出部(仮位置偏差取得部)
13C 仮曲率勾配算出部(仮位置偏差取得部)
13D 仮位置偏差取得部
14 走行制御部
100 自動走行制御装置
11 Target
13C Temporary curvature gradient calculation unit (provisional position deviation acquisition unit)
13D Temporary position
Claims (10)
上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出部と、
上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出部により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御部と、
上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御部とを備え、
上記位置・方位制御部は、
所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得する仮位置偏差取得部と、
上記仮位置偏差取得部により取得された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する目標曲率設定部とを備えたことを特徴とする自動走行制御装置。 It is an automatic driving control device that automatically drives the vehicle along a preset target route.
Based on the current position and current azimuth of the vehicle detected based on the output from the position / orientation sensor mounted on the vehicle, the current directional deviation of the vehicle azimuth with respect to the target route and the vehicle with respect to the target route. A deviation detector that detects the current position deviation of the position, and
As the curvature of the path used as the travel control value when the vehicle turns, the position / direction control unit for calculating the directional deviation detected by the deviation detection unit and the target curvature for eliminating the position deviation, and the position / azimuth control unit.
It is provided with a travel control unit that controls the travel of the vehicle so that the actual curvature matches the target curvature calculated by the position / orientation control unit.
The above position / orientation control unit
The above azimuth deviation is eliminated according to the relational expression including the non-linear element consisting of a triangular function according to the curvature gradient which is the amount that the curvature changes per predetermined mileage obtained from the azimuth deviation and the curvature by the predetermined relational expression. A temporary position deviation acquisition unit that acquires the temporary position deviation, which is a position deviation from a predetermined position which is either the target route or the current position of the vehicle, which occurs when it is assumed that the vehicle has been driven up to.
While comparing the temporary position deviation acquired by the temporary position deviation acquisition unit with the current position deviation detected by the deviation detection unit, the target curvature required to eliminate the temporary position deviation is set. An automatic travel control device characterized by having a target curvature setting unit.
上記目標曲率設定部は、暫定目標曲率を設定して上記仮位置偏差を算出するよう上記仮位置偏差算出部に指示した後、上記仮位置偏差算出部が算出した上記仮位置偏差を受け取り、当該受け取った仮位置偏差が所定の条件を満たす場合は、上記仮位置偏差算出部が上記仮位置偏差を算出する際に使用した上記暫定目標曲率を目標曲率として上記走行制御部に対して出力し、上記受け取った仮位置偏差が上記所定の条件を満たしていない場合は、新たに暫定目標曲率を設定して上記仮位置偏差を再算出するよう上記仮位置偏差算出部に指示し、
上記仮位置偏差算出部は、上記目標曲率設定部から与えられた上記暫定目標曲率と、上記偏差検出部により検出された上記現在の方位偏差と、上記仮曲率勾配算出部により算出された仮曲率勾配とに基づいて、上記仮曲率勾配に従って上記車両を走行させたと仮定した場合に生じる上記所定位置との位置偏差である上記仮位置偏差を、上記三角関数からなる非線形要素が含まれる演算によって算出する処理と、上記仮曲率勾配算出部に指示する仮曲率および仮方位偏差を算出する処理とを含むステップ処理を、上記仮方位偏差が解消されるまで順次繰り返し、算出した上記仮位置偏差を上記目標曲率設定部に出力し、
上記仮曲率勾配算出部は、与えられた仮曲率および仮方位偏差をゼロに収束させるような仮曲率勾配を算出する関係式に基づいて、上記仮位置偏差算出部から指示された仮曲率および仮方位偏差を用いて上記仮曲率勾配を算出し、これを上記仮位置偏差算出部に出力する
ことを特徴とする請求項1に記載の自動走行制御装置。 The temporary position deviation acquisition unit includes a temporary position deviation calculation unit and a temporary curvature gradient calculation unit.
The target curvature setting unit receives the temporary position deviation calculated by the temporary position deviation calculation unit after instructing the temporary position deviation calculation unit to set the provisional target curvature and calculate the temporary position deviation. When the received temporary position deviation satisfies a predetermined condition, the provisional target curvature used by the temporary position deviation calculation unit for calculating the temporary position deviation is output to the travel control unit as the target curvature. If the received temporary position deviation does not meet the predetermined conditions, the temporary position deviation calculation unit is instructed to newly set a provisional target curvature and recalculate the temporary position deviation.
The temporary position deviation calculation unit includes the provisional target curvature given by the target curvature setting unit, the current orientation deviation detected by the deviation detection unit, and the temporary curvature calculated by the temporary curvature gradient calculation unit. Based on the gradient, the temporary position deviation, which is the positional deviation from the predetermined position that occurs when the vehicle is assumed to be driven according to the temporary curvature gradient, is calculated by an operation including a nonlinear element consisting of the triangular function. The step process including the process of calculating the temporary curvature and the process of calculating the temporary curvature and the temporary direction deviation instructed to the temporary curvature gradient calculation unit is sequentially repeated until the temporary direction deviation is eliminated, and the calculated temporary position deviation is calculated as described above. Output to the target curvature setting section
The tentative curvature gradient calculation unit calculates the tentative curvature and tentative curvature instructed by the tentative position deviation calculation unit based on the relational expression for calculating the tentative curvature gradient so as to converge the given tentative curvature and the tentative orientation deviation to zero. The automatic traveling control device according to claim 1, wherein the temporary curvature gradient is calculated using the azimuth deviation and is output to the temporary position deviation calculation unit.
上記目標曲率設定部は、上記仮位置偏差算出部により算出された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差との差を微小化させるような上記目標曲率を設定することを特徴とする請求項2~5の何れか1項に記載の自動走行制御装置。 The temporary position deviation calculation unit calculates the position deviation from the current position that occurs when it is assumed that the vehicle is driven until the temporary orientation deviation is eliminated according to the temporary curvature gradient calculated by the temporary curvature gradient calculation unit. Calculated as the above temporary position deviation,
The target curvature setting unit sets the target curvature so as to minimize the difference between the temporary position deviation calculated by the temporary position deviation calculation unit and the current position deviation detected by the deviation detection unit. The automatic traveling control device according to any one of claims 2 to 5, wherein the automatic traveling control device is characterized.
上記目標曲率設定部は、上記仮位置偏差算出部により算出された上記仮位置偏差を微小化させるような上記目標曲率を設定することを特徴とする請求項2~5の何れか1項に記載の自動走行制御装置。 The temporary position deviation calculation unit calculates the position deviation from the target route that occurs when it is assumed that the vehicle is driven until the temporary orientation deviation is eliminated according to the temporary curvature gradient calculated by the temporary curvature gradient calculation unit. Calculated as the above temporary position deviation,
The item according to any one of claims 2 to 5, wherein the target curvature setting unit sets the target curvature so as to minimize the temporary position deviation calculated by the temporary position deviation calculation unit. Automatic driving control device.
上記目標曲率設定部は、上記暫定目標曲率を設定して上記仮位置偏差を取得するよう上記仮位置偏差取得部に指示した後、上記仮位置偏差取得部が取得した上記仮位置偏差を受け取り、当該受け取った仮位置偏差が所定の条件を満たす場合は、上記仮位置偏差取得部が上記仮位置偏差を取得する際に使用した上記暫定目標曲率を目標曲率として上記走行制御部に対して出力し、上記受け取った仮位置偏差が上記所定の条件を満たしていない場合は、新たに暫定目標曲率を設定して上記仮位置偏差を再取得するよう上記仮位置偏差取得部に指示する
ことを特徴とする請求項1に記載の自動走行制御装置。 The temporary position deviation acquisition unit refers to table information by inputting the current orientation deviation and the current position deviation detected by the deviation detection unit and the provisional target curvature set by the target curvature setting unit. It is configured to obtain the above temporary position deviation by
The target curvature setting unit receives the temporary position deviation acquired by the temporary position deviation acquisition unit after instructing the temporary position deviation acquisition unit to set the provisional target curvature and acquire the temporary position deviation. When the received temporary position deviation satisfies a predetermined condition, the provisional target curvature used when the temporary position deviation acquisition unit acquires the temporary position deviation is output to the travel control unit as the target curvature. If the received temporary position deviation does not meet the predetermined conditions, the temporary position deviation acquisition unit is instructed to newly set a provisional target curvature and reacquire the temporary position deviation. The automatic traveling control device according to claim 1.
自動走行制御装置の偏差検出部が、上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出ステップと、
上記自動走行制御装置の位置・方位制御部が、上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出部により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御ステップと、
上記自動走行制御装置の走行制御部が、上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御ステップとを有し、
上記位置・方位制御ステップは、仮位置偏差取得ステップと目標曲率設定ステップとを含み、
上記仮位置偏差取得ステップでは、上記位置・方位制御部が、所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得し、
上記目標曲率設定ステップでは、上記位置・方位制御部が、上記仮位置偏差取得ステップで取得された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する
ことを特徴とする自動走行制御方法。 It is an automatic driving control method that automatically drives the vehicle along a preset target route.
The deviation detection unit of the automatic driving control device detects the current position and current direction of the vehicle based on the output from the position / orientation sensor mounted on the vehicle, and the current position of the own vehicle with respect to the target route. Deviation detection step to detect the directional deviation of the vehicle and the current position deviation of the vehicle position with respect to the target route, and
In order to eliminate the directional deviation and the position deviation detected by the deviation detection unit as the curvature of the path used by the position / orientation control unit of the automatic travel control device as the travel control value when the vehicle turns. Position / orientation control step to calculate the target curvature of
The travel control unit of the automatic travel control device has a travel control step that controls the travel of the vehicle so that the actual curvature matches the target curvature calculated by the position / orientation control unit.
The position / orientation control step includes a temporary position deviation acquisition step and a target curvature setting step.
In the temporary position deviation acquisition step, the position / orientation control unit obtains from the orientation deviation and the curvature by a predetermined relational expression, and from the triangular function according to the curvature gradient which is the amount that the curvature changes per predetermined mileage. The position deviation from a predetermined position, which is either the target route or the current position of the vehicle, which occurs when the vehicle is driven until the orientation deviation is eliminated according to the relational expression including the non-linear element. Obtain the above temporary position deviation and
In the target curvature setting step, the position / orientation control unit compares the temporary position deviation acquired in the temporary position deviation acquisition step with the current position deviation detected by the deviation detection unit. An automatic traveling control method characterized by setting the target curvature required to eliminate the temporary position deviation.
上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出手段、
上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出手段により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御手段、および
上記位置・方位制御手段により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御手段、
としてコンピュータを機能させるためのプログラムであって、
上記位置・方位制御手段は、
所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得する仮位置偏差取得手段と、
上記仮位置偏差取得手段により取得された上記仮位置偏差と上記偏差検出手段により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する目標曲率設定手段とを備える
ことを特徴とする自動走行制御用プログラム。 It is a program for automatic driving control applied to an automatic driving control device that automatically drives a vehicle along a preset target route.
Based on the current position and current azimuth of the vehicle detected based on the output from the position / orientation sensor mounted on the vehicle, the current directional deviation of the vehicle azimuth with respect to the target route and the vehicle with respect to the target route. Deviation detecting means, which detects the current position deviation of the position,
As the curvature of the path used as the travel control value when the vehicle turns, the position / directional control means for calculating the directional deviation detected by the deviation detecting means and the target curvature for eliminating the position deviation, and the position / directional control means. A travel control means that controls the travel of the vehicle so that the actual curvature matches the target curvature calculated by the position / orientation control means.
It is a program to make a computer function as
The above position / orientation control means is
The above azimuth deviation is eliminated according to the relational expression including the non-linear element consisting of a triangular function according to the curvature gradient which is the amount that the curvature changes per predetermined mileage obtained from the azimuth deviation and the curvature by the predetermined relational expression. Temporary position deviation acquisition means for acquiring the temporary position deviation, which is a position deviation from a predetermined position which is either the target route or the current position of the vehicle, which occurs when it is assumed that the vehicle has been driven up to.
While comparing the temporary position deviation acquired by the temporary position deviation acquisition means with the current position deviation detected by the deviation detecting means, the target curvature required to eliminate the temporary position deviation is set. A program for automatic driving control, characterized in that it is provided with a target curvature setting means.
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018214670A JP7100358B2 (en) | 2018-11-15 | 2018-11-15 | Automatic driving control device, automatic driving control method and program for automatic driving control |
PCT/JP2019/042832 WO2020100617A1 (en) | 2018-11-15 | 2019-10-31 | Automatic travel control device, automatic travel control method, and program for automatic travel control |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018214670A JP7100358B2 (en) | 2018-11-15 | 2018-11-15 | Automatic driving control device, automatic driving control method and program for automatic driving control |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2020082752A JP2020082752A (en) | 2020-06-04 |
JP7100358B2 true JP7100358B2 (en) | 2022-07-13 |
Family
ID=70730625
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2018214670A Active JP7100358B2 (en) | 2018-11-15 | 2018-11-15 | Automatic driving control device, automatic driving control method and program for automatic driving control |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP7100358B2 (en) |
WO (1) | WO2020100617A1 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20180113821A (en) * | 2017-04-07 | 2018-10-17 | 케이씨에프테크놀로지스 주식회사 | Graphite Heat Dissipation Film With Copper Foil Lamination |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113859233B (en) * | 2021-09-17 | 2023-06-27 | 江苏英拓动力科技有限公司 | Track keeping method under steering and braking working conditions of bilateral independent electric drive tracked vehicle |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130304300A1 (en) | 2012-05-11 | 2013-11-14 | Trimble Navigation Ltd. | Path planning autopilot |
JP2016155491A (en) | 2015-02-25 | 2016-09-01 | 株式会社クボタ | Automatic traveling work vehicle |
EP3231270A1 (en) | 2016-04-13 | 2017-10-18 | CLAAS E-Systems KGaA mbH & Co KG | Method for controlling an agricultural working machine and operator assistance system |
US20170297621A1 (en) | 2016-04-13 | 2017-10-19 | Cnh Industrial America Llc | System and method for controlling a vehicle |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008092818A (en) * | 2006-10-06 | 2008-04-24 | Yanmar Co Ltd | Agricultural work vehicle |
-
2018
- 2018-11-15 JP JP2018214670A patent/JP7100358B2/en active Active
-
2019
- 2019-10-31 WO PCT/JP2019/042832 patent/WO2020100617A1/en active Application Filing
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130304300A1 (en) | 2012-05-11 | 2013-11-14 | Trimble Navigation Ltd. | Path planning autopilot |
JP2016155491A (en) | 2015-02-25 | 2016-09-01 | 株式会社クボタ | Automatic traveling work vehicle |
EP3231270A1 (en) | 2016-04-13 | 2017-10-18 | CLAAS E-Systems KGaA mbH & Co KG | Method for controlling an agricultural working machine and operator assistance system |
US20170297621A1 (en) | 2016-04-13 | 2017-10-19 | Cnh Industrial America Llc | System and method for controlling a vehicle |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20180113821A (en) * | 2017-04-07 | 2018-10-17 | 케이씨에프테크놀로지스 주식회사 | Graphite Heat Dissipation Film With Copper Foil Lamination |
Also Published As
Publication number | Publication date |
---|---|
JP2020082752A (en) | 2020-06-04 |
WO2020100617A1 (en) | 2020-05-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11317557B2 (en) | Automatic steering system and automatic steering method | |
CN109421709B (en) | Driving support device for vehicle | |
JP6420173B2 (en) | Automated traveling field work vehicle | |
US10416675B2 (en) | Line acquisition path generation using curvature profiles | |
CN109154817B (en) | Automatic traveling work vehicle | |
US10118639B2 (en) | Method and system for steering control of an autonomous vehicle using proportional, integral, and derivative (PID) controller | |
US20180011495A1 (en) | Route search method, route search system, non-transitory computer-readable storage medium, and work vehicle | |
JP5396873B2 (en) | Driver operation prediction apparatus and program | |
JP7100358B2 (en) | Automatic driving control device, automatic driving control method and program for automatic driving control | |
CN111868801A (en) | Route generation device and vehicle control system | |
JP7072496B2 (en) | Control device for self-driving work vehicles | |
Taghia et al. | A sliding mode controller with a nonlinear disturbance observer for a farm vehicle operating in the presence of wheel slip | |
JP2014228495A (en) | Self-position estimation device and method | |
CN107943060A (en) | A kind of automatic pilot, method and computer-readable medium along tracking straight line guiding vehicle | |
EP4110671A1 (en) | System and method for adjusting a planned trajectory of an autonomous vehicle | |
JP7058761B2 (en) | Mobile control device, mobile control learning device, and mobile control method | |
JP6954553B2 (en) | Autonomous vehicle, controller, computer program, control method of autonomous vehicle | |
JP2018008550A (en) | Steering control device | |
JP2007269180A (en) | Steering control device | |
JP7204612B2 (en) | POSITION AND POSTURE ESTIMATION DEVICE, POSITION AND POSTURE ESTIMATION METHOD, AND PROGRAM | |
JP2022151104A (en) | Automatic travel control device, automatic travel control method, and program for automatic travel control | |
JP2005250696A (en) | System and method for controlling autonomous travelling of vehicle | |
JP4517298B2 (en) | Correction coefficient calculation device and calculation program for direction sensor | |
US11299137B2 (en) | Lateral control for vehicle wireless charging guidance | |
JP4366660B2 (en) | Correction coefficient calculation device and calculation program for direction sensor |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20210624 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20220607 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20220624 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7100358 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |