Modern Robotics指数坐标推导雅可比（Jacobian）

Modern Robotics指数坐标推导雅可比（Jacobian）

空间雅可比(Space Jacobian)

假设一个开链机构的正运动学以指数积形式写成
$$T(\theta )=e^{[{\mathcal{S}}_1]{\theta }_1}\dots e^{[{\mathcal{S}}_n]{\theta }_n}M.$$
空间雅可比$J_s(\theta )\in {\mathbb{R}}^{6\times n}$关联了关节速度向量$\dot{\theta }\in {\mathbb{R}}^n$和空间旋量${\mathcal{V}}_s$，即${\mathcal{V}}_s=J_s(\theta )\dot{\theta }$。$J_s(\theta )$第$i$列由下式确定
$$J_{si}(\theta )=A{d}_{e^{[{\mathcal{S}}_1]{\theta }_1}...e^{[{\mathcal{S}}_{i-1}]{\theta }_{i-1}}}({\mathcal{S}}_i),$$
对于$i=2,\dots ,n,$第1列$J_{s1}={\mathcal{S}}_1$。螺旋矢量$J_{si}$的物理意义是在任意非零关节角度$\theta$下，第$i$个关节在空间固定坐标系下的表达。

推导：

已知正运动学指数形式表达为
$$T(\theta )=e^{[{\mathcal{S}}_1]{\theta }_1}\dots e^{[{\mathcal{S}}_n]{\theta }_n}M.\text{\hspace{1em}(5.5)}$$
空间旋量${\mathcal{V}}_s$由齐次变换矩阵给出$[{\mathcal{V}}_s]=\dot{T}{T}^{-1}$, 其中
$$\begin{gathered} \dot{T}=(\frac{d}{dt}{e}^{[{\mathcal{S}}_1]{\theta }_1})\dots e^{[{\mathcal{S}}_n]{\theta }_n}M+e^{[{\mathcal{S}}_1]{\theta }_1}(\frac{d}{dt}{e}^{[{\mathcal{S}}_2]{\theta }_2})\dots e^{[{\mathcal{S}}_n]{\theta }_n}M+\dots \\ =[{\mathcal{S}}_1]{\dot{\theta }}_1{e}^{[{\mathcal{S}}_1]{\theta }_1}\dots e^{[{\mathcal{S}}_n]{\theta }_n}M+e^{[{\mathcal{S}}_1]{\theta }_1}[{\mathcal{S}}_2]{\dot{\theta }}_2{e}^{[{\mathcal{S}}_2]{\theta }_2}\dots e^{[{\mathcal{S}}_n]{\theta }_n}M+\dots \end{gathered}$$
也有
$$T^{-1}=M^{-1}{e}^{-[{\mathcal{S}}_n]{\theta }_n}\dots e^{-[{\mathcal{S}}_1]{\theta }_1}.$$
计算$\dot{T}{T}^{-1}$得到
$$[{\mathcal{V}}_s]=[{\mathcal{S}}_1]{\dot{\theta }}_1+e^{[{\mathcal{S}}_1]{\dot{\theta }}_1}[{\mathcal{S}}_2]e^{-[{\mathcal{S}}_1]{\dot{\theta }}_1}+e^{[{\mathcal{S}}_1]{\dot{\theta }}_1}{e}^{[{\mathcal{S}}_2]{\dot{\theta }}_2}[{\mathcal{S}}_3]e^{-[{\mathcal{S}}_1]{\dot{\theta }}_1}{e}^{-[{\mathcal{S}}_2]{\dot{\theta }}_2}+\dots$$
上式可以进一步写成向量形式的伴随映射
$${\mathcal{V}}_s=\underset{J_{s1}}{\underbrace{{\mathcal{S}}_1}}{\dot{\theta }}_1+\underset{J_{s2}}{\underbrace{{\text{Ad}}_{e^{[{\mathcal{S}}_1]{\theta }_1}}({\mathcal{S}}_2)}}{\dot{\theta }}_2+\underset{J_{s3}}{\underbrace{{\text{Ad}}_{e^{[{\mathcal{S}}_1]{\theta }_1}{e}^{[{\mathcal{S}}_2]{\theta }_2}}({\mathcal{S}}_3)}}{\dot{\theta }}_3+\dots \text{\hspace{1em}(5.6)}$$
观察发现，${\mathcal{V}}_s$是$n$个空间旋量的和的形式
$${\mathcal{V}}_s=J_{s1}{\dot{\theta }}_1+J_{s2}(\theta ){\dot{\theta }}_2+\cdots +J_{sn}(\theta ){\dot{\theta }}_n,\text{\hspace{1em}(5.7)}$$
这里对任意$i=2,\dots ,n$, 每个$J_{si}(\theta )=({\omega }_{si}(\theta ,v_{si}(\theta )))$显式地决定于关节变量$\theta \in {\mathbb{R}}^n$. 写成矩阵形式
$${\mathcal{V}}_s=\left[\begin{array}{cccc}{J}_{s1}& J_{s2}(\theta )& \dots & J_{sn}(\theta )\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ \dot{\theta }\\ ⋮\\ \dot{\theta }\end{array}\right]=J_s(\theta )\dot{\theta }.\text{\hspace{1em}(5.8)}$$
矩阵$J_s(\theta )$称为在（空间）固定坐标系的雅可比，或称为空间雅可比。

