【机器人学】机器人开源项目KDL源码学习：（5）KDL如何求解几何雅克比矩阵

这篇文章试图说清楚两件事：（1）几何雅克比矩阵的本质；（2）KDL如何巧妙地求解几何雅克比矩阵。

机器人的关节空间的速度可以映射到执行器末端在操作空间的速度，这种映射可以通过一个矩阵来描述，就是几何雅克比矩阵，了解雅克比矩阵需要了解这种映射关系的本质，否则在编写机器人运动学算法的时候会无从下手。

机器人执行器末端的速度v和角速度w和角速度的关系可以用下式表示：

（1）

把上式写成：

（2）

为什么要整理成（2）式呢？因为机械臂末端的速度可以认为是各个关节的运动导致末端的速度的叠加之后的效果。

所以，（2）式中等号后的每一列（（3）式）就代表第i个关节对末端的造成的线速度和角速度。

（3）

现在介绍KDL中如何逐列的求雅克比矩阵，理解下边的内容需要回顾一下刚体运动的知识，刚体内任一点的速度等于基点的速度与该点随刚体绕基点转动速度的和，角速度一样。

如果有同学看过我的上一篇博客（【机器人学】机器人开源项目KDL源码学习：（2）牛顿拉普森迭代法求机器人的数值解）的话，应该明白我们的目的不是求机械臂末端的速度，而是在已知机械臂构型和每个关节角度位移的情况下，求解雅克比矩阵各项的值。

好的，在编程的时候，我们依然采用上边叙述的运动传递的思想，希望能够逐列地将雅克比矩阵求出来，明白了它的每一列表示什么意思，才能动手写代码，那它的每一列（（4）式）代表什么意思呢？代表的是关节i在角速度为1的时候对机械臂最末端造成的速度！如果再乘以关节i的角速度的值，就得到了机械臂最末端的速度。从这个角度看（2）和（3）式就比较直观了，所有的关节对末端这也是KDL的巧妙之处。

（4）

有了这种理念，就可以看懂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）