基于改進蟻群算法的海上風電運維船系泊系統逆運動學求解研究*

郭辰

（華能新能源股份有限公司，北京 100036）

基于改進蟻群算法的海上風電運維船系泊系統逆運動學求解研究*

郭辰

（華能新能源股份有限公司，北京 100036）

本文針對一種球形電動機驅動的系泊系統，提出了基于改進蟻群算法的系泊系統逆運動學求解方法。通過對改進蟻群算法應用于系泊系統逆運動學求解的合理性，及參數設置規律等進行仿真研究，對采用此逆運動學求解方法后，系泊系統的控制效果進行了驗證。仿真結果表明，本文提出的逆運動學求解方法能夠快速準確地找到系泊系統逆運動學解，從而使系泊系統實現精確的軌跡跟蹤控制。

系泊系統；逆運動學求解；改進蟻群算法；全局搜索；局部搜索；球面定位；軸向伸縮

0 引言

截至2012年年底，全球海上風電裝機容量已達到5410MW。歐洲海上風電發展較快，其中，英國海上風電裝機容量超過2900MW，位居世界第一；我國海上風電裝機容量也接近400MW[1]。隨著我國風電產業技術的發展和相關支持性政策的逐步出臺，我國海上風電場將在未來得到迅速發展。

海上風電場運行維護主要依靠海上運維船只。這種船只在進入目標風電機組數米范圍內水域時，需依靠其接近系統（根據船只大小及需要，接近系統可能包括搭乘系統和系泊系統）將船身與風電機組連接起來，以起到固定船身、搭載人員或小型部件、維修工具等目的。

1 一種海上運維船舶系泊系統

海上運維船舶的搭乘系統和系泊系統需要建立船身與風電機組特定構件間的穩定聯系，故上述系統需要具備空間定位能力。海上運維船及系泊系統示意圖如圖1所示。

一種系泊系統的執行機構可采用球形電動機。球形電動機結構簡單、體積小、重量輕、損耗小、里能指標高、便于控制，能實現自轉、俯仰、偏航等三個自由度的運動，可應用于機器人關節等做空間多自由度運動的精密裝置中[2-3]。一種球形電動機如圖2、圖3所示[4-5]。

圖1 海上運維船舶及系泊系統示意圖

圖2 一種球形電動機結構示意圖

圖3 轉子的三自由度運動

配合機械臂等裝置，可在一定角度范圍內實現空間定位。用球形電動機作為驅動機構實現的系泊系統示意圖如圖4所示。球形電動機輸出軸與機械臂實現剛性連接；末端為機械鎖扣裝置（機械爪環），可在接觸到風電機組掛靠桿時，通過機械機構實現扣鎖。

球形電動機實現俯仰、偏航運動時，可帶動機械臂進行固定球面內的空間定位；球形電動機自轉時，通過輸出軸法蘭、螺栓桿、傳動螺母等機械結構，機械臂能實現伸縮運動。于是，通過對球形電動機的廣義歐拉角進行控制，就能實現機械臂末端機械抓環的空間定位，從而實現船身與風電機組結構的可靠連接。

要實現上述目的，關鍵在于通過對球形電動機歐拉角的控制，實現精確、快速的空間定位；這就需要對球形電動機的逆運動學進行求解，即根據空間定位的要求，迅速求解得到歐拉角的控制要求。本文結合圖4所示系泊系統，提出一種基于改進蟻群算法的逆運動學求解方法，從而實現對系泊系統進行精確快速的控制。

球形電動機的運動學分為正運動學和逆運動學。其中，前者是根據各個自由度的歐拉角[4]變化情況來求解電動機轉子輸出軸的運動情況，是從歐拉角空間到笛卡爾空間的映射的求解問題；后者則是相反的過程，是從笛卡爾空間到歐拉角空間的映射的求解問題。球形電動機的逆運動學求解問題是其進行運動控制，運動分析，離線編程和軌跡規劃等的基礎。目前，關于球形電動機逆運動學分析求解的文獻還很少。

