机械手位置控制之欧拉-拉格朗日方程仿真
问题背景
机械手的动态方程通常可以由欧拉-拉格朗日方程(Euler-Lagrange equation)描述,下面是一个简单的平面双铰链机械手的示意图。其中,坐标系 ( x 0 , y 0 ) (x_0,y_0) (x0,y0)和 ( x 1 , y 1 ) (x_1,y_1) (x1,y1)为机械手两个关节的轴坐标系,坐标系 ( x 2 , y 2 ) (x_2,y_2) (x2,y2)为机械手末端,实际应用中带负载使用,其位置可有由工作关节角度向量 q q q来描述,执行机构输入由作用于关节的两维力矩向量 τ \tau τ组成,EL方程描述如下: M ( q ) q ¨ + C ( q , q ˙ ) q ˙ + g ( q ) = τ M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau M(q)q¨+C(q,q˙)q˙+g(q)=τ式中, M ( q ) M(q) M(q)为2x2维机械手惯量矩阵(对称正定), C ( q , q ˙ ) q ˙ C(q,\dot{q})\dot{q} C(q,q˙)q˙为两维向心和哥氏力矩向量 ( C ( q , q ˙ ) C(q,\dot{q}) C(q,q˙)为2x2维矩阵), g ( q ) g(q) g(q)为两维重力力矩向量, q q q和 q ˙ \dot{q} q˙分别为关节角度向量和关节速度向量。
至此,我们给出了描述该机械手的动力学的方程。那么接下来,我们的任务是实现机械手的位置控制和轨迹控制。显然,给定一组角度向量 q q q,我们可以唯一的确定机械手的位置,也就是说,要实现对机械手位置的控制,只需让 q q q按照我们期望的规律变化即可,也即跟踪问题。
控制率设计
对于机械手位置控制来说,我们可以采用比例-微分(PD)控制器来实现,即按照各关节测量误差 q − q d q-q_d q−