二自由度机器人运动控制方法

摘要

本发明公开了一种二自由度机器人运动控制方法，用于解决现有机器人控制方法控制精度差的技术问题。技术方案是首先首先对二自由度机器人的客观物理动力学特性进行建模，抽象出二自由度机器人一般形式的动力学特性方程。在此基础上，采用逐次逼近，最终收敛的方法完成转角控制，从根本上消除了θ→0时，二自由度机器人的震荡，为后续线运动控制奠定基础。然后引入实半径ρ₂和轨迹曲率半径ρ₁，变换二自由度机器人在二维极坐标下的运动学方程，令ρ₂正比于cotθ，再利用已有的转角控制，得到线运动控制方程，完成跑点控制。提高了二自由度机器人的控制精度，而且方程中的参数物理意义明确，不需人工调试即可确定。

主权项

1.一种二自由度机器人运动控制方法，其特征在于包括以下步骤：（I）获取二自由度机器人运动控制系统的动力学特性方程：$\left\{\begin{array}{c}\stackrel{·}{\upsilon }=f_{\upsilon }(\upsilon ,V)\\ \stackrel{·}{\omega }=f_{\omega }(\omega ,T)\\ g(V,T)\le 0\\ V_{\min }\le V\le V_{\max }\\ T_{\min }\le T\le T_{\max }\end{array}\right.,$其中：υ——二自由度机器人的线速度，ω——二自由度机器人角速度；——二自由度机器人的线加速度，——二自由度机器人角加速度；V——二自由度机器人线速度的控制输入，T——二自由度机器人角速度的控制输入；V_min——V的下限；V_max——V的上限；T_min——T的下限；T_max——T的上限；f_υ——函数映射关系；f_ω——函数映射关系；当V_min≤V≤V_max时，当T_min≤T≤T_max时，V＝V_c,T＝T_c时，二自由度机器人处于无控制自然状态；（II）以二自由度机器人中心为原点，二自由度机器人正方向为极轴建立极坐标系；（III）计算目标点的极坐标θ和ρ；（IV）转角控制；步骤1：获取当前转角速度ω₀，计算完全靠惯性使二自由度机器人状态达到ω₀=0，所需转过的角度θ_{o_cri}：则由得ω(t,T)＝F_ω(t,ω₀,T)，由于无控制自然状态下，运动物体最终一定会静止；所以，令T＝T_c，则有：ω(∞,T_c)＝F_ω(∞,ω₀,T_c)＝0；${\theta }_{o\_\mathrm{cri}}=\mathrm{sign}\left(\theta \right)·{\int }_0^{\infty }\omega (\tau ,T_c)·\mathrm{d\tau }$是存在的；其中，F_ω表示由微分方程及初值条件ω(0,T)＝ω₀，所确定ω关于t和T的函数关系；步骤2：确定控制器输出T_o；若|θ|＞θ_{o_cri}，则使机器人以最大力度转角，即T＝max{T·sign(θ)|T_min≤T≤T_max}；若|θ|≤θ_{o_cri}，则对任意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)＜+∞，此时机器人转过的有利角度为：${\theta }_o\left(T\right)=\mathrm{sign}\left(\theta \right)·{\int }_0^{t_o\left(T\right)}\omega (\tau ,T)·\mathrm{d\tau };$求解T*使得θ_o(T*)＝|θ|；当T_min≤T^*≤T_max时，T_o＝T^*；否则，T_o＝min{f_ω(ω₀,T)·sign(ω₀)|T_min≤T≤T_max}；（V）如果任务是原地转角，则线速度控制输出V_o＝V_c，跳转到步骤（VII）；否则转到步骤（VI）进行线运动控制；（VI）线运动控制；步骤1：变换运动方程；目标点在极坐标系中的运动方程为：令轨迹曲率半径实半径${\rho }_2=\frac{\stackrel{·}{\rho }}{\stackrel{·}{\theta }},$则运动方程为：$\frac{1}{{\rho }_1·\sin \theta }=\frac{1}{\rho }+\frac{1}{{\rho }_2·\tan \theta };$步骤2：控制ρ₂，驱动ρ₁；令ρ₂＝a·cotθ，则上述步骤1得到的运动方程变为驱动方程：${\rho }_1=\frac{1}{\sin \theta \times (1/\rho +1/a)};$其中，a是模型参数，a＝V_max；步骤3：将时间向前推进Δt，计算Δt时间后二自由度机器人的角速度和需要的速度：${\omega }_{\mathrm{\Delta t}}\approx {\omega }_0+\stackrel{·}{\omega }·\mathrm{\Delta t}={\omega }_0+f_{\omega }({\omega }_0,T_o)·\mathrm{\Delta t};$令${\rho }_1=\frac{{\upsilon }_{\mathrm{\Delta t}}}{{\omega }_{\mathrm{\Delta t}}},$代入驱动方程得：${\upsilon }_{\mathrm{\Delta t}}=\frac{{\omega }_{\mathrm{\Delta t}}}{\sin \theta \times (1/\rho +1/a)};$其中，Δt＝1f，f是控制器与机器人接收器的数据交换频率；步骤4：模型修正：由于θ→0时，会在1和-1上震荡，故模型修正为：${\upsilon }_{\mathrm{\Delta t}}=\frac{(1-\alpha )·{\omega }_{\mathrm{\Delta t}}+\alpha ·\cos \theta }{[(1-\alpha )·{\omega }_{\mathrm{\Delta t}}+\alpha ·\cos \theta ]\times (1/\rho +1/a)};$其中，ω'为环境对ω期望为0的随机扰动，模型参数α取为该扰动的幅值：α＝|ω'|；步骤5：确定控制器输出V_o；计算Δt时间段内二自由度机器人需要的加速度：$\bar{a}\approx \frac{{\upsilon }_{\mathrm{\Delta t}}-{\upsilon }_0}{\mathrm{\Delta 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驱动二自由度机器人。