**补充：**这里伴随映射的定义：给定一个齐次变换矩阵$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\in SE(3)$，其伴随表示$[{\text{Ad}}_T]$是
$${\text{Ad}}_T=\left[\begin{array}{cc}R& R\\ [p]R& 0\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$
这里$[p]$ 表示向量$p$对应的反对称矩阵。 $[{\text{Ad}}_T]$的物理意义实际是对6维旋量进行坐标变换，类似于齐次矩阵对3维向量进行坐标变换。例如，坐标系{s}到坐标系{b}的其次变换为$T_{sb}$，对一个旋量${\mathcal{V}}_b$进行伴随表示可以写成
$${\mathcal{V}}_s=[{\text{Ad}}_{T_{sb}}]{\mathcal{V}}_b={\text{Ad}}_{T_{sb}}({\mathcal{V}}_b)$$

物体雅可比（Body Jacobian）

类似的，若旋量在物体坐标系里描述，那么可以推导出在物体坐标系描述的运动学表达式，进而推导出物体雅可比,即

假设一个开链机构的正运动学以指数积形式写成
$$T(\theta )=M{e}^{[{\mathcal{B}}_1]{\theta }_1}\dots e^{[{\mathcal{B}}_n]{\theta }_n}.$$
空间雅可比$J_b(\theta )\in {\mathbb{R}}^{6\times n}$关联了关节速度向量$\dot{\theta }\in {\mathbb{R}}^n$和空间旋量${\mathcal{V}}_b$，即${\mathcal{V}}_b=J_b(\theta )\dot{\theta }$。$J_b(\theta )$第$i$列由下式确定
$$J_{bi}(\theta )=A{d}_{e^{[{\mathcal{B}}_1]{\theta }_1}...e^{[{\mathcal{B}}_{i-1}]{\theta }_{i-1}}}({\mathcal{B}}_i),$$
对于$i=n-1,\dots ,1,$第n列$J_{B_n}={\mathcal{B}}_n$。螺旋矢量$J_{bi}$的物理意义是在任意非零关节角度$\theta$下，第$i$个关节在物体坐标系下的表达。

空间雅可比与物体雅可比的联系

物体雅可比和空间雅可比的关系用下式表达
$$\begin{gathered} J_s(\theta )=[{\text{Ad}}_{T_{sb}}]J_b(\theta ), \\ {J}_b(\theta )=[{\text{Ad}}_{T_{bs}}]J_s(\theta ), \end{gathered}$$
这里$T_{sb}=T(\theta )$。

##例子：雅可比计算

Example 1: 计算如图的RRRP空间机器人的空间雅可比。

我们知道雅可比的第$i$列，就是第$i$个关节的螺旋轴（旋量）在空间坐标系（固定坐标）的表示$J_{si}=[{\omega }_{si},v_{si}{]}^T$。有时不能直接看出其旋量时，可以通过$[{\text{Ad}}_{T_{i-1}}]$矩阵对螺旋轴进行坐标变换，得到其在固定坐标系的表示。

(1)观察发现${\omega }_{s1}$始终沿${\hat{z}}_s$方向不变，因此${\omega }_{s1}=[0,0,1{]}^T$。选择坐标原点为$q_1$,那么$v_{s1}=[0,0,0{]}^T$.

(2)${\omega }_{s2}$始终沿${\hat{z}}_s$方向不变，因此${\omega }_{s2}=[0,0,1{]}^T$。选择轴线上$[L_1{c}_1,L_1{s}_1,0{]}^T$为$q_2$,其中$c_1=\text{cos}{\theta }_1,s_1=\text{sin}({\theta }_2)$,那么$v_{s2}=-{\omega }_2\times q_2=[L_1{s}_1,-L_1{c}_1,0{]}^T$.

(3)不管如何运动，${\omega }_{s3}$也是沿固定坐标系${\hat{z}}_s$方向，因此${\omega }_{s3}=[0,0,1{]}^2$，选择$q_3=[L_1{c}_1+L_2{c}_{12},L_1{s}_1+L_2{s}_{12},0{]}^T$，其中$c_{12}=\text{cos}({\theta }_1+{\theta }_2),s_{12}=\text{sin}({\theta }_1+{\theta }_2)$，那么$v_{s3}=(L_1{s}_1+L_2{s}_{12},-L_1{c}_1-L_2{c}_{1}2,0)$.

(4)最后一个关节时移动副，${\omega }_{s4}=[0,0,0{]}^2$,轴的方向为$v_{s4}=[0,0,1{]}^2$

综合上述分析，空间雅可比为
$$J_s(\theta )=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 1& 1& 1& 0\\ 0& L_1{s}_1& L_1{s}_1+L_2{s}_{12}& 0\\ 0& -L_1{c}_1& -L_1{c}_1-L_2{c}_{12}& 0\\ 0& 0& 0& 1\end{array}\right]$$
Example 2: 计算如图的RRPRRR空间机器人的空间雅可比。

