郑州排名前十的科技公司,天津网站优化实战,微信小程序云开发收费标准,餐饮网站开发机械臂运动学逆解#xff08;牛顿法#xff09; 常用的工业6轴机械臂采用6轴串联结构#xff0c;虽然其运动学正解比较容易#xff0c;但是其运动学逆解非常复杂#xff0c;其逆解的方程组高度非线性#xff0c;且难以化简。 由于计算机技术的发展#xff0c;依靠其…机械臂运动学逆解牛顿法 常用的工业6轴机械臂采用6轴串联结构虽然其运动学正解比较容易但是其运动学逆解非常复杂其逆解的方程组高度非线性且难以化简。 由于计算机技术的发展依靠其强大的算力可以通过数值解的方式对机械臂的运动学逆解方程组进行求解。以下将使用牛顿法详解整个求解过程。
算法的过程
机械臂运动学正解方程组如式1所示。 [ f 11 f 12 f 13 f 14 f 21 f 22 f 23 f 24 f 31 f 32 f 33 f 34 0 0 0 1 ] f ( θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ) (1) \begin{bmatrix} f_{11}f_{12}f_{13} f_{14}\\ f_{21}f_{22}f_{23} f_{24}\\ f_{31}f_{32}f_{33} f_{34}\\ 000 1\\ \end{bmatrix} f(\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) \tag1 f11f21f310f12f22f320f13f23f330f14f24f341 f(θ1,θ2,θ3,θ4,θ5,θ6)(1) 对于运动学正解式1右边是已知量对于运动学逆解式1左边式已知量。采用牛顿法求解运动学逆解已知机械臂末端姿态为 [ r 11 r 12 r 13 r 14 r 21 r 22 r 23 r 24 r 31 r 32 r 33 r 34 0 0 0 1 ] \begin{bmatrix} r_{11}r_{12}r_{13} r_{14}\\ r_{21}r_{22}r_{23} r_{24}\\ r_{31}r_{32}r_{33} r_{34}\\ 000 1\\ \end{bmatrix} r11r21r310r12r22r320r13r23r330r14r24r341 构造目标函数如式2所示。 F ( θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ) ( f 14 − r 14 ) 2 ( f 24 − r 24 ) 2 ( f 34 − r 34 ) 2 0.08 ∗ ( f 11 − r 11 ) 2 0.08 ∗ ( f 12 − r 12 ) 2 0.08 ∗ ( f 13 − r 13 ) 2 0.08 ∗ ( f 31 − r 31 ) 2 0.08 ∗ ( f 32 − r 32 ) 2 0.08 ∗ ( f 34 − r 34 ) 2 (2) F(\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) (f_{14}-r_{14})^2(f_{24}-r_{24})^2(f_{34}-r_{34})^20.08*(f_{11}-r_{11})^20.08*(f_{12}-r_{12})^20.08*(f_{13}-r_{13})^20.08*(f_{31}-r_{31})^20.08*(f_{32}-r_{32})^20.08*(f_{34}-r_{34})^2 \tag2 F(θ1,θ2,θ3,θ4,θ5,θ6)(f14−r14)2(f24−r24)2(f34−r34)20.08∗(f11−r11)20.08∗(f12−r12)20.08∗(f13−r13)20.08∗(f31−r31)20.08∗(f32−r32)20.08∗(f34−r34)2(2)
目标函数的雅可比矩阵为 J [ ∂ F ∂ θ 1 , ∂ F ∂ θ 2 , ∂ F ∂ θ 3 , ∂ F ∂ θ 4 , ∂ F ∂ θ 5 , ∂ F ∂ θ 6 ] J [\frac{\partial F}{\partial\theta_1}, \frac{\partial F}{\partial\theta_2},\frac{\partial F}{\partial\theta_3},\frac{\partial F}{\partial\theta_4},\frac{\partial F}{\partial\theta_5},\frac{\partial F}{\partial\theta_6}] J[∂θ1∂F,∂θ2∂F,∂θ3∂F,∂θ4∂F,∂θ5∂F,∂θ6∂F] 目标函数的雅克比矩阵为 H [ ∂ 2 F ∂ θ 1 2 ∂ 2 F ∂ θ 1 ∂ θ 2 ∂ 2 F ∂ θ 1 ∂ θ 3 ∂ 2 F ∂ θ 1 ∂ θ 4 ∂ 2 F ∂ θ 1 ∂ θ 5 ∂ 2 F ∂ θ 1 ∂ θ 6 ∂ 2 F ∂ θ 2 ∂ θ 1 ∂ 2 F ∂ θ 2 2 ∂ 2 F ∂ θ 2 ∂ θ 3 ∂ 2 F ∂ θ 2 ∂ θ 4 ∂ 2 F ∂ θ 2 ∂ θ 5 ∂ 2 F ∂ θ 2 ∂ θ 6 ∂ 2 F ∂ θ 3 ∂ θ 1 ∂ 2 F ∂ θ 3 ∂ θ 2 ∂ 2 F ∂ θ 3 2 ∂ 2 F ∂ θ 3 ∂ θ 4 ∂ 2 F ∂ θ 3 ∂ θ 5 ∂ 2 F ∂ θ 3 ∂ θ 6 ∂ 2 F ∂ θ 4 ∂ θ 1 ∂ 2 F ∂ θ 4 ∂ θ 2 ∂ 2 F ∂ θ 4 ∂ θ 3 ∂ 2 F ∂ θ 4 2 ∂ 2 F ∂ θ 4 ∂ θ 5 ∂ 2 F ∂ θ 4 ∂ θ 6 ∂ 2 F ∂ θ 5 ∂ θ 1 ∂ 2 F ∂ θ 5 ∂ θ 2 ∂ 2 F ∂ θ 5 ∂ θ 3 ∂ 2 F ∂ θ 5 ∂ θ 4 ∂ 2 F ∂ θ 5 2 ∂ 2 F ∂ θ 5 ∂ θ 6 ∂ 2 F ∂ θ 6 ∂ θ 1 ∂ 2 F ∂ θ 6 ∂ θ 2 ∂ 2 F ∂ θ 6 ∂ θ 3 ∂ 2 F ∂ θ 6 ∂ θ 4 ∂ 2 F ∂ θ 6 ∂ θ 5 ∂ 2 F ∂ θ 6 2 ] H\begin{bmatrix} \frac{\partial^2 F}{\partial\theta_1^2} \frac{\partial^2 F}{\partial\theta_1 \partial\theta_2} \frac{\partial^2 F}{\partial\theta_1 \partial\theta_3} \frac{\partial^2 F}{\partial\theta_1 \partial\theta_4} \frac{\partial^2 F}{\partial\theta_1 \partial\theta_5} \frac{\partial^2 F}{\partial\theta_1 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_2 \partial\theta_1} \frac{\partial^2 F}{\partial\theta_2^2} \frac{\partial^2 F}{\partial\theta_2 \partial\theta_3} \frac{\partial^2 F}{\partial\theta_2 \partial\theta_4} \frac{\partial^2 F}{\partial\theta_2 \partial\theta_5} \frac{\partial^2 F}{\partial\theta_2 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_3 \partial\theta_1} \frac{\partial^2 F}{\partial\theta_3 \partial\theta_2} \frac{\partial^2 F}{\partial\theta_3^2} \frac{\partial^2 F}{\partial\theta_3 \partial\theta_4} \frac{\partial^2 F}{\partial\theta_3 \partial\theta_5} \frac{\partial^2 F}{\partial\theta_3 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_4 \partial\theta_1} \frac{\partial^2 F}{\partial\theta_4 \partial\theta_2} \frac{\partial^2 F}{\partial\theta_4 \partial\theta_3} \frac{\partial^2 F}{\partial\theta_4^2} \frac{\partial^2 F}{\partial\theta_4 \partial\theta_5} \frac{\partial^2 F}{\partial\theta_4 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_5 \partial\theta_1} \frac{\partial^2 F}{\partial\theta_5 \partial\theta_2} \frac{\partial^2 F}{\partial\theta_5 \partial\theta_3} \frac{\partial^2 F}{\partial\theta_5 \partial\theta_4} \frac{\partial^2 F}{\partial\theta_5^2} \frac{\partial^2 F}{\partial\theta_5 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_6 \partial\theta_1} \frac{\partial^2 F}{\partial\theta_6 \partial\theta_2} \frac{\partial^2 F}{\partial\theta_6 \partial\theta_3} \frac{\partial^2 F}{\partial\theta_6 \partial\theta_4} \frac{\partial^2 F}{\partial\theta_6 \partial\theta_5} \frac{\partial^2 F}{\partial\theta_6^2} \\ \end{bmatrix} H ∂θ12∂2F∂θ2∂θ1∂2F∂θ3∂θ1∂2F∂θ4∂θ1∂2F∂θ5∂θ1∂2F∂θ6∂θ1∂2F∂θ1∂θ2∂2F∂θ22∂2F∂θ3∂θ2∂2F∂θ4∂θ2∂2F∂θ5∂θ2∂2F∂θ6∂θ2∂2F∂θ1∂θ3∂2F∂θ2∂θ3∂2F∂θ32∂2F∂θ4∂θ3∂2F∂θ5∂θ3∂2F∂θ6∂θ3∂2F∂θ1∂θ4∂2F∂θ2∂θ4∂2F∂θ3∂θ4∂2F∂θ42∂2F∂θ5∂θ4∂2F∂θ6∂θ4∂2F∂θ1∂θ5∂2F∂θ2∂θ5∂2F∂θ3∂θ5∂2F∂θ4∂θ5∂2F∂θ52∂2F∂θ6∂θ5∂2F∂θ1∂θ6∂2F∂θ2∂θ6∂2F∂θ3∂θ6∂2F∂θ4∂θ6∂2F∂θ5∂θ6∂2F∂θ62∂2F
迭代步长 Δ Θ − H ∗ J T \Delta \Theta -H*J^T ΔΘ−H∗JT
程序验证
clear;
clc;rng(1); %固定随机数种子%构造运动学模型
syms a0 a1 a2 a3 a4 a5;
FK FKinematics(a0, a1, a2, a3, a4, a5);%构造目标函数
syms T14 T24 T34 T11 T12 T13 T31 T32 T33;
opt_F(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) (FK(1, 4) - T14)^2 ...(FK(2, 4) - T24)^2 ...(FK(3, 4) - T34)^2 ...0.08 * (FK(1, 1) - T11)^2 ...0.08 * (FK(1, 2) - T12)^2 ...0.08 * (FK(1, 3) - T13)^2 ...0.08 * (FK(3, 1) - T31)^2 ...0.08 * (FK(3, 2) - T32)^2 ...0.08 * (FK(3, 3) - T33)^2
opt_F matlabFunction(opt_F);%构造目标函数的雅可比函数矩阵
J(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) jacobian(opt_F, [a0 a1 a2 a3 a4 a5])
J matlabFunction(J);%构造目标函数的海塞矩阵
H(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) jacobian(J, [a0 a1 a2 a3 a4 a5])
H matlabFunction(H);T FKinematics(2, 0.5, -1.6, 0.6, 1.5, -0.9)
X IKinematics(opt_F, J, H, T);
X
T
FKinematics(X(1), X(2), X(3), X(4), X(5), X(6))function T FKinematics(x1, x2, x3, x4, x5, x6)T1 urdfJoint(0, 0, 0.3015, 0, 0, 0, x1);T2 urdfJoint(0.077746, -0.0869967, 0.1465, 1.5708, 1.5708, 0, x2);T3 urdfJoint(-0.64, 0, -0.015, 0, 0, 0, x3);T4 urdfJoint(-0.195, 0.9055, -0.072, -1.5708, 0, 0, x4);T5 urdfJoint(0, 0, 0, -1.6876, -1.5708, -3.0248, x5);T6 urdfJoint(0, 0, 0, -1.5708, 0, -1.5708, x6);T7 urdfJoint(0, 0, 0.08, 0, 0, 0, 0); %法兰盘的位姿T T1 * T2 * T3 * T4 * T5 * T6 * T7;
endfunction X IKinematics(opt_F, J, H, T)X [0; 0; 0; 0; 0; 0];X0 [0; 0; 0; 0; 0; 0];min_opt_value opt_F(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));X_opt X0;last_opt_value min_opt_value;t0 clock;for i 1 : 1000iJn J(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));Hn H(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));%[U, S, V] svd(Hn);%det_X V * inv(S) * U * Jn;det_X inv(Hn) * Jn;X0 X0 - det_X;X0;opt_value opt_F(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));if(min_opt_value opt_value) min_opt_value opt_value;X_opt X0;endif(min_opt_value 0.0001)break;endif(abs(last_opt_value - opt_value) 0.00001)fprintf(陷入局部最小解将重新生成迭代初始值);X0 randn(6, 1);endlast_opt_value opt_value;endt etime(clock,t0);fprintf(solve time: %f, t);T;X X_opt;T1 FKinematics(X_opt(1), X_opt(2), X_opt(3), X_opt(4), X_opt(5), X_opt(6));
endfunction T urdfJoint(x0, y0, z0, R0, P0, Y0, theta)r1 [1 0 0;0 cos(R0) -sin(R0);0 sin(R0) cos(R0)];r2 [ cos(P0) 0 sin(P0);0 1 0;-sin(P0) 0 cos(P0)];r3 [cos(Y0) -sin(Y0) 0;sin(Y0) cos(Y0) 0;0 0 1];r r3 * r2 * r1;T0 [r(1, 1) r(1, 2) r(1, 3) x0;r(2, 1) r(2, 2) r(2, 3) y0;r(3, 1) r(3, 2) r(3, 3) z0;0 0 0 1];T T0 * [cos(theta) -sin(theta) 0 0;sin(theta) cos(theta) 0 0;0 0 1 0;0 0 0 1];
end
注意事项
matlab在构造雅可比函数、函数矩阵的时候比较慢使用四元数建立运动学模型效率更低暂时未发现什么原因可通过设置迭代的初始值获得其它的逆解
