專利名稱：制導(dǎo)車(chē)輛行駛的控制裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及制導(dǎo)車(chē)輛行駛的控制裝置，該裝置存儲(chǔ)預(yù)定行駛路線的位置數(shù)據(jù)，根據(jù)該位置數(shù)據(jù)控制車(chē)輛所操的舵角，制導(dǎo)車(chē)輛沿所述預(yù)定行駛路線行駛。
過(guò)去，對(duì)于制導(dǎo)車(chē)輛沿預(yù)定行駛路線行駛的裝置已進(jìn)行種種開(kāi)發(fā)，并申請(qǐng)了專利等。
特公平5-22925號(hào)乃至22926號(hào)公報(bào)中公開(kāi)了這樣一種制導(dǎo)行駛的技術(shù)，即使多條直線結(jié)合起來(lái)形成近似的行駛路線，制導(dǎo)車(chē)輛沿該直線行駛。
根據(jù)上述公報(bào)所記載的技術(shù)，車(chē)輛只是直線前進(jìn)即可，所以可簡(jiǎn)易地控制車(chē)輛的所操的舵角運(yùn)算等，這是有利之點(diǎn)，但是，行駛路線是曲線的，其所經(jīng)路線是近似的直線，所以車(chē)輛不可能正確沿預(yù)定行駛路線行駛，這是一個(gè)缺點(diǎn)。
還有，根據(jù)上述公報(bào)記載的技術(shù)，車(chē)輛所操的舵角的設(shè)定與車(chē)輛現(xiàn)在的速度無(wú)關(guān)。
因此，車(chē)輛的速度高時(shí)，所操的舵角過(guò)大，導(dǎo)致過(guò)度轉(zhuǎn)向，脫離曲線的行駛路線。同樣，車(chē)輛的速度低時(shí)，所操的舵角過(guò)小，導(dǎo)致操縱不靈，脫離曲線的行駛路線。
還有，根據(jù)上述公報(bào)記載的技術(shù)，在所操的舵角運(yùn)算處理上，在不脫離直線路線行駛時(shí)，所操的舵角設(shè)定為零。
然而，在自卸卡車(chē)等車(chē)輛的情況下，由于裝載的貨物或裝載的狀態(tài)不同，車(chē)輛四輪的調(diào)整發(fā)生變化。這樣，調(diào)整一旦發(fā)生變化，而仍使所操的舵角保持為零繼續(xù)行駛時(shí)，車(chē)輛會(huì)脫離直線路線，在實(shí)用上成為很大的問(wèn)題。
還有，特公平5-503775號(hào)公報(bào)中公開(kāi)了所操的舵角運(yùn)算，求出了由5次多項(xiàng)式所表示的、可從現(xiàn)在位置順利地沿行駛路線行駛的路線，車(chē)輛沿著這條路線行駛。
根據(jù)該公報(bào)記載的技術(shù)，考慮的是沿曲線的行駛路線行駛，其運(yùn)算繁雜，且費(fèi)時(shí)，這是個(gè)缺點(diǎn)。
還有，根據(jù)上述公報(bào)記載的技術(shù)，仍像所述特公平5-22925號(hào)乃至22926號(hào)公報(bào)的技術(shù)那樣，車(chē)輛所操的舵角的設(shè)定與車(chē)輛現(xiàn)在的速度無(wú)關(guān)。因此，也導(dǎo)致脫離曲線的行駛路線的問(wèn)題。
還有，同所述特公平5-22925號(hào)乃至22926號(hào)公報(bào)的技術(shù)那樣，在不偏離直線行駛路線行駛的情況下，因所操的舵角設(shè)定為零，所以若車(chē)輛前后輪的調(diào)整不完善，則要不偏離直線路線行駛是極端困難的。
如上所述，根據(jù)已有技術(shù)，存在著不能通過(guò)簡(jiǎn)易處理正確地在曲線行駛路線上行駛的問(wèn)題。
還有，存在著由于什么樣的車(chē)輛速度而導(dǎo)致脫離曲線行駛路線的問(wèn)題。
還有，由于車(chē)輛的調(diào)整變化，例如即使是直線行駛路線，也會(huì)有脫離預(yù)定路線的問(wèn)題。
本發(fā)明的目的在于，提供一種可解決這些問(wèn)題的裝置。
因此，本發(fā)明的第一發(fā)明目的是，通過(guò)簡(jiǎn)易處理使車(chē)輛正確地在曲線行駛路線上行駛，同時(shí)不管車(chē)輛的速度如何，不會(huì)使其脫離曲線行駛路線。
這一目的，可通過(guò)下列方式達(dá)到，即這種制導(dǎo)車(chē)輛行駛的控制裝置存儲(chǔ)預(yù)定行駛路線的位置數(shù)據(jù)，根據(jù)該位置數(shù)據(jù)控制車(chē)輛所操的舵角，制導(dǎo)車(chē)輛沿所述預(yù)定行駛路線行駛。這種裝置具有存儲(chǔ)手段，存儲(chǔ)表示所述預(yù)定行駛路線各點(diǎn)上的各該點(diǎn)坐標(biāo)位置和所述車(chē)輛應(yīng)行駛方向的數(shù)據(jù)；位置檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的坐標(biāo)位置；速度檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的行駛速度；選擇手段，從所述存儲(chǔ)手段的存儲(chǔ)內(nèi)容中，選擇最接近于以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置的、預(yù)定行駛路線上點(diǎn)的坐標(biāo)位置數(shù)據(jù)，以及該最接近點(diǎn)的行駛方向數(shù)據(jù)；
偏差值運(yùn)算手段，根據(jù)以所述選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在的坐標(biāo)位置，運(yùn)算沿車(chē)輛行駛方向的垂直方向上車(chē)輛位置現(xiàn)在的偏差值；所操的舵角運(yùn)算手段，根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在行駛速度，為使所速偏差值為零，所述檢測(cè)行駛速度越大，運(yùn)算車(chē)輛所操的舵角越??；控制手段，根據(jù)以所述所操的舵角運(yùn)算手段運(yùn)算的車(chē)輛所操的舵角，控制所述車(chē)輛所操的舵角。
還有，本發(fā)明的第二發(fā)明目的，除加進(jìn)上述第一發(fā)明的目的外，可吸收消除車(chē)輛調(diào)整的變化，制導(dǎo)車(chē)輛正確地沿預(yù)定行駛路線行駛。
在這個(gè)目的中加進(jìn)了上述第一發(fā)明的構(gòu)成，并具有積分運(yùn)算手段，以積分法運(yùn)算用所述偏差值運(yùn)算手段運(yùn)算的偏差值，同時(shí)還具有所操的舵角運(yùn)算手段。
根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述積分運(yùn)算手段運(yùn)算的積分值，和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，為使所述的偏差值為零，所述檢測(cè)行駛速度越大，則運(yùn)算車(chē)輛所操的舵角越小。
還有，本發(fā)明的第三發(fā)明目的，除加進(jìn)上述第一發(fā)明目的外，可制導(dǎo)車(chē)輛沿著曲率半徑復(fù)雜變化的路線正確地行駛。
在這個(gè)目的中，加進(jìn)了上述第一發(fā)明的構(gòu)成，并具有積分運(yùn)算手段，運(yùn)算以所述偏差值運(yùn)算手段運(yùn)算的偏差值，同時(shí)不采用所操的舵角運(yùn)算手段，而具有第一所操的舵角運(yùn)算手段，該手段根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述積分運(yùn)算手段運(yùn)算的積分值，和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，為使所述偏差值為零，所述檢測(cè)速度越大，則運(yùn)算第一車(chē)輛所操的舵角越小；還有，第二所操的舵角運(yùn)算手段，根據(jù)所述最近點(diǎn)的所述預(yù)定行駛路線的曲率半徑，運(yùn)算第二車(chē)輛所操的舵角；第三所操的舵角運(yùn)算手段，根據(jù)以所述第一所操的舵角運(yùn)算手段運(yùn)算的第一車(chē)輛所操的舵角，和所述第二所操的舵角運(yùn)算手段運(yùn)算的第二車(chē)輛所操的舵角，運(yùn)算第三車(chē)輛所操的舵角；
控制手段，根據(jù)以所述第三所操的舵角運(yùn)算手段運(yùn)算的第三車(chē)輛所操的舵角，控制所述車(chē)輛所操的舵角。
根據(jù)上述第一發(fā)明的構(gòu)成，根據(jù)以選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置，運(yùn)算在車(chē)輛行駛方向的垂直方向上車(chē)輛位置的現(xiàn)在偏差值，即可簡(jiǎn)易地運(yùn)算與曲線行駛路線間的偏差值。
而且，根據(jù)以偏差值運(yùn)算手段運(yùn)算的偏差值和以速度檢測(cè)手段檢測(cè)的現(xiàn)在行駛速度，使偏差值為零，檢測(cè)行駛速度越大，簡(jiǎn)易運(yùn)算車(chē)輛所操的舵角越小，即，簡(jiǎn)易而正確地運(yùn)算與車(chē)輛現(xiàn)在速度相應(yīng)的車(chē)輛所操的舵角。
而且，根據(jù)以所操的舵角運(yùn)算手段運(yùn)算的車(chē)輛所操的舵角，控制車(chē)輛所操的舵角，高精度地制導(dǎo)車(chē)輛在預(yù)定行駛路線上行駛。
還有，根據(jù)上述第二發(fā)明的構(gòu)成，以偏差值運(yùn)算手段運(yùn)算的偏差值，由積分運(yùn)算。而且，根據(jù)以偏差值運(yùn)算手段運(yùn)算的偏差值和以積分運(yùn)算手段運(yùn)算的積分值，和以速度檢測(cè)手段檢測(cè)的現(xiàn)在行駛速度，為使偏差值為零、檢測(cè)行駛速度越大，運(yùn)算車(chē)輛所操的舵角越小。即，考慮偏差值的積分，運(yùn)算所操的舵角，因此，可正確地求出能夠吸收并消除車(chē)輛的調(diào)整變化，高精度地制導(dǎo)車(chē)輛在預(yù)定行駛路線上行駛。
還有，根據(jù)上述第三發(fā)明的構(gòu)成，根據(jù)最接近車(chē)輛現(xiàn)在位置的預(yù)定行駛路線的曲率半徑，運(yùn)算第二車(chē)輛所操的舵角。而且，根據(jù)基于上述偏差值的第一車(chē)輛所操的舵角和第二車(chē)輛所操舵角，運(yùn)算第三車(chē)輛所操的舵角，并根據(jù)該第三車(chē)輛所操的舵角，控制車(chē)輛所操的舵角。其結(jié)果，通過(guò)迅速地保持基于路線曲率的第三所操的舵角，可大致地制導(dǎo)車(chē)輛沿路線行駛，同時(shí)，通過(guò)基于偏差值的第一所操的舵角來(lái)修正離開(kāi)路線的偏差，即可高精度地制導(dǎo)車(chē)輛沿曲率半徑復(fù)雜變化的路線行駛。
以下，對(duì)附圖進(jìn)行簡(jiǎn)要說(shuō)明。


