机器人逆运动学——以六自由度机器人为例（详解、易懂，附全部Matlab代码）

Ne0inhk

— 18 min read
机器人逆运动学——以六自由度机器人为例（详解、易懂，附全部Matlab代码）

前言

前面机器人正运动学主要讲关节变量到末端执行器位姿的关系，也就是知道了关节变量与连杆参数就可以利用D-H参数表来表达末端位姿。而逆运动学就是已知末端的位姿与连杆参数，来求得关节变量的过程。本文首先介绍何为逆运动学，再以例子的形式利用D-H参数表与齐次变换矩阵对机器人进行逆解。

**阅读提醒1：在运动学逆解前，需要掌握运动学正解的相关知识，也要掌握一定的矩阵运算规则。（相关知识点有在我之前的文章提到，我也在本文进行了引用，如有需要可以查阅；我对机器人正运动学相关的matlab分析单独发了一篇博客，有需要也可以查阅）

**阅读提醒2：下文灰色补充块是用于解释正文的，用来补充正文没讲到的知识或细节。

一、运动学逆解

上面提到，已知末端执行器的位姿来求解这一位姿对应的全部关节变量就是逆解，然而由于机械结构的差异，有些时候一个末端位姿可能对应着不同的反解情况(多解)。逆运动学问题实质就是非线性超越方程组的求解问题，其解法分为两大类（封闭解法和数值解法），本文主要讲封闭解法。

1.【 封闭解法 】概述

封闭解法是指具有解析形式的解法，其计算速度快、效率高，更便于实时控制，具体又可以分为代数解法和几何解法。Pieper准则是机械臂存在封闭逆解的充分条件，并且满足其一即可，具体内容为：

Pieper准则：

1. 三个相邻关节轴相交于一点；

2. 三个相邻关节轴相互平行。

为简化逆解运算，如今机械臂厂家的大多机械臂结构都满足这一准则，比如下面常见的PUMA560机械臂(6R机械臂)，属于关节式机器人，一共有6个关节且都是转动关节，前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位：

为了方便写下面的逆变换求解，先把该机器人对应的D-H参数表列在下方（具体得到过程之前有写）：

2. 逆解具体过程（代数详解）

PUMA560六自由度机器人的2、3、4关节轴平行，所以满足Pieper准则，运用封闭解法来对其进行运动学逆解如下：

首先先列写其一般变换方程如下式（要知道

_{6}^{0}\textrm{}T

可以通过连杆参数等，一步步乘出结果）：

_{6}^{0}\textrm{}T=\begin{bmatrix} n_{x} & o_{x} & a_{x} & p_{x}\\ n_{y} & o_{y} & a_{y} &p_{y} \\ n_{z} & o_{z} & a_{z} &p_{z} \\ 0 & 0& 0& 1 \end{bmatrix}=_{1}^{0}\textrm{}T\left ( \theta _{1} \right )_{2}^{1}\textrm{}T\left ( \theta _{2} \right )_{3}^{2}\textrm{}T\left ( \theta _{3} \right )_{4}^{3}\textrm{}T\left ( \theta _{4} \right )_{5}^{4}\textrm{}T\left ( \theta _{5} \right )_{6}^{5}\textrm{}T\left ( \theta _{6} \right )

末端执行器的位姿为

\vec{n}

、

\vec{o}

、

\vec{a}

、

\vec{p}

，问题也就转变为知道这些参量，来求解 

\theta

 这6个关节变量。过程就是用未知的连杆逆变换来左乘上式的等号两边，这样就能把关节变量分离出来，进而就能求解，具体如下：

①

\theta _{1}

求解

将一般变换方程两端左乘

_{1}^{0}\textrm{}T^{-1}\left (\theta _{1} \right )

得到下式，这样左侧就分离出只有

\theta _{1}

的变量：

