雅克比矩阵求导推导

首先，引入雅克比矩阵公式：

$$\mathbf{J}=\left[\begin{array}{c}\begin{array}{lllc}{\mathbf{u}}_1& {\mathbf{u}}_2& \cdots & {\mathbf{u}}_n\end{array}\\ \begin{array}{lllc}{\mathbf{e}}_1& {\mathbf{e}}_2& \cdots & {\mathbf{e}}_n\end{array}\end{array}\right]$$

$$\dot{\mathbf{J}}=\left[\begin{array}{c}\begin{array}{lllc}{\dot{\mathbf{u}}}_1& {\dot{\mathbf{u}}}_2& \cdots & {\dot{\mathbf{u}}}_n\end{array}\\ \begin{array}{lllc}{\dot{\mathbf{e}}}_1& {\dot{\mathbf{e}}}_2& \cdots & {\dot{\mathbf{e}}}_n\end{array}\end{array}\right]$$

其中 ${\mathbf{u}}_i={\mathbf{e}}_i\times {\mathbf{r}}_i$ ， ${\dot{\mathbf{u}}}_i={\dot{\mathbf{e}}}_i\times {\mathbf{r}}_i+{\mathbf{e}}_i\times {\dot{\mathbf{r}}}_i$

为了求得 $\dot{\mathbf{J}}$ ,我们已知 ${\mathbf{e}}_i$ 、${\mathbf{r}}_i$

${\mathbf{e}}_i$ :坐标系{i}的Z轴单位方向向量在基坐标系下{0}下的坐标，由 $T_{\mathrm{i}}^0$ 的第三列的前三个元素给出

${\mathbf{r}}_i$：${\mathbf{r}}_i={\mathbf{o}}_e^0-{\mathbf{o}}_i^0$

${\mathbf{o}}_i^0$ 为坐标系{i}原点在基坐标{0}下的三维坐标，由 $T_{\mathrm{i}}^0$ 的第四列的前三个元素给出

${\mathbf{r}}_i$ 为在基坐标系下，坐标系{i}原点到末端点的距离

${\mathbf{r}}_i$ 还有另一种表示方式：${\mathbf{r}}_i={\mathbf{a}}_i+{\mathbf{a}}_{i+1}+\cdots +{\mathbf{a}}_n$ (式中的 ${\mathbf{a}}_i$ 必须化为基坐标下表示)

其中 ${\mathbf{a}}_i^i$ 为坐标系{i+1}的原点在坐标系{i}下的三维坐标，由 $T_{\mathrm{i}+\mathrm{i}}^i$ 的的第四列的前三个元素给出，若要化为基坐标下表示，需乘以一个旋转变换矩阵 ${\mathbf{R}}_i^0$

需要我们求：${\dot{\mathbf{e}}}_i$ 、${\dot{\mathbf{r}}}_i$

其中 ${\dot{\mathbf{e}}}_i={\mathbf{\omega }}_i\times {\mathbf{e}}_i$ 、${\dot{\mathbf{r}}}_i={\mathbf{\omega }}_i\times {\mathbf{a}}_i+{\mathbf{\omega }}_{i+1}\times {\mathbf{a}}_{i+1}+\cdots +{\mathbf{\omega }}_n\times {\mathbf{a}}_n$

求解 ${\mathbf{\omega }}_i$ ：

$${\mathbf{\omega }}_{i+1}^{i+1}={\mathbf{R}}_i^{i+1}{\mathbf{\omega }}_i^i+{\dot{\theta }}_{i+1}{\mathbf{z}}_{i+1}^{i+1}$$

$${\mathbf{\omega }}_{i+1}={\mathbf{R}}_{i+1}^0{\mathbf{\omega }}_{i+1}^{i+1}$$

可以求得 ${\dot{\mathbf{e}}}_i$ ：

$${\dot{\mathbf{e}}}_i={\mathbf{\omega }}_i\times {\mathbf{e}}_i$$

也可求得 ${\dot{\mathbf{r}}}_i$ ：

$${\dot{\mathbf{r}}}_i={\mathbf{\omega }}_i\times {\mathbf{a}}_i+{\mathbf{\omega }}_{i+1}\times {\mathbf{a}}_{i+1}+\cdots +{\mathbf{\omega }}_n\times {\mathbf{a}}_n$$

综上，所有参数求得，最后求得

$${\dot{\mathbf{u}}}_i={\dot{\mathbf{e}}}_i\times {\mathbf{r}}_i+{\mathbf{e}}_i\times {\dot{\mathbf{r}}}_i$$

参考文献：《Fundamentals of Robotic Mechanical Systems》5.5节

matlab seriaLink 也有相应的求雅克比矩阵的函数 jacob_dot （注：该函数求的是雅克比矩阵导数与关节速度的乘积，即$\dot{\mathbf{J}}\dot{\mathbf{q}}$ ）