專利名稱：浮游移動(dòng)體的控制系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及水下、空中、宇宙空間等中的浮游移動(dòng)體、或在平面上滑行的移動(dòng)體的控制系統(tǒng)。
背景技術(shù)：
首先，參照?qǐng)D11～圖14，對(duì)已知的浮游移動(dòng)體的控制方法進(jìn)行說明。圖11是已知的浮游移動(dòng)體的概略圖，圖11～圖14是表示已知的浮游移動(dòng)體的控制方法的概略圖。
如圖11所示，為了控制以水下機(jī)器人為代表的浮游移動(dòng)體10主體的位置和速度，在該主體上設(shè)有傳感器13，由此來檢測(cè)出浮游移動(dòng)體10主體的位置和速度，并提供給計(jì)算機(jī)P內(nèi)的減法運(yùn)算電路11。另外，還由操作者H向減法運(yùn)算電路11提供表示成為目標(biāo)的位置和速度的信號(hào)。而減法運(yùn)算電路11的輸出提供給推進(jìn)器控制電路12，由此控制浮游移動(dòng)體10的位置和速度(所謂的位置/速度反饋控制)，該推進(jìn)器控制電路12用于控制設(shè)置在浮游移動(dòng)體10上的作為推進(jìn)力產(chǎn)生裝置(執(zhí)行器)的推進(jìn)器T。
圖12是對(duì)該圖11所示的浮游移動(dòng)體10進(jìn)行控制的框圖。作為推進(jìn)器控制電路12的輸出的執(zhí)行器(effector)推力指令信號(hào)，被提供給具有規(guī)定的推力特性14的推進(jìn)器T，由此把執(zhí)行器推力賦予浮游移動(dòng)體10，如參照符號(hào)15所示，獲得浮游移動(dòng)體10的動(dòng)力特性(dynamics)、即動(dòng)力學(xué)輸入輸出特性，并利用傳感器13如上述所述來檢測(cè)出其主體的位置和速度，并提供給減法運(yùn)算電路11。
這樣，在以往的浮游移動(dòng)體的控制中，在通過反饋成為控制對(duì)象的浮游移動(dòng)體的位置和速度的信息來直接對(duì)執(zhí)行器的推力指令進(jìn)行操作時(shí)，在一般的執(zhí)行器的響應(yīng)速度比較慢的浮游移動(dòng)體中，從向執(zhí)行器發(fā)出指令，到浮游移動(dòng)體的位置和速度反映出其應(yīng)變，并被傳感器得到為止，需要一定的時(shí)間，因而存在著對(duì)控制性能產(chǎn)生不良影響這樣的問題(所謂的傳感延遲的問題)。而且浮游移動(dòng)體的質(zhì)量越大、或者執(zhí)行器的推力越小該傾向越明顯。
另外，在水下、空中等流體中的浮游移動(dòng)體，除了經(jīng)常受波浪、潮流、風(fēng)等流體力學(xué)上的干擾的影響外，還如圖11所示，由于水下的浮游移動(dòng)體10連接有來自水上的控制線纜C(供電線纜和通信線纜等)等，所以經(jīng)常出現(xiàn)該線纜抻拽浮游移動(dòng)體10的情況，按上述的控制方法，對(duì)未知的干擾作出響應(yīng)耗費(fèi)時(shí)間，因而也存在著對(duì)控制性能產(chǎn)生不良影響這樣的問題。
接下來，當(dāng)如圖11所示，在浮游移動(dòng)體10上還設(shè)置有具有多個(gè)連桿的機(jī)械臂A時(shí)，則構(gòu)成圖13所圖示的框圖。參照符號(hào)16所示的機(jī)械臂A的反作用力和扭矩，如作為加法運(yùn)算電路17所舉例說明的那樣，被加到推進(jìn)器T的執(zhí)行器推力上。因此，來自機(jī)械臂A的反作用力和扭矩使得浮游移動(dòng)體10的動(dòng)作，即位置和速度發(fā)生紊亂，需要控制推進(jìn)器T來修正因來自該機(jī)械臂A的反作用力和扭矩而產(chǎn)生的浮游移動(dòng)體10的位置和速度的誤差。這樣，由于來自機(jī)械臂A的反作用力和扭矩使得浮游移動(dòng)體10的動(dòng)作(位置和速度)發(fā)生紊亂，所以，在以往的姿勢(shì)控制中，通過使推進(jìn)器T動(dòng)作來修正所產(chǎn)生的位置和速度的誤差。
即，在已知的控制方法中，在浮游移動(dòng)體10因機(jī)械臂A的反作用力和扭矩而動(dòng)作后，使推進(jìn)器T動(dòng)作，因而，依然沒有解決對(duì)干擾的響應(yīng)速度慢的所謂的傳感延遲的問題。
另外，圖14是表示其它現(xiàn)有技術(shù)的框圖。該控制方法考慮到機(jī)械臂A的運(yùn)動(dòng)與浮游移動(dòng)體10主體的運(yùn)動(dòng)之間的相互干擾，同時(shí)確定執(zhí)行器推力和機(jī)械臂關(guān)節(jié)扭矩，使得各自的動(dòng)作與目的一致。在該現(xiàn)有技術(shù)中，把具備了機(jī)械臂A的浮游移動(dòng)體10作為多體連桿系統(tǒng)，根據(jù)其運(yùn)動(dòng)方程式，把各個(gè)機(jī)械臂的各個(gè)關(guān)節(jié)扭矩對(duì)浮游移動(dòng)體10主體的運(yùn)動(dòng)所產(chǎn)生的影響利用算式來描述，通過求解該算式來確定浮游移動(dòng)體10主體的推進(jìn)力(所謂的基于模型的控制)。
在該圖14的現(xiàn)有技術(shù)中，把來自于推進(jìn)器/機(jī)械臂控制電路18的執(zhí)行器推力指令信號(hào)提供給推進(jìn)器T，另外，把機(jī)械臂A的扭矩指令信號(hào)提供給關(guān)節(jié)致動(dòng)器19。這樣，由從推進(jìn)器T輸出的執(zhí)行器推力和關(guān)節(jié)致動(dòng)器19的關(guān)節(jié)扭矩形成了浮游移動(dòng)體10主體和機(jī)械臂A的復(fù)合系統(tǒng)動(dòng)力特性20，傳感器13檢測(cè)浮游移動(dòng)體10主體的位置和速度，并提供給減法器11，該減法器11還被提供了表示浮游移動(dòng)體10主體的成為目標(biāo)的位置和姿勢(shì)的信號(hào)。另外，來自復(fù)合系統(tǒng)動(dòng)力特性20的表示機(jī)械臂控制變量、即位置、姿勢(shì)、關(guān)節(jié)角度、速度、指尖反作用力等的信號(hào)被提供給減法運(yùn)算電路21，該減法運(yùn)算電路21還被提供了機(jī)械臂A的控制變量的目標(biāo)值。這些減法器11、21的輸出，被提供給推進(jìn)器/機(jī)械臂控制電路18。
即，在該控制方法中，考慮機(jī)械臂A的運(yùn)動(dòng)和浮游移動(dòng)體10主體的運(yùn)動(dòng)之間的相互干擾來同時(shí)確定執(zhí)行器推力和機(jī)械臂關(guān)節(jié)扭矩，使得各自的動(dòng)作與目的一致。由此，具有可準(zhǔn)確地控制浮游移動(dòng)體10主體的動(dòng)作這樣的優(yōu)點(diǎn)，然而還存在以下的(1)～(3)的問題。
(1)對(duì)象的動(dòng)力特性一般都很復(fù)雜，如果機(jī)械臂A為6軸，則加上浮游移動(dòng)體10主體，將成為7個(gè)剛體連接在一起的連桿的動(dòng)力特性。如果機(jī)械臂A有多個(gè)，則將進(jìn)一步變成非常復(fù)雜的動(dòng)力特性。對(duì)于這樣的控制系統(tǒng)的運(yùn)算需要花費(fèi)大量的工夫，給計(jì)算機(jī)帶來沉重的負(fù)荷。因此，在實(shí)際中使用小型計(jì)算機(jī)來構(gòu)成上述的控制系統(tǒng)是非常困難的。
(2)另外，在上述的控制系統(tǒng)中，需要機(jī)械臂A的各個(gè)連桿和主體的質(zhì)量、慣性力矩、重心位置等多種力學(xué)參數(shù)，對(duì)這些力學(xué)參數(shù)而言，如果不是全部使用正確的值，則不能實(shí)現(xiàn)正確的控制。因此，在用機(jī)械臂A抓取物體等情況下，必須正確地提供被抓取的物體的質(zhì)量、慣性力矩、和重心位置等。因此，需要具備可抓取的物體的數(shù)據(jù)庫，或測(cè)量對(duì)象的力學(xué)特性，在實(shí)際中存在其用途受到限制的問題。
(3)另外，在水下、空中等流體中的浮游移動(dòng)體，不僅經(jīng)常受到波浪、潮流、風(fēng)等流體力學(xué)上未知的干擾的影響，而且如圖11所示，水下的浮游移動(dòng)體10還連接有來自水上的控制線纜C(供電線纜和通信線纜等)等，因而時(shí)常會(huì)發(fā)生該線纜拖拽浮游移動(dòng)體10的情況，這樣的未知的干擾不僅不能模型化，而且一般也很難對(duì)該干擾的影響進(jìn)行預(yù)測(cè)，所以這樣的基于模型的控制方法存在著根本不能適應(yīng)浮游移動(dòng)體的控制的問題。
另外，作為已知的浮游移動(dòng)體的控制方法，還有專利文獻(xiàn)1所公開的機(jī)械手根部的力反饋控制，或非專利文獻(xiàn)1所提出的推力局部反饋控制等。
根據(jù)專利文獻(xiàn)1的機(jī)械手根部的力反饋控制，在浮游移動(dòng)體主體因機(jī)械手的干擾而動(dòng)作之前，先使推進(jìn)器動(dòng)作來消除對(duì)浮游移動(dòng)體主體的影響，從而可防止浮游移動(dòng)體主體因機(jī)械手的干擾而動(dòng)作。
另外，作為用于控制浮游移動(dòng)體的位置/速度的執(zhí)行器而使用的推進(jìn)器，由于利用流體來產(chǎn)生推力，所以在對(duì)推進(jìn)器的輸入指令與實(shí)際輸出的推力之間存在起因于流體的較強(qiáng)的非線性特性，而且構(gòu)成這樣的非線性特性的要素涉及多方面，以往難以進(jìn)行將這些全盤考慮了的控制(所謂的執(zhí)行器動(dòng)態(tài)特性的非線性特性的問題)，但根據(jù)非專利文獻(xiàn)1涉及的推力局部反饋控制，可防止因上述執(zhí)行器動(dòng)態(tài)特性的非線性特性而導(dǎo)致的浮游移動(dòng)體的控制性能的惡化。
然而，上述專利文獻(xiàn)1和非專利文獻(xiàn)1涉及的控制方法，雖然都是測(cè)量一部分的力并進(jìn)行反饋，但都不能消除作為對(duì)浮游移動(dòng)體的最大干擾的波浪、潮流、風(fēng)等的影響。
總之，在上述任意一種現(xiàn)有技術(shù)中都還未實(shí)現(xiàn)以下這樣的浮游移動(dòng)體的控制系統(tǒng)即使有機(jī)械臂的反作用等的干擾，以及處于水下或空中等流體中的浮游移動(dòng)體經(jīng)常受到的波浪、潮流、風(fēng)等流體力學(xué)上的未知的干擾，也能夠自動(dòng)地對(duì)干擾的影響進(jìn)行補(bǔ)償，從而可使浮游移動(dòng)體高精度地靜止，或高精度地追蹤目標(biāo)軌道。
另一方面，除了上述的問題外，作為使浮游移動(dòng)體航行的已知方法，在多數(shù)情況下使用慣性導(dǎo)航，然而該慣性導(dǎo)航存在著以下的問題。即，慣性導(dǎo)航使用根據(jù)浮游移動(dòng)體求出的加速度，在浮游移動(dòng)體自身很重的情況下，暫時(shí)把加速度轉(zhuǎn)換為位移、然后把加速度的值作為電信號(hào)輸出的已知的加速度傳感器，從其構(gòu)造上講，不能高精度地檢測(cè)出微小的加速度的變化，存在著從傳感器獲得的加速度值與實(shí)際的加速度值之間產(chǎn)生明顯誤差這樣的問題。這樣的加速度值的誤差，對(duì)慣性導(dǎo)航自身的精度也產(chǎn)生明顯的影響，由此產(chǎn)生了不能沿著所要的軌道準(zhǔn)確地引導(dǎo)浮游移動(dòng)體的不良情況。
專利文獻(xiàn)1JP特開平5-119837號(hào)公報(bào)非專利文獻(xiàn)1金岡、中山、林、川村著“用于實(shí)現(xiàn)水下高精度運(yùn)動(dòng)的推進(jìn)器推力局部反饋控制”，日本機(jī)械學(xué)會(huì)機(jī)器人學(xué)及機(jī)械電子講演會(huì)′03講演論文集2P1-2F-A6，2003年發(fā)明內(nèi)容因此，本發(fā)明的目的是提供一種浮游移動(dòng)體的控制系統(tǒng)，該控制系統(tǒng)即使在受到例如波浪、潮流、風(fēng)以及安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾的情況下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道。
本發(fā)明的另一個(gè)目的是提供一種能夠以比較簡(jiǎn)單的結(jié)構(gòu)，準(zhǔn)確地控制浮游移動(dòng)體的位置和姿勢(shì)等的浮游移動(dòng)體的控制系統(tǒng)。
另外，本發(fā)明的再一個(gè)目的是提供一種浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)，該加速度檢測(cè)系統(tǒng)能夠高精度檢測(cè)出浮游移動(dòng)體的加速度，甚至是微小的加速度變化，而不依賴于浮游移動(dòng)體的大小、重量，或加速度傳感器的性能。
本發(fā)明的發(fā)明人發(fā)現(xiàn)如果把浮游移動(dòng)體分成占浮游移動(dòng)體的一部分、并可視為單一剛體的主體部、和對(duì)浮游移動(dòng)體產(chǎn)生推力的執(zhí)行器部，并在對(duì)該主體部和執(zhí)行器部進(jìn)行力學(xué)結(jié)合的唯一部分(推力傳輸門)上設(shè)置力/扭矩傳感器，則由于該力/扭矩傳感器所檢測(cè)出的值是從執(zhí)行器部施加給主體部的推力，所以可將施加在主體部與執(zhí)行器部之間的力和扭矩全部檢測(cè)出來，如果反饋該值，則可直接控制對(duì)浮游移動(dòng)體施加的推力，由此，即使在受到波浪、潮流等流體力學(xué)上的干擾下，也能夠高精度地使浮游移動(dòng)體靜止在規(guī)定位置，或高精度地追蹤目標(biāo)軌道，并據(jù)此完成了本發(fā)明。
能夠解決上述問題的本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)的特征在于(1)在浮游移動(dòng)體的控制系統(tǒng)中，上述浮游移動(dòng)體包括主體部，占上述浮游移動(dòng)體的一部分、并可視為單一性剛體；執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力；以及推力傳輸門，是對(duì)上述主體部和上述執(zhí)行器部進(jìn)行力學(xué)結(jié)合的部分，構(gòu)成為可實(shí)際測(cè)定從上述執(zhí)行器部作用于上述主體部的推力；使用來自上述推力傳輸門的推力測(cè)定值來得到對(duì)上述執(zhí)行器部的推力指令。
另外，本發(fā)明的特征還在于，在上述的浮游移動(dòng)體的控制系統(tǒng)中，(2)上述主體部具有可測(cè)量上述主體部的加速度的加速度測(cè)定裝置，上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置；使用轉(zhuǎn)換來自操作者的目標(biāo)位置速度軌道指令而得到的目標(biāo)加速度軌道指令、來自上述主體部的上述加速度測(cè)定裝置的輸出、以及來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出，來得到對(duì)上述執(zhí)行器部的推力指令。
另外，除了后面的實(shí)施方式和實(shí)施例所說明的在三維空間內(nèi)浮游的浮游移動(dòng)體以外，在把本發(fā)明的控制系統(tǒng)和加速度檢測(cè)系統(tǒng)應(yīng)用于在斜面上滑行的浮游移動(dòng)體的情況下，由于需要對(duì)重力加速度的影響進(jìn)行補(bǔ)償，所以，需要如后面的(4)或(8)的發(fā)明所規(guī)定的那樣，具有可測(cè)量主體部的傾斜的傾斜角測(cè)定裝置。
但是，在將本發(fā)明的控制系統(tǒng)或加速度檢測(cè)系統(tǒng)應(yīng)用于在根本沒有重力作用的宇宙空間中浮游的浮游移動(dòng)體、和在不需要考慮傾斜或可忽略傾斜的水平面上滑行的浮游移動(dòng)體時(shí)，如上述(2)或下面所說明的(3)或下述(7)的發(fā)明所規(guī)定的那樣，不需要傾斜角測(cè)定裝置。
這在應(yīng)用了虛擬推力傳輸門系統(tǒng)的構(gòu)思的下述(5)的發(fā)明的情況下也一樣，在將本發(fā)明的使用了虛擬推力傳輸門系統(tǒng)的構(gòu)思的控制系統(tǒng)應(yīng)用于在三維空間內(nèi)浮游的浮游移動(dòng)體等時(shí)，如下述(6)的發(fā)明所規(guī)定的那樣，需要具備可測(cè)量浮游移動(dòng)體的傾斜的傾斜角測(cè)定裝置。
同樣，本發(fā)明的特征還在于，在上述的浮游移動(dòng)體的控制系統(tǒng)中，(3)上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置，并且上述主體部只與上述推力傳輸門進(jìn)行力學(xué)結(jié)合，而且構(gòu)成為作用于上述主體部的力實(shí)質(zhì)上全部通過上述執(zhí)行器部和上述推力傳輸門來輸入；使用轉(zhuǎn)換來自操作者的目標(biāo)位置速度軌道指令而得到的目標(biāo)加速度軌道指令、以及來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出，來得到對(duì)上述執(zhí)行器部的推力指令。
另外，作為上述主體部構(gòu)成為只與上述推力傳輸門進(jìn)行力學(xué)結(jié)合、而且作用于上述主體部的力實(shí)質(zhì)上全部通過上述執(zhí)行器部和上述推力傳輸門來輸入的典型例子，可列舉出上述執(zhí)行器部呈包圍主體部的殼狀體。
但是，這樣的構(gòu)造是極端的例子，由呈殼狀體的執(zhí)行器部來密封主體部不是必要的條件?？傊?，在由該(3)所規(guī)定的發(fā)明中，只要構(gòu)成為對(duì)主體部不直接施加水壓、較強(qiáng)的風(fēng)壓等干擾以及其它外力的構(gòu)造即可，例如，空氣與被收納于在水上或地上滑行的浮游移動(dòng)體內(nèi)部的主體部接觸這樣的情況是足以忽略的程度，因此在該(3)涉及的發(fā)明的應(yīng)用上不會(huì)出現(xiàn)問題。
因此，可理解為由該(3)所規(guī)定的發(fā)明不限于圖3所示的構(gòu)造。
另外，本發(fā)明的特征還在于，在上述(2)或(3)的浮游移動(dòng)體的控制系統(tǒng)中，(4)上述主體部還具有可測(cè)量上述主體部的傾斜的傾斜角測(cè)定裝置；還利用來自上述主體部的上述傾斜角測(cè)定裝置的輸出來得到對(duì)上述執(zhí)行器部的推力指令。
另外，本發(fā)明的特征還在于，(5)在浮游移動(dòng)體的控制系統(tǒng)中，上述浮游移動(dòng)體在計(jì)算上被虛擬地分成主體部，占上述浮游移動(dòng)體的一部分、并可視為單一性剛體；以及執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力，上述浮游移動(dòng)體具有可測(cè)量上述浮游移動(dòng)體的加速度的加速度測(cè)定裝置，使用來自上述加速度測(cè)定裝置的輸出，在計(jì)算上推定來自虛擬推力傳輸門的力或扭矩，得到對(duì)上述執(zhí)行器部的推力指令。
另外，本發(fā)明的特征還在于，在上述(5)的浮游移動(dòng)體的控制系統(tǒng)中，(6)上述浮游移動(dòng)體還具有可測(cè)量上述浮游移動(dòng)體的傾斜的傾斜角測(cè)定裝置；使用來自上述傾斜角測(cè)定裝置和上述加速度測(cè)定裝置的輸出，在計(jì)算上推定來自虛擬推力傳輸門的力或扭矩，由此得到對(duì)上述執(zhí)行器部的推力指令。
另外，如下所述，在上述本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)中，浮游移動(dòng)體的控制通過“推力計(jì)劃部”和“推力控制部”2級(jí)結(jié)構(gòu)來進(jìn)行，“推力計(jì)劃部”為了實(shí)現(xiàn)目標(biāo)軌道而計(jì)算出主體部應(yīng)從推力傳輸門接受的推力并發(fā)出指令，“推力控制部”把推力傳輸門推力的當(dāng)前值反饋到執(zhí)行器部來直接驅(qū)動(dòng)執(zhí)行器部，使得在推力傳輸門實(shí)現(xiàn)指令推力。
在本發(fā)明中，把上述的機(jī)構(gòu)設(shè)計(jì)和控制方法統(tǒng)稱為“推力傳輸門系統(tǒng)”。作為推力傳輸門系統(tǒng)的實(shí)施方式，主要有以下3種i)使主體部露出在外部的狀態(tài)，即、采用使主體部可直接受到水壓、較強(qiáng)的風(fēng)壓等干擾、以及其它外力的影響的結(jié)構(gòu)，并且具有推力傳輸門的方式(參照下述的第1實(shí)施方式的第1例)；ii)具有推力傳輸門，并且主體部只與推力傳輸門進(jìn)行力學(xué)結(jié)合，且作用于主體部的力實(shí)質(zhì)上全部通過執(zhí)行器部和推力傳輸門輸入，以便使水壓、較強(qiáng)的風(fēng)壓等干擾以及其它外力不直接施加于主體部(例如利用執(zhí)行器部覆蓋、密封主體部的狀態(tài)等)的方式(參照下述的第1實(shí)施方式的第2例)；iii)不改變浮游移動(dòng)體的硬件，但在計(jì)算上將浮游移動(dòng)體分為主體部和執(zhí)行器部，并在計(jì)算機(jī)內(nèi)預(yù)先準(zhǔn)備主體部的慣性矩陣，使用該慣性矩陣、設(shè)在浮游移動(dòng)體上的加速度傳感器的輸出、以及來自根據(jù)需要而設(shè)置在浮游移動(dòng)體上的傾斜角傳感器的輸出，來得到對(duì)執(zhí)行器部的推力指令的方式(虛擬推力傳輸門、參照下述的第1實(shí)施方式的第3例)。
對(duì)于i)和ii)而言，雖然為了實(shí)現(xiàn)推力傳輸門系統(tǒng)，需要變更浮游移動(dòng)體的硬件設(shè)計(jì)，并且在推力傳輸門中需要力/扭矩傳感器，但所實(shí)現(xiàn)的控制性能較高。
另一方面，對(duì)于iii)而言，雖然與i)和ii)的方法相比，控制性能降低，但不需要改變浮游移動(dòng)體的硬件設(shè)計(jì)，只要在現(xiàn)有的浮游移動(dòng)體上附加加速度傳感器、另外根據(jù)需要而附加傾斜角傳感器，即可使用推力傳輸門系統(tǒng)，從而能提高控制性能。
特別是，目前在航空器等中裝備的公知的慣性導(dǎo)航裝置，也可以作為在本發(fā)明中所使用的加速度傳感器、傾斜角傳感器、主體部位置/速度傳感器來使用，如果使用這些，則與現(xiàn)有的控制相比，可實(shí)現(xiàn)高速化、高精度化，而無須進(jìn)行硬件的改變。
另外，本發(fā)明的特征還在于，(7)在用于檢測(cè)浮游移動(dòng)體的加速度系統(tǒng)中，上述浮游移動(dòng)體包括主體部，占上述浮游移動(dòng)體的一部分、且可視為單一性剛體；執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力；和推力傳輸門，是對(duì)上述主體部和上述執(zhí)行器部進(jìn)行力學(xué)結(jié)合的部分，構(gòu)成為可實(shí)際測(cè)定從上述執(zhí)行器部作用于上述主體部的推力；上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置，并且，上述主體部只與上述推力傳輸門進(jìn)行力學(xué)結(jié)合，而且構(gòu)成為作用于上述主體部的力實(shí)質(zhì)上全部通過上述執(zhí)行器部和上述推力傳輸門來輸入；使用來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出來得到上述主體部的加速度的推定值。
另外，本發(fā)明的特征還在于，在上述(7)的浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)中，(8)上述主體部還具有可測(cè)量上述主體部的傾斜的傾斜角測(cè)定裝置；還利用來自上述主體部的上述傾斜角測(cè)定裝置的輸出，生成上述主體部的加速度的推定值。
根據(jù)本發(fā)明，可提供一種能夠高速且高精度地控制浮游移動(dòng)體的系統(tǒng)結(jié)構(gòu)，即使在例如波浪、潮流、風(fēng)或安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道。
另外，以下對(duì)在本發(fā)明的說明中使用的技術(shù)用語進(jìn)行定義。
所謂“浮游移動(dòng)體”是指在水下、空中或宇宙空間等中浮游的各種移動(dòng)體。而且，浮游移動(dòng)體的概念還包括在平面上、或在軌道上滑行的移動(dòng)體。
本發(fā)明所述的“浮游移動(dòng)體”的概念，根據(jù)不同的環(huán)境，包括如下列舉的具體的例子
(1)宇宙宇宙機(jī)器人、宇宙飛船、人造衛(wèi)星等(2)空中航空機(jī)器人、航空器、直升飛機(jī)、飛船等(3)水下水下機(jī)器人、潛水艇等(4)水上船舶、氣墊船等(5)地上汽車、火車、氣墊船、或雪橇、摩托雪橇以及其它在冰上或雪上滑行的移動(dòng)體等。
關(guān)于汽車等地上車輛，可理解為在例如車輪打滑的狀態(tài)下，能成為“浮游移動(dòng)體”。因此，通過應(yīng)用本發(fā)明，在汽車的情況下，無論車輪打滑還是不打滑，都可以同樣進(jìn)行車輛控制。
不過，在車輪打滑的狀態(tài)下，由現(xiàn)有的發(fā)動(dòng)機(jī)和輪胎構(gòu)成的推力傳輸機(jī)構(gòu)不能發(fā)揮作用。如果考慮到這一點(diǎn)，在該狀況下應(yīng)用本發(fā)明時(shí)，最好使上述地上車輛具備另外不同的推力傳輸機(jī)構(gòu)(各種形式的推進(jìn)器等)，對(duì)此，通過以下的各個(gè)說明可以理解。
所謂“主體部”是指，占浮游移動(dòng)體的一部分、且可視為單一剛體的物理構(gòu)造。原則上，在主體部中不包括可機(jī)械性活動(dòng)的部分。而且，即使有可機(jī)械性活動(dòng)的部分，如果其影響小到可以被忽略的程度也可以，除此以外，即使不能忽略，只要能夠預(yù)測(cè)其影響，就能將該影響消除也可以。
另外，在應(yīng)用虛擬推力傳輸門系統(tǒng)的構(gòu)思的本發(fā)明中的、與下述第1實(shí)施方式的第3例相當(dāng)?shù)陌l(fā)明中，只不過是單純?yōu)榱吮阌谡f明而進(jìn)行了主體部和執(zhí)行器的區(qū)別，將哪個(gè)部分規(guī)定為主體部，也可以由用戶來判斷。因此，在實(shí)際實(shí)施與下述第1實(shí)施方式的第3例相當(dāng)?shù)陌l(fā)明時(shí)，除可以把主體部規(guī)定為占浮游移動(dòng)體的質(zhì)量的主要部分的部分以外，也可以相反地把追加設(shè)置在浮游移動(dòng)體中的一部分上的物體(例如，數(shù)千克左右的錘等)規(guī)定為主體部。
另外，即使在不應(yīng)用虛擬推力傳輸門系統(tǒng)的構(gòu)思的、與下述第1實(shí)施方式的第1例和第2例相當(dāng)?shù)陌l(fā)明中，區(qū)別主體部和執(zhí)行器部的判斷基準(zhǔn)也只是兩者是否由下面說明的推力傳輸門進(jìn)行結(jié)合，只要滿足這一點(diǎn)，對(duì)浮游移動(dòng)體中的主體部所占的比例就沒有特殊的限定。
因此，在本發(fā)明中，主體部不限于由占浮游移動(dòng)體的質(zhì)量的主要部分的單一剛體所構(gòu)成的物理構(gòu)造。
所謂“執(zhí)行器部”是指，對(duì)浮游移動(dòng)體產(chǎn)生推力的物理構(gòu)造。其包括所有的用于控制浮游移動(dòng)體的位置、速度等的推進(jìn)力產(chǎn)生裝置(執(zhí)行器)。
所謂“力學(xué)結(jié)合”是指，在作為對(duì)象的物體之間產(chǎn)生了某種力的相互作用的狀態(tài)。雖然幾乎都是機(jī)械性結(jié)合的狀態(tài)，但也包括非機(jī)械性結(jié)合，例如由電磁力等形成非接觸性的力的相互作用的狀態(tài)。
另一方面，所謂“力學(xué)分離”是指，在作為對(duì)象的物體之間不產(chǎn)生力的相互作用的狀態(tài)。
所謂“推力傳輸門”是指，是對(duì)主體部和執(zhí)行器部進(jìn)行力學(xué)結(jié)合的唯一部分，具備能夠檢測(cè)施加在主體部與執(zhí)行器部之間的所有的力和扭矩的傳感器，從而可檢測(cè)出從執(zhí)行器部作用于主體部的所有的推力。
所謂“虛擬推力傳輸門”是指，假設(shè)設(shè)置在浮游移動(dòng)體內(nèi)而在物理上不存在的推力傳輸門，在為了便于計(jì)算而分成主體部和執(zhí)行器部的現(xiàn)有的浮游移動(dòng)體中，構(gòu)成為根據(jù)主體部加速度測(cè)定值，能夠推定施加在主體部和執(zhí)行器部之間的力和扭矩、即執(zhí)行器部作用于主體部的所有的推力。
另外，在本發(fā)明中，只稱“推力傳輸門”時(shí)，如果沒有特殊的指定，則包括“虛擬推力傳輸門”。
所謂“虛擬推力傳輸門的推力推定值”是指，不是在物理上存在的推力傳輸門處實(shí)際測(cè)量出的值，而是在虛擬推力傳輸門處根據(jù)加速度傳感器的測(cè)定值(主體部加速度測(cè)定值)通過計(jì)算而求出的虛擬值。
所謂“推力計(jì)劃部”是指，為了實(shí)現(xiàn)目標(biāo)軌道而計(jì)算出主體部在推力傳輸門處應(yīng)從執(zhí)行器部受到的目標(biāo)推力，并對(duì)推力控制部發(fā)出指令的計(jì)算機(jī)上的一部分或者功能。
所謂“推力控制部”是指，通過反饋推力傳輸門處的測(cè)定值或推力推定值而向執(zhí)行器輸出推力指令，以便在推力傳輸門實(shí)現(xiàn)所希望的目標(biāo)推力的計(jì)算機(jī)上的一部分或功能。
所謂“目標(biāo)推力”是指，為了實(shí)現(xiàn)目標(biāo)軌道而計(jì)算出主體部在推力傳輸門處應(yīng)從執(zhí)行器部受到的推力。
所謂“目標(biāo)加速度”是指，通過對(duì)所希望的目標(biāo)軌道或目標(biāo)速度進(jìn)行微分運(yùn)算而求出的加速度。
所謂“推力傳輸門系統(tǒng)(TTGSThrust Transfer Gate System)”是指，利用包括推力計(jì)劃部和推力控制部的控制系統(tǒng)來控制具備推力傳輸門的浮游移動(dòng)體的控制系統(tǒng)。
所謂“執(zhí)行器推力特性”是指，輸入到推進(jìn)器等執(zhí)行器的執(zhí)行器推力指令與實(shí)際輸出的執(zhí)行器推力之間的關(guān)系。另外，由于推進(jìn)器等執(zhí)行器一般是通過流體來產(chǎn)生推力，所以，在執(zhí)行器推力特性中，除了包含執(zhí)行器自身的機(jī)械性動(dòng)力特性之外，還包含受流體力學(xué)中的阻力/升力影響等的復(fù)雜的動(dòng)力特性。
以往，反饋浮游移動(dòng)體的位置/速度的傳感器信息來控制推進(jìn)器的推力，但在本發(fā)明中，直接反饋所產(chǎn)生的推力來控制執(zhí)行器的推力，所以從原理上實(shí)現(xiàn)了高速的響應(yīng)。因此，可緩解上述的傳感延遲的問題。特別是，只要采用具有作為硬件的推力傳輸門的結(jié)構(gòu)(參照第1實(shí)施方式的第1例和第2例等)，則無論是浮游移動(dòng)體的質(zhì)量大，還是執(zhí)行器的推力小，都不會(huì)增加傳感延遲的影響。
在本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)中，如下述的那樣，推力控制部通過反饋推力傳輸門的推力來控制執(zhí)行器的推力。由此，在實(shí)現(xiàn)了上述的高速響應(yīng)的同時(shí)，如果針對(duì)非線性特性對(duì)推力控制部應(yīng)用魯棒(robust)的現(xiàn)有控制方法(滑模(sliding mode)控制等)，則可抑制執(zhí)行器動(dòng)態(tài)特性的非線性特性，從而可防止對(duì)控制性能的不良影響。以往，通過基于執(zhí)行器的動(dòng)態(tài)特性模型的前饋補(bǔ)償來進(jìn)行執(zhí)行器的動(dòng)態(tài)特性的線性化，但是需要大量的工夫和計(jì)算工作。然而，在本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)中，由于通過不必使用動(dòng)態(tài)特性模型的推力反饋來實(shí)現(xiàn)線性化，所以能夠通過少量的計(jì)算工作來構(gòu)筑控制系統(tǒng)。
另外，在以往的技術(shù)中，如上述的那樣，從進(jìn)行傳感到實(shí)際向推進(jìn)器發(fā)出推力指令為止的期間將產(chǎn)生不能忽略的時(shí)間延遲，所以，例如在把浮游移動(dòng)體維持在規(guī)定軌道上時(shí)，必須反復(fù)地進(jìn)行很多次過沖(overshoot)和下沖(undershoot)的修正等，結(jié)果經(jīng)常使浮游移動(dòng)體進(jìn)行無用的動(dòng)作。確切地進(jìn)行控制系統(tǒng)的增益調(diào)節(jié)，除了費(fèi)時(shí)費(fèi)力，還需要具備經(jīng)驗(yàn)，因而負(fù)擔(dān)非常大。
而根據(jù)本發(fā)明，由于能夠適時(shí)地提供推進(jìn)器真正需要的推力，所以不會(huì)使推進(jìn)器和浮游移動(dòng)體進(jìn)行無用的動(dòng)作。而且不會(huì)使浮游移動(dòng)體消耗不必要的推進(jìn)器驅(qū)動(dòng)能量，結(jié)果還可獲得能夠減少浮游移動(dòng)體的耗能的效果。
另外，在本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)中，未知的干擾的影響，可作為推力傳輸門的推力誤差被立刻檢測(cè)出來，并且通過推力控制部的局部反饋控制得到補(bǔ)償，所以在推力計(jì)劃部以上的級(jí)中，不必考慮干擾。即，在推力計(jì)劃部中，只需進(jìn)行“靜止”、“直進(jìn)”等指示，就可在推力控制部中自動(dòng)地對(duì)干擾的影響進(jìn)行補(bǔ)償，能夠?qū)崿F(xiàn)預(yù)先計(jì)劃的所希望的動(dòng)作。另外，與以往那樣的根據(jù)位置/速度誤差來檢測(cè)干擾的影響的情況相比，可實(shí)現(xiàn)高速的響應(yīng)。
另一方面，根據(jù)本發(fā)明的浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)，由于使用預(yù)先確認(rèn)了的主體部的慣性矩陣、來自推力傳輸門的力/扭矩傳感器的輸出、以及根據(jù)需要從主體部的傾斜角傳感器進(jìn)行的輸出，可求出主體部的加速度推定值，所以可高精度地檢測(cè)出浮游移動(dòng)體的加速度，甚至是微小的加速度變化，而與浮游移動(dòng)體的重量和大小無關(guān)，或者不依賴于加速度傳感器的性能，在使用慣性導(dǎo)航引導(dǎo)浮游移動(dòng)體時(shí)，能夠把浮游移動(dòng)體準(zhǔn)確地引導(dǎo)到所希望的軌道上。
下面，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)的說明。