_{1}^{0}\textrm{}T^{-1}\left (\theta _{1} \right )_{6}^{0}\textrm{}T=_{2}^{1}\textrm{}T\left ( \theta _{2} \right )_{3}^{2}\textrm{}T\left ( \theta _{3} \right )_{4}^{3}\textrm{}T\left ( \theta _{4} \right )_{5}^{4}\textrm{}T\left ( \theta _{5} \right )_{6}^{5}\textrm{}T\left ( \theta _{6} \right )
补充：如何求？

我们已知齐次变换矩阵实际是由旋转矩阵与平移向量的复合也就是：



套用公式，其实际上变换矩阵的逆矩阵为：



也就是齐次变换矩阵求逆：让对应的旋转矩阵处求逆（也就是转置），位移列向量处左乘一个负的旋转矩阵的逆（转置）。

*（在之前的文章中已经用Matlab求过了，可以再回头看一下。）

结合上面补充分析，等号左右两式矩阵可得下式：

具体计算过程：

想要逆解出变量结果，就需要真正算出左侧两矩阵相乘后的结果（主要是为了得到、、），这里我用Matlab确实具体算了一遍，结果如下：

①首先先算一下、和的值：









以上是用齐次变换矩阵定义，利用matlab计算出的。

--------------------------------------------------------------------------------------------------------------------

②所以计算上面式等号左边两红框相乘的结果：



这一步就是按部就班代入、乘开这个式子，然后合并消除后就只剩下了，不要被大长式子吓到。

所以可以得到：

--------------------------------------------------------------------------------------------------------------------

③同理也可以去计算矩阵两端（1，4）和（3，4）的元素：





*(  就是，就是，就是，就是，这里不赘述。)

式中：

^{1}\textrm{}p_{y}=d_{2}

之后令等号两端的（2，4）元素相等（红框已经圈出）：

补充：三角代换知识。如何求  中的  ？

解：利用中间变量进行变换

其中，，

所以：

通过三角代换：

p_{x}=\rho cos\phi

，

p_{y}=\rho sin\phi

。其中

\rho =\sqrt{p_{x}^{2}+p_{y}^{2}}

，

\phi =Atan2\left ( p_{x}, p_{y}\right )

。

代入后得到：

\theta_1 = \text{Atan2}(p_y, p_x) - \text{Atan2}\left(d_2, \pm \sqrt{p_x^2 + p_y^2 - d_2^2}\right)

式中，正负号代表了

\theta _{1}

的两个可能解。

②

\theta _{3}

求解

选定

\theta _{1}

一个解之后，再令

_{6}^{1}\textrm{}T

两端元素（1，4）与（3，4）分别相等，可以得到下面方程组：

\left\{ \begin{aligned} c_1 p_x + s_1 p_y &= a_3 c_{23} - d_4 s_{23} + a_2 c_2 \\ p_z &= -\left [ a_3s_{23}+d_4c_{23}-a_2s_{2} \right ] \end{aligned} \right.

之后让

-s_1p_x+c_1p_y=d_2

与此方程组平方求和，可以得到：

a_3c_3-d_4s_3=k

式中，

k = \frac{p_x^2 + p_y^2 + p_z^2 - a_2^2 - a_3^2 - d_2^2 - d_4^2}{2 a_2 }

这样就可以实现式中只有一个变量

\theta _{3}

，并且这个等式形式和上面求

\theta _{1}

时一致，所以同理：

\theta_3 = \text{Atan2}(a_3, d_4) - \text{Atan2}\left(k, \pm \sqrt{a_3^2 + d_4^2 - k^2}\right)

式中，正负号代表了

\theta _{3}

的两个可能解。

③

\theta _{2}

求解

将一般变换方程等式两边左乘一个

_{3}^{0}\textrm{}T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} \right )

，如下：

_{3}^{0}\textrm{}T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3}\right )_{6}^{0}\textrm{}T=_{4}^{3}\textrm{}T\left ( \theta _{4} \right )_{5}^{4}\textrm{}T\left ( \theta _{5} \right )_{6}^{5}\textrm{}T\left ( \theta _{6} \right )

