這篇文章試圖說清楚兩件事：1. 幾何雅克比矩陣的本質；2. KDL如何求解機械臂的幾何雅克比矩陣。

一、幾何雅克比矩陣的本質

機械臂的關節空間的速度可以對映到執行器末端在操作空間的速度，這種對映可以通過一個矩陣來描述，就是幾何雅克比矩陣，瞭解雅克比矩陣需要了解這種對映關係的本質，這有助於用程式碼實現。

機械臂是一種開鏈式的機構，如下圖所示，那麼每個連桿的速度和加速度可以表示為式（1），這樣機械臂末端的速度可以認為是各個關節的運動導致末端的速度的疊加之後的效果。

（1）

機器人學教材和KDL中都會把機器人末端的線速度和角速度組合成一個向量，那麼末端的速度v和角速度w和機械臂關節角速度的關係可以用下式表示：

（2）

把上式寫成：

（3）

（3）式中等號後的每一列（（4）式）就代表第i個關節對末端的造成的線速度和角速度。

（4）

二、KDL如何求幾何雅克比矩陣

現在介紹KDL中如何逐列的求雅克比矩陣。

如果有同學看過我的上一篇部落格（【機器人學】機器人開源專案KDL原始碼學習：（2）牛頓拉普森迭代法求機器人的數值解）的話，應該明白我們的目的不是求機械臂末端的速度，而是在已知機械臂構型和每個關節角度位移的情況下，求解雅克比矩陣各項的值。

好的，在程式設計的時候，我們依然採用上邊敘述的運動傳遞的思想，希望能夠逐列地將雅克比矩陣求出來，明白了它的每一列表示什麼意思，才能動手寫程式碼，那它的每一列代表什麼意思呢？代表的是關節i在角速度為1的時候對機械臂最末端造成的速度！

（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）