意大利學者M. Dorigo在1991年首次提出了蟻群算法[6]。蟻群算法是一種全局優化的搜索算法,具有較強的魯棒性，且易于與其他算法結合，已經成功地應用于旅行商問題(TSP)、資源二次分配等經典優化問題，取得了良好的效果[7-9]。

圖4 系泊系統結構示意圖

2 系泊系統運動學逆問題

由前所述，系泊系統運動學逆問題，實際上是球形電動機運動學逆問題與軸向伸縮問題的組合，因此，球形電動機的逆運動學問題是難點所在。以下對其進行研究。

球形電動機的定子位置和轉子位置分別用靜坐標系xyz和動坐標系dqp來定義，轉子輸出軸與dqp系中的p坐標軸重合。若x軸從xyz系旋轉α角到x1y1z1系，y軸從x1y1z1旋轉β角到x2y2z2系，z軸從x2y2z2系旋轉γ角到dqp系，則所產生的角度θ＝(α,β,r)T稱為廣義歐拉角[5-10]。角度余弦cos簡記為c，角度正弦sin簡記為s，則所產生的旋轉矩陣A可表示如下：

此旋轉矩陣滿足如下關系式：

由式（2）可知，永磁球形電動機輸出軸在某一時刻t的位置向量和歐拉角向量之間的關系可以表示如下：

其中，F為一個(3×s)的矩陣，其中，s的具體取值取決于初始坐標值式（3）即為球形電動機的正向運動學方程。

一種逆運動學求解策略是建立球形電動機的微分運動關系，根據轉子輸出軸在xyz坐標系中的速度向量來求解其對應的歐拉角速度向量。對式（3）兩邊同時求導數，可得：

于是可以求出歐拉角速度向量：

式中，J-1(θ(t))為雅可比矩陣的逆矩陣。

然后根據初始條件對式（5）兩邊同時求積分，即可得到永磁球形電動機的逆運動學方程。

用上述方法求球形電動機的雅可比矩陣及其逆矩陣，計算比較復雜；且對于不同的初始位置坐標，雅可比矩陣的形式不同。因此采用這種方法進行逆運動學求解比較困難。

3 一種改進蟻群算法及其實現

蟻群算法利用一群人工螞蟻來模擬真實螞蟻的行為，通過人工螞蟻之間的協作來尋找較為優化的解。每只人工螞蟻代表一個計算單元，在每次迭代中，負責構建解問題的一條路徑，計算并存儲該路徑的目標函數值，同時在所經過的路徑上釋放一定數量的信息素。在后一步的迭代中，螞蟻能夠檢測到前一步迭代中積累的信息素的濃度，并據此選擇自己的前進方向；同時，信息素會隨著時間的推移逐漸揮發掉。于是，路徑的長短及該路徑上通過螞蟻的多少就對殘余信息素的濃度產生了影響；同樣，殘余信息素濃度的大小又指導著后來螞蟻的行動方向。因此，某條路徑上走過的螞蟻越多，則后來的螞蟻選擇該路徑的概率就越大。

由式（3）可知，本文中的逆運動學問題是一個三維函數F(θ)的求解問題，即通過優化算法求得3個歐拉角α，β和γ的數值。由轉子輸出軸的初始位置坐標和所求得的歐拉角，可以確定其旋轉后的位置坐標令目標函數為：

式中(xd, yd, zd)為給定的轉子輸出軸的位置坐標值。目標函數值越小，代表根據逆運動學解法得到的(xe, ye, ze)與給定的(xd, yd, zd)越接近，即該解法的求解精度越高。

在永磁球形電動機逆運動學求解中，待求解的參數共有3個，分別為α，β和γ。普通蟻群算法在每個參數的取值范圍內隨機產生N個值作為備選數值點，這樣產生的備選數據點就有可能沒有覆蓋最優解附近的范圍，從而導致尋優結果不理想。本文中，這3個參數的取值范圍分別設置為[-1, 1]，則逆運動學的解空間為[-1, 1]×[-1, 1]×[-1, 1]。將這3個參數的取值范圍分別均勻地離散化為N個值，在本文中，稱N為單維離散化率。

3.1 全局搜索

