这篇文章试图说清楚两件事：1. 几何雅克比矩阵的本质；2. KDL如何求解机械臂的几何雅克比矩阵。
一、几何雅克比矩阵的本质
机械臂的关节空间的速度可以映射到执行器末端在操作空间的速度，这种映射可以通过一个矩阵来描述，就是几何雅克比矩阵，了解雅克比矩阵需要了解这种映射关系的本质，这有助于用代码实现。
机械臂是一种开链式的机构，如下图所示，那么每个连杆的速度和加速度可以表示为式（1），这样机械臂末端的速度可以认为是各个关节的运动导致末端的速度的叠加之后的效果。
机器人学教材和KDL中都会把机器人末端的线速度和角速度组合成一个向量，那么末端的速度v和角速度w和机械臂关节角速度的关系可以用下式表示：
把上式写成：
（3）式中等号后的每一列（（4）式）就代表第i个关节对末端的造成的线速度和角速度。
二、KDL如何求几何雅克比矩阵
现在介绍KDL中如何逐列的求雅克比矩阵。
如果有同学看过我的上一篇博客（【机器人学】机器人开源项目KDL源码学习：（2）牛顿拉普森迭代法求机器人的数值解）的话，应该明白我们的目的不是求机械臂末端的速度，而是在已知机械臂构型和每个关节角度位移的情况下，求解雅克比矩阵各项的值。
好的，在编程的时候，我们依然采用上边叙述的运动传递的思想，希望能够逐列地将雅克比矩阵求出来，明白了它的每一列表示什么意思，才能动手写代码，那它的每一列代表什么意思呢？代表的是关节i在角速度为1的时候对机械臂最末端造成的速度！如果再乘以关节i的角速度的值，就得到了机械臂最末端的速度。从这个角度看（2）和（3）式就比较直观了，所有的关节对末端这也是KDL的巧妙之处。
有了这种理念，就可以看懂KDL中的求雅克比矩阵的代码了（orocos_kinematics_dynamics-master\orocos_kdl\src\chainjnttojacsolver.cpp）
形参q_in 表示机械臂在某一时刻的所有关节角位移，jac用来存放雅克比矩阵的值。
int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
{
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
//Initialize Jacobian to zero since only segmentNr colunns are computed
SetToZero(jac) ;
if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints())
return (error = E_SIZE_MISMATCH);
else if(segmentNr>chain.getNrOfSegments())
return (error = E_OUT_OF_RANGE);
T_tmp = Frame::Identity();
SetToZero(t_tmp);
int j=0;
int k=0;
Frame total;
for (unsigned int i=0;i<segmentNr;i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//pose of the new end-point expressed in the base
total = T_tmp*chain.getSegment(i).pose(q_in(j));
//changing base of new segment's twist to base frame if it is not locked
//t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
if(!locked_joints_[j])
t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
}else{
total = T_tmp*chain.getSegment(i).pose(0.0);
}
//Changing Refpoint of all columns to new ee
changeRefPoint(jac,total.p-T_tmp.p,jac);
//Only increase jointnr if the segment has a joint
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//Only put the twist inside if it is not locked
if(!locked_joints_[j])
jac.setColumn(k++,t_tmp);
j++;
}
T_tmp = total;
}
return (error = E_NOERROR);
}
}关键代码详解：
total和T_tmp用来处理坐标变换。
total = T_tmp*chain.getSegment(i).pose(q_in(j));//表示第i个连杆末端在基座标中的表示T_tmp = total;//这行在循环体的最后，表示第i个连杆的起点在基坐标中的表示t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);//求的是第i个关节对第i个连杆末端的速度（在基座标中的表示），设关节i的角速度为1，changeRefPoint(jac,total.p-T_tmp.p,jac);//将雅克比矩阵更新，第k列表示第k个关节角速度为1时对末端的造成的线速度和角速度 jac.setColumn(k++,t_tmp);//为雅克比矩阵添加新的一列（t_tmp）