这样就可以使得等号左侧只有三个变量，而前面求了

\theta _{1}

与

\theta _{3}

，因此就只有

\theta _{2}

这一个变量了。等号左右两式矩阵可得下式：

通过计算可以得到：



然后再根据矩阵求逆的方法去计算，这里不再赘述~

上式中等号左边的矩阵就是计算后的

^{_{3}^{0}\textrm{}}T^{-1}

(这里简化表达)，然后令上式的（1，4）与（2，4）元素相等进行计算，最后合并消除可以得到：

\theta_{23} = \theta_2 + \theta_3 = \text{atan2}\left[ -(a_3 + a_2 c_3) p_z + (c_1 p_x + s_1 p_y)(a_2 s_3 - d_4), (-d_4 + a_2 s_3) p_x + (c_1 p_x + s_1 p_y)(a_2 c_3 + a_3) \right]
\theta _{2}=\theta _{23}-\theta _{3}

其中

\theta _{3}

的解与

\theta _{1}

和

\theta _{2}

有关，因此得出的

\theta _{3}

有四种可能的解。

④

\theta _{4}

求解

因为上面

_{6}^{3}\textrm{}T

左式均为已知，令两边的元素（1，3）和（3，3）分别相等，则可以得到：

\left\{ \begin{aligned} a_x c_{1} c_{23} + a_y s_{1} c_{23} - a_z s_{23} &= -c_4 s_5 \\ -a_x s_y + a_y c_1 &= s_4 s_5 \end{aligned} \right.

这里面只要

s_{5}

不为0，消去之后就可以得到

\theta _{4}

：

\theta_4 = \text{Atan2}\left( -a_4 s_1 + a_y c_1, -a_x c_1 c_{23} - a_y s_1 c_{23} + a_z s_{23} \right)

如果

s_{5}=0

，此时机械臂处于奇异位置，这时候关节4与6重合，不能解出来具体值，这个奇异值我们后面讨论。

⑤

\theta _{5}

求解

可以让一般变换方程等式两边左乘一个

_{4}^{0}\textrm{}T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} , \theta _{4} \right )

，如下：

_{4}^{0}\textrm{}T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3} ,\theta _{4} \right )_{6}^{0}\textrm{}T= _{5}^{4}\textrm{}T\left ( \theta _{5} \right )_{6}^{5}\textrm{}T\left ( \theta _{6} \right )

这时候前面四个变量都解出来了，所以左侧只有

\theta _{5}

一个变量未知。因此可以求出

_{4}^{0}\textrm{}T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} , \theta _{4} \right )

矩阵：

\left[ \begin{array}{cccc} c_1 c_{23} c_4 + s_1 s_4 & s_1 c_{23} c_4 - c_1 s_4 & -s_{23} c_4 & -a_2 c_3 c_4 + d_2 s_4 - a_3 c_4 \\ -c_1 c_{23} s_4 + s_1 c_4 & -s_1 c_{23} s_4 - c_1 c_4 & s_{23} s_4 & a_2 c_3 s_4 + d_2 c_4 + a_3 s_4 \\ -c_1 s_{23} & -s_1 s_{23} & -c_{23} & a_2 s_3 - d_4 \\ 0 & 0 & 0 & 1 \end{array} \right]

同理，可以让等式两侧元素（1，3）和（3，3）分别相等，可以得到：

\left\{ \begin{aligned} a_x \left( c_1 c_{23} c_4 + s_1 s_4 \right) + a_y \left( s_1 c_{23} c_4 - c_1 s_4 \right) - a_z \left( s_{23} c_4 \right) &= -s_5 \\ a_x \left( -c_1 s_{23} \right) + a_y \left( -s_1 s_{23} \right) + a_z \left( -c_{23} \right) &= c_5 \end{aligned} \right.

由此得到

\theta _{5}

：

\theta _5=Atan2\left ( s_5, c_5 \right )

⑥

\theta _{6}

求解

同理让一般变换方程等式两边左乘一个

_{5}^{0}\textrm{}T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} , \theta _{4} , \theta _{5} \right )