由前所述，逆運動學的解空間中備選數值點的數目為N×N×N。對于其中任意一個參數，將其取值范圍內的每個值看作一個元素，則N個元素形成一個集合，設為Ii(i=1,2,3)。定義螞蟻的數目為m，全部螞蟻從蟻巢出發尋找食物。每只螞蟻從集合I1出發，根據集合中每個元素的信息素狀態和式（7），獨立隨機地從每個集合Ii中唯一地選擇一個元素；螞蟻在所有集合中完成元素的選擇（即完成全局搜索），然后在所選擇元素周圍完成局部搜索后，它就到達了食物源，之后調節集合中各個元素的信息素。這一過程反復進行，直至找到最優解。全局搜索的步驟如下：

（1）初始條件：令集合Ii(i=1,2,3)中的元素j(j=1,…,N)的信息素初始值phj(Ii)(0)=C，迭代次數初值Nc=1，設置最大迭代次數Nc_max。

（2）啟動所有螞蟻，每只螞蟻從集合Ii(i=1,2,3)開始，按照下述規則依次在每個集合中選擇一個元素，直到螞蟻全部選擇完畢。

路徑選擇規則：對于集合Ii(i=1,2,3)，任意一只螞蟻k(k=1,…,m)，根據下式計算的概率隨機地選擇它的第j個元素。

（3）當每只螞蟻在每個集合中都選擇一個元素后，計算由各個螞蟻所選數值作為歐拉角參數時的目標函數值，并記錄其中的最小值及其對應的歐拉角參數。設上述螞蟻覓食過程經歷了m個時間單位，對所有集合Ii(i=1,2,3)中的各個元素的信息素按照下式進行調整：

其中，參數ρ(0≤ρ＜1)表示信息素的持久性，則1-ρ表示信息素的消逝程度。表示在本次循環中第k只螞蟻在集合Ii的第j個元素上留下的信息素，可以用下式來表示：

其中，Q為常數，用來調節信息素的調整速度；Fk是以螞蟻k選擇的三個元素分別作為歐拉角數值時的目標函數值；η為Fk的指數，不同的η值可以得到不同的尋優效果。由式（10）可以看出，目標函數越小，信息素的增量就越大。普通蟻群算法中，η=1；在本文所提出的改進蟻群算法中，η=2。由后面的仿真分析可以看出，η=2時蟻群對歐拉角參數的尋優速度會明顯提高。當然，η取值過大可能會造成尋優過程不穩定，并且容易陷入局部極小。

3.2 局部搜索

由上所述，全局搜索是根據解空間中的N×N×N個備選數值對應的目標函數值來確定每次最優的目標函數值對應的數值點。全局搜索的對象是各個離散點，點與點之間的值就會被忽略掉，由此影響解的質量。為了得到更加優化的解，本改進蟻群算法相比普通蟻群算法增加了局部搜索，即讓螞蟻在其所選擇的數值點周圍的一個小的鄰域內進行搜索，并對原數值點進行相應的微量移動，以使得數值點對應的目標函數值更小，由此提高解的質量。

在局部搜索過程中，需要判斷螞蟻進行微量移動的方向和移動的步長。為了確定移動的方向，先根據當前集合Ii(i=1,2,3)中第j(j=1,…,N)個元素對應的信息素來確定該元素上應該有的螞蟻數目。具體計算公式如下：

要判斷螞蟻的移動方向，還要借助于螞蟻所在元素左邊和右邊的所有元素上的應有螞蟻數目和實際螞蟻數目。集合Ii(i=1,2,3)中第j(j=1,…,N)個元素左邊所有元素上的應有螞蟻數目如下：

集合Ii(i=1,2,3)中第j(j=1,…,N)個元素右邊所有元素上的應有螞蟻數目如下：

集合Ii(i=1,2,3)中第j(j=1,…,N)個元素左邊、右邊以及第j個元素上的實際螞蟻數目Nsl-j(Ii)、 Nsr-j(Ii)和Nsj(Ii)可以通過螞蟻當前的分布直接得出。螞蟻的移動方向如表1所示。

