看到专栏在短短时间之内就受到不少机器人与控制大佬的点赞,关注和收藏,本菜感觉鸭梨山大呢。今天,本菜继续带领小白们白嫖,额不,站在巨人的肩上做科研?

首先,大家打开《A Systematic Approach to Learning Robot Programming with ROS》by Wyatt S. Newman,翻到第四部分Robot Arms in ROS第11章Low Level Control第4节Example force controller。这本书我感觉是我目前所读过的有限的若干本关于ROS的书中最系统的,最良心的,全书每个示例都讲解的比较细致,都有ROS package可供下载运行。中华人民共和国机械工业出版社出了此书的中文翻译版《ROS机器人编程:原理与应用》。我也不知道怎么翻译成这个鬼样子,最重要的Systematic竟然直接扔了...

通过阅读,我们发现作者是个标题党,“底层控制器之力控制”--- 你以为是传统的动力学建模外加计算力矩控制,就像上一个例程那样,结果发现第一句话竟然出现了“interaction forces”,“programmable compliance”。或许写书的大佬所认为的low level就是如此吧,不过从侧面也说明机器人就是应该具备“优雅地”与外界交互的基本能力。大佬的风格很非主流,很符合我们“非典型”的特质,哈哈。

下面来说说“interaction forces”和“programmable compliance”。

交互力------特指机械臂与环境之间的相互作用力,也叫外部力(External force),是笛卡尔空 间的,如果你在机器人或者环境中安装了力/力矩传感器,就可以得到交互力。也有人把笛卡尔空间的交互力通过机器人交互点处的雅克比(Jacobian)的转置变换到关节空间,然后把变换得到的关节力/力矩称为交互力或者外部力的,当然如果你没有外部力传感器只有关节力/力矩传感器,你可以通过机器人动力学模型计算出期望运动所需的关节力/力矩,然后拿关节力/力矩传感器所反馈的实际关节力/力矩减去期望值就得到了关节空间外部力/力矩

可编程柔顺------意思是让机械臂能够有可改变的对于外力的顺应性。一般的做法是把机械臂与环境交互看成一个二阶系统(阻抗模型),这个阻抗模型联系了交互力交互处的加速度。如果反馈力,输出加速度,然后让机械臂接触处叠加上这样一个加速度,就叫做导纳控制;如果反过来,就叫做阻抗控制可改变就体现在所等效的阻抗模型参数可改变。

继续往后读,作者先在单自由度竖直平动机械臂上做了点镇定导纳控制。这个例子中机器人的关节空间刚好和笛卡尔空间一致(如果只考虑z轴平动自由度的话),作者直接把阻尼模型的质量部分取为连杆质量,这种用机械臂系统的等效质量作为阻尼模型中质量的做法被称为自然阻尼/导纳(Natural Impedance/Admittance)。然而,对于多自由度的复杂机器人系统,等效质量就是个随关节位置变化的矩阵,大概长这个样子 [公式] ,因为我们这里说的是交互处的等效,所以出来了雅克比。即便是关节空间的质量矩阵 [公式] ,其也是变化的。为了简化(主要是本菜太懒),我们就还是取个常矩阵,尽量跟实际的等效质量靠近,阻尼弹性矩阵根据二阶线性系统设计方法确定。

万事具备,只欠一顿胖揍,作者设计的交互是------天降重物,为了安全起见,在七自由度机械臂末端加了个大托盘。仿真结果如下两图所示。图片白嫖自

我们看到由于具有了柔顺性,交互力没有变的特别大,两个机械臂都稳稳地,优雅地接住了重物并一直保持。

单自由度机械臂自然导纳控制
七自由度自然导纳控制

感觉,不是特别复杂鸭。开始改造吧!

1. 正向运动学和瞬时运动学

我们的LBR/iiwa刚好也是个七自由度机械臂,不过跟作者的结构不同,那么我们得手撸正向运动学和瞬时运动学(不能白嫖好伤心的说...)。