，如下：

_{5}^{0}\textrm{}T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3} ,\theta _{4} , \theta _{5} \right )_{6}^{0}\textrm{}T= _{6}^{5}\textrm{}T\left ( \theta _{6} \right )

通过上面等式（3，1）与（1，1）的元素相等，可以得到

\theta _{6}

：

\begin{cases} -n_x \left( c_1 c_{23} s_4 - s_1 c_4 \right) - n_y \left( s_1 c_{23} s_4 + c_1 c_4 \right) + n_z \left( s_{23} s_4 \right) = s_6 \\ n_x \left[ \left( c_1 c_{23} c_4 + s_1 s_4 \right) c_5 - c s_{23} s_5 \right] + n_y \left[ \left( s_1 c_{23} c_4 - c_1 s_4 \right) c_5 - s_1 s_{23} s_5 \right] - n_z \left( s_{23} c_4 c_5 + c_{23} s_5 \right) = c_6 \end{cases}
\theta _6=Atan2\left ( s_6, c_6 \right )

至此六个变量求解完成，以上存在多解的关节还需要考虑关节结构，是否能够到达这一位置，如果因结构限制则可能这对应的解不存在。

二、Matlab求解过程

1. 整体代码

先把具体代码展示如下：

clc; clear; %输入已知的位姿矩阵（正解可得）---------------------- a=[ 0.0000 0.0000 1.0000 800.0000; -0.0000 -1.0000 0.0000 120.0000; 1.0000 -0.0000 -0.0000 10.0000; 0 0 0 1.0000 ]; %------------------------------------------------- %定义连杆的D-H参数 %连杆偏移 d1 = 0; d2 = 120; d3 = 0; d4 = 400; d5 = 0; d6 = 0; %连杆长度 a2 = 400; a3 = 10; %连杆扭角 alpha1 = 0; alpha2 = -pi/2; alpha3 = 0; alpha4 = -pi/2; alpha5 = pi/2; alpha6 = -pi/2; %建立机器人模型 % theta d a alpha L1=Link([0 d1 0 alpha1 ],'modified'); L2=Link([0 d2 0 alpha2 ],'modified'); L3=Link([0 d3 a2 alpha3 ],'modified'); L4=Link([0 d4 a3 alpha4 ],'modified'); L5=Link([0 d5 0 alpha5 ],'modified'); L6=Link([0 d6 0 alpha6 ],'modified'); %限制关节空间 L1.qlim = [(-165/180)*pi,(165/180)*pi]; L2.qlim = [(-150/180)*pi, (60/180)*pi]; L3.qlim = [(-150/180)*pi, (90/180)*pi]; L4.qlim = [(-180/180)*pi,(180/180)*pi]; L5.qlim = [(-115/180)*pi,(115/180)*pi]; L6.qlim = [(-360/180)*pi,(360/180)*pi]; %连接连杆，机器人取名为robot robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','6Rrobot'); robot.display(); robot.teach(); nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4); ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4); nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4); %解关节1 theta1_1 = atan2(py,px)-atan2(d2,abs(sqrt(px^2+py^2-d2^2))); theta1_2 = atan2(py,px)-atan2(d2,-abs(sqrt(px^2+py^2-d2^2))); %解关节3 kk为中间值 kk = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2); theta3_1 = atan2(a3,d4)-atan2(kk,abs(sqrt(a3^2+d4^2-kk^2))); theta3_2 = atan2(a3,d4)-atan2(kk,-abs(sqrt(a3^2+d4^2-kk^2))); %解关节2 ko2为theta23计算式中第一部分,kt2为第二部分 % theta1_1 theta3_1对应解 ko2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4); kt2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3); theta23_1 = atan2(ko2_1,kt2_1); theta2_1 = theta23_1 - theta3_1; % theta1_2 theta3_1对应解 ko2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4); kt2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3); theta23_2 = atan2(ko2_2,kt2_2); theta2_2 = theta23_2 - theta3_1; % theta1_1 theta3_2对应解 ko2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4); kt2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3); theta23_3 = atan2(ko2_3,kt2_3); theta2_3 = theta23_3 - theta3_2; % theta1_2 theta3_2对应解 ko2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4); kt2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3); theta23_4 = atan2(ko2_4,kt2_4); theta2_4 = theta23_4 - theta3_2; % 求解关节4 % theta1_1 theta2_1 theta3_1对应解 ko4_1=-ax*sin(theta1_1)+ay*cos(theta1_1); kt4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1); theta4_1=atan2(ko4_1,kt4_1); % theta1_2 theta2_2 theta3_1对应解 ko4_2=-ax*sin(theta1_2)+ay*cos(theta1_2); kt4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2); theta4_2=atan2(ko4_2,kt4_2); % theta1_1 theta2_3 theta3_2对应解 ko4_3=-ax*sin(theta1_1)+ay*cos(theta1_1); kt4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3); theta4_3=atan2(ko4_3,kt4_3); % theta1_2 theta2_4 theta3_2对应解 ko4_4=-ax*sin(theta1_2)+ay*cos(theta1_2); kt4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4); theta4_4=atan2(ko4_4,kt4_4); % 求解关节5 % theta1_1 theta2_1 theta3_1 theta4_1 对应解 ko5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1)); kt5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1)); theta5_1=atan2(ko5_1,kt5_1); % theta1_2 theta2_2 theta3_1 theta4_2 对应解 ko5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2)); kt5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2)); theta5_2=atan2(ko5_2,kt5_2); % theta1_1 theta2_3 theta3_2 theta4_3 对应解 ko5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3)); kt5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3)); theta5_3=atan2(ko5_3,kt5_3); % theta1_2 theta2_4 theta3_2 theta4_4 对应解 ko5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4)); kt5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4)); theta5_4=atan2(ko5_4,kt5_4); % 求解关节角6 % theta1_1 theta2_1 theta3_1 theta4_1 theta5_1 对应解 ko6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1)); kt6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1)); theta6_1=atan2(ko6_1,kt6_1); % theta1_2 theta2_2 theta3_1 theta4_2 theta5_2 对应解 ko6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2)); kt6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2)); theta6_2=atan2(ko6_2,kt6_2); % theta1_1 theta2_3 theta3_2 theta4_3 theta5_3 对应解 ko6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3)); kt6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3)); theta6_3=atan2(ko6_3,kt6_3); % theta1_2 theta2_4 theta3_2 theta4_4 theta5_4 对应解 ko6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4)); kt6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4)); theta6_4=atan2(ko6_4,kt6_4); % 整理得到4组运动学非奇异逆解 theta_MOD1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1; theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2; theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3; theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4; ]*(180/pi); % 将操作关节‘翻转’可得到另外4组解 theta_MOD2 = ... [ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi; theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi; theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi; theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi; ]*(180/pi); %输出关节变量值 J = [theta_MOD1;theta_MOD2]

