[發明專利]基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法

有效

• 申請號: 202011065470.8
• 申請日: 2020-09-30
• 公開（公告）號: CN112428262B
• 公開（公告）日: 2022-04-12
• 發明（設計）人: 耿新宇;李萌;鄭威
• 申請（專利權）人: 中國空間技術研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 任林沖

1.基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于，該方法的步驟包括：

步驟(1)、確定并聯冗余柔索機構運動部件的質量m、轉動慣量I_xx、I_yy與I_zz，定義狀態向量為x＝(p^T,Θ^T)^T，其中p＝(p_x,p_y,p_z)^T為并聯冗余柔索機構運動部件質心位移向量，為并聯冗余柔索機構運動部件姿態角向量，為滾動角、θ為俯仰角、ψ為偏航角；定義PID控制參數矩陣K_p、K_I與K_D，控制參數矩陣維度為6×6；

步驟(2)、通過傳感器提取與處理信息，獲得并聯冗余柔索機構運動部件狀態信息x，確定該時刻目標軌跡x_d，求解運動誤差e＝x_d-x；

步驟(3)、根據步驟(2)所獲得的運動部件狀態信息向量x，求解并聯冗余柔索機構的雅可比矩陣J，雅可比矩陣維度為6×m，m≥6為控制柔索數量；

步驟(4)、通過步驟(1)所獲得的m、I_xx、I_yy、I_zz與步驟(2)所獲得的并聯冗余柔索機構狀態信息x、運動誤差e，求解伺服控制偽指令f，伺服控制偽指令的維度為6×1；

步驟(5)、通過步驟(2)所獲得的并聯冗余柔索機構狀態信息x與步驟(3)所獲得的雅可比矩陣J求解控制張力最小超球直徑r；

步驟(6)、根據步驟(4)所獲得的伺服控制偽指令與步驟(5)所獲得的控制張力超球最小直徑r，求解控制張力分配向量T，控制張力分配向量維度為m×1，將控制張力分配向量T分配到柔索驅動電機執行；

步驟(7)、運行時間每間隔控制步長Δt，重復步驟(2)至(6)直至并聯冗余柔索機構完整生成目標運動軌跡；

步驟(4)中的伺服控制偽指令f通過下式進行求解：

其中M(x)為質量矩陣，為柯氏力與向心力矩陣，x為狀態變量，為狀態變量速度，為目標軌跡加速度，g為外部力與扭矩所組成的外載荷向量；為運動誤差對時間的一階導；

步驟(5)中的控制張力最小超球半徑r通過下式進行求解

其中a、b與c為中間變量；

步驟(6)中的控制張力分配向量T通過下式進行求解

T＝-J^T(JJ^T)f+rηη^TI₃₁

其中η為矩陣-J的零空間。

2.根據權利要求1所述的基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于：步驟(3)中的雅可比矩陣J通過下式進行求解

上式中u_i為全局坐標系下第i根柔索的單位方向向量，b_i為并聯冗余柔索機構運動部件隨體坐標系下運動部件質心與第i根柔索連接點間的向量，R為姿態角轉換矩陣。

3.根據權利要求2所述的基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于：R通過任意轉換方法，以3-2-1轉換順序，通過下式進行求解

其中c_α與s_α分別表示cos(α)與sin(α)。

4.根據權利要求1所述的基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于：

其中Q為姿態角速度轉換矩陣，m為質量，I_xx、I_yy、I_zz為x、y、z三個方向上的轉動慣量；為目標姿態角向量對時間的一階導。

5.根據權利要求1所述的基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于：以3-2-1轉換順序，通過下式進行求解，

6.根據權利要求1所述的基于超橢球映射解析算法并聯冗余柔索機構伺服控制方法，其特征在于：b＝I₃₁J^T(JJ^T)f,c＝f^T(JJ^T)f,