表1中所列的7種情況以外的其他情況下，螞蟻將不移動。由表1可以看出，螞蟻的移動方向總是向著實際螞蟻數目少于應有螞蟻數目的方向。離散空間中的離散點越多，即元素數目越大，各個元素上應有螞蟻數跟實際螞蟻數的差別就越小，此時的離散空間也就越接近于連續空間。本文的局部搜索就是要在不改變解空間中元素數目的情況下，讓離散解空間最大限度地接近連續空間，從而提高解的質量。定義差別度變量D，對于集合Ii(i=1,2,3)中第j(j=1,…,N)個元素來說，差別度D如下所示：

其中，b由如下規則賦值：

于是，每次微量移動的步長定義如下：

其中，ε為局部搜索步長因子。在本文中，取ε=0.02/m，其中m為螞蟻數目。

在螞蟻完成局部搜索后，將新的元素值代替局部搜索前的元素值，重新進行全局搜索。全局搜索和局部搜索交替進行，直到得到最優解，或者達到最大迭代次數為止。事實上，局部搜索的運算量較小，蟻群算法的運算量和運算耗時主要體現在全局搜索階段。螞蟻數目m和單維離散化率N較小時，蟻群算法的運算量和運算耗時也較小。

本文提出的改進蟻群算法運行流程圖如圖2所示。圖中，Nc表示迭代次數，Nc_max表示最大迭代次數。

表1 螞蟻移動方向判斷規則

改進蟻群算法實現了在任意初始位置下，從笛卡爾空間到廣義歐拉角空間的轉換。然而，對于球面求解問題而言，笛卡爾空間三個變量x, y, z之間并非完全獨立，而滿足球形約束關系，只有其中兩個向量是完全獨立的；這就是說，（3）式中F在固定球面內的秩為2；廣義歐拉角向量也只需兩個歐拉角變量即可在特定球面上確定空間位置。結合歐拉角的物理意義可知，自轉歐拉角γ在固定球面空間的定位問題中為冗余解；也就是說，在球面定位過程中，自轉歐拉角γ的控制指令為零。

由以上的分析可知，系泊系統要想從圖1(a)的狀態到達圖1(b)的狀態，需要首先實現球面內的定位，然后實現伸縮運動，即通過自轉運動推進此機械臂，使系泊系統末端的機械抓環到達掛靠桿。

以下建立自轉運動對應的廣義歐拉角與伸縮量之間的關系。設螺栓桿的螺距為，則兩者關系如下：

4 仿真研究

圖5 改進蟻群算法運行流程圖

以下對本文提出的改進蟻群算法進行仿真研究，取Q=200，ρ=0.7。設轉子球體的半徑為R，仿真中，電動機輸出軸的轉子球面位置點的初始坐標為(xi,yi,zi)T=(0, 0, R)T，轉子旋轉后的坐標為(xe, ye, ze)T=(0.433R, 0.500R, 0.750R)T。首先比較在采用普通蟻群算法和改進蟻群算法時目標函數最小值隨迭代次數的變化情況，如圖6所示。圖6中，螞蟻數目m=40，解空間內的單維離散化率N=20，最大循環次數設置為Nc_max=30。

由上圖可以看出，在具有相同起點的情況下，采用本文提出的改進蟻群算法時目標函數最小值的收斂要明顯快于普通蟻群算法。事實上，當m和N取其他數值時，也有類似的規律。這是由于改進蟻群算法中η=2，信息素的增量對不同大小的目標函數值更加敏感，每次迭代中使得目標函數值最小的歐拉角數值就會被加速強化，提高其下次被選擇的概率，從而提高整個算法的收斂速度。由于局部搜索的作用，改進蟻群算法更容易找到最優解，因此圖6中兩種算法所得到的目標函數最小值不同。采用改進蟻群算法，到第5步時目標函數最小值為0.0370，到第20步時目標函數最小值為0.0351。這是由于在第3步以后，局部搜索起主要作用，找到的數值點仍然向著最優的方向微量移動。

為了便于理解蟻群算法，以下對圖6中采用改進蟻群算法時螞蟻分布隨迭代次數的變化情況進行仿真，如圖7所示。其中，圖7(a)表示Nc=1，即迭代開始時的蟻群分布情況，圖7(b)表示Nc=5時的蟻群分布情況，圖7(c)表示Nc=10時的蟻群分布情況，圖7(d)表示Nc=15時的蟻群分布情況。

