[發(fā)明專利]一種機(jī)械臂控制精度協(xié)同優(yōu)化方法

在審

• 申請?zhí)枺?/td>: 201711040862.7
• 申請日: 2017-10-31
• 公開（公告）號: CN107717991A
• 公開（公告）日: 2018-02-23
• 發(fā)明（設(shè)計）人: 程遠(yuǎn)超;楊帆;李科;楊濤;劉嘉宇
• 申請（專利權(quán)）人: 北京精密機(jī)電控制設(shè)備研究所
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 核工業(yè)專利中心11007
• 代理人: 閆兆梅

1.一種機(jī)械臂控制精度協(xié)同優(yōu)化方法，其特征在于，包括：

步驟一：對機(jī)械臂進(jìn)行構(gòu)型分析及正運(yùn)動學(xué)分析；

步驟二：對機(jī)械臂的逆運(yùn)動學(xué)進(jìn)行分析；

步驟三：機(jī)械臂末端軌跡規(guī)劃；

步驟四：機(jī)械臂關(guān)節(jié)空間軌跡規(guī)劃；

步驟五：機(jī)械臂桿拓?fù)鋬?yōu)化；

步驟六：控制參數(shù)協(xié)同優(yōu)化；

步驟七：對比驗(yàn)證。

2.根據(jù)權(quán)利要求1所述一種機(jī)械臂控制精度協(xié)同優(yōu)化方法，其特征在于：所述步驟一：對機(jī)械臂進(jìn)行構(gòu)型分析及正運(yùn)動學(xué)分析，包括：根據(jù)實(shí)際六自由度輕型機(jī)械臂構(gòu)型，建立該機(jī)械臂的機(jī)構(gòu)簡圖，并利用標(biāo)準(zhǔn)D-H參數(shù)法建立機(jī)械臂的D-H坐標(biāo)系。

3.根據(jù)權(quán)利要求1所述一種機(jī)械臂控制精度協(xié)同優(yōu)化方法，其特征在于：所述步驟二：對機(jī)械臂的逆運(yùn)動學(xué)進(jìn)行分析，包括：

步驟2.1：

T60=A10A21A43A54A65=r11r12r13pxr21r22r23pyr31r32r33pz0001]]>

其中：₆⁰T-坐標(biāo)系6到坐標(biāo)系0的位姿變換矩陣；_i-1ⁱA-坐標(biāo)系i到坐標(biāo)系i-1的位姿變換矩陣；r_ij-姿態(tài)參數(shù)；p_i-位置參數(shù)。

令公式(1)

等式兩側(cè)(2，4)元素對應(yīng)相等，求得：

公式(2)

式中

等式兩側(cè)(2，3)元素對應(yīng)相等，求得：

θ₅＝±arccos[sin(θ₁)·r₁₃-cos(θ₁)·r₂₃]————————(3)

公式(3)等式兩側(cè)(2，1)(2，2)元素對應(yīng)相等，求得：

θ6=Atan2(-sin(θ1)·r12+cos(θ1)·r22sin(θ5),sin(θ1)·r11-cos(θ1)·r21sin(θ5))---(4)]]>

步驟二2.2：

令公式(5)

等式兩側(cè)(1，4)(3，4)元素對應(yīng)相等，求得：

θ2=Atan2(G4E42+F42,±1-G42E42+F42)-Atan2(E4,F4)----(6)]]>

其中：

E₄＝d₅·s₆·c₁·r₁₁+d₅·s₆·s₁·r₂₁+d₅·c₆·c₁·r₁₂+d₅·c₆·s₁·r₂₂+c₁·p_x+s₁·p_y

F₄＝d₅·s₆·r₃₁+d₅·c₆·r₃₂+p_z-d₁

G4=a32-E42-F42-a22-2a2]]>

式中s_i代表sin(θ_i)，c_i代表cos(θ_i)；

步驟2.3：

令

等式兩側(cè)(1，3)(2，3)元素對應(yīng)相等，求得：

θ3+θ4=Atan2(F5-sin(θ5),E5-sin(θ5))(θ5≠kπ,k=-1,0,1)]]>

其中：E₅＝c₂·c₁·r₁₃+c₂·s₁·r₂₃+s₂·r₃₃

F₅＝-s₂·c₁·r₁₃-s₂·s₁·r₂₃+c₂·r₃₃補(bǔ)充其他相應(yīng)數(shù)學(xué)符號含義

等式兩側(cè)(1，4)(2，4)元素對應(yīng)相等，求得：

θ₃＝Atan2(H₆,G₆)

補(bǔ)充其他相應(yīng)數(shù)學(xué)符號含義

其中：

E₆＝c₂·c₁·p_x+c₂·s₁·p_y+s₂·p_z-s₂·d₁

F₆＝-s₂·c₁·p_x-s₂·s₁·p_y+c₂·p_z-c₂·d₁

G6=E6-a2+d5·F5sin(θ5)a3]]>

H6=F6-d5·E5sin(θ5)a3]]>

在上述的逆運(yùn)動學(xué)求解過程中，關(guān)節(jié)角度θ₁、θ₂、θ₅均有兩組解析解，因此該型機(jī)械臂的逆運(yùn)動學(xué)有八組解析解。