圖1是表示應(yīng)用了本發(fā)明的控制系統(tǒng)的浮游移動(dòng)體的一個(gè)例子的概略圖。
圖2是表示本發(fā)明的控制系統(tǒng)的一個(gè)結(jié)構(gòu)例的方框圖。
圖3是表示應(yīng)用了本發(fā)明的控制系統(tǒng)的浮游移動(dòng)體的另一例子的概略圖。
圖4是表示本發(fā)明的控制系統(tǒng)的另一結(jié)構(gòu)例的方框圖。
圖5是表示應(yīng)用了本發(fā)明的控制系統(tǒng)的浮游移動(dòng)體的另一例子的概略圖。
圖6是表示本發(fā)明的控制系統(tǒng)的另一結(jié)構(gòu)例的方框圖。
圖7是表示本發(fā)明的控制系統(tǒng)的第2實(shí)施方式的方框圖。
圖8是表示本發(fā)明的控制系統(tǒng)的一個(gè)實(shí)施例的方框圖。
圖9是表示本發(fā)明的控制系統(tǒng)的另一實(shí)施例的方框圖。
圖10是表示本發(fā)明的控制系統(tǒng)的另一實(shí)施例的方框圖。
圖11是表示已知的浮游移動(dòng)體的概要的圖。
圖12是表示已知的浮游移動(dòng)體的控制系統(tǒng)的方框圖。
圖13是表示已知的浮游移動(dòng)體的控制系統(tǒng)的其它的方框圖。
圖14是表示已知的浮游移動(dòng)體的控制系統(tǒng)的其它的方框圖。
圖中符號(hào)A、機(jī)械臂部；B、主體部；B1、主體部靜力學(xué)；B2、主體部慣性；B2’、主體部慣性；B3、主體部慣性；B4、積分；C、線纜；E、執(zhí)行器部；E1、執(zhí)行器推力特性；E2、執(zhí)行器部靜力學(xué)；G、推力傳輸門；G’、虛擬推力傳輸門；H、操作者；P、計(jì)算機(jī)；P1、微分；P2、推力計(jì)劃部；P3、推力控制部；P4、位置控制/速度控制；S、耐壓殼；T、推進(jìn)器；1、控制系統(tǒng)；2、加速度傳感器；3、傾斜角傳感器；4、位置/速度傳感器；5、空洞；10、浮游移動(dòng)體；11、減法運(yùn)算電路；12、推進(jìn)器控制電路；13、傳感器；14、執(zhí)行器推力特性；15、浮游移動(dòng)體動(dòng)力特性；16、機(jī)械臂的反作用力/扭矩；17、加法運(yùn)算電路；18、推進(jìn)器/機(jī)械臂控制電路；19、關(guān)節(jié)致動(dòng)器；20、浮游移動(dòng)體主體/機(jī)械臂復(fù)合系統(tǒng)動(dòng)力特性；21、減法運(yùn)算電路。
具體實(shí)施例方式
(第1實(shí)施方式)下面，基于圖1～圖7對(duì)作為實(shí)施本發(fā)明的對(duì)象的代表例的水下機(jī)器人的本發(fā)明一個(gè)實(shí)施方式及其作用進(jìn)行說明。水下機(jī)器人是在三維空間浮游的浮游移動(dòng)體，在對(duì)該水下機(jī)器人應(yīng)用本發(fā)明的控制系統(tǒng)的情況下，由于需要對(duì)重力加速度的影響進(jìn)行補(bǔ)償，所以作為如上述(4)涉及的發(fā)明所規(guī)定的那樣，需要具備可測(cè)量主體部的傾斜的傾斜角測(cè)定裝置。這與應(yīng)用虛擬推力傳輸門系統(tǒng)的構(gòu)思的上述(5)的發(fā)明的情況一樣，在對(duì)浮游于三維空間內(nèi)的浮游移動(dòng)體等應(yīng)用使用了虛擬推力傳輸門系統(tǒng)的構(gòu)思的本發(fā)明的控制系統(tǒng)時(shí)，如作為上述(6)涉及的發(fā)明所規(guī)定的那樣，需要具有可測(cè)量浮游移動(dòng)體的傾斜的傾斜角測(cè)定裝置。
作為上述(4)涉及的發(fā)明、即還具備傾斜角測(cè)定裝置的上述(2)或(3)涉及的發(fā)明，分別相當(dāng)于本第1實(shí)施方式的第1例或第2例、以及下述的實(shí)施例1或?qū)嵤├?。而且，上述(6)涉及的發(fā)明相當(dāng)于第1實(shí)施方式的第3例和下述的實(shí)施例3。
另外，根據(jù)上述定義部分和下述的實(shí)施例中的描述，可充分得知使用對(duì)象不限于水下機(jī)器人。而且，即使是水下機(jī)器人，也不限于圖中所示的用于深?？碧降鹊牡湫偷氖纠捎糜谒聽I救、油田勘探等的比較小型的機(jī)器人也屬于本水下機(jī)器人的范疇。
這里，圖1是表示應(yīng)用了本發(fā)明的控制系統(tǒng)的浮游移動(dòng)體的一個(gè)例子的概略圖，而圖2是表示本發(fā)明的控制系統(tǒng)的一個(gè)結(jié)構(gòu)例的方框圖。另外，圖3和圖5是表示應(yīng)用了本發(fā)明的控制系統(tǒng)的浮游移動(dòng)體的其它例子的概略圖，圖4和圖6是表示本發(fā)明的控制系統(tǒng)的其它結(jié)構(gòu)例的方框圖。圖7是表示本發(fā)明的控制系統(tǒng)的第2實(shí)施方式的方框圖。
在上述圖1～圖7中，對(duì)于與之前所示的圖11～圖14中相同的部分，使用相同的符號(hào)進(jìn)行說明。
在圖2、4、6以及7中，連接在各方框之間的細(xì)實(shí)線表示信號(hào)。另一方面，連接在浮游移動(dòng)體10內(nèi)的各方框之間的雙線，表示在浮游移動(dòng)體10上作為物理作用而產(chǎn)生的現(xiàn)象(力學(xué)上的轉(zhuǎn)換作用等)。
在描述以下內(nèi)容的本發(fā)明的說明中，把固定在慣性系統(tǒng)的絕對(duì)位置上的基準(zhǔn)坐標(biāo)系設(shè)為∑R，而把以浮游移動(dòng)體的重心位置為原點(diǎn)、固定在浮游移動(dòng)體主體部上的主體坐標(biāo)系設(shè)為∑B。另外，只要沒有特別說明，則矢量和矩陣是從主體坐標(biāo)系∑B看到的表現(xiàn)。并且，浮游移動(dòng)體存在于m維空間(m≤6)，浮游移動(dòng)體的執(zhí)行器能夠產(chǎn)生n級(jí)自由度(m≤n)的推力。
第1例本實(shí)施方式涉及的推力傳輸門系統(tǒng)的第1例的結(jié)構(gòu)如圖1和2所示。圖1是浮游移動(dòng)體的概略圖，圖2是控制系統(tǒng)的方框圖。
在本例中，浮游移動(dòng)體10被力學(xué)分離成主體部B和執(zhí)行器部E，在主體部B與執(zhí)行器部E之間，作為硬件具有推力傳輸門G。另外，主體部B具有加速度傳感器2和傾斜角傳感器3，而推力傳輸門G具有力/扭矩傳感器。在本例中，主體部B和推力傳輸門G與執(zhí)行器部E一樣，構(gòu)成為露出在外部。
另外，表現(xiàn)下述的主體部B的慣性矩陣MB和浮游移動(dòng)體10的幾何學(xué)構(gòu)造的JGB、JEG，采用預(yù)先測(cè)定的已知的矩陣。
以下，對(duì)本例的結(jié)構(gòu)進(jìn)行詳細(xì)說明。
參照?qǐng)D2的主體部B和執(zhí)行器部E的內(nèi)部，則表示浮游移動(dòng)體的物理動(dòng)力特性的運(yùn)動(dòng)方程式一般可寫成下式。
(算式1)JGBTuG(t)＝MB(aB(t)-gB(t))+MAaB(t)-fB(t)(1)uG(t)＝JEGT(t)uE(t)+fE(t) (2)式(1)是表示主體部的式子，而式(2)是表示執(zhí)行器部的式子。各個(gè)變量的定義如下aB(t)∈Rm浮游移動(dòng)體主體部(重心)的加速度gB(t)∈Rm施加于浮游移動(dòng)體主體部(重心)的重力加速度uG(t)∈Rm在推力傳輸門處從執(zhí)行器部作用于主體部的推力JGB∈Rm×m進(jìn)行從主體部速度向推力傳輸門速度的轉(zhuǎn)換的雅可比矩陣MB∈Rm×m浮游移動(dòng)體主體部的慣性矩陣
MA∈Rm×m因流體的影響而對(duì)浮游移動(dòng)體主體部附加的附加慣性矩陣fB(t)∈Rm不能用式(1)的其它項(xiàng)來表現(xiàn)的作用于浮游移動(dòng)體主體部的推力(基于潮流、波浪、風(fēng)的外力等)uE(t)∈Rn各執(zhí)行器所產(chǎn)生的推力JEG(t)∈Rn×m進(jìn)行從推力傳輸門速度向各執(zhí)行器速度的轉(zhuǎn)換的雅可比矩陣fG(t)∈Rm從推力傳輸門觀察到的不能用式(2)的其它項(xiàng)來表現(xiàn)的從浮游移動(dòng)體執(zhí)行器作用于推力傳輸門的推力(執(zhí)行器部的慣性力；基于潮流、波浪、風(fēng)的外力等)的表現(xiàn)。
另外，在本例的推力傳輸門系統(tǒng)1中，采用了在主體部B的單一剛體構(gòu)造上固定推力傳輸門G的構(gòu)造。由此，雅可比矩陣JGB和慣性矩陣MB為常數(shù)矩陣。
下面，對(duì)本例的推力計(jì)劃部進(jìn)行說明。
如圖2的計(jì)算機(jī)P內(nèi)的推力計(jì)劃部P2的方框部分所示，把實(shí)現(xiàn)主體部B的目標(biāo)加速度aBd(t)所需的推力傳輸門G上的目標(biāo)推力uGd(t)定義為下式。
(算式2)uGd(t)＝uGs(t)+JGB-TMB(aBd(t)-B(t)) (3)其中，aBd(t)、B(t)(m維矢量)分別是主體加速度aB(t)的目標(biāo)值和推定值，而uGd(t)(m維矢量)是門推力uG(t)的目標(biāo)值，uGs(t)(m維矢量)是由推力傳輸門所具有的力/扭矩傳感器測(cè)量的uG(t)的測(cè)定值。
這里，在圖2的計(jì)算機(jī)P內(nèi)，在由參照符號(hào)P3所示的推力控制部中，通過控制uG(t)，使得利用推力反饋實(shí)現(xiàn)該uGd(t)，可以構(gòu)成利用推力反饋來控制推力的、延遲少的反饋控制系統(tǒng)。
在推力控制部P3中，為了抑制在圖2的執(zhí)行器部E內(nèi)由參照符號(hào)E1所表示的執(zhí)行器推力特性、和對(duì)執(zhí)行器部的干擾或動(dòng)態(tài)力fE(t)等執(zhí)行器部動(dòng)態(tài)特性的未知的非線性特性，應(yīng)使用對(duì)非線性特性系統(tǒng)可發(fā)揮良好的魯棒性的控制方法。例如，如果應(yīng)用現(xiàn)有的滑?？刂?，則推力控制部P3可如下構(gòu)成。
在本例的推力控制部P3中，把實(shí)現(xiàn)目標(biāo)推力uGd(t)所需的對(duì)執(zhí)行器的推力指令uEd(t)定義為下式。
(算式3)sG(t)=JEG+T(t){(uGs(t)-uGd(t))+TGddtuGs(t)}---(4)]]>[uEd(t)]i=uEdimax([sG(t)]i<0)0([sG(t)]i=0)uEdimin([sG(t)]i>0)---(5)]]>各變量的定義如下。
sG(t)∈Rn滑?？刂频那袚Q函數(shù)TG∈R 由切換函數(shù)限定的一次延遲系統(tǒng)的時(shí)間常數(shù)uEd(t)∈R 對(duì)各執(zhí)行器的指令推力uEdimax∈R 執(zhí)行器推力的第i分量中可指令的最大推力uEdimin∈R 執(zhí)行器推力的第i分量中可指令的最小推力(*)i∈R 矢量*的第i分量*+T矩陣*的虛擬逆矩陣的轉(zhuǎn)置在本例中，對(duì)推力計(jì)劃部、推力控制部的反饋所需的值是uGs(t)和B(t)。由推力傳輸門所具備的力/扭矩傳感器測(cè)量的測(cè)定值直接用于門推力測(cè)定值uGs(t)。
下面，對(duì)主體部加速度aB(t)的推定方法進(jìn)行說明。為了考察傳感器特性、推力控制特性，以下主要是在頻率范圍內(nèi)展開論述。
首先，在圖2的主體部B內(nèi)，利用由參照符號(hào)2所示的主體部加速度傳感器所獲得的加速度的測(cè)定值aBs(t)(m維矢量)表示為下式。由于在該測(cè)定值中包含重力加速度，所以，不能直接用作主體加速度aB(t)的推定值。
(算式4)aBs(s)＝GaBs(s)(aB(s)-gB(s)) (6)另外，*(s)是矢量或矩陣*的拉普拉斯變換式*(s)＝L(*(t))，GaBs(s)是表示主體部加速度傳感器特性的m行m列的傳輸函數(shù)矩陣。
同樣，由主體部?jī)A斜角傳感器所獲得的重力加速度的測(cè)定值gBs(t)(m維矢量)表示為下式。
(算式5)gBs(s)＝GgBs(s)gB(s) (7)其中，GgBs(s)是表示主體部?jī)A斜角傳感器特性的m行m列的傳輸函數(shù)矩陣。
同樣，由推力傳輸門所獲得的門推力測(cè)定值uGs(t)(m維矢量)表示為下式。
(算式6)uGs(s)＝GuGs(s)uG(s) (8)其中，GuGs(s)是表示推力傳輸門的力/扭矩傳感器特性的m行m列的傳輸函數(shù)矩陣。
下面，說明本例中的主體加速度aB(t)的推定。
把主體加速度aB(t)的推定值B(t)定義為下式。
(算式7)B(t)＝aBs(t)+gBs(t)(9)這里，考察上述主體加速度推定的合理性。把該式(9)進(jìn)行拉普拉斯變換，并代入上面的式(6)、式(7)中，則成為，B(s)＝GgBs(s)(aB(s)-gB(s))+GgBs(s)gB(s)(9)’在滿足上述“所有種類、所有自由度的傳感器特性相同，各傳感器、各自由度之間不存在干擾(傳感器特性一致條件)”的條件的情況下，關(guān)于主體部加速度傳感器特性、主體部?jī)A斜角傳感器特性、和推力傳輸門的力/扭矩傳感器特性，可以表示為GaBs(s)＝GgBs(s)＝GuGs(s)＝Gs(s)I(10)其中，Gs(s)是表示同一傳感器特性的函數(shù)，I是單位矩陣。即，aBs(s)＝Gs(s)(aB(s)-gB(s)) (11)gBs(s)＝Gs(s)gB(s)(12)uGs(s)＝Gs(s)uG(s)(13)此時(shí)，式(9)’成為(算式8)B(s)＝Gs(s)aB(s) (14)利用式(8)可進(jìn)行主體部加速度aB(t)的推定。由于在式(14)中未明示出重力加速度gBs(t)，所以，只要滿足上述傳感器特性一致條件，就可視為直接測(cè)定了主體部加速度aB(t)。
另外，通過選擇例如與浮游移動(dòng)體的動(dòng)力特性相比具有足夠高速的響應(yīng)、且高精度的傳感器，可實(shí)現(xiàn)上述傳感器特性的一致條件。
第2例下面，對(duì)本實(shí)施方式的推力傳輸門系統(tǒng)的第2例進(jìn)行說明。
推力傳輸門系統(tǒng)的第2例的結(jié)構(gòu)如圖3、4所示。圖3是浮游移動(dòng)體的概略圖。圖4是控制系統(tǒng)的方框圖。
在本例中，也是把浮游移動(dòng)體10力學(xué)分離成主體部B和執(zhí)行器部E，并且在主體部B與執(zhí)行器部E之間，作為硬件具有推力傳輸門G。而且在主體部B上具有傾斜角傳感器3，在推力傳輸門G上具有力/扭矩傳感器。
與第1例大的不同之處是在本例中執(zhí)行器部E形成覆蓋主體部B的殼(圖3所示的耐壓殼S)，由該殼S將主體部B和推力傳輸門G與外界完全隔離。因?yàn)檫@樣的構(gòu)造而不需要在第1例中所需的主體部B的加速度傳感器2。
與第1例一樣，主體部的慣性矩陣MB和表現(xiàn)浮游移動(dòng)體的幾何學(xué)構(gòu)造的JGB、JEG采用預(yù)先測(cè)定的已知的矩陣。
下面，對(duì)本例的結(jié)構(gòu)進(jìn)行詳細(xì)說明。
參照?qǐng)D4的主體部B和執(zhí)行器部E的內(nèi)部，則表示浮游移動(dòng)體的物理動(dòng)力特性的運(yùn)動(dòng)方程式一般可寫成下式。
(算式9)JGBTuG(t)＝MB(aB(t)-gB(t)) (15)uG(t)＝JEGT(t)uE(t)+fE(t)(16)式(15)是表示主體部的式子，式(16)是表示執(zhí)行器部的式子。這里，如下式所示，重新定義UG2(t)(m維矢量)。
(算式10)uG2(t)＝uG(t)+JGB-TMBgB(t) (17)下面，對(duì)本例的推力計(jì)劃部進(jìn)行說明。
如圖4的計(jì)算機(jī)P內(nèi)的推力計(jì)劃部P2的方框部分所示，把實(shí)現(xiàn)主體部B的目標(biāo)加速度aBd(t)所需的推力傳輸門G的目標(biāo)推力uG2d(t)定義為下式。
(算式11)uG2d(t)＝JGB-TMBaBd(t) (18)在圖4的計(jì)算機(jī)P內(nèi)，在由參照符號(hào)P3表示的推力控制部中，通過控制uG2(t)，使得利用推力反饋實(shí)現(xiàn)該uG2d(t)，可以構(gòu)成利用推力反饋來控制推力的、延遲少的反饋控制系統(tǒng)。
這里，關(guān)于本例的推力控制，如果應(yīng)用例如滑?？刂?，則可構(gòu)成如下的推力控制部P3。
在本例的推力控制部P3中，把實(shí)現(xiàn)目標(biāo)推力uG2d(t)所需的對(duì)執(zhí)行器的推力指令uEd(t)定義為下式。
(算式12)
sG(t)=JEG+T(t){(u^G2(t)-uG2d(t))+TGddtu^G2(t)}---(19)]]>[uEd(t)]i=uEdimax([sG(t)]i<0)0([sG(t)]i=0)uEdimin([sG(t)]i>0)---(20)]]>其中， (m維矢量)是uG2(t)的推定值。
在本例中，不需要對(duì)推力計(jì)劃部P2的反饋，對(duì)推力控制部P3的反饋所需的值只是式(17)的uG2(t)的推定值 這里，說明該uG2(t)的推定方法。
下面，對(duì)本例中的門推力uG2(t)的推定進(jìn)行說明。把門推力uG2(t)的推定值 定義為下式(算式13)u^G2(t)=uGs(t)+JGB-TMBgBs(t)---(21)]]>這里，考察上述門推力推定的合理性。對(duì)式(21)進(jìn)行拉普拉斯變換，成為U^G2(s)=uGs(s)+JGB-TMBgBs(s)---(21),]]>這里，如果滿足上述的傳感器特性一致條件，則式(12)、(13)也同樣成立。將它們代入式(17)，則成為(算式14)u^G2(s)=Gs(s)uG2(s)---(22)]]>根據(jù)式(21)可進(jìn)行門推力uG2(t)的推定。由于在式(22)中未明示出重力加速度gBs(t)，所以，只要滿足傳感器特性一致條件，即可視為直接測(cè)定了門推力uG2(t)。
第3例下面，對(duì)本實(shí)施方式的推力傳輸門系統(tǒng)的第3例進(jìn)行說明。
推力傳輸門系統(tǒng)的第3例的結(jié)構(gòu)如圖5、6所示。圖5是浮游移動(dòng)體的概略圖，圖6是控制系統(tǒng)的方框圖。
在該第3例中，浮游移動(dòng)體10未被力學(xué)分離成主體部B和執(zhí)行器部E，并且也沒有硬件的推力傳輸門G。但本例的浮游移動(dòng)體10具有加速度傳感器2和傾斜角傳感器3。
在本例中，通過適當(dāng)?shù)卮_定主體部B的慣性矩陣MB，并且根據(jù)該MB，從傳感器的測(cè)定值計(jì)算在圖6的計(jì)算機(jī)P內(nèi)所示出的虛擬推力傳輸門G’的推力推定值，視為存在推力傳輸門來構(gòu)成推力傳輸門系統(tǒng)。關(guān)于表現(xiàn)浮游移動(dòng)體10的幾何學(xué)構(gòu)造的JGB、JEG，與上述第1例子、第2例子一樣，采用預(yù)先測(cè)定的已知的矩陣。
另外，作為主體部B的慣性矩陣MB的確定方法，例如，可列舉出如下的方法等，即，利用計(jì)算機(jī)根據(jù)設(shè)計(jì)浮游移動(dòng)體10時(shí)的設(shè)計(jì)圖、主要規(guī)格等預(yù)先求出主體部B的慣性矩陣MB，并把其存儲(chǔ)在未圖示的存儲(chǔ)器等中。
下面，對(duì)本例的構(gòu)造進(jìn)行詳細(xì)說明。
參照?qǐng)D6的主體部B和執(zhí)行器部E的內(nèi)部，則表示浮游移動(dòng)體的物理動(dòng)力特性的運(yùn)動(dòng)方程式一般可寫成下式。
(算式15)JGBTuGv(t)＝MBaB(t) (23)uGv(t)＝JEGT(t)uE(t)+fE(t)+JGB-TMBgB(t) (24)式(23)是表示主體部的式子，式(24)是表示執(zhí)行器部的式子。雖然這些與第2例的情況大致相同，但在該第3例中，由于不存在物理上的推力傳輸門，所以需要注意推力傳輸門的推力uGv(m維矢量)是虛擬值。
下面，對(duì)本例的推力計(jì)劃部進(jìn)行說明。
如圖6的計(jì)算機(jī)P內(nèi)的推力計(jì)劃部P2的方框部分所示，把實(shí)現(xiàn)主體部B的目標(biāo)加速度aBd(t)所需的虛擬推力傳輸門G’的目標(biāo)推力uGvd(t)定義為下式。
(算式16)uGvd(t)＝JGB-TMBaBd(t)(25)在圖6的計(jì)算機(jī)P內(nèi)，在由參照符號(hào)P3表示的推力控制部中，通過控制uGv(t)，使得利用反饋實(shí)現(xiàn)該uGvd(t)，可以構(gòu)成利用推力反饋來控制推力的、延遲少的反饋控制系統(tǒng)。
這里，如果在本例的推力控制中應(yīng)用例如滑模控制，則可構(gòu)成如下的推力控制部P3。
在本例的推力控制部P3中，把實(shí)現(xiàn)目標(biāo)推力uGvd(t)所需的對(duì)執(zhí)行器的推力指令uEd定義為下式。
(算式17)sG(t)=JEG+T(t){(u^Gv(t)-uGvd(t))+TGddtu^Gv(t)}---(26)]]>[uEd(t)]i=uEdimax([sG(t)]i<0)0([sG(t)]i=0)uEdimin([sG(t)]i>0)---(27)]]>其中， (m維矢量)是uGv(t)的推定值。
另外，在本例中與上述第2例一樣，不需要對(duì)推力計(jì)劃部P2的反饋，對(duì)推力控制部P3的反饋所需的值只是uGv(t)的推定值 這里，對(duì)該uGv(t)的推定方法進(jìn)行說明。
下面，對(duì)本例中的門推力uGv(t)的推定進(jìn)行說明。
將門推力uGv(t)的推定值 定義為下式。
(算式18)u^Gv(t)=JGB-TMB(aBs(t)+gBs(t))---(28)]]>這里，考察上述門推力推定的合理性。對(duì)式(28)進(jìn)行拉普拉斯變換，而成為u^Gv(s)=JGB-TMB(aBs(s)+gBs(s))---(28),]]>
這里也是如果滿足上述的傳感器特性一致條件，則式(11)、(12)同樣成立。如果把它們代入式(23)，則成為(算式19)u^Gv(s)=Gs(s)uGv(s)---(29)]]>利用式(28)可進(jìn)行門推力uGv(t)的推定。由于在式(29)中未明示出重力加速度gBs(t)，所以只要滿足傳感器特性一致條件，就可視為直接測(cè)定了門推力uGv(t)。
(第2實(shí)施方式)下面，對(duì)本發(fā)明的浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)的一個(gè)實(shí)施方式進(jìn)行說明。與第1實(shí)施方式一樣，本實(shí)施方式也是把水下機(jī)器人作為適用對(duì)象來進(jìn)行以下說明。另外，水下機(jī)器人是在三維空間中浮游的浮游移動(dòng)體，在對(duì)該水下機(jī)器人應(yīng)用本發(fā)明的加速度檢測(cè)系統(tǒng)的情況下，與第1實(shí)施方式一樣，需要對(duì)重力加速度的影響進(jìn)行補(bǔ)償，因此，如作為上述(8)涉及的發(fā)明所規(guī)定的那樣，需要具備可測(cè)量主體部的傾斜的傾斜角測(cè)定裝置。
在本實(shí)施方式中，利用推力傳輸門進(jìn)行主體部加速度的推定。本實(shí)施方式涉及的加速度檢測(cè)系統(tǒng)的結(jié)構(gòu)如圖3和圖7所示。圖3是浮游移動(dòng)體的概略圖，圖7是加速度檢測(cè)系統(tǒng)的方框圖。
在此前的第1實(shí)施方式的各例中，作為浮游移動(dòng)體10的控制系統(tǒng)1構(gòu)筑了推力傳輸門系統(tǒng)，而在第2實(shí)施方式中，作為浮游移動(dòng)體10的高精度的加速度傳感系統(tǒng)，將利用推力傳輸門G。
本實(shí)施方式的硬件結(jié)構(gòu)與前面的第1實(shí)施方式涉及的第2例(圖3)完全相同。即，將浮游移動(dòng)體10力學(xué)分離成主體部B和執(zhí)行器部E，在主體部B中具有傾斜角傳感器3，在推力傳輸門G中具有力/扭矩傳感器。
不過，由于在本實(shí)施方式中的目的是高精度地推定主體部加速度aB(t)，而不是進(jìn)行控制，所以在本實(shí)施例中不包含推力計(jì)劃部和推力控制部。
另外，主體部的慣性矩陣MB和表現(xiàn)浮游移動(dòng)體的幾何學(xué)構(gòu)造的JGB，采用預(yù)先測(cè)定的已知的常數(shù)矩陣。通過考慮推力傳輸門的力/扭矩傳感器的測(cè)定范圍、和在本實(shí)施方式中應(yīng)檢測(cè)的加速度范圍來適當(dāng)?shù)卦O(shè)計(jì)該主體部慣性矩陣MB，可實(shí)現(xiàn)高精度的加速度測(cè)定。
下面，對(duì)本實(shí)施方式的結(jié)構(gòu)進(jìn)行詳細(xì)說明。
在圖3和圖7所示的本實(shí)施方式的硬件結(jié)構(gòu)中，對(duì)式(15)進(jìn)行變形而將主體部加速度aB(t)表示為下式。
(算式20)aB(t)＝MB-1JGBTuG(t)+gB(t) (30)接著，對(duì)本實(shí)施方式的主體部加速度aB(t)的推定進(jìn)行說明。
把主體部加速度aB(t)的推定值B(t)定義為下式。
(算式21)B(t)＝MB-1JGBTuGs(t)+gBs(t)(31)這里，考察上述主體部加速度推定的合理性。對(duì)式(31)進(jìn)行拉普拉斯變換，成為下式。
(算式22)B(s)＝MB-1JGBTuGs(s)+gBs(s)(32)這里，如果假設(shè)滿足上述的傳感器特性一致條件，則與第1實(shí)施方式一樣，式(12)、(13)成立。如果把它們代入式(30)，則成為下式。
(算式23)B(s)＝Gs(s)aB(s) (33)利用式(31)可進(jìn)行主體加速度aB(t)的推定。
實(shí)施例1如在上述第1實(shí)施方式中說明的那樣，在本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)中，為了進(jìn)行推力控制，嚴(yán)格地講，所實(shí)現(xiàn)的不是位置/速度軌道，而是加速度的軌道。這里，因加速度的積分誤差而產(chǎn)生的漂移的影響不可避免，因此，本發(fā)明可對(duì)比較高的頻帶的運(yùn)動(dòng)單獨(dú)發(fā)揮顯著的效果。而對(duì)低頻的緩慢運(yùn)動(dòng)需要并用以往的反饋位置/速度的控制方法，但是，即便是在這種情況下，與單獨(dú)使用以往的方法相比，也能提高精度。
下面，作為本發(fā)明的一個(gè)實(shí)施例，對(duì)在上述第1實(shí)施方式中所說明的本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)的各例中并用位置/速度反饋控制的控制系統(tǒng)進(jìn)行說明。
這里，圖8是表示本發(fā)明的控制系統(tǒng)的一個(gè)實(shí)施例的方框圖，其相當(dāng)于上述第1實(shí)施方式的第1例，圖9是表示本發(fā)明的控制系統(tǒng)的另一實(shí)施例的方框圖，其相當(dāng)于上述第1實(shí)施方式的第2例，圖10是表示本發(fā)明的控制系統(tǒng)的另一實(shí)施例的方框圖，其相當(dāng)于上述第1實(shí)施方式的第3例。在上述圖8～10中，對(duì)與上述的圖1～7以及圖11～14相同的部分賦予相同的符號(hào)來進(jìn)行說明。
另外，與圖2、4、6和7一樣，在圖8～10中連接在各個(gè)方框間的細(xì)實(shí)線表示信號(hào)。另一方面，在浮游移動(dòng)體10內(nèi)的連接在各方框之間的雙線，表示作為物理作用而在浮游移動(dòng)體10上產(chǎn)生的現(xiàn)象(力學(xué)轉(zhuǎn)換作用等)。
實(shí)施例1相當(dāng)于上述第1實(shí)施方式的第1例。圖8表示本實(shí)施例涉及的控制系統(tǒng)1的方框圖。本實(shí)施例的浮游移動(dòng)體10的概略結(jié)構(gòu)如先前的圖1所示。
根據(jù)圖8的方框圖可知，本實(shí)施例是把在上述第1實(shí)施方式的第1例中所說明的圖2所示的結(jié)構(gòu)，應(yīng)用在已知的位置/速度反饋控制系統(tǒng)中的實(shí)施例。因此，基于前面的圖1和圖2來說明本發(fā)明的推力傳輸門系統(tǒng)部分的結(jié)構(gòu)以及控制系統(tǒng)的動(dòng)作等。
本實(shí)施例涉及的控制系統(tǒng)1、即推力傳輸門系統(tǒng)的主要特征是，把由參照符號(hào)10所示的浮游移動(dòng)體的機(jī)構(gòu)力學(xué)分離成接受推力的主體部B和產(chǎn)生推力的執(zhí)行器部E，并在其結(jié)合部分附加了推力傳輸門G。這里，推力傳輸門G是將主體部B和執(zhí)行器部E結(jié)合起來的唯一部分，并具有能夠測(cè)量施加在主體部B與執(zhí)行器部E之間的所有力和扭矩的傳感器(力/扭矩傳感器)。另外，對(duì)于力/扭矩傳感器，可使用市場(chǎng)銷售的通用產(chǎn)品(可輸出多維的力和扭矩分量的多通道產(chǎn)品等)。
這樣，在本實(shí)施例中，通過利用推力傳輸門G來結(jié)合主體部B和執(zhí)行器部E，構(gòu)成了能夠?qū)嶋H檢測(cè)出從執(zhí)行器部E作用于主體部B的所有的推力。如圖1所示，應(yīng)用了推力傳輸門系統(tǒng)的本實(shí)施例的浮游移動(dòng)體10，在主體部B中具有加速度傳感器2和傾斜角傳感器3。
具有上述結(jié)構(gòu)的本實(shí)施例涉及的控制系統(tǒng)1，在進(jìn)行已知的位置/速度反饋控制的同時(shí)，還進(jìn)行基于本發(fā)明的推力傳輸門系統(tǒng)的推力反饋控制。
根據(jù)本實(shí)施例，可并用以往的反饋位置/速度的控制方法來避免因加速度的積分誤差而引起的漂移的影響，并且通過使用本發(fā)明的控制系統(tǒng)、即推力傳輸門系統(tǒng)，即使在例如波浪、潮流、風(fēng)、或者安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道，因此，能夠?qū)Ω∮我苿?dòng)體進(jìn)行更高速且更高精度的控制。
實(shí)施例2本實(shí)施例相當(dāng)于上述第1實(shí)施方式的第2例。圖9是表示本實(shí)施例涉及的控制系統(tǒng)1的方框圖。本實(shí)施例的浮游移動(dòng)體10的概略結(jié)構(gòu)如前面的圖3所示。
與實(shí)施例1一樣，本實(shí)施例涉及的控制系統(tǒng)1的主要特征也是，把水下機(jī)器人10的機(jī)構(gòu)力學(xué)分離成接受推力的主體部B、和產(chǎn)生推力的執(zhí)行器部E，并在其結(jié)合部分附加了推力傳輸門G。
但是本實(shí)施例與上述實(shí)施例1的不同之處在于主體部B被收納在執(zhí)行器部E的內(nèi)部，構(gòu)成為來自外部的干擾和機(jī)械臂A的反作用等全都對(duì)作為耐壓殼S的執(zhí)行器部E產(chǎn)生影響。這里，主體部B在執(zhí)行器部E的內(nèi)部，只通過推力傳輸門G來承擔(dān)。因此，在推力傳輸門G是將主體部B和執(zhí)行器部E結(jié)合起來的唯一部分這一點(diǎn)，與上述實(shí)施例1相同。
因此，本實(shí)施例也構(gòu)成為通過利用推力傳輸門G將主體部B與執(zhí)行器部E結(jié)合起來，而能夠?qū)嶋H檢測(cè)出執(zhí)行器部E作用于主體部B的所有推力的結(jié)構(gòu)。
根據(jù)本實(shí)施例2，也可并用以往的反饋位置/速度的控制方法來避免因加速度的積分誤差而引起的漂移的影響，并且通過使用本發(fā)明的控制系統(tǒng)、即推力傳輸門系統(tǒng)，即使是在例如波浪、潮流、風(fēng)、或者安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道，因此，能夠?qū)Ω∮我苿?dòng)體進(jìn)行更高速且更高精度的控制。
實(shí)施例3本實(shí)施例相當(dāng)于上述第1實(shí)施方式的第3例。圖10是表示本實(shí)施例涉及的控制系統(tǒng)1的方框圖。本實(shí)施例的浮游移動(dòng)體10的概略結(jié)構(gòu)如前面的圖5所示。
本實(shí)施例涉及的控制系統(tǒng)1的主要的特征是，把水下機(jī)器人的機(jī)構(gòu)在計(jì)算上分成接受推力的主體部B、和產(chǎn)生推力的執(zhí)行器部E，并在兩者之間虛擬地設(shè)置了虛擬推力傳輸門G’。
另外，虛擬推力傳輸門G’能夠推定出施加在主體部B和執(zhí)行器部E之間的全部的力和扭矩。
根據(jù)本實(shí)施例3，也能夠并用以往的反饋位置/速度的控制方法來避免因加速度的積分誤差所引起的漂移的影響，并且通過使用本發(fā)明的控制系統(tǒng)、即推力傳輸門系統(tǒng)，即使是在例如波浪、潮流、風(fēng)、或者安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道，因此，能夠?qū)Ω∮我苿?dòng)體進(jìn)行更高速且更高精度的控制。
(利用了慣性導(dǎo)航裝置的安裝)被廣泛用于航空器等浮游移動(dòng)體的作為現(xiàn)有技術(shù)的慣性導(dǎo)航裝置可作為本發(fā)明中所使用的加速度傳感器、傾斜角傳感器、主體部位置/速度傳感器使用。
即，只要是裝備了慣性導(dǎo)航裝置的浮游移動(dòng)體，無須任何硬件的追加、變更，而只需改變軟件，即可安裝例如本實(shí)施例3或上述第1實(shí)施方式的第3例涉及的本發(fā)明的控制系統(tǒng)，因此，可更容易地實(shí)現(xiàn)現(xiàn)有的浮游移動(dòng)體的高速、高精度化。
更詳細(xì)地講，在現(xiàn)有的慣性導(dǎo)航裝置中，現(xiàn)狀是無論是否以足夠高的精度測(cè)量加速度，在控制中都是只使用通過對(duì)其積分而獲得的位置/速度信息。然而，在本發(fā)明的推力傳輸門系統(tǒng)的構(gòu)成中，通過在推力測(cè)定中使用至今為止還未被加以利用的加速度信息，在硬件與以往相同的情況下，與現(xiàn)有的控制相比，能實(shí)現(xiàn)更高的速度、更高的精度。
特別是，對(duì)于上述慣性導(dǎo)航裝置而言，最近把加速度傳感器、陀螺儀、GPS(Global Positioning System)以及其它的傳感器組件化來獲得加速度、速度和位移信息的混合型慣性導(dǎo)航裝置已成為主流，從整體上提高了測(cè)量速度。因此，可以說上述慣性導(dǎo)航裝置是可在安裝本發(fā)明的控制系統(tǒng)時(shí)加以利用的優(yōu)選的傳感器組件。
另外，本發(fā)明并不限于上述各例所記載的結(jié)構(gòu)，能夠進(jìn)行各種設(shè)計(jì)變化以及其它的變形。
首先，在上述各例中，針對(duì)把本發(fā)明的控制系統(tǒng)應(yīng)用于水下機(jī)器人的示例進(jìn)行了說明，但作為適用本發(fā)明的控制系統(tǒng)的對(duì)象不限于水下機(jī)器人。本發(fā)明的控制系統(tǒng)除了水下機(jī)器人以外，如上述定義部分中所列舉的示例所示，可適用于潛水艇、直升飛機(jī)、飛船、航空器、宇宙飛船或宇宙機(jī)器人等水下、空中或宇宙空間等中的浮游移動(dòng)體。另外，本發(fā)明的控制系統(tǒng)也可適用于在水上或地上滑行的氣墊式交通工具、和在車輪處于打滑狀態(tài)下的汽車等在平面上滑行的各種移動(dòng)體。
另外，在上述實(shí)施方式的各例以及上述各實(shí)施例中，對(duì)把本發(fā)明的控制系統(tǒng)或加速度檢測(cè)系統(tǒng)應(yīng)用于在三維空間中浮游的浮游移動(dòng)體的情況進(jìn)行了說明。但是，在把本發(fā)明的控制系統(tǒng)或加速度檢測(cè)系統(tǒng)應(yīng)用于在根本沒有重力作用的宇宙空間浮游的浮游移動(dòng)體、可以不考慮傾斜或者可忽略傾斜的在水平面上滑行的浮游移動(dòng)體時(shí)，如作為上述(2)、(3)、(5)或(7)的發(fā)明所規(guī)定的那樣可不需要傾斜角測(cè)定裝置，此時(shí)，在推力計(jì)劃部、推力控制部中通過使施加于浮游移動(dòng)體主體部(重心)的重力加速度gB＝0，可采用更簡(jiǎn)單的控制法則。
另外，在上述各例中，作為推力控制部P3的控制法則的一個(gè)例子，對(duì)執(zhí)行器動(dòng)態(tài)特性的非線性，使用了作為魯棒的現(xiàn)有控制方法的滑?？刂?，但推力控制部P3的控制法則不限于此。因此，例如作為推力控制部P3的控制法則，也可以對(duì)推力傳輸門G的目標(biāo)推力與推力傳輸門G的推力測(cè)定或推定值的誤差使用推進(jìn)器T輸入電壓的PID控制。
并且，在上述各例中，作為推進(jìn)器T使用了基于與作為驅(qū)動(dòng)源的馬達(dá)機(jī)械連接的螺旋槳的推力發(fā)生設(shè)備，但推進(jìn)器T的結(jié)構(gòu)不限于此。因此，推進(jìn)器T也可以是噴氣噴射式推進(jìn)器，而且該驅(qū)動(dòng)源也不限于馬達(dá)，可采用內(nèi)燃機(jī)、蒸汽渦輪機(jī)、原子能渦輪機(jī)等各種形式的推進(jìn)器。
如上所述，本發(fā)明是一種新型且非常有用的發(fā)明，其提供了一種可高速且高精度地控制浮游移動(dòng)體的系統(tǒng)結(jié)構(gòu)，即使在例如波浪、潮流、風(fēng)、或者安裝在浮游移動(dòng)體上的機(jī)械臂的反作用等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道。
權(quán)利要求
1.一種浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述浮游移動(dòng)體，包括主體部，占上述浮游移動(dòng)體的一部分，并可視為單一性剛體；執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力；以及推力傳輸門，是對(duì)上述主體部和上述執(zhí)行器部進(jìn)行力學(xué)結(jié)合的部分，構(gòu)成為可實(shí)際測(cè)定從上述執(zhí)行器部作用于上述主體部的推力，使用來自上述推力傳輸門的推力測(cè)定值來得到對(duì)上述執(zhí)行器部的推力指令。
2.根據(jù)權(quán)利要求1所述的浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述主體部具有可測(cè)量上述主體部的加速度的加速度測(cè)定裝置，上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置，使用轉(zhuǎn)換來自操作者的目標(biāo)位置速度軌道指令而得到的目標(biāo)加速度軌道指令；來自上述主體部的上述加速度測(cè)定裝置的輸出；以及來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出，來得到對(duì)上述執(zhí)行器部的推力指令。
3.根據(jù)權(quán)利要求1所述的浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置，并且上述主體部構(gòu)成為只與上述推力傳輸門進(jìn)行力學(xué)結(jié)合，而且作用于上述主體部的力實(shí)質(zhì)上全部通過上述執(zhí)行器部和上述推力傳輸門來輸入，使用轉(zhuǎn)換來自操作者的目標(biāo)位置速度軌道指令而得到的目標(biāo)加速度軌道指令、和來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出，來得到對(duì)上述執(zhí)行器部的推力指令。
4.根據(jù)權(quán)利要求2或3所述的浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述主體部還具有可測(cè)量上述主體部的傾斜的傾斜角測(cè)定裝置，還利用來自上述主體部的上述傾斜角測(cè)定裝置的輸出來得到對(duì)上述執(zhí)行器部的推力指令。
5.一種浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述浮游移動(dòng)體在計(jì)算上被虛擬地分成主體部，占上述浮游移動(dòng)體的一部分，并可視為單一性剛體；和執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力，上述浮游移動(dòng)體具有可測(cè)量上述浮游移動(dòng)體的加速度的加速度測(cè)定裝置，使用來自上述加速度測(cè)定裝置的輸出，在計(jì)算上推定來自虛擬推力傳輸門的力或扭矩，得到對(duì)上述執(zhí)行器部的推力指令。
6.根據(jù)權(quán)利要求5所述的浮游移動(dòng)體的控制系統(tǒng)，其特征在于，上述浮游移動(dòng)體還具有可測(cè)量上述浮游移動(dòng)體的傾斜的傾斜角測(cè)定裝置，使用來自上述傾斜角測(cè)定裝置和上述加速度測(cè)定裝置的輸出，在計(jì)算上推定來自虛擬推力傳輸門的力或扭矩，由此得到對(duì)上述執(zhí)行器部的推力指令。
7.一種浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)，用于檢測(cè)浮游移動(dòng)體的加速度，其特征在于，上述浮游移動(dòng)體，包括主體部，占上述浮游移動(dòng)體的一部分，并可視為單一性剛體；執(zhí)行器部，對(duì)上述浮游移動(dòng)體產(chǎn)生推力；以及推力傳輸門，是對(duì)上述主體部和上述執(zhí)行器部進(jìn)行力學(xué)結(jié)合的部分，構(gòu)成為可實(shí)際測(cè)定從上述執(zhí)行器部作用于上述主體部的推力，上述推力傳輸門具有可測(cè)量施加在上述主體部與上述執(zhí)行器部之間的力或扭矩的力或扭矩測(cè)定裝置，上述主體部構(gòu)成為只與上述推力傳輸門進(jìn)行力學(xué)結(jié)合，而且作用于上述主體部的力實(shí)質(zhì)上全部通過上述執(zhí)行器部和上述推力傳輸門來輸入，使用來自上述推力傳輸門的上述力或扭矩測(cè)定裝置的輸出來生成上述主體部的加速度的推定值。
8.根據(jù)權(quán)利要求7所述的浮游移動(dòng)體的加速度檢測(cè)系統(tǒng)，其特征在于，上述主體部還具有可測(cè)量上述主體部的傾斜的傾斜角測(cè)定裝置，還使用來自上述主體部的上述傾斜角測(cè)定裝置的輸出來生成上述主體部的加速度的推定值。
全文摘要
本發(fā)明的目的是提供一種浮游移動(dòng)體的控制系統(tǒng)，即便是在波浪、潮流等的干擾下，也能夠使浮游移動(dòng)體高精度地靜止在規(guī)定位置，或者高精度地追蹤目標(biāo)軌道?？梢赃_(dá)到上述目的的本發(fā)明的浮游移動(dòng)體的控制系統(tǒng)是浮游移動(dòng)體(10)的控制系統(tǒng)(1)，上述浮游移動(dòng)體包括主體部(B)，占浮游移動(dòng)體的一部分，并可視為單一性剛體；執(zhí)行器部(E)，對(duì)浮游移動(dòng)體產(chǎn)生推力；以及推力傳輸門(G)，是對(duì)主體部和執(zhí)行器部進(jìn)行力學(xué)結(jié)合的部分，構(gòu)成為能夠?qū)嶋H測(cè)定從執(zhí)行器部作用于主體部的推力；使用來自推力傳輸門(G)的推力測(cè)定值來得到對(duì)執(zhí)行器部(E)的推力指令。
文檔編號(hào)G05D3/12GK1934513SQ200580009479
公開日2007年3月21日 申請(qǐng)日期2005年2月25日 優(yōu)先權(quán)日2004年2月25日
發(fā)明者金岡克彌 申請(qǐng)人:學(xué)校法人立命館