[發(fā)明專利]二自由度機(jī)器人運(yùn)動(dòng)控制方法

無(wú)效

• 申請(qǐng)?zhí)枺?/td>: 201310080212.0
• 申請(qǐng)日: 2013-03-13
• 公開（公告）號(hào): CN103170976A
• 公開（公告）日: 2013-06-26
• 發(fā)明（設(shè)計(jì)）人: 史豪斌;徐林程;郭志巍;黃伯崢;李小婷;沈楷
• 申請(qǐng)（專利權(quán)）人: 西北工業(yè)大學(xué)
• 主分類號(hào): B25J13/00
• 分類號(hào): B25J13/00
• 代理公司: 西北工業(yè)大學(xué)專利中心 61204
• 代理人: 王鮮凱

1.一種二自由度機(jī)器人運(yùn)動(dòng)控制方法，其特征在于包括以下步驟：

（I）獲取二自由度機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的動(dòng)力學(xué)特性方程：

υ·=fυ(υ,V)ω·=fω(ω,T)g(V,T)≤0Vmin≤V≤VmaxTmin≤T≤Tmax,]]>

其中：

υ——二自由度機(jī)器人的線速度，ω——二自由度機(jī)器人角速度；

——二自由度機(jī)器人的線加速度，——二自由度機(jī)器人角加速度；

V——二自由度機(jī)器人線速度的控制輸入，T——二自由度機(jī)器人角速度的控制輸入；

V_min——V的下限；V_max——V的上限；

T_min——T的下限；T_max——T的上限；

f_υ——函數(shù)映射關(guān)系；f_ω——函數(shù)映射關(guān)系；

當(dāng)V_min≤V≤V_max時(shí)，當(dāng)T_min≤T≤T_max時(shí)，

V＝V_c,T＝T_c時(shí)，二自由度機(jī)器人處于無(wú)控制自然狀態(tài)；

（II）以二自由度機(jī)器人中心為原點(diǎn)，二自由度機(jī)器人正方向?yàn)闃O軸建立極坐標(biāo)系；

（III）計(jì)算目標(biāo)點(diǎn)的極坐標(biāo)θ和ρ；

（IV）轉(zhuǎn)角控制；

步驟1：獲取當(dāng)前轉(zhuǎn)角速度ω₀，計(jì)算完全靠慣性使二自由度機(jī)器人狀態(tài)達(dá)到ω₀=0，所需轉(zhuǎn)過的角度θ_{o_cri}：

則由得ω(t,T)＝F_ω(t,ω₀,T)，由于無(wú)控制自然狀態(tài)下，運(yùn)動(dòng)物體最終一定會(huì)靜止；所以，令T＝T_c，則有：

ω(∞,T_c)＝F_ω(∞,ω₀,T_c)＝0；

θo_cri=sign(θ)·∫0∞ω(τ,Tc)·dτ]]>是存在的；

其中，F(xiàn)_ω表示由微分方程及初值條件ω(0,T)＝ω₀，所確定ω關(guān)于t和T的函數(shù)關(guān)系；

步驟2：確定控制器輸出T_o；

若|θ|＞θ_{o_cri}，則使機(jī)器人以最大力度轉(zhuǎn)角，即T＝max{T·sign(θ)|T_min≤T≤T_max}；

若|θ|≤θ_{o_cri}，則對(duì)任意T∈{T|f_ω(ω₀,T)·ω₀＜0,T_min≤T≤T_max}，令ω(t,)T＝_ω(Fω_,0)t＝,，T解0方程F_ω(t,ω₀,T)＝0，得：t_o(T)且t_o(T)＜+∞，此時(shí)機(jī)器人轉(zhuǎn)過的有利角度為：

θo(T)=sign(θ)·∫0to(T)ω(τ,T)·dτ;]]>

求解T*使得θ_o(T*)＝|θ|；

當(dāng)T_min≤T^*≤T_max時(shí)，T_o＝T^*；否則，T_o＝min{f_ω(ω₀,T)·sign(ω₀)|T_min≤T≤T_max}；

（V）如果任務(wù)是原地轉(zhuǎn)角，則線速度控制輸出V_o＝V_c，跳轉(zhuǎn)到步驟（VII）；否則轉(zhuǎn)到步驟（VI）進(jìn)行線運(yùn)動(dòng)控制；

（VI）線運(yùn)動(dòng)控制；

步驟1：變換運(yùn)動(dòng)方程；

目標(biāo)點(diǎn)在極坐標(biāo)系中的運(yùn)動(dòng)方程為：令軌跡曲率半徑實(shí)半徑ρ2=ρ·θ·,]]>則運(yùn)動(dòng)方程為：1ρ1·sinθ=1ρ+1ρ2·tanθ;]]>

步驟2：控制ρ₂，驅(qū)動(dòng)ρ₁；令ρ₂＝a·cotθ，則上述步驟1得到的運(yùn)動(dòng)方程變?yōu)轵?qū)動(dòng)方程：ρ1=1sinθ×(1/ρ+1/a);]]>

其中，a是模型參數(shù)，a＝V_max；

步驟3：將時(shí)間向前推進(jìn)Δt，計(jì)算Δt時(shí)間后二自由度機(jī)器人的角速度和需要的速度：

ωΔt≈ω0+ω··Δt=ω0+fω(ω0,To)·Δt;]]>

令ρ1=υΔtωΔt,]]>代入驅(qū)動(dòng)方程得：υΔt=ωΔtsinθ×(1/ρ+1/a);]]>

其中，Δt＝1f，f是控制器與機(jī)器人接收器的數(shù)據(jù)交換頻率；

步驟4：模型修正：由于θ→0時(shí)，會(huì)在1和-1上震蕩，故模型修正為：

υΔt=(1-α)·ωΔt+α·cosθ[(1-α)·ωΔt+α·cosθ]×(1/ρ+1/a);]]>

其中，ω'為環(huán)境對(duì)ω期望為0的隨機(jī)擾動(dòng)，模型參數(shù)α取為該擾動(dòng)的幅值：α＝|ω'|；步驟5：確定控制器輸出V_o；計(jì)算Δt時(shí)間段內(nèi)二自由度機(jī)器人需要的加速度：

a‾≈υΔt-υ0Δt,]]>

求解V^*使得

若V^*≥V_max，則令V^*＝V_max，否則若V^*≤V_min，則令V^*＝V_min；

若g(V^*,T_o)≤0，則V_o＝V^*，否則由g(V_l,T_o)≤0解出V_l，令V_o＝V_l；

（VII）用獲得的V_o,T_o驅(qū)動(dòng)二自由度機(jī)器人。