Python | C++ | MATLAB机器人正逆向运动学动力学求解器及算法

🎯求解器算法

📜机器人-用例

📜ROS2(Cpp或Python)机器学习路径选择三维模拟平衡车及YOLOv8视觉消息

🍇MATLAB矩阵解析库卡机器人正逆向运动学

💦正向运动学

在机器人运动学中，正向运动学是指使用机器人的运动学方程根据关节参数的指定值来计算末端执行器的位置。机器人的运动学方程用于机器人技术、计算机游戏和动画。计算实现末端执行器指定位置的关节参数的逆过程称为逆运动学。

机器人串联链的运动学方程是使用刚性变换 [Z] 来表征每个关节允许的相对运动，并使用单独的刚性变换 [X] 来定义每个连杆的尺寸而获得的。结果是一系列刚性变换，交替进行关节和连杆变换，从链的底部到其末端连杆，相当于末端连杆的指定位置，
$[T]=\left[Z_1\right]\left[X_1\right]\left[Z_2\right]\left[X_2\right] \ldots\left[X_{n-1}\right]\left[Z_n\right]$
其中 $[T]$ 是定位末端链接的变换。这些方程称为串联链的运动学方程。

💦Denavit-Hartenberg 矩阵

与这些操作相关的矩阵是：
$\operatorname{Trans}_{Z_i}\left(d_i\right)=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{array}\right], \quad \operatorname{Rot}_{Z_i}\left(\theta_i\right)=\left[\begin{array}{cccc} \cos \theta_i & -\sin \theta_i & 0 & 0 \\ \sin \theta_i & \cos \theta_i & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]$
类似于，
$\operatorname{Trans}_{X_i}\left(a_{i, i+1}\right)=\left[\begin{array}{cccc} 1 & 0 & 0 & a_{i, i+1} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right], \quad \operatorname{Rot}_{X_i}\left(\alpha_{i, i+1}\right)=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos \alpha_{i, i+1} & -\sin \alpha_{i, i+1} & 0 \\ 0 & \sin \alpha_{i, i+1} & \cos \alpha_{i, i+1} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] .$
使用 Denavit-Hartenberg 约定产生链接变换矩阵 $\left[{ }^{j-1} T_i\right]$ 为
$}^{i-1} T_i=\left[\begin{array}{cccc} \cos \theta_i & -\sin \theta_i \cos \alpha_{i, i+1} & \sin \theta_i \sin \alpha_{i, i+1} & a_{i, i+1} \cos \theta_i \\ \sin \theta_i & \cos \theta_i \cos \alpha_{i, i+1} & -\cos \theta_i \sin \alpha_{i, i+1} & a_{i, i+1} \sin \theta_i \\ 0 & \sin \alpha_{i, i+1} & \cos \alpha_{i, i+1} & d_i \\ 0 & 0 & 0 & 1 \end{array}\right],$
称为 Denavit-Hartenberg 矩阵。

库卡机器人的线性轴为机器人增加了一个轴，因此大大扩展了机器人的工作空间。冗余机器人的优点是增加了指定方向的可操作性、速度和加速度的均匀分布、最大限度地降低能耗、优化执行时间等。然而，也存在一些缺点，例如逆向运动学和运动控制的计算复杂，结构复杂性更高。冗余解决方案是必要的，因为它可以避免奇点、障碍物并平滑工作空间周围的操作。冗余的解决方案是成本函数优化，其中成本函数可以是基于能量或最小化距离，冗余解决方案包括三种方法，即

阻尼最小二乘和加权伪逆
零空间
任务增强

本文通过冗余解析实现正逆向运动学。正向运动学可以写成如下：
$\stackrel{T}{=} T_y\left(q_0\right) T_z\left(d_0\right) R_z\left(q_1\right) T_x\left(d_1\right) R_y\left(q_2\right) T_x\left(d_2\right) R_y\left(q_3\right) T_x\left(d_3\right) T_z\left(d_4\right) R_x\left(q_4\right) R_y\left(q_5\right) R_x\left(q_6\right) T_x\left(d_6\right)$
我们有初始关节状态 q_0，需要获得将操纵器移动到给定所需位置的关节状态 q。总体思路如下：

while norm(p_global-cur_pos)>1e-02
     .Damped_LS()
     .Null_Space()
     .PseudoInverse()
     .TaskAugmentation()

基于雅可比行列式的方法：阻尼最小二乘法
$\begin{gathered} \min H(\dot{q})=\frac{\mu^2}{2}|| \dot{q}\left\|^2+\frac{1}{2}\right\| \dot{r}-J \dot{q} \|^2 \\ \dot{q}=J_{D L S}(q) \dot{r}=J^T\left(J J^T+\mu^2 I_M\right)^{-1} \dot{r} \end{gathered}$
备注：大关节速度和任务准确性之间的折衷。