從上圖可以看出，在迭代開始時，螞蟻分散地分布在解空間的元素上。根據前述信息素的初值及各個元素被選擇概率的關系，蟻群此時的分布是隨機的，選擇解空間內任意元素的概率是相等的。隨著迭代的進行，蟻群的分布越來越集中，并逐漸匯集于一點，如圖7(d)所示。這個點正是蟻群尋優的收斂點，是改進蟻群算法在迭代中找到的最優解。

由圖6可以看出，改進蟻群算法在運行至第5步迭代時，目標函數最小值已穩定在一個基值上（以后各步中局部搜索起主要作用），說明目標函數最小值基本已經找到，但是并不說明此時所有螞蟻都聚集在一點。這從圖7(b)中也可以看出。

改進蟻群算法中的各個參數設置會改變仿真的結果。為了進行準確的逆運動學求解，應當選取合適的參數。與普通蟻群算法一樣，本文提出的算法中多數參數根據經驗設置。在仿真實驗中，Q和ρ兩個參數的取值對仿真結果的影響不大。對仿真結果影響最大的是解空間的單維離散化率N和人工蟻群中螞蟻的數目m。當蟻群中螞蟻數目m=15，最大迭代次數為Nc_max=20時，目標函數最小值和逆運動學求解結果隨單維離散化率N的變化情況如圖8所示。

由上圖可以看出，隨著N的增大，目標函數最小值逐漸減小，逆運動學求解結果也趨于穩定。事實上，N越大，解空間的劃分就越細密，越有利于螞蟻在較好的解空間區域中進行更加細密的局部搜索。因此，N越大就越容易搜索到較好的目標函數最小值和全局優化的解。

當單維離散化率N=20，最大迭代次數為Nc_max=20時，目標函數最小值和逆運動學求解結果隨蟻群螞蟻數目m的變化情況如圖9所示。

由上圖可以看出，螞蟻數目較小時，目標函數的最小值出現波動，且逆運動學求解的結果也不穩定，這是由于，此時蟻群的正反饋作用較強，全局隨機搜索能力較弱，容易陷入局部極小。隨著螞蟻數目m的增大，目標函數最小值開始穩定減小，表明逆運動學求解結果在逐漸變得精確。這是因為，螞蟻數目較大時，蟻群的全局隨機搜索能力就會加強，就越容易搜索到較好的解。

圖10表示系泊系統將采用本文提出逆運動學求解方法得到的歐拉角作為控制系統的輸入量，并采用PD控制策略時，系泊系統末端機械抓環在笛卡爾空間的運動軌跡。設球形電動機轉子中心為圓心，轉子中心到機械抓環之間的距離為半徑，此半徑設為單位1。圖中，球面上的紅色圓圈為球面定位運動的指令位置，球面以外的紅色菱形表示伸縮運動的指令位置。

從圖10(a)可以看出，系泊系統完成球面定位后，機械抓環的位置與紅色圓圈表示的指令位置重合度較高。從圖10(b)可以看出，完成伸縮運動后，機械抓環末端位置與紅色菱形表示的指令位置幾乎完全重合。圖10說明，采用本文提出的逆運動學求解方法后，系泊系統進行系泊操作的定位精度較高。

5 結論

圖6 普通蟻群算法和改進蟻群算法中目標函數最小值的變化情況

圖7 螞蟻分布隨迭代次數的變化情況

圖8 單維離散化率N對目標函數最小值和逆運動學求解結果的影響

本文針對一種球形電動機驅動的系泊系統，提出基于改進蟻群算法的系泊系統逆運動學求解方法。改進蟻群算法能夠在解空間中對逆運動學解進行全局與局部搜索尋優，得到的解具有較高的精度，且比普通蟻群算法的收斂速度快。基于改進蟻群算法的逆運動學求解方法，能夠實現準確的球面定位和伸縮定位，從而實現系泊系統的軌跡跟蹤控制。仿真結果驗證了該算法的合理性，對該算法中螞蟻數目和單維離散化率參數與求解結果之間的影響機制進行了分析研究，為算法的參數設置提供了依據；仿真結果顯示，以逆運動學求解得到的歐拉角作為控制系統輸入信號，在采用PD控制算法時，系泊系統可以得到良好的軌跡跟蹤效果。

