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

🎯求解器算法

🎯C++ 计算机器人正向和逆向运动学，碰撞检测和可视化： | 🎯C++运动学和动力学串行机器人建模和计算运动链 | 🎯C++运动规划框架：逆运动学求解器，连接深度传感器和点云 | 🎯Python和C++模拟分析运动规划及现实算法环境 | 🎯三维机器人模拟：DH 参数可视化、正逆向运动学、正逆向动力学、虚拟机器人模块，与MATLAB交互 | 🎯适用于MATLAB的机器人姿态和运动学及动力学及算法数学工具，包含四旋翼飞行机器人Simulink 模型 | 🎯C++和Python运动学可视化动态模拟器 。| 🎯Python | C# | MATLAB 库卡机器人微分运动学 | 欧拉-拉格朗日动力学 | 混合动力控制

📜机器人-用例

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

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

💦正向运动学

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

机器人串联链的运动学方程是使用刚性变换 [Z] 来表征每个关节允许的相对运动，并使用单独的刚性变换 [X] 来定义每个连杆的尺寸而获得的。结果是一系列刚性变换，交替进行关节和连杆变换，从链的底部到其末端连杆，相当于末端连杆的指定位置，
[ T ] = [ Z 1 ] [ X 1 ] [ Z 2 ] [ X 2 ] ... [ X n − 1 ] [ Z n ] [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]=[Z1][X1][Z2][X2]...[Xn−1][Zn]

其中 [ T ] [T] [T] 是定位末端链接的变换。这些方程称为串联链的运动学方程。

💦Denavit-Hartenberg 矩阵

与这些操作相关的矩阵是：
Trans ⁡ Z i ( d i ) = [ 1 0 0 0 0 1 0 0 0 0 1 d i 0 0 0 1 ] , Rot ⁡ Z i ( θ i ) = [ cos ⁡ θ i − sin ⁡ θ i 0 0 sin ⁡ θ i cos ⁡ θ i 0 0 0 0 1 0 0 0 0 1 ] \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] TransZi(di)= 10000100001000di1 ,RotZi(θi)= cosθisinθi00−sinθicosθi0000100001

类似于，
Trans ⁡ X i ( a i , i + 1 ) = [ 1 0 0 a i , i + 1 0 1 0 0 0 0 1 0 0 0 0 1 ] , Rot ⁡ X i ( α i , i + 1 ) = [ 1 0 0 0 0 cos ⁡ α i , i + 1 − sin ⁡ α i , i + 1 0 0 sin ⁡ α i , i + 1 cos ⁡ α i , i + 1 0 0 0 0 1 ] . \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] . TransXi(ai,i+1)= 100001000010ai,i+1001 ,RotXi(αi,i+1)= 10000cosαi,i+1sinαi,i+100−sinαi,i+1cosαi,i+100001 .

使用 Denavit-Hartenberg 约定产生链接变换矩阵 [ j − 1 T i ] \left[{ }^{j-1} T_i\right] [j−1Ti] 为
i − 1 T i = [ cos ⁡ θ i − sin ⁡ θ i cos ⁡ α i , i + 1 sin ⁡ θ i sin ⁡ α i , i + 1 a i , i + 1 cos ⁡ θ i sin ⁡ θ i cos ⁡ θ i cos ⁡ α i , i + 1 − cos ⁡ θ i sin ⁡ α i , i + 1 a i , i + 1 sin ⁡ θ i 0 sin ⁡ α i , i + 1 cos ⁡ α i , i + 1 d i 0 0 0 1 ] , { }^{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], i−1Ti= cosθisinθi00−sinθicosαi,i+1cosθicosαi,i+1sinαi,i+10sinθisinαi,i+1−cosθisinαi,i+1cosαi,i+10ai,i+1cosθiai,i+1sinθidi1 ,

称为 Denavit-Hartenberg 矩阵。

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

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

本文通过冗余解析实现正逆向运动学。正向运动学可以写成如下：
= T T y ( q 0 ) T z ( d 0 ) R z ( q 1 ) T x ( d 1 ) R y ( q 2 ) T x ( d 2 ) R y ( q 3 ) T x ( d 3 ) T z ( d 4 ) R x ( q 4 ) R y ( q 5 ) R x ( q 6 ) T x ( d 6 ) \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) =TTy(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)

我们有初始关节状态 q_0，需要获得将操纵器移动到给定所需位置的关节状态 q。总体思路如下：
FK(q_0) 目前位置

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

基于雅可比行列式的方法：阻尼最小二乘法
min ⁡ H ( q ˙ ) = μ 2 2 ∣ ∣ q ˙ ∥ 2 + 1 2 ∥ r ˙ − J q ˙ ∥ 2 q ˙ = J D L S ( q ) r ˙ = J T ( J J T + μ 2 I M ) − 1 r ˙ \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} minH(q˙)=2μ2∣∣q˙ 2+21 r˙−Jq˙∥2q˙=JDLS(q)r˙=JT(JJT+μ2IM)−1r˙

备注：大关节速度和任务准确性之间的折衷。

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

基于雅可比行列式的方法：加权伪逆
q ˙ = J W # ( q ) r ˙ J W # = W − 1 J T ( J W − 1 J T ) − 1 , if J is full (row) rank \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} q˙=JW#(q)r˙JW#=W−1JT(JW−1JT)−1, if J is full (row) rank

备注：大权重>>小 q ˙ \dot{q} q˙

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

零空间
q ˙ = J # r ˙ + ( I − J # J ) q ˙ 0 \dot{q}=J^{\#} \dot{r}+\left(I-J^{\#} J\right) \dot{q}_0 q˙=J#r˙+(I−J#J)q˙0

由于解现在不再属于 J J J 的行空间，因此它不会给出最小 2-范数解 q ˙ \dot{q} q˙

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

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

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

我们首先处理具有最高优先级的任务
q ˙ = J 1 # r ˙ 1 + ( I − J 1 # J 1 ) v 1 \dot{q}=J_1^{\#} \dot{r}_1+\left(I-J_1^{\#} J_1\right) v_1 q˙=J1#r˙1+(I−J1#J1)v1

然后选择 v 1 v 1 v1作为次要任务，使其位于 J J J的零空间中，这样就不会影响主要任务。
v 1 = ( J 2 P 1 ) # ( r ˙ 2 − J 2 J 1 # r ˙ 1 ) v_1=\left(J_2 P_1\right)^{\#}\left(\dot{r}_2-J_2 J_1^{\#} \dot{r}_1\right) v1=(J2P1)#(r˙2−J2J1#r˙1)

💦正向运动学代码

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

👉参阅一：计算思维

👉参阅二：亚图跨际