(1)第一个轴线始终保持不变，${\omega }_{s1}=[0,0,1{]}^2$，选择$q_1=[0,0,L1{]}^2$,得到$w_{s1}=-{\omega }_{s1}\times q_1=[0,0,0{]}^T$

(2)第二个关节的轴线随${\theta }_1$变化，${\omega }_{s2}=[-c_1,-s_1,0{]}^2$ ，选择$q_1=(0,0,L_1)$ ，那么$v_{s2}=-{\omega }_{s2}\times q_2=[L_1{s}_1,-L_1{c}_1,0]$。

(3)第三个关节是移动副，因此${\omega }_{s3}=[0,0,0{]}^T$,轴线方向

$$v_{s3}=Rot(\hat{z},{\theta }_1)Rot(\hat{x},-{\theta }_2)[0,1,0{]}^T=[-s_1{c}_2,c_1{c}_2,-s_2{]}^T$$
(4)后三个轴交于一点

$$q_w=\left[\begin{array}{c}0\\ 0\\ L_1\end{array}\right]+Rot(\hat{z},{\theta }_1)Rot(\hat{x},-{\theta }_2)\left[\begin{array}{c}0\\ L_2+{\theta }_3\\ 0\end{array}\right]=\left[\begin{array}{c}-(L_2+{\theta }_3)s_1{c}_2\\ (L_2+{\theta }_3)c_1{c}_2\\ L_1-(L_2+{\theta }_3)s_2\end{array}\right]$$
后三个轴的轴线方向为
$$\begin{gathered} {\omega }_{s4}=Rot(\hat{z},{\theta }_1)Rot(\hat{x},-{\theta }_2)\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}-s_1{s}_2\\ c_1{s}_2\\ c_2\end{array}\right], \\ {\omega }_{s5}=Rot(\hat{z},{\theta }_1)Rot(\hat{x},-{\theta }_2)Rot(\hat{z},{\theta }_4)\left[\begin{array}{c}-1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}-c_1{c}_4+s_1{c}_2{s}_4\\ -s_1{c}_4-c_1{c}_2{s}_4\\ s_2{s}_4\end{array}\right], \\ {\omega }_{s6}=Rot(\hat{z},{\theta }_1)Rot(\hat{x},-{\theta }_2)Rot(\hat{z},{\theta }_4)Rot(\hat{x},-{\theta }_5)\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right] \\ =\left[\begin{array}{c}-c_5(s_1{c}_2{c}_4+c_1{s}_4)+s_1{s}_2{s}_5\\ c_5(c_1{c}_2{c}_4-s_1{s}_4)-c_1{s}_2{s}_5\\ -s_2{c}_4{c}_5-c_2{s}_5\end{array}\right] \end{gathered}$$
因此，空间雅可比可以写成矩阵的形式为
$$J_s(\theta )=\left[\begin{array}{cccccc}{\omega }_{s1}& {\omega }_{s2}& 0& {\omega }_{s4}& {\omega }_{s5}& {\omega }_{s6}\\ 0& -{\omega }_{s2}\times q_2& v_{s3}& -{\omega }_{s4}\times q_w& -{\omega }_{s5}\times q_w& -{\omega }_{s6}\times q_w\end{array}\right]$$
注意到，我们不需要对正运动学模型进行微分，就可以直接获得整个雅可比。

算法实现（C语言）

