[發(fā)明專利]一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法

在審

• 申請?zhí)枺?/td>: 202011534828.7
• 申請日: 2020-12-22
• 公開（公告）號: CN112720481A
• 公開（公告）日: 2021-04-30
• 發(fā)明（設(shè)計(jì)）人: 明良杰;張雨濃;李中華
• 申請（專利權(quán)）人: 中山大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 張金福

1.一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，包括以下步驟：

S1：設(shè)定機(jī)械臂最小運(yùn)動(dòng)規(guī)劃的軌跡以及物理限制；

S2：根據(jù)所設(shè)定的軌跡以及物理限制，將機(jī)械臂的最小運(yùn)動(dòng)規(guī)劃和控制問題轉(zhuǎn)化為急突度層二次規(guī)劃問題；

S3：對急突度層二次規(guī)劃問題進(jìn)行求解，機(jī)械臂根據(jù)求解結(jié)果進(jìn)行最小運(yùn)動(dòng)。

2.根據(jù)權(quán)利要求1所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S1中設(shè)定的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃的軌跡為：

r_d(t)

其中，r_d(t)為機(jī)械臂在t時(shí)刻末端執(zhí)行器的期望坐標(biāo)。

3.根據(jù)權(quán)利要求1所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S1中設(shè)定的物理限制為：

θ^-≤θ(t)≤θ⁺

其中，θ(t)為機(jī)械臂各關(guān)節(jié)在t時(shí)刻的角度向量，θ⁺為角度向量上限，θ^-為角度向量下限，為t時(shí)刻的角速度向量，為角速度向量上限，為角速度向量下限，為t時(shí)刻的角加速度向量，為角加速度向量上限，為角加速度下限，為t時(shí)刻的角突加度向量，為角突加度上限，為角突加度下限，為t時(shí)刻的角急突度向量，為角急突度上限，為角急突度下限。

4.根據(jù)權(quán)利要求1所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S2具體為：

S201：將軌跡以及物理限制轉(zhuǎn)化到急突度層，得到急突度層約束，并通過在急突度層設(shè)定機(jī)械臂末端執(zhí)行器回到初始點(diǎn)完成最小運(yùn)動(dòng)，得到最小化目標(biāo)；

S202：將急突度層約束和最小化目標(biāo)組成急突度層二次規(guī)劃問題。

5.根據(jù)權(quán)利要求4所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S201中獲取的急突度層約束為：

其中，J為末端執(zhí)行器的雅可比矩陣，

為J的時(shí)間導(dǎo)數(shù)，為的時(shí)間導(dǎo)數(shù)，為的時(shí)間導(dǎo)數(shù)，為r_d(t)的時(shí)間導(dǎo)數(shù)，為的時(shí)間導(dǎo)數(shù)，為的時(shí)間導(dǎo)數(shù)，為的時(shí)間導(dǎo)數(shù)，f(θ(t))為根據(jù)角向量得到的t時(shí)刻末端執(zhí)行器的實(shí)際坐標(biāo)，收斂參數(shù)γ0。

6.根據(jù)權(quán)利要求4所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S201中獲取的最小化目標(biāo)為：

其中，Z為最小化目標(biāo)；上標(biāo)T為向量轉(zhuǎn)置標(biāo)志。

7.根據(jù)權(quán)利要求1所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，所述步驟S3具體為：

S301：對急突度層二次規(guī)劃問題進(jìn)行求解；

S302：將求解結(jié)果轉(zhuǎn)化為機(jī)械臂動(dòng)力源的驅(qū)動(dòng)信號；

S303：機(jī)械臂動(dòng)力源接收到驅(qū)動(dòng)信號后，驅(qū)動(dòng)機(jī)械臂進(jìn)行最小運(yùn)動(dòng)。

8.根據(jù)權(quán)利要求7所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，在所述步驟S301中，采用二次規(guī)劃求解器對急突度層二次規(guī)劃問題進(jìn)行求解。

9.根據(jù)權(quán)利要求7所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，在所述步驟S302中，采用控制器將求解結(jié)果轉(zhuǎn)化為機(jī)械臂動(dòng)力源的驅(qū)動(dòng)信號。

10.根據(jù)權(quán)利要求7所述的一種基于急突度的機(jī)械臂最小運(yùn)動(dòng)規(guī)劃和控制方法，其特征在于，在所述步驟S303中，當(dāng)機(jī)械臂所進(jìn)行的最小運(yùn)動(dòng)結(jié)束后，獲取機(jī)械臂末端的運(yùn)動(dòng)軌跡。