基于URWPGSim2D仿真平臺水中機器魚搬運策略分析

奉榆杰 田錦波

摘?要：基于URWPGSim2D仿真平臺，提出在水中搬運項目上解決平臺隨機性給魚帶球動作帶來的影響。設計魚到洞的最佳路以及魚帶球進洞的最佳路線，設計出較好的魚姿，同時在魚運動到目標點的過程中采用虛擬洞的算法，讓魚能夠高效的頂球進洞，最大程度上避免失誤。實驗結果表明：采用上述算法，能夠有效的減少平臺帶來的隨機性誤差，魚頂球進洞的效率大大提高。

關鍵詞：URWPGSim2D仿真平臺;魚姿;虛擬洞

隨著科技的發展，人類對未知領域了解的越來越多，海洋就是其中的一個代表。但是人類最大的下潛深度不足水下機器人下潛深度的十分之一，所以對于水下機器人的開發尤為重要。而URWPGSim2D仿真平臺就是對魚的動作的仿真，分析魚的運動姿態和水波對運動造成的影響，控制這些隨機因素，讓魚的運動更加順暢。同時仿真就是解決前期存在的明顯問題，為制造出更好的實物機器打下基礎。

1 目標任務及尺寸

在URWPGSim2D仿真平臺中兩條魚需要將如下圖所示的右邊六個球按順序運到左邊的洞里，魚體矩形長*寬為160mm*45mm;仿真水球半徑為58mm;洞的半徑為80mm。

2 頂球的順序及魚的運動路線規劃策略

1號魚先去頂5號球，再去頂4號球，最后頂2號球;2號魚先頂3號球，再頂1號球，最后頂0號球。在找到最佳擊球點的基礎上我們去規劃路徑，在實踐中我們會發現如果魚從球的左邊去頂球，最后會出現魚追著魚跑的情況，所以在魚運動到球的過程中我們往往會重新設定一個X值大于魚坐標的目標點，就會避免這種。但是這種策略安排會造成兩條魚的同步性非常強，最后會造成兩條魚到達目標的同時性非常強，所以我們會在時間上將兩條魚錯開。

3 頂球進洞失誤分析

經過多次的實驗，我們發現此項目中出現次數最高的失誤便是：

3.1 魚推球的路徑起始階段偏離目標點過遠

在大量的實踐中我們發現，使用Dribble函數控制魚帶球時，魚從觸球開始到運動到接近洞的過程中常常不能保持直線前進，而是起先朝偏離洞的方向前進，隨后在球的水平坐標接近洞時才開始重新觸球調整。一旦出現這種情況將耗費大量的時間。

3.2 球在即將入洞時受干擾影響“錯過”球洞

如下圖所示，在球快要入洞時，我們發現此時球常常會偏移一段距離而導致球壓線而過，錯失進洞機會。如果在錯失進洞后依舊不改變策略很可能還會導致魚將球帶入死角，使球再無進洞的可能性。

這兩個問題存在共性，便是都存在球的偏移，在實驗中我們還發現，這種偏移很大一部分都是沿著Z的正方向的。經過分析發現，問題產生是由于平臺引入的隨機干擾，以及魚推著頂球點跑引起的。

4 解決措施

4.1 控制觸球點及觸球角度

首先通過posetopose函數讓魚運動到球的后方，為了提早調整時間，posetopose的目標點便應設置在洞與球連線的反向延長線上。隨后便使用Dribble函數，設置較低的速度擋輸入參數值，使魚更精確地接近球。

4.2 運球優化策略

為了更大程度的避免兩種情況的發生，優化運球過程，我們引入了一種虛擬洞算法，即在運球過程中，并不始終以真正的洞坐標點為最終目標點，而是根據球的位置不停地變化洞的坐標，夸大偏移量，以達到讓魚更快做出環繞球調整動作以及增強對抗干擾能力的目的。

4.3 算法描述

①虛擬洞坐標確定。在B處的球需要被運送到H處的洞，球與洞之間Z方向的坐標差的絕對值是deltaZ=H.Z-B.Z，以此參數為標準進行調整。當deltaZ>0時，此時球在洞的下方，如圖所示（圖），為使得魚在Dribble函數的控制下能夠更早地繞球調整方向，將假象的目標點向H的上方偏移，從而由洞口坐標算得的頂球點坐標相應的往球下方滑動。得到的虛擬的洞口坐標：

MH=（H.X，H.Y，H.Z+deltaZ）

在deltaZ<0時有類似的關系，此時球位于洞的上方，需將虛擬的洞口設置在H的下方，不過在實際測試中，球總是往Z軸正方向偏移的，此時依舊沿用上式可能會出現接近洞口時，向下調整過度而使球直接偏洞而過，所以我們在這里設置一個系數k來對deltaZ的值進行適當縮小，即：

MH=（H.X，H.Y，H.Z+k*deltaZ）

經過多次測試發現，當球與洞的橫向距離deltaX>=100時k=0.4比較合適，而當deltaX<100時k=-0.7比較合適，使魚既不會向Z軸正方向調整過度，也不會向Z軸負方向持續偏離控球。

②計算新的頂球點。在虛擬洞坐標的基礎上計算頂球點，以虛擬洞和球中心的連線反向延長，得到隨球的位置的變化不停調整的頂球點坐標MB，計算步驟如下：

θ=Arctan（（MH.Z-B.Z）/（MH.X-B.X））

MB=（B.X+cosθ*r，B.Y，B.Z+sinθ*r）

③參數輸入Dribble函數中。將得到的新的頂球點帶入Dribble函數的輸入參數中，實現運球過程中對目標點的控制，使魚一次運球進洞的成功率提高。