这部分常规操作,具体代码参见iiwa14_kinematics文件夹,里边包含了iiwa7和iiwa14两种,她们只是几何尺寸不同,用robot_type参数区别 ---- iiwa7 (0) or iiwa14 (~0)。

2. 制作场景文件

我们取一组关节位置值,让iiwa的末端关节轴竖直朝上,把末端坐标系的位值记录下来作为点镇定的笛卡尔空间目标值。然后我们添加一个力传感器(Add->Force sensor),让它与末端关节坐标系重合,并在模型树中使其成为末端连杆的子节点。最后再添加一个托盘(Add->Primitive shape->Cuboid),填入适当的尺寸值和质量,设置为dynamics enabled并调整位姿,在模型树中拖动,使其成为力传感器的子节点。接着,在空中添加一个小重物(书中的机械臂是很大的,iiwa很娇小的)并给定合适质量设置为dynamics enabled。最后添加一个ContactInfoDisplay,这个是V-REP自带示例中的,用于计算场景中的平均接触力并显示各个接触点处接触力,我们在其附带的脚本中添加两句话---将平均接触力打包成字符串数组然后通过名为ContactForce的信号发出。当然你也可以通过获取力传感器的值间接得到接触力,为什么是间接呢?因为接触发生在托盘的上面,力传感器在托盘的下面,那么力传感器的数值表示的是接触处的力系在力传感器处的等效力系,无奈本菜太懒了就不转换了。成品如下:

3. 编写控制代码

控制部分的流程是:

1. 获取当前反馈的机器人关节位置和速度值,计算末端坐标系的位姿和速度,并进一步根据阻抗模型计算点镇定的吸引力(这部分相当于建立了末端坐标系期望笛卡尔位姿与实际笛卡尔位姿之间的阻尼弹簧,她把实际末端坐标系拉向期望位姿),形式就是传统的PD控制器;

2. 获取接触力,将其与1中计算的吸引力求合力(接触力是把末端坐标系推离期望位姿的,稳定状态是吸引力和接触力平衡),然后通过雅克比转置得到对应的关节力矩;

3. 关节空间的等效质量取为常数对角阵(当然严格的应取为 M(q) ,我承认我懒,只要最终能稳定即可),基于2中的关节力矩加上关节速度反馈项(主要是为了关节空间控制稳定),得到关节加速度;

4. 数值积分更新期望的关节速度,进一步数值积分更新期望的关节位置,然后发给V-REP。

但是,考虑到iiwa的特殊构型(实际是白嫖了书作者的),后三个关节用于调整末端姿态,前四个关节用于调整末端位置,在给定末端位姿以及前四个关节位置值的基础上,可以解析地求解后三个关节位置。因此1中我们没有考虑姿态误差,而是在4中通过逆运动学求解期望的后三个关节位置值,然后把后三个关节期望位置值与当前位置值的误差乘以比例系数当做后三个关节的期望速度。当然,你也可以整体一锅炖了(主要是我懒,尽量白嫖)。

4. 运行

操作没什么好说的,只是当vrep中机械臂下降至稳定状态之后,你可以鼠标点击小重物,然后键盘delete一下,小重物就消失了,基于控制器的设计,我们知道机械臂会回到目标位姿,果不其然(好想发生点意外鸭)。

5. 数据处理

这里我们记录末端坐标系的位置与速度,我们在m文件中把 z 方向的弹性系数取为80N/m,在V-REP中把小重物设为 1kg ,重力加速度设为-9.81m/s^2,那么理论上估算稳定点在初始点以下 0.1226m 处,与仿真结果比较吻合。在去掉小重物以后,末端回升。

再看看外部力(接触力),吸引力(虚拟力)与两者合力。也是意料之中。

其他期望关节加速度,关节速度与关节力矩如下:

----------------------------------------------------

本例程源文件链接(切记只可用于学习,科研等非商业用途)

-----------------------------------------------------

做人呐,最重要的就是开心喽。能帮到别人是最大的快乐,希望本菜鸡能对小白们有所帮助,希望大佬们的批评能帮助本菜鸡成长!