matlab机器人工具箱学习笔记——状态空间方程 matlab机器人工具箱

机器人动力学概述对于机器人动力学分析，有两种经典方法：一种是牛顿—欧拉法，另一种是拉格朗日法。与机器人运动学相似，机器人动力学也有两个相反的问题：
（1）动力学正问题是已知机械臂各关节的作用力或力矩，求各关节的位移、速度和加速度，即机器人的运动轨迹 ( τ → q , q ′ , q ′ ′ ) (\tau\to q,q' ,q' ' ) (τ→q,q′,q′′),这可以用于对机械臂的仿真
（2）动力学逆问题是已知机械臂的运动轨迹，即各关节的位移、速度和加速度，求各关节所需要的驱动力或力矩 ( q ′ ′ , q ′ , q → τ ) (q' ' ,q' ,q \to\tau) (q′′,q′,q→τ)，这可以用于对机械臂的控制。
状态空间方程其中一种用状态空方程表示动力学方程。不考虑一切摩擦因素：
M ( q ) q ′ ′ + C ( q , q ′ ) q ′ + G ( q ) + F ( q ′ ) + J ( q ) T f = τ M(q)q' ' +C(q,q' )q' +G(q)+F(q' )+J(q)^Tf=\tau M(q)q′′+C(q,q′)q′+G(q)+F(q′)+J(q)Tf=τ
M ( q ) q ′ ′ M(q)q' ' M(q)q′′，为机器人的惯性矩阵。
C ( q , q ′ ) q ′ C(q,q' )q' C(q,q′)q′，为科里奥利矩阵。
G ( q ) G(q) G(q)，为重力矩阵。
F ( q ′ ) F(q' ) F(q′)，为摩擦力矩。
J ( q ) T f J(q)^Tf J(q)Tf,表示关节力， f f f表示扭力， J ( q ) J(q) J(q)为雅克比矩阵。
各项参数的获取（正向动力学的计算）
【matlab机器人工具箱学习笔记——状态空间方程】1）运动学和动力学参数
使用SerialLink.dyn()来显示运动学参数和动力学参数。
输入命令：

>> mdl_puma560 >> p560.links(6).dyn theta=q, d=0, a=0, alpha=0, offset=0 (R,stdDH) m=0.09 r=000.032 I= |0.0001500 | |00.000150 | |004e-05 | Jm=3.3e-05 Bm=3.67e-05 Tc=0.00396(+)-0.0105(-) G=76.69 qlim = -4.642576 to 4.642576

2）惯性矩阵
当关节角为 q q q时，使用SerialLink.inertia()获取机器人的惯性矩阵：
输入：

>> q=[0 0 0 0 0 0]; >> p560.inertia(q)ans =3.9611-0.1627-0.13890.0016-0.00040.0000 -0.16274.45660.37270.00000.00190.0000 -0.13890.37270.93870.00000.00190.0000 0.00160.00000.00000.19240.00000.0000 -0.00040.00190.00190.00000.17130.0000 0.00000.00000.00000.00000.00000.1941

3）科里奥利矩阵
当机器人的关节角为 q q q，关节角的速度为 q ′ q' q′，可以通过函数SerialLink.coriolis()获取科里奥利矩阵：
例如：

>> q=[0 0 0 0 0 0]; >>qd=[pi/6 pi/6 pi/6 pi/6 pi/6 pi/6]; >>> p560.coriolis(q,qd)ans =-0.4206-0.5773-0.2121-0.0007-0.00140.0000 0.2118-0.2029-0.4050-0.0000-0.00200 0.20810.2021-0.00000.0000-0.00010 0.00000.00000.0000000 0.00070.00070.0001000 000000

4）重力矩阵
当机器人的关节角为 q q q，可通过函数SerialLink.gravload()获取机器人的重力矩阵：

>> q=[0 0 0 0 0 0]; >>p560.gravload(q) ans =037.48370.2489000 G(q)=(037.48370.2489000)'

5）摩擦力矩
使用SerialLink.dyn()
其中：
Bm为粘性摩擦系数
Tc为库仑摩擦系数
G为齿轮传动比
逆向动力学的计算使用SerialLink.rne()计算动力学的逆问题，主要参数为 q q q(关节角), q d qd qd（速度）, q d d qdd qdd（加速度）, g r a v grav grav（重力项，有默认值）
输入：

>> mdl_puma560; >> q=[0 0 0 pi/6 pi/6 pi/6]; >> qd=[0 0 0 pi/18 pi/18 pi/18]; >> qdd=[0 0 0 0 0 0]; >> t1=p560.rne(q,qd,qdd) t1 =-0.000037.47130.23660.92350.72650.3413%%%驱动力矩

当忽略状态方程的重力项时，输入语句：

>>t2=p560.rne(q,qd,qdd,[0 0 0]') t2 =-0.0000-0.0001-0.00010.92350.74060.3413%%%驱动力矩

此外，函数SerialLink.rne()也可以计算机器人沿着一条轨迹时，每一时刻下的驱动力矩。
输入：

>> T1=transl(0.3,0.1,0)*trotx(pi); %设置初始位姿 >>q1=p560.ikine6s(T1); %计算对应关节角 >>T2=transl(0.2,0.4,0)*trotx(pi/2); %设置最终位姿 >>q2=p560.ikine6s(T2); %计算对应关节角 >>t=[0:0.1:6]'; %设置时间及步长 >>[q,qd,qdd]=jtraj(q1,q2,t); %生成相应的轨迹 >>tu=p560.rne(q,qd,qdd); %计算轨迹上每个点的驱动力矩

可以得到一个61×6的矩阵每一行对应某一个时间的驱动力矩。
我们可以
plot(t,tu(:1)); …等
得到驱动力矩关于时间的图像
参考资料：
杨辰光, 李智军, 许扬，机器人仿真与编程技术[M].北京:清华大学出版社,2018