圖9 蟻群螞蟻數目m對目標函數最小值和逆運動學求解結果的影響

圖10 系泊系統采用本文提出逆運動學求解方法及PD控制策略時的軌跡跟蹤效果

[1] 2012年中國風電裝機容量統計. 中國可再生能源協會風能專業委員會.

[2] Liang Yan, I-Ming Chen, Chee Kian Lim, Guilin Yang, Wei Lin, Kok-Meng Lee. Design and Analysis of a Permanent Magnet Spherical Actuator[C]. IEEE/ASME Transactions on Mechatronics. 2008, 13(2): 239-248.

[3] Klemens Kahlen, Ingo Voss, Christian Priebe, Rik W. De Doncker. Torque Control of a Spherical Machine with Variable Pole Pitch[J]. IEEE Transactions on Power Electronics, 2004, 19(6): 1628-1634.

[4] 夏長亮, 李洪鳳, 宋鵬. 基于Halbach陣列的永磁球形電動機磁場研究[J]. 電工技術學報, 2007, 22(7): 126-130.

Xia Changliang, Li Hongfeng, Song Peng. Magnetic field model of a PM spherical motor[J]. Transactions of China Electrotechnical Society, 2007, 22(7): 126-130 (in Chinese).

[5] Changliang Xia, Chen Guo and Tingna Shi. A Neural Network Identifier and Fuzzy Controller Based Algorithm for Dynamic Dycoupling Control of Permanent Magnet Sperical Motor [J], IEEE Transactions on Industrial Electronics, pp. 361-372, vol. 59(1), 2012.

[6] Marco Dorigo, Luca Maria Gambardella. Ant Colony System: a Cooperative Learning Approach to the Traveling Salesman Problem[J]. IEEE Transactions on Evolutionary Computation, 1997, 1(1): 53-66.

[7] 翟海保, 程浩忠, 呂干云, 陳小良, 馬則良.基于模式記憶并行蟻群算法的輸電網規劃[J]. 中國電機工程學報, 2005, 5, 25(9): 17-22.

[8] 趙慶杞, 黎明, 張化光. 基于蟻群算法的靈敏負荷調度[J]. 中國電機工程學報, 2006, 12, 26(25): 15-21.

[9] 余玲, 劉康, 李開世. 蟻群算法的連續空間算法研究[J]. 機械設計與研究, 2006, 4, 22(2): 6-9.

[10] 黃聲華. 三維電動機及其控制系統[M]. 武漢:華中科技大學出版社, 1998.

Study on Problem of Inverse Kinematics of Mooring System of Offshore O&M Vassel Based on Advanced Ant Colony Algorithm

Guo Chen
（Huaneng Renewables Co., Ltd., Beijing 100036, China）

For a Spherical Motor drived MS of ofshore O&M vessel, one Advanced Ant Colony Algorithm (AACA) based IKS strategy is proposed in this paper. Through the axial moment of Spherical Motor, Axial Telescopic (AT) control of MS can be completed and the Trajectory Tracking of MS is realized. The proposed method for problem of Inverse Kinematics is validated, and the parameter seThing rule of AACA is researched. Simulation result indicated that the proposed IKS strategy for MS can solve the problem of IKS accurately and actual trajectory tracking.

Mooring System; Inverse Kinematics Solution; Advanced Ant Colony Algorithm; global search; local search; Spherical Positioning; telescopic positioning

TM614

A

1674-9219（2013）12-0064-08

國家863計劃課題(2012AA051706)和(2012AA051703)資助項目。

2013-10-02。

郭辰（1982-），男，工學博士，主要從事風能資源評估及微觀選址技術、海上風電場接近技術及系統、非線性系統動力學建模及仿真、智能控制策略及其應用、特種電機及其智能控制等方面的研究。