上面需要我们输入改变的是a矩阵的具体内容，这个a实际上就是齐次变换矩阵

_{6}^{0}\textrm{}T

。如果要求逆解，那么这个矩阵是知道的，如果不知道怎么求的伙伴，可以看我前一篇正运动学的文章。

2. 举例分析正确性

先看上面代码我的a是什么条件下得来的，这是我的转角变量所赋值的内容：

这是对应变量下的齐次变换矩阵

_{6}^{0}\textrm{}T

：

现在假设我们有了这个齐次变换矩阵

_{6}^{0}\textrm{}T

的信息，我们把这个数据替换掉上面代码的a矩阵，然后运行程序：

至此完成了逆运动学解算~

总结

本文以之前所写的正运动学《机器人正运动学实例——PUMA560机械臂（附Matlab机器人工具箱建模代码）》为基础，从基本公式推导出发，结合matlab工具完成了六自由度机器人的运动学逆解。最终能够实现输入指定的齐次变换矩阵，能够得到对应的所有变量解。这一过程有助于理解正逆运动学的分析过程，对于机器人设计及运动控制等有一定参考意义。

声明，本文的主要学习资料为   机器人运动学——林沛群老师课程

本文以学习为目的，会不断将所学总结更新，如有不足还望不吝赐教。