基于指数坐标计算雅可比的算法，我已用C语言实现，包括两个函数JacobianBody和JacobianSpace 。下面只列出这两个函数的实现。代码不断更新迭代，完整的最新源码参见github–链接

/**
 * @brief			Description: Algorithm module of robotics, according to the
 *					book[modern robotics : mechanics, planning, and control].
 * @file:			RobotAlgorithmModule.c
 * @author:			Brian
 * @date:			2019/03/01 12:23
 * Copyright(c) 	2019 Brian. All rights reserved.
 *					  
 * Contact 			https://blog.csdn.net/Galaxy_Robot
 * @warning: 		
*/

/**
*@brief Description: Computes the body Jacobian Jb(theta) in 6×n given a list of joint screws Bi 
expressed in the body frame and a list of joint angles.
*@param[in]		Blist		The joint screw axes in the end - effector frame when the manipulator is 
*							at the home position, in the format of a matrix with the screw axes as the row.
*@param[in]		thetalist	A list of joint coordinates.
*@param[out]	Jb			Body Jacobian matrix.
*@return        No return value.
*@note:			 when Blist and Jb are matrixes ,make sure that columns number of Slist or Jb is equal to JointNum,
*				 rows number of Slist or Jb is 6 .The function call should be written as
*				 JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Jb).
*@waring:
*/
void JacobianBody(int JointNum,double *Blist, double *thetalist,double *Jb)
{
	int i;
	int j;
	int k;
	double T1[4][4];
	double T2[4][4];
	double se3mat[4][4];
	double V[6];
	double AdT[6][6];
	double T[4][4] = {
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1
	};
	//Fist column of Jbn.
	for (i=0;i<6;i++)
	{
		Jb[i*JointNum+JointNum -1] = Blist[i*JointNum + JointNum-1];
	}
	//Jbi for i=n-1,n-2,...,1.
	for (i= JointNum -2;i>=0;i--)
	{
		for (j=0;j<6;j++)
		{
			V[j] = -1.0*Blist[j*JointNum+i+1];
		}
		VecTose3(V, se3mat);
		Matrix4MultValue(se3mat, thetalist[i + 1], se3mat);
		MatrixExp6(se3mat, T1);
		Matrix4Mult(T, T1,T2);
		Matrix4Equal(T2, T);
		Adjoint(T, AdT);
		for (j=0;j<6; j++)
		{
			Jb[j*JointNum + i] = 0;
			for (k=0;k<6;k++)
			{
				Jb[j*JointNum + i] = Jb[j*JointNum + i] + AdT[j][k] * Blist[k*JointNum + i];
			}
		}
	}
	return ;
}

/**
*@brief			Description:Computes the space Jacobian Js(theta) in R6 x n given a list of joint screws Si 
*				expressed in the fixed space frame and a list of joint angles.
*@param[in]		Slist		The joint screw axes expressed in the fixed space frame when the manipulator is
*							at the home position, in the format of a matrix with the screw axes as the column.
*@param[in]		thetalist	A list of joint coordinates.
*@param[out]	Js			Space Jacobian matrix.
*@return		 No return value.
*@note:			 when Slist and Js are matrixes ,make sure that columns number of Slist or Js is equal to JointNum,
*				 rows number of Slist or Js is 6 .The function call should be written as 
*				 JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Js).
*@waring:
*/
void JacobianSpace(int JointNum, double *Slist, double *thetalist, double *Js)
{
	int i;
	int j;
	int k;
	double T1[4][4];
	double T2[4][4];
	double se3mat[4][4];
	double V[6];
	double AdT[6][6];
	double T[4][4] = {
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1
	};
	//Fist column of Js.
	for (i = 0; i < 6; i++)
	{
		Js[i*JointNum + 0] = Slist[i*JointNum + 0];
	}
	//Jsi for i=2,3,...,n.
	for (i = 1; i <JointNum; i++)
	{
		for (j = 0; j < 6; j++)
		{
			V[j] = Slist[j*JointNum + i - 1];
		}
		VecTose3(V, se3mat);
		Matrix4MultValue(se3mat, thetalist[i - 1], se3mat);
		MatrixExp6(se3mat, T1);
		Matrix4Mult(T, T1, T2);
		Matrix4Equal(T2, T);
		Adjoint(T, AdT);
		for (j = 0; j < 6; j++)
		{
			Js[j*JointNum + i] = 0;
			for (k = 0; k < 6; k++)
			{
				Js[j*JointNum + i] = Js[j*JointNum + i] + AdT[j][k] * Slist[k*JointNum + i];
			}
		}
	}
	return;
}

---

参考文献

Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017