圖1為表示有關(guān)本發(fā)明制導(dǎo)車(chē)輛行駛的控制裝置實(shí)施例構(gòu)成的框圖。
圖2為說(shuō)明通過(guò)模擬取得的預(yù)定行駛路線數(shù)據(jù)處理的框圖。
圖3為用于說(shuō)明運(yùn)算車(chē)輛所操的舵角幾何學(xué)關(guān)系圖。
圖4為表示自動(dòng)制導(dǎo)車(chē)輛行駛處理程序的程序圖。
下面，參照附圖，說(shuō)明有關(guān)本發(fā)明制導(dǎo)車(chē)輛行駛的控制裝置的實(shí)施例。
圖1為表示實(shí)施例的制導(dǎo)行駛控制裝置的框圖。圖2為說(shuō)明在實(shí)際制導(dǎo)行駛前所進(jìn)行的模擬內(nèi)容的框圖。
預(yù)定行駛路線的模擬本實(shí)施例假定以圖2所示的無(wú)人駕駛的自卸卡車(chē)(以下稱“車(chē)輛”)14作為應(yīng)行駛的車(chē)輛。
例如，車(chē)輛14是在預(yù)定行駛路線S上無(wú)人駕駛行駛的。表示該預(yù)定行駛路線S的數(shù)據(jù)，是通過(guò)模擬，即通過(guò)有人駕駛在應(yīng)行駛路線上行駛一周的模擬作業(yè)而設(shè)定并存儲(chǔ)在車(chē)輛14上設(shè)置的存儲(chǔ)器5。
車(chē)輛14上裝有檢測(cè)行駛方向的陀螺儀1、檢測(cè)行駛距離的距離編碼器2、和控制陀螺儀1與距離編碼器2的位置計(jì)測(cè)控制器3。
在教學(xué)行駛中，車(chē)輛14依次的坐標(biāo)位置、車(chē)輛14依次的行駛方向、車(chē)輛14依次的速度，依據(jù)由陀螺儀1、距離編碼器2輸出的檢測(cè)信號(hào)，通過(guò)位置計(jì)測(cè)控制器進(jìn)行運(yùn)算，并輸出，作為全部路線數(shù)據(jù)4。
但是，全部路線數(shù)據(jù)4以一秒鐘輸出數(shù)次至數(shù)十次，數(shù)據(jù)規(guī)模大，所以為使該數(shù)據(jù)尺寸成為適于儲(chǔ)存器5的存儲(chǔ)容量的尺寸，而以給定的時(shí)間間隔取樣。取樣的間隔τ定為不能過(guò)稀、過(guò)密的最佳間隔。
這樣取樣的車(chē)輛14依次的坐標(biāo)位置P1(X1、Y1)、P2(X2Y2)...Pi(Xi、Yi)...、而車(chē)輛14依次的行駛方向(方位角)ψ1、ψ2...ψi...、而車(chē)輛14依次的速度V1、V2...Vi...，均作為路線數(shù)據(jù)6被存儲(chǔ)在存儲(chǔ)器5中。
這樣，在存儲(chǔ)器5中，在預(yù)定行駛路線S各點(diǎn)的每個(gè)P1、P2...Pi...上均記憶和存儲(chǔ)著車(chē)輛14應(yīng)處位置的坐標(biāo)位置(Xi、Yj)、車(chē)輛14應(yīng)行駛的行駛方向ψ1、車(chē)輛14應(yīng)行駛的速度Vi。
還有，在本實(shí)施例中，是通過(guò)模擬作業(yè)求出路線數(shù)據(jù)6，其求出的方法是任意的，也可以通過(guò)測(cè)量等方法求出。
自動(dòng)制導(dǎo)行駛下面，參照?qǐng)D4的程序圖，說(shuō)明自動(dòng)制導(dǎo)車(chē)輛14行駛的處理程序。
模擬行駛后，開(kāi)始自動(dòng)制導(dǎo)行駛(重復(fù))時(shí)，則由陀螺儀1、距離編碼器2輸出檢測(cè)信號(hào)，車(chē)輛14依次的現(xiàn)在位置Q(X、Y)通過(guò)位置計(jì)測(cè)控制器3運(yùn)算，并輸入制導(dǎo)控制器7(步驟101)。
然后，在制導(dǎo)控制器7中，運(yùn)算如后所述的所操的目標(biāo)舵角ζ，通過(guò)操舵控制器8由液壓伺服系統(tǒng)11驅(qū)動(dòng)車(chē)輛14未圖示的操舵，以可得到上述所操的目標(biāo)舵角ζ而使車(chē)輛14的方向發(fā)生變化(步驟102-106)。
還有，在制導(dǎo)控制器7中，運(yùn)算如后所述的車(chē)輛14目標(biāo)速度，通過(guò)加速控制器9和制動(dòng)控制器10由機(jī)械伺服系統(tǒng)12、氣動(dòng)伺服系統(tǒng)13驅(qū)動(dòng)車(chē)輛14圖中未示出的加速器及制動(dòng)器，以可得到上述目標(biāo)速度而使車(chē)輛14的速度發(fā)生變化。
下面，將更詳細(xì)地說(shuō)明關(guān)于上述所操的目標(biāo)舵角的運(yùn)算。
即是說(shuō)，在車(chē)輛14行駛方向的前方選擇最接近車(chē)輛14的現(xiàn)在位置Q(X、Y)的行駛路線S上的點(diǎn)Pi(步驟102，參閱圖3)。
于是，與該選擇點(diǎn)Pi相對(duì)應(yīng)的位置坐標(biāo)數(shù)據(jù)(Xi、Yi)、方位角數(shù)據(jù)ψi，可由存儲(chǔ)器5中讀取。
還有，對(duì)于與上述最近點(diǎn)Pi的行駛路線S的曲率半徑對(duì)應(yīng)的所操的基本舵角ζP，進(jìn)行如后所述的運(yùn)算(步驟103)。
然后，根據(jù)這樣得到的路線上位置坐標(biāo)(Xi、Yi)、方位角ψi、所操的基本舵角ζp和車(chē)輛現(xiàn)在的位置Q(X、Y)，進(jìn)行下列(1)、(2)、(3)式的運(yùn)算，運(yùn)算車(chē)輛14所操的目標(biāo)舵角ζ。
yi＝-(X-Xi)sinφi+(Y-Yi)cosφi …(1)αs＝-(K4+K5·K6)yi-K4·K6 yidt-K5dyi/dt…(2)ζ＝tan-1(αsL/V2)+Kp·ζp…(3)式中，K4、K5、K6、Kp為控制常數(shù)；L為車(chē)輛14的前后軸距(前、后車(chē)軸之間的距離)。
V是車(chē)輛14的現(xiàn)在速度，通過(guò)位置計(jì)測(cè)控制器3而被輸入制導(dǎo)控制器7。
若就上述(1)-(3)式加以說(shuō)明，則首先可由下列(4)-(7)式求出(3)式所示的所操的基本舵角ζp。
也就是說(shuō)，若使預(yù)定行駛路線S上的點(diǎn)Pi與下一點(diǎn)Pi+1相聯(lián)結(jié)的路線上的圓弧長(zhǎng)度為G，以車(chē)輛14的現(xiàn)在的速度為V，取樣時(shí)間為τ，則可求下式G＝V·τ...(4)而且，若點(diǎn)Pi與下一點(diǎn)Pi+1之間的圓弧的曲率半徑為Ri，則圓弧的長(zhǎng)度G可由下式求得G＝Ri·(ψi+1-ψi)...(5)由上述(4)、(5)式，可通過(guò)下式(6)求曲率半徑RiRi＝V·τ/(ψi+1-ψi)...(6)所以，利用這樣求得的曲率半徑Ri和軸距L，可得到所操的基本舵角ζp，下式(7)ζp＝arctan(L/Ri)...(7)。
且說(shuō)，上述(1)式中，X-Y固定坐標(biāo)系中車(chē)輛14的Y軸方向坐標(biāo)位置Y(現(xiàn)在位置)，可變換為局部坐標(biāo)系xi-yi上的yi。
也就是說(shuō)，如圖3所示，以預(yù)定行駛路線S上的點(diǎn)Pi為原點(diǎn)，在Pi點(diǎn)中，以車(chē)輛14應(yīng)行駛的速度Vi(矢量)的方向，即行駛方向ψ1作為xi軸，進(jìn)而，在線路S的法線方向上取yi軸時(shí)的xi-yi坐標(biāo)系，則可以局部坐標(biāo)系的形式設(shè)定。因此，經(jīng)坐標(biāo)轉(zhuǎn)換的坐標(biāo)位置yi就意味著在預(yù)定行駛路線S的法線方向(沿行駛方向的垂直方向)上車(chē)輛14的偏差值。然后，通過(guò)(3)式即可求以該法線方向偏差值yi變?yōu)榱愕乃俚哪繕?biāo)舵角ζ。
還有，X-Y固定坐標(biāo)系上車(chē)輛14的X軸方向坐標(biāo)位置X(現(xiàn)在位置)，坐標(biāo)轉(zhuǎn)換為局部坐標(biāo)系xi-yi上的xi，如下列(8)所示xi＝(X-X1)cosψi+(Y-Yi)sinψi...(8)將上述(1)式所求出的法線方向偏差值yi代入上述(2)式中，求得車(chē)輛14的橫(法線)方向加速度αs，通過(guò)將該橫向加速度αs代入上述(3)式的方法，運(yùn)算所操的目標(biāo)舵角ζ。
這里，橫方向加速度αs中含有法線方向偏差值yi的積分值-K4·K6yidt，該積分值接近所操的目標(biāo)舵角ζ。
因此，若將該所操的目標(biāo)舵角ζ成為零那樣來(lái)控制車(chē)輛14的操舵，則車(chē)輛14的調(diào)整即使有毛病，也會(huì)通過(guò)上述積分值將其吸收、消除出去，從而有可能使車(chē)輛14不偏離直線路線行駛。
還有，由上述(3)式求的所操的目標(biāo)舵角ζ與1/V2成比例。因此，車(chē)輛14的現(xiàn)在速度V越高，所操的舵角越小，速度越低，所操的舵角越大。其結(jié)果，可排除只與速度相適的適當(dāng)角度的操舵，可防止車(chē)輛14由路線S上脫離。
還有，以上的運(yùn)算不是求復(fù)雜多項(xiàng)式的復(fù)雜運(yùn)算，而是簡(jiǎn)易的運(yùn)算。因此，不需要繁雜的運(yùn)算而以簡(jiǎn)易的處理，即可制導(dǎo)車(chē)輛14在曲線路線上正確地行駛(步驟103、104、105)。
但是，通過(guò)位置計(jì)測(cè)控制器3可運(yùn)算車(chē)輛14現(xiàn)在的方位角θ，并將其輸入制導(dǎo)控制器7中。然后，根據(jù)該現(xiàn)在方位角θ和現(xiàn)在速度V，和對(duì)應(yīng)點(diǎn)Pi的方位角ψi，通過(guò)下列(9)、(10)、(11)式運(yùn)算車(chē)輛14的目標(biāo)速度Vxi、Vyiθi＝θ-ψi…(9)Vxi＝Vcosθi…(10)Vyi＝Vsinθi…(11)但是，上述(9)式中，若θ和ψ1相同，也可以相似，因此，θi成為零，橫(法線)方向速度Vyi成為零.結(jié)果，只要控制(10)式所示的縱(切線)方向速度Vxi就可以了，因此，可以目標(biāo)速度的形式輸入加速控制器9和制動(dòng)控制器10，以控制加速器、制動(dòng)器。
如上所述，根據(jù)本發(fā)明，通過(guò)簡(jiǎn)易運(yùn)算可求車(chē)輛所操的舵角，因此，通過(guò)簡(jiǎn)易處理可制導(dǎo)車(chē)輛簡(jiǎn)易地在曲線行駛路線上行駛。
還有，由于考慮到了行駛路線法線方向偏差值的積分值，而運(yùn)算所操的舵角，所以即使車(chē)輛調(diào)整出了毛病，也能制導(dǎo)車(chē)輛高精度地沿預(yù)定行駛路線行駛。
還有，根據(jù)基于行駛路線曲率的基本所舵角，預(yù)先給予所操的必需舵角，進(jìn)而使基于編差值的所操的舵角接近，修正來(lái)自路線的偏差，所以即使對(duì)于曲率半徑復(fù)雜變化的曲線路線，也能及時(shí)、高精度地制導(dǎo)行駛。
本發(fā)明對(duì)于不論是無(wú)人駕駛的車(chē)輛，還是有人駕駛的車(chē)輛均可適用。此外，本發(fā)明對(duì)于不論是室外行駛的自卸等車(chē)輛，還是室內(nèi)行駛的無(wú)人駕駛的搬運(yùn)車(chē)等車(chē)輛，也均可適用。
權(quán)利要求
1.一種制導(dǎo)車(chē)輛行駛的控制裝置，存儲(chǔ)預(yù)定行駛路線的位置數(shù)據(jù)，根據(jù)該位置數(shù)據(jù)控制車(chē)輛所操的舵角，制導(dǎo)車(chē)輛沿所述預(yù)定行駛路線行駛，其特征在于，該裝置具有存儲(chǔ)手段，存儲(chǔ)表示所述預(yù)定行駛路線各點(diǎn)上的各該點(diǎn)的坐標(biāo)位置和所述車(chē)輛應(yīng)行駛方向的數(shù)據(jù)；位置檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的坐標(biāo)位置；速度檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的行駛速度；選擇手段，從所述存儲(chǔ)手段的存儲(chǔ)內(nèi)容中，選擇最接近于以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置的、預(yù)定行駛路線上各坐標(biāo)位置的數(shù)據(jù)，以及該最接近點(diǎn)的行駛方向數(shù)據(jù)；偏差值運(yùn)算手段，根據(jù)以所述選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在的坐標(biāo)位置，運(yùn)算沿車(chē)輛行駛方向的垂直方向上車(chē)輛位置現(xiàn)在的偏差值；所操的舵角運(yùn)算手段，根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，為使所述偏差值為零，所述檢測(cè)行駛速度越大，則運(yùn)算車(chē)輛所操的舵角越小；控制手段，根據(jù)以所述所操的舵角運(yùn)算手段運(yùn)算的車(chē)輛所操的舵角，控制所述車(chē)輛所操的舵角。
2.一種制導(dǎo)車(chē)輛行駛的控制裝置，存儲(chǔ)預(yù)定行駛路線的位置數(shù)據(jù)，根據(jù)該位置數(shù)據(jù)控制車(chē)輛所操的舵角，制導(dǎo)車(chē)輛沿所述預(yù)定路線行駛，其特征在于，該裝置具有存儲(chǔ)手段，存儲(chǔ)表示所述預(yù)定行駛路線的各點(diǎn)上的各該點(diǎn)的坐標(biāo)位置和所述車(chē)輛應(yīng)行駛方向的數(shù)據(jù)；位置檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的坐標(biāo)位置；速度檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的行駛速度；選擇手段，從所述存儲(chǔ)手段的存儲(chǔ)內(nèi)容中，選擇最接近于以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置的、預(yù)定行駛路線上坐標(biāo)位置的數(shù)據(jù)，以及該最近點(diǎn)的行駛方向數(shù)據(jù)；偏差值運(yùn)算手段，根據(jù)以所述選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置，運(yùn)算沿車(chē)輛行駛方向的垂直方向上車(chē)輛位置現(xiàn)在的偏差值；積分運(yùn)算手段，以積分法運(yùn)算以所述偏差值運(yùn)算手段運(yùn)算的偏差值；所操的舵角運(yùn)算手段，根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述積分運(yùn)算手段運(yùn)算的積分值，和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在行駛速度，為使所述偏差值為零，所述檢測(cè)行駛速度越大，則運(yùn)算車(chē)輛所操的舵角越??；控制手段，根據(jù)以所述所操的舵角運(yùn)算手段運(yùn)算的車(chē)輛所操的舵角，控制所述車(chē)輛所操的舵角。
3.一種制導(dǎo)車(chē)輛行駛的控制裝置，存儲(chǔ)預(yù)定行駛路線的位置數(shù)據(jù)，根據(jù)該位置數(shù)據(jù)控制車(chē)輛所操的舵角，制導(dǎo)車(chē)輛沿所述預(yù)定路線行駛，其特征在于，該裝置具有存儲(chǔ)手段，存儲(chǔ)表示所述預(yù)定行駛路線的各點(diǎn)上的各該點(diǎn)的坐標(biāo)位置和所述車(chē)輛應(yīng)行駛方向的數(shù)據(jù)；位置檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的坐標(biāo)位置；速度檢測(cè)手段，檢測(cè)所述車(chē)輛現(xiàn)在的行駛速度；選擇手段，從所述存儲(chǔ)手段的存儲(chǔ)內(nèi)容中，選擇最接近于以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置的、預(yù)定行駛路線上坐標(biāo)位置的數(shù)據(jù)，以及該最近點(diǎn)的行駛方向數(shù)據(jù)；偏差值運(yùn)算手段，根據(jù)以所述選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以所述位置檢測(cè)手段檢測(cè)的現(xiàn)在坐標(biāo)位置，運(yùn)算沿車(chē)輛行駛方向上的垂直方向上車(chē)輛位置現(xiàn)在的偏差值；積分運(yùn)算手段，以積分法運(yùn)算以所述偏差值運(yùn)算手段運(yùn)算的偏差值；第一所操的舵角運(yùn)算手段，根據(jù)以所述偏差值運(yùn)算手段運(yùn)算的偏差值和以所述積分運(yùn)算手段運(yùn)算的積分值，和以所述速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，為使所述偏差值為零，所述檢測(cè)行駛速度越大，則運(yùn)算車(chē)輛所操的舵角越?。坏诙俚亩娼沁\(yùn)算手段，根據(jù)所述最近點(diǎn)的所述預(yù)定行駛路線的曲率半徑，運(yùn)算第二車(chē)輛所操的舵角；第三所操的舵角運(yùn)算手段，根據(jù)以所述第一所操的舵角運(yùn)算手段運(yùn)算的第一車(chē)輛所操的舵角，和以所述第二所操的舵角運(yùn)算手段運(yùn)算的第二的車(chē)輛所操的舵角，運(yùn)算第三的車(chē)輛所操的舵角；控制手段，根據(jù)以所述第三所操的舵角運(yùn)算手段運(yùn)算的第三車(chē)輛所操的舵角，控制所述車(chē)輛所操的舵角。
全文摘要
本發(fā)明控制裝置的目的在于，提高制導(dǎo)車(chē)輛沿預(yù)定行駛路線行駛時(shí)的制導(dǎo)行駛的精確度，也即是說(shuō)，根據(jù)以選擇手段選擇的坐標(biāo)位置數(shù)據(jù)以及行駛方向數(shù)據(jù)和以檢測(cè)位置手段檢測(cè)的現(xiàn)在坐標(biāo)位置，運(yùn)算沿車(chē)輛行駛方向上的垂直方向車(chē)輛位置的現(xiàn)在偏差值。隨后，根據(jù)以偏差值運(yùn)算手段運(yùn)算的偏差值和以速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，使偏差值為零，檢測(cè)行駛速度越大，運(yùn)算車(chē)輛所操的舵角越小。然后，根據(jù)以所操的舵角運(yùn)算手段運(yùn)算的車(chē)輛所操的舵角，控制車(chē)輛所操的舵角，高精度地制導(dǎo)車(chē)輛沿預(yù)定行駛路線行駛。還有，用積分運(yùn)算以偏差手段運(yùn)算的偏差值。然后，根據(jù)以運(yùn)算偏差值手段運(yùn)算的偏差值、以積分演運(yùn)手段運(yùn)算的積分值和以速度檢測(cè)手段檢測(cè)的現(xiàn)在的行駛速度，使偏差值為零，檢測(cè)行駛速度越大，演算的車(chē)輛所操的舵角越小。
文檔編號(hào)G05D1/02GK1163005SQ95196139
公開(kāi)日1997年10月22日 申請(qǐng)日期1995年11月10日 優(yōu)先權(quán)日1995年11月10日
發(fā)明者大川幸男 申請(qǐng)人:株式會(huì)社小松制作所