[發(fā)明專利]一種基于冗余自由度利用的旋翼飛行機械臂協(xié)調運動規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 202310138230.3
• 申請日: 2023-02-20
• 公開（公告）號: CN116237938A
• 公開（公告）日: 2023-06-09
• 發(fā)明（設計）人: 余翔;陳瑞;郭克信;李文碩;郭雷;劉錢源;王萌
• 申請（專利權）人: 北京航空航天大學杭州創(chuàng)新研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 江亞平

1.一種基于冗余自由度利用的旋翼飛行機械臂協(xié)調運動規(guī)劃方法，其特征在于，包括以下步驟：

第一步、根據(jù)旋量理論與指數(shù)積公式，建立機械臂的運動學模型；

第二步、建立飛行機械臂的一體化運動學模型，計算相應的雅各比矩陣，將運動學模型分解為可控部分與不可控部分；

第三步、建立飛行機械臂的水平方向質心偏移模型，推導與質心偏移變化率對應的雅各比矩陣；

第四步、將機械臂末端軌跡跟蹤精度與質心偏移量同時作為優(yōu)化目標，建立并求解分層二次規(guī)劃問題模型，得到滿足末端目標跟蹤性能且最小化質心偏移的飛行機械臂運動軌跡。

2.根據(jù)權利要求1所述的一種基于冗余自由度利用的旋翼飛行機械臂協(xié)調運動規(guī)劃方法，其特征在于，所述第一步包括：

定義當機械臂位于初始位姿時，機械臂第i關節(jié)的運動旋量ξ_i：

其中，ω_i表示在機械臂基坐標系下關節(jié)i的單位角速度矢量,v_i表示在機械臂基坐標系下關節(jié)i的線速度矢量，r_i表示機械臂基坐標系下關節(jié)i轉軸上任意一點的坐標。所述初始位姿為機械臂各個關節(jié)角為0時的位姿；

將運動旋量ξ_i轉化為矩陣形式

其中，S(·)為反對稱矩陣運算符；

根據(jù)指數(shù)積公式，計算得到機械臂的前向運動學齊次變換矩陣

其中，表示機械臂末端相對于機械臂基坐標系的旋轉矩陣，表示機械臂末端在機械臂基坐標系下的位置，n是機械臂關節(jié)數(shù)目，q_i是關節(jié)i的關節(jié)角度，是關節(jié)i的指數(shù)映射，T₀是機械臂在初始位姿下的齊次變換矩陣，且：

計算機械臂的雅各比矩陣

其中，表示關節(jié)i在任意位姿下的運動旋量。可由ξ_i經過伴隨變換運算后得到：

3.根據(jù)權利要求2所述的一種基于冗余自由度利用的旋翼飛行機械臂協(xié)調運動規(guī)劃方法，其特征在于，所述第二步包括：

依據(jù)齊次變換原理建立飛行機械臂的一體化前向運動學模型：

其中，P_e表示機械臂執(zhí)行器在慣性坐標系下的位置，R_e是機械臂末端在慣性坐標下的旋轉矩陣，P_b表示無人機機體坐標系在慣性坐標系下的位置，R_b是無人機機體坐標系相對于慣性坐標系下的旋轉矩陣；

將上式對時間求導，得到飛行機械臂的微分運動學方程：

其中，Ω_b和Ω_e分別代表無人機質心和飛行機械臂末端在慣性坐標系下的角速度；是飛行機械臂末端在機體標系下的角速度；是機械臂的關節(jié)角速度向量；分別是P_e，P_b，R_b，對時間的導數(shù)；

進一步得到飛行機械臂末端在慣性坐標系下的表示：

其中，代表飛行機械臂末端坐標系的線速度和角速度，代表無人機機體坐標系的線速度和角速度；

定義機械臂末端和無人機的角速度與歐拉角速度之間的轉換矩陣分別為T_e與T_b，得到飛行機械臂末端位姿在慣性坐標系的表達式為：

其中，x_e＝[P_e?Θ_e]^T,x_b＝[P_b?Θ_b]^T；Θ_e＝[φ_e?θ_e?ψ_e]^T，Θ_b＝[φ_b?θ_b?ψ_b]^T分別是機械臂末端與無人機的歐拉角；θ_e，ψ_b分別為飛行機械臂末端和無人機的滾轉角；θ_e，θ_e分別為飛行機械臂末端和無人機的俯仰角；ψ_e，ψ_b分別為飛行機械臂末端和無人機的偏航角；

由于四旋翼無人機是一個欠驅動系統(tǒng)，其三維位置和偏航角通道可控，而俯仰角和滾轉角通道不可控，故將微分運動學方程重新排列，得到：

其中，ζ_c＝[P_b?ψ_b?q]^T，ζ_uc＝[φ_b?θ_b]^T，J_c與J_uc是重排列后得到的可控部分雅各比矩陣與不可控部分雅各比矩陣。