r = p_global - cur_pos;
r_dot = r./k; 
q_dot = J_DLS * r_dot;
q = q_0+ (q_dot . * deltaT)';

基于雅可比行列式的方法：加权伪逆
$\begin{gathered} \dot{q}=J_W^{\#}(q) \dot{r} \\ J_W^{\#}=W^{-1} J^T\left(J W^{-1} J^T\right)^{-1}, \quad \text { if J is full (row) rank } \end{gathered}$
备注：大权重>>小 $\dot{q}$

r = p_global - cur_pos;
r_dot = r./k; 
q_dot = J_Inv * r_dot;
q = q_0+ (q_dot .* deltaT)';

零空间
$\dot{q}=J^{\#} \dot{r}+\left(I-J^{\#} J\right) \dot{q}_0$
由于解现在不再属于 $J$ 的行空间，因此它不会给出最小 2-范数解 $\dot{q}$

备注： $J^{\#}$ 是正确的伪逆

r = p_global - cur_pos;
r_dot = r./k; 
q_dot = J_Inv * r_dot;
q = q_0+ (q_dot . * deltaT)';

如果原始（主要）任务 $\dot{r}_1=J_1(q) \dot{q}$ 的优先级高于辅助（次要）任务 $\dot{r}_2=J_2(q) \dot{ q}$

我们首先处理具有最高优先级的任务
$\dot{q}=J_1^{\#} \dot{r}_1+\left(I-J_1^{\#} J_1\right) v_1$
然后选择 $v 1$ 作为次要任务，使其位于 $J$ 的零空间中，这样就不会影响主要任务。
$v_1=\left(J_2 P_1\right)^{\#}\left(\dot{r}_2-J_2 J_1^{\#} \dot{r}_1\right)$
💦正向运动学代码

function [T, T1, T2, T3, T4, T5, T6, Pos] =  FK(q, link_lengths) 

d0 = link_lengths(1);
d1 = link_lengths(2);
d2 = link_lengths(3);
d3 = link_lengths(4);
d4 = link_lengths(5);
d6 = link_lengths(6);

q0 = q(1);
q1 = q(2);
q2 = q(3);
q3 = q(4);

T1 = Ty(q0);

T2 = T1 * Tz(d0);

T3 = T2 * Rz(q1) * Tx(d1);

T4 = T3 * Ry(q2) * Tx(d2);

T5 = T4 * Ry(q3) * Tx(d3) * Tz(d4);

T6 = T5 * Rx(q4)* Ry(q5) * Rx(q6);

T = T6 * Tx(d6);

phi_x = atan2(T(3,1),T(3,2));
phi_z = atan2(T(1,3),-T(2,3));
phi_y = atan2(sqrt(T(1,3)^2+T(2,3)^2),T(3,3));

Pos = [T(1:3,4);phi_x;phi_y;phi_z];
end

💦雅可比计算代码

function [J, J1, J2, J3, J4, J5, J6, J7] =  Jacobian(q, link_lengths)

d0 = link_lengths(1);
d1 = link_lengths(2);
d2 = link_lengths(3);
d3 = link_lengths(4);
d4 = link_lengths(5);
d6 = link_lengths(6);

q0 = q(1);
q1 = q(2);
q2 = q(3);
q3 = q(4);
q4 = q(5);
q5 = q(6);
q6 = q(7);

T =  FK(q, link_lengths);
T(1:3, 4) = 0;

Td = Tyd(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ry(q5)*Rx(q6)*Tx(d6) / T;
 
J1 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rzd(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ry(q5)*Rx(q6)*Tx(d6) / T;
 
J2 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ryd(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ry(q5)*Rx(q6)*Tx(d6) / T;
 
J3 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ryd(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ry(q5)*Rx(q6)*Tx(d6) / T;
 
J4 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rxd(q4)*Ry(q5)*Rx(q6)*Tx(d6) / T;
 
J5 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ryd(q5)*Rx(q6)*Tx(d6) / T;
 
J6 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

Td = Ty(q0)*Tz(d0)*Rz(q1)*Tx(d1)*Ry(q2)*Tx(d2)*Ry(q3)*Tx(d3)*Tz(d4)*Rx(q4)*Ry(q5)*Rxd(q6)*Tx(d6) / T;
 
J7 = [ Td(1,4); Td(2,4); Td(3,4); Td(3,2); Td(1,3); Td(2,1)];

J = [J1, J2, J3, J4, J5, J6, J7];
end