本發明屬于柔性機械臂路徑規劃，具體涉及一種水下柔性機械臂空間路徑規劃方法的設計。
背景技術：
1、地球有近70%被水覆蓋，其中的資源更是占巨大比值，而人類在不斷發展過程中必將想方設法開發水下資源。而水下世界錯綜復雜，針對如何使用機械設備來替代人類完成這一任務，開發水下機器人、機械臂等是目前比較熱門的方式。
2、傳統水下機械臂類似人類四肢，使用多個單維度關節來實現多自由度。而這必定會導致機械臂體積過大，且控制復雜。在某些特定環境可能需要特定彎曲，而傳統機械臂無法實現。并且傳統機械臂在控制過程中，需要對各個關節進行控制，需要對每個關節的位置與動作進行計算，控制較復雜。其中一個關節的控制精度出現偏差，必將影響到整體的動作，導致末端無法到達指定位置。此外傳統機械臂對路徑中的障礙物規避較為復雜，無法直觀地對障礙物坐標進行規避，需要對多個關節進行調整，才能實現避開障礙物。
技術實現思路
1、本發明的目的是為了解決現有機械臂控制復雜且精度容易被影響的問題，提出了一種水下柔性機械臂空間路徑規劃方法，同時對路徑中的障礙物進行規避，快速有效地規劃水下柔性機械臂路徑來完成抓取任務。
2、本發明的技術方案為：一種水下柔性機械臂空間路徑規劃方法，包括以下步驟：
3、s1、獲取水下柔性機械臂的長度、世界坐標系、目標點的世界坐標以及方向向量。
4、s2、根據水下柔性機械臂的長度得到局部坐標系原點，并構建局部坐標系。
5、s3、在世界坐標系中確定第一段圓弧所在的第一平面，在局部坐標系中確定第二段圓弧所在的第二平面。
6、s4、根據目標點的世界坐標以及方向向量計算第一平面到第二平面的旋轉坐標變換矩陣。
7、s5、根據旋轉坐標變換矩陣將第二平面的世界坐標轉換為局部坐標，進而計算得到第二段圓弧上每個點的局部坐標。
8、s6、根據旋轉坐標變換矩陣將第一段圓弧與第二段圓弧的交點的局部坐標轉換為世界坐標，進而計算得到第一段圓弧上每個點的世界坐標。
9、s7、根據旋轉坐標變換矩陣將第二段圓弧上每個點的局部坐標轉換為世界坐標。
10、s8、將第一段圓弧上每個點的世界坐標、第二段圓弧上每個點的世界坐標以及初始直線段的世界坐標組合得到水下柔性機械臂中心的路徑。
11、s9、根據水下柔性機械臂中心的路徑計算得到水下柔性機械臂四根繩的路徑。
12、進一步地，步驟s2包括以下分步驟：
13、s21、在世界坐標系 o-xyz的 y軸上獲取使水下柔性機械臂中心的路徑長度大于水下柔性機械臂的長度的節點。
14、s22、將節點與世界坐標系原點 o之間的任意一點作為局部坐標系原點 m。
15、s23、獲取第一段圓弧與第二段圓弧的交點 b。
16、s24、將局部坐標系原點 m與交點 b連線的方向作為 x’軸負方向，并使目標點 c位于 mx’ y’平面內，構建局部坐標系 m-x’ y’ z’。
17、進一步地，步驟s3具體為：將世界坐標系的 oxy平面作為第一平面，將局部坐標系的 mx’ y’平面作為第二平面。
18、進一步地，步驟s4包括以下分步驟：
19、s41、根據目標點 c的世界坐標以及方向向量 s得到第二平面的法向量，并將平行于 z軸的向量作為第一平面的法向量。
20、s42、根據第一平面的法向量和第二平面的法向量計算得到第一平面和第二平面的夾角。
21、s43、根據第一平面和第二平面的夾角得到第一平面到第二平面的旋轉坐標變換矩陣：
22、
23、
24、
25、其中表示第一平面到第二平面繞 x軸的旋轉坐標變換矩陣，表示第一平面到第二平面繞 x軸的旋轉角度，表示第一平面到第二平面繞 y軸的旋轉坐標變換矩陣，表示第一平面到第二平面繞 y軸的旋轉角度，表示第一平面到第二平面繞 z軸的旋轉坐標變換矩陣，表示第一平面到第二平面繞 z軸的旋轉角度。
26、進一步地，步驟s5包括以下分步驟：
27、s51、將第二平面的世界坐標乘以旋轉坐標變換矩陣，轉換為第二平面的局部坐標。
28、s52、根據第二平面的局部坐標計算得到第二段圓弧上每個點的局部坐標：
29、
30、
31、
32、
33、其中表示求解步數，表示第二段圓弧的圓心角的角度，表示從0到依次增加的固定角度，表示均分計算函數，表示絕對值函數，表示第二段圓弧上每個點的局部橫坐標，表示第二段圓弧上每個點的局部縱坐標，表示第一段圓弧與第二段圓弧的交點 b的局部橫坐標，表示第二段圓弧的圓心的局部坐標。
34、進一步地，步驟s6包括以下分步驟：
35、s61、將第一段圓弧與第二段圓弧的交點 b的局部坐標除以旋轉坐標變換矩陣，轉換為交點 b的世界坐標。
36、s62、根據交點 b的世界坐標計算得到第一段圓弧上每個點的世界坐標：
37、
38、
39、
40、
41、其中表示求解步數，表示第一段圓弧的圓心角的角度，表示從0到依次增加的固定角度，表示均分計算函數，表示絕對值函數，表示第一段圓弧上每個點的世界橫坐標，表示第一段圓弧上每個點的世界縱坐標，表示第一段圓弧與第二段圓弧的交點 b的世界橫坐標，表示第一段圓弧與初始直線段的交點 a的世界縱坐標。
42、進一步地，步驟s7具體為：將第二段圓弧上每個點的局部坐標除以旋轉坐標變換矩陣，轉換為第二段圓弧上每個點的世界坐標。
43、進一步地，步驟s9包括以下分步驟：
44、s91、根據第一段圓弧上每個點的世界坐標和初始直線段的世界坐標計算得到水下柔性機械臂四根繩的第一段路徑的世界坐標：
45、
46、
47、
48、
49、
50、
51、
52、
53、
54、
55、
56、
57、其中表示水下柔性機械臂第一根繩的第一段路徑中每個點的世界坐標，表示水下柔性機械臂第二根繩的第一段路徑中每個點的世界坐標，表示水下柔性機械臂第三根繩的第一段路徑中每個點的世界坐標，表示水下柔性機械臂第四根繩的第一段路徑中每個點的世界坐標，表示初始直線段 oa和第一段圓弧 aeb上每個點的世界橫坐標，表示初始直線段 oa和第一段圓弧 aeb上每個點的世界縱坐標，表示水下柔性機械臂的寬度。
58、s92、將水下柔性機械臂四根繩的第一段路徑的終點作為第二段路徑的起點，將第二段路徑的起點的世界坐標乘以旋轉坐標變換矩陣，轉換為第二段路徑的起點的局部坐標，并投影到 y’軸，得到投影距離。
59、s93、根據投影距離得到水下柔性機械臂四根繩的第二段路徑的半徑：
60、
61、其中表示水下柔性機械臂四根繩的第二段路徑的初始設計半徑。
62、s94、根據水下柔性機械臂四根繩的第二段路徑的半徑得到水下柔性機械臂四根繩的第二段路徑的局部坐標：
63、
64、其中表示水下柔性機械臂四根繩的第二段路徑的圓弧角度，表示第二段路徑上的點在局部坐標系 z’軸的坐標值。
65、s95、將水下柔性機械臂四根繩的第二段路徑的局部坐標除以旋轉坐標變換矩陣，得到水下柔性機械臂四根繩的第二段路徑的世界坐標。
66、s96、根據水下柔性機械臂四根繩的第一段路徑的世界坐標和第二段路徑的世界坐標組合得到水下柔性機械臂四根繩的路徑。
67、本發明的有益效果是：
68、（1）本發明只需要輸入目標點坐標和機械臂抓取方向向量等信息，即可直接規劃出水下柔性機械臂四根繩的路徑，進而整合出整個水下柔性機械臂的路徑，其基于數學計算和邏輯判斷的輕量化算法，大幅降低了計算復雜度，在保證控制精度的前提下，可有效減少水下柔性機械臂每次抓取的時間。
69、（2）本發明對水下柔性機械臂四根繩的路徑進行規劃，由繩索驅動機械臂，其柔性傳動特性使機械臂能夠靈活繞過復雜障礙物，實現仿生纏繞運動，尤其適合狹窄或非結構化的水下環境，而繩索的高效傳動（效率超90%）和近乎靜音的運行特點使其在對噪聲敏感的任務中具有獨特優勢。
70、（3）本發明采用四根繩冗余驅動控制，其容錯能力進一步增強了在深海探測、水下救援及海洋牧場等場景中的實用性，未來結合智能算法優化和先進材料應用，性能還可進一步提升。