Read more

面向数据工程的 SQL 与 Python 代码自动生成：6 款大模型深度评测

面向数据工程的 SQL 与 Python 代码自动生成：6 款大模型深度评测

面向数据工程的 AI 代码助手：6 款 SQL 与 Python 工具深度评测 摘要：本文对 GitHub Copilot、Cursor、Claude、ChatGPT、Gemini Code Assist 和 Amazon CodeWhisperer 六款主流 AI 代码助手进行了深度评测，重点考察它们在数据工程工作流（如 SQL 转换、Python ETL、dbt 模型生成等）中的表现。作者详细对比了各工具的优缺点、适用场景及成本效益，为个人开发者和数据团队提供了切实可行的选型建议，强调了“混合使用”策略的优势。 免责声明：本评测反映了 2026 年 1 月时的工具能力。AI 代码助手发展迅速，功能、定价和模型能力频繁变化。

By Ne0inhk

嵌入式软件代码架构详解，超清晰图解为什么需要软件架构，以及告诉你怎么实现软件架构

我希望你能够带着几个问题进入到下面的文章中，我会用生动的例子告诉你为什么需要软件架构，以及一个简单的软件架构是什么样子的。在看文章的过程中，你要有意识的思考这几个问题，希望看完这篇文章，你就能回答出下面几个问题了。 1.为什么需要软件架构？ 2.好的软件架构有哪些标准，能够解决掉什么问题？ 3.软件架构长什么样子？文章看完了你能够画出来嘛？ 一、 告别“面条代码”，嵌入式为何更需要软件架构？ 1.1 从两个场景说起 当你拿到一块新的开发板，兴致勃勃地开始你的嵌入式项目时，是不是经常这样开始你的main.c？ 场景A（新手期）：功能堆砌的“面条代码” 这就是经典的“面条代码”（Spaghetti Code）——所有逻辑像一碗面条一样缠绕在一起。 它有些什么样的问题呢？ * main函数无限膨胀： 所有功能都堆在while(1)循环里，代码越来越长，越来越难阅读。 * “牵一发而动全身”： 你想修改按键的逻辑，可能会影响到ADC采样；想移除蜂鸣器功能，得小心翼翼地从一大坨代码里找出所有相关行。 * 高度耦合： 业务逻辑（按键控制LED）

By Ne0inhk
【Kafka进阶篇】深入Kafka内部：日志存储的设计思路，藏着中间件高性能的真相

【Kafka进阶篇】深入Kafka内部：日志存储的设计思路，藏着中间件高性能的真相

🍃 予枫：个人主页 📚 个人专栏: 《Java 从入门到起飞》《读研码农的干货日常》 💻 Debug 这个世界，Return 更好的自己！ 引言 做分布式开发的同学，几乎都用过Kafka，但多数人只停留在“生产者发消息、消费者收消息”的表层使用，很少深究：百万级消息并发下，Kafka如何快速定位目标消息？底层的.log、.index、.timeindex文件各司其职，又是如何配合实现高效读写的？今天就从物理层面拆解Kafka日志存储与索引机制，吃透这部分，不仅能搞定面试难点，更能在生产环境中精准优化Kafka性能。 文章目录 * 引言 * 一、前言：为什么要搞懂Kafka日志与索引机制？ * 二、核心前提：Kafka日志的“分段存储”设计 * 三、深度拆解：三种核心文件的作用与结构 * 3.1 .log文件：消息的“真正存储载体” * 关键细节： * 3.2

By Ne0inhk