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

这篇文章试图说清楚两件事：1. 几何雅克比矩阵的本质；2. KDL如何求解机械臂的几何雅克比矩阵。

一、几何雅克比矩阵的本质

机械臂的关节空间的速度可以映射到执行器末端在操作空间的速度，这种映射可以通过一个矩阵来描述，就是几何雅克比矩阵，了解雅克比矩阵需要了解这种映射关系的本质，这有助于用代码实现。

机械臂是一种开链式的机构，如下图所示，那么每个连杆的速度和加速度可以表示为式（1），这样机械臂末端的速度可以认为是各个关节的运动导致末端的速度的叠加之后的效果。

（1）

机器人学教材和KDL中都会把机器人末端的线速度和角速度组合成一个向量，那么末端的速度v和角速度w和机械臂关节角速度的关系可以用下式表示：

（2）

把上式写成：

（3）

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

（4）

二、KDL如何求几何雅克比矩阵

现在介绍KDL中如何逐列的求雅克比矩阵。

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

好的，在编程的时候，我们依然采用上边叙述的运动传递的思想，希望能够逐列地将雅克比矩阵求出来，明白了它的每一列表示什么意思，才能动手写代码，那它的每一列代表什么意思呢？代表的是关节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();elsesegmentNr = seg_nr;//Initialize Jacobian to zero since only segmentNr colunns are computedSetToZero(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_eeif(chain.getSegment(i).getJoint().getType()!=Joint::None){//pose of the new end-point expressed in the basetotal = 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 eechangeRefPoint(jac,total.p-T_tmp.p,jac);//Only increase jointnr if the segment has a jointif(chain.getSegment(i).getJoint().getType()!=Joint::None){//Only put the twist inside if it is not lockedif(!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）

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

今年我读了四个开源项目的源码，来分享下心得
今年来看了 RocketMQ.Kafka.Dubbo .Tomcat 的源码,之前也有读者询问过如何读源码,索性就来分享一下. 其实还看了一点点 Linux.Redis.jdk8,这几个阅读的目的和上 ...
开源项目实例源码_今年我读了四个开源项目的源码，来分享下心得
今年来看了 RocketMQ.Kafka.Dubbo .Tomcat 的源码,之前也有读者询问过如何读源码,索性就来分享一下. 其实还看了一点点 Linux.Redis.jdk8,这几个阅读的目的和上 ...
Go实现的5G核心网开源项目free5gc源码分析系列 | Gopher Daily (2021.01.08) ʕ◔ϖ◔ʔ
每日一谚:"Abstractions should be discovered, not created." Go技术新闻 Go实现的5G核心网开源项目free5gc源码分析系列 ...
鸿蒙开源源码,基于鸿蒙系统开源项目OpenHarmony源码静态分析
#ifndef __scc #define __scc(X) ((long) (X)) // 转为long类型 typedef long syscall_arg_t; #endif #define _ ...
BT开源项目Snark源码分析
BT开源项目Snark源码分析 Snark是国外一个开源Java的项目,实现了BitTorrent协议,通过分析此项目的源程序,可以更利于我们更加深入的了解当前流行的BT软件的原理,进而可以指导我们的 ...
开源项目Telegram源码 Telegram for Android Source
背景介绍 Telegram 是一款跨平台的即时通信软件,它的客户端是自由及开放源代码软件.用户可以相互交换加密与自毁消息,发送照片.影片等所有类型文件.官方提供手机版.桌面版和网页版等多种平台客户端. ...
python写个围棋程序_经典python实战项目blog源码学习
这是廖雪峰老师实际项目的源码, python知识学得再多,都不如实战用一回. Day 1 – 搭建开发环境 Day 2 – 编写Web App骨架 Day 3 – 编写ORM Day 4 – 编写Mo ...
区块链开源项目Asch源码初探
Asch这个名字是 App Side Chain 的缩写. 是一种基于区块链跨链技术的应用开发平台,目前全部核心代码已经在GitHub上开源. 区块链是比特币的底层技术,但是名气低于比特币,但是个人认 ...
基于‘纯洁的微笑’开源项目 — Favorites 源码分析
引言: 对于某语言不算熟悉的话自创项目是很痛苦的过程,即便笔者是一位掌握java的Android码农,对于java入门也是深感无力,毕竟语言是基础,但框架设计模式却与Android有出入,且学习成本较 ...

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

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

最新文章

热门文章