一種水下無人航行器和機械手系統的手艇協調控制方法與流程
本發明涉及一種水下無人航行器和機械手系統的手艇協調控制方法。
背景技術：
隨著海洋開發與研究的日益蓬勃開展，水下無人航行器搭載機械手在進行水下打撈、援潛救生、海底設施的維護與裝置的回收、海底生物及巖石標本的采樣等方面得到了越來越廣泛地應用。
首先，雖然目前搭載機械手的水下航行器通過遙控操作可以完成大量的作業工作，但其在結構化環境中的作業能力等方面存在較大缺陷，作業范圍也受到較大限制，并且在水下機器人的作業中如果單純依賴人的遙控，將難以實現海流擾動中機械手末端的準確定位和作業。專利文獻“一種ROV水下機器人的運動控制方法(CN105404303A)”和“浮游式水下機器人的S面控制方法(CN1718378A)”，涉及到的水下機器人運動控制方法，雖然也可以完成水下機器人的運動控制，但由于作業過程中機械手的運動會給航行器帶來較大的擾動作用，并且擾動作用力根據作業姿態隨時變化，所以上述根據干擾誤差反饋進行補償的控制方式難以實現作業過程中的航行器穩定控制。
技術實現要素：
本發明的目的是為了提供一種水下無人航行器和機械手系統的手艇協調控制方法，能夠通過航行器和機械手當前的位置和姿態的反饋實時估計作業過程中水下無人航行器所受到的機械手的擾動力，并在控制中進行補償，從而提高作業精度。該方法用于水下無人航行器的遙控作業可以減小操作人員的操作時間，提高遙控精度，用于水下無人航行器的自主作業可以提高控制精度，尤其是機械手末端的控制精度，實現航行器的自主作業。
本發明的目的是這樣實現的：包括水下無人航行器控制系統和協調控制器，
步驟一、協調運動控制器通過隔離串口板采集水下無人航行器控制系統中的多普勒測速儀和磁羅經的數據，并通過水下無人航行器控制系統中的PC104模塊進行船位推算，獲得水下無人航行器當前位姿，通過水下無人航行器控制系統中的can數據采集板、機械手直流伺服電機驅動器和編碼器獲取機械手各關節當前姿態；
步驟二、建立擾動力觀測器，并設計協調控制器的兩條控制率；
步驟三、通過協調控制器的兩條控制率控制并控制水下無人航行器和機械手的運動。
本發明還包括這樣一些結構特征：
1.步驟二中所述建立擾動力觀測器是機械手對航行器的擾動力觀測器，其中，傾斜力矩觀測公式是：
式中，和分別為機械手第i個廣義連桿相對于水下無人航行器的重力和浮力的矩陣表達形式，0RV為艇體坐標系到機械手基座坐標系的轉換矩陣和分別為機械手第i個廣義連桿的重心和浮心相對于水下無人航行器的位置，而和分別為機械手艇體重心和浮心相對于水下無人航行器的位置，mig和FBi分別為機械手第i個廣義連桿相對于水下無人航行器的重力和浮力；
機械手和水下無人航行器的耦合運動造成的耦合力的觀測公式是：
式中：0f1,0和0n1,0是水下無人航行器在手臂基關節受到的機械手的耦合作用力，1f1,0和1n1,0是水下無人航行器在手臂基座受到的機械手的耦合作用力，0R1是機械手基關節的隨動坐標系到機械手基座坐標系的轉換矩陣，0f1,0和0n1,0將由下式遞推得到：
式中：ifi,i-1和ini,i-1為第i個廣義連桿在其第i關節對第i-1關節的約束反力，iri和irci分別為機械手第i個廣義連桿在其坐標系下的位置矢量和中心位置矢量，iFi,i和iMi,i分別為機械手第i個廣義連桿在水中運動所受到的流體作用力，ifi*和分別為機械手第i個廣義連桿質心的慣性力和力矩，可以通過牛頓-歐拉方程遞推得到；
基于傾斜力矩觀測公式、機械手和水下無人航行器的耦合運動造成的耦合力的觀測公式得出擾動力矩的觀測公式是：
2.所設計協調運動控制器是水下無人航行器和機械手的協調運動控制器，包括運動學控制律和動力學控制律兩部分。
3.所述動力學控制率為：
式中：代表質量、慣性以及附加質量的觀測矩陣，代表由質量和附加質量引起的科氏力的觀測矩陣，代表由流體粘性引起的阻力的觀測矩陣，Kd和Kp代表微分系數和比例系數，q代表實際位置量，qd代表期望位置量，代表位置偏差。
與現有技術相比，本發明的有益效果是：本發明根據水下機器人精確作業控制的需要設計一種水下無人航行器和機械手系統的手艇協調運動控制方法，通過測量和估計作業過程中水下無人航行器所受到的機械手的擾動力，并進行補償，保證水下無人航行器的平穩運動，實現精確的定位和作業控制。該發明具有控制準確、適應性強、魯棒性高等優點。
附圖說明
圖1是水下無人航行器控制系統的內部硬件系統框圖；
圖2是水下無人航行器協調控制的流程圖；
圖3是水下無人航行器和機械手的協調運動控制器框圖；
圖4(a)至圖4(d)分別是無協調控制時觀測到機械手的各關節位移、艇體產生的縱搖和橫搖、機械手運動產生的傾斜力矩以及機械手運動產生的耦合力對水下無人航行器的干擾力；
圖5是應用本發明對水下無人航行器和機械手協調運動的控制結果；
圖6(a)至圖6(f)分別是觀測得到的協調運動過程中機械手各關節位移、艇體姿態、機械手運動中觀測到的擾動力、機械手運動中觀測到的擾動力矩、機械手運動中產生的耦合力、機械手運動中產生的傾斜力矩對水下無人航行器的擾動力；
具體實施方式
下面結合附圖與具體實施方式對本發明作進一步詳細描述。
本發明為：
a、水下無人航行器機械手系統的手艇協調控制方法由水下無人航行器控制系統和協調控制控制器兩部分組成。水下無人航行器控制系統主要由PC104核心模塊、I/O板、can數據采集板、隔離串口板、數據采集板、直流伺服電機控制板、機械手直流伺服電機驅動器、編碼器、機械手電機、推進器螺旋槳及其電機驅動器、磁羅經、深度計、多普勒測速儀組成。控制器通過隔離串口板采集多普勒測速儀和磁羅經的數據，并通過PC104模塊進行船位推算，獲得水下無人航行器當前位姿；進而建立擾動力觀測器，并設計協調運動控制器控制機械手的運動；通過can數據采集板、機械手直流伺服電機驅動器和編碼器獲取機械手各關節當前姿態，并控制機械手的運動；通過直流伺服電機控制板、推進器螺旋槳及其電機驅動器控制水下無人航行器螺旋槳的運動。
b、所建立擾動力觀測器為機械手對航行器的擾動力觀測器。擾動力主要包括機械手和水下無人航行器的耦合運動造成的耦合力，作業過程中系統浮心和重心變化派生的傾斜力矩兩部分。其中傾斜力矩可以結合水下無人航行器和機械手廣義連桿的當前位置姿態，通過下面的公式來觀測：
式中，和分別為機械手第i個廣義連桿相對于水下無人航行器的重力和浮力的矩陣表達形式，0RV為艇體坐標系到機械手基座坐標系的轉換矩陣；和分別為機械手第i個廣義連桿的重心和浮心相對于水下無人航行器的位置，而和分別為機械手艇體重心和浮心相對于水下無人航行器的位置，mig和FBi分別為機械手第i個廣義連桿相對于水下無人航行器的重力和浮力。機械手和水下無人航行器的耦合運動造成的耦合力可以通過下面的公式來觀測：
式中0f1,0和0n1,0是水下無人航行器在手臂基座受到的機械手的耦合作用力，1f1,0和1n1,0是水下無人航行器在手臂基座受到的機械手的耦合作用力。0R1是機械手基關節的隨動坐標系到機械手基座坐標系的轉換矩陣。0f1,0和0n1,0將由下式遞推得到：
式中ifi,i-1和ini,i-1為第i個廣義連桿在其第i關節對第i-1關節的約束反力(力矩)。iri和irci分別為機械手第i個廣義連桿在其坐標系下的位置矢量和中心位置矢量。iFi,i和iMi,i分別為機械手第i個廣義連桿在水中運動所受到的流體作用力。ifi*和分別為機械手第i個廣義連桿質心的慣性力和力矩，可以通過牛頓-歐拉方程遞推得到。
由于航行器受到的擾動力(矩)主要由上述兩種組成，所以擾動力矩可按如下公式進行觀測：
c、所設計協調運動控制器為水下無人航行器和機械手的協調運動控制器。控制器包括運動學控制律和動力學控制律兩部分。將專利文獻“基于浮游式水下機器人的S面控制方法(CN1718378A)”作為運動學控制律，主要作用是保證水下無人航行器和機械手的協調控制器全局收斂；在動力學控制律的設計中，將基于系統的動力學模型，觀測水下無人航行器在作業過程中受到的機械手擾動力，并控制推進器運動進行補償，實現作業過程中航行器穩定和精確控制。協調控制器如下：
式中：代表質量，慣性以及附加質量的觀測矩陣,代表由質量和附加質量引起的科氏力的觀測矩陣，代表由流體粘性引起的阻力的觀測矩陣，Kd和Kp代表微分系數和比例系數，q代表實際位置量，qd代表期望位置量，代表位置偏差。
下面結合附圖進行說明：
結合圖1，水下無人航行器控制系統的核心是PC104核心模塊，PC104模塊通過隔離串口板采集多普勒測速儀和磁羅經的數據，并進行船位推算，獲得水下無人航行器當前位姿；PC104模塊通過can數據采集板、由機械手直流伺服電機驅動器和編碼器獲取機械手各關節的當前姿態，并發送控制指令控制機械手的運動；PC104模塊根據協調運動控制器的結果，通過直流伺服電機控制板向電機驅動器發送控制指令，控制水下無人航行器螺旋槳運動實現作業過程中水下無人航行器的穩定精確運動。
結合圖2，水下無人航行器機械手系統的手艇協調控制的流程是：首先，傳感器采集航行器位姿和機械手的運動姿態數據；然后，通過運動學控制律調整位置偏差使其收斂，同時，擾動力觀測器按照式(4)觀測擾動力，并通過動力學控制律對其進行補償；最后，兩種控制律通過推進器同時作用在航行器上，傳感器獲取航行器當前時刻運動姿態數據，繼續進行下一輪的協調控制。
結合圖3協調運動控制器將S面控制方法(CN1718378A)作為運動學控制律，以保證水下無人航行器和機械手的協調控制器全局收斂；并基于系統的動力學模型，觀測水下無人航行器在作業過程中受到的機械手擾動力，并控制推進器運動進行補償，實現作業過程中航行器穩定和精確控制。
結合圖4(a)至圖4(d)，在沒有經過協調運動控制器的實時控制條件下，機械手三個關節的運動對艇體產生了較大的傾斜力和耦合力矩，其結果是對艇造成了較大的橫傾和縱傾。
結合圖5，水下無人航行器機械手系統按照軌跡規劃的對航行器和機械手各關節的規劃軌跡，通過協調控制精確地實現了航行器和機械手末端的位姿控制，說明控制器可以有效地應用于在水下無人航行器機械手系統的水下作業精確控制。
結合圖6(a)至圖6(f)，表示經過動態控制后，觀測到的機械手產生的干擾力(矩)，與圖4對比可發現，協調運動控制器顯著地減小了傾斜力(矩)對艇體的影響，較好地控制了機械手運動造成的航行器本體在橫傾和縱傾方向的運動。
綜上，本發明提供的是水下無人航行器機械手系統(Underwater Vehicle Manipulator System-UVMS)的手艇協調控制方法。用以實現水下無人航行器的穩定的高精度作業。為實現上述目的，水下無人航行器和機械手系統的手艇協調控制方法包括：通過PC104、數據采集板、機械手電機、螺旋槳推進器、磁羅經、多普勒測速儀等設備構建水下無人航行器控制系統，獲取系統各自由度的姿態信息，同時進行硬件層面的控制；根據航行器和機械手的位置姿態建立內部擾動力觀測器，依據牛頓-歐拉方法觀測各時刻因系統姿態變化產生的傾斜力矩和耦合力矩；建立航行器和機械手的協調運動控制器，在控制中對航行器作業過程中受到的機械手擾動力進行補償，實現作業過程中航行器穩定和精確控制。該方法可實際應用于UVMS控制系統設計，對于水下機器人在自主作業和遙控作業方面具有重要意義。
- 還沒有人留言評論。精彩留言會獲得點贊！