嗨伙计们，忙到爆炸的月更侠罗伯特祥又来和大家见面了！今天我们来聊聊协作机器人中柔顺控制那点事儿~ 我们先来讲下本次文章的安排，首先我们要在webots中建立一个连杆模型，然后我们需要对它进行动力学建模，基于这个模型，我们通过阻抗控制使它呈现出一个柔顺的效果！ 那进入正题之前呢，请允许我先来打波广告~
之前呢，几篇文章大体介绍了webots的基本使用，也开过一次公开课《如何入门webots机器人仿真神器 · 罗伯特祥》，但是这些都是基于python或者c++的，为了打通ROS与webots的联合仿真，博主又倾情打造了一套ROS与webots的联合仿真教程：《如何实现ROS与Webots联合仿真 · 罗伯特祥》，在这套课程中，我将带你了解webots这款仿真神器中机器人的建模及常见执行器与传感器的使用，也会手把手教你如何在ROS中编写机器人的控制程序以及数据的可视化，同时呢还会教你一些小技巧，例如利用上帝节点实现机器人的3D轨迹绘制等等。 
整套课程使用的是rospy来写的控制器，带有丰富的案例，让你更轻松入门~（小声BB：朋友都说我的声音像李诞，为啥不说他的声音像我呢哈哈哈哈，普通话不太标准，还请各位见谅）
建模教程已经实现的差不多了，所以以后的文章中，我将会结合理论性的知识点，在webots中实现一些有意思的仿真小案例。嘿嘿当然啦，这也得在我个人的能力和时间允许的情况下~ okok！广告打完了，我们来进入正题！ 近几年呢协作机器人发展的很快，人机交互以及人机的安全协作技术也跟着走进了我们的视野，其中柔顺控制就是这其中的一项重要技术！ 正好笔者的研究方向与此相关，所以打算跟大家聊聊柔顺控制的实现过程~ 机器人的柔顺性一般包括运动柔顺性和力柔顺性，运动柔顺性指的是机器人奇异位形逃逸等相关的一些内容，主要是为了避免运动过程中的冲击；而我们常说的柔顺控制呢，一般是指后者这个力的柔顺性。 本文中，我们将使用一个单连杆来模拟它的柔顺控制。
1. webots建模
详细的建模过程，我感觉貌似不用再一步步带着大家走了，那么就直接上模型的结构吧~ 
我们在连杆上加了一个陀螺仪，为啥要加它呢，这是为了测量连杆的角速度。 一个刚体由惯性张量、质量和质心等10个参数来定义，所以这次我们详细定义了连杆的物理参数。 相信你能看明白是怎么建的，如果有些吃力，翻翻以前的文章或者古月学院的课程安排一波呀！ 
具体的参数，可以参考我的设置，图在文末。 建完的模型也很简单，是这个样子的~ 
2.动力学建模
终于到了正题，我们先来温习一下拉格朗日法的动力学求解。 首先第一步，定义拉格朗日函数：
那么它的运动方程为：
也就是：
不太熟悉的朋友记得翻翻《机器人学导论》，或者古月居论坛上一些大佬们的文章~
下面开始对单连杆进行动力学建模： 
连杆在质心坐标系下的惯性张量矩阵为： 
对于单连杆，总动能为： 
假设连杆运动最低点为零势面，那么总势能为： 
偏导数： 
所以，系统动力学方程为：
当然，更简单的是我们直接对连杆进行受力分析，然后通过平行移轴定理得到连杆在旋转轴的转动惯量，计算出由于加速度引起的加速力矩和重力力矩的和。
这里要注意，公式里的惯性张量要跟webots中对应起来，因为webots中的坐标系大多数情况下跟我们理论建模的时候是不一致的！
3.柔顺控制器
现在到正主儿了，它的发展说来话长，篇幅有限就不多啰嗦了。 我们怎么实现它的柔顺性呢？ 在此之前，我们需要先回顾一下质量-弹簧-阻尼模型，这个模型可太经典了！当年恩师讲这个的情景还历历在目~（怀念我的母校，明德笃学、弘毅拓新，致敬母校，祝母校70周岁生日快乐~）
声明：上图摘自《Modern Robotics Mechanics Planning and Control》，版权归原作者所有。
为什么要讲它呢，因为这就是我们体现系统柔顺性的一个关键！ 那什么是柔顺性？在外力作用下，系统能表现出自然顺从，这就叫柔顺性。 为什么又说质量-弹簧-阻尼模型是我们实现柔顺性的一个关键呢？这让我想起了邵总在知乎上的那篇文章，他举了一个简单形象的例子，机器人开门的问题，理想情况下机器人是可以实现开门过程，然而现实世界存在着误差，这就导致外力与门的关节限制发生冲突，从而导致门被破坏或者机器人被扯断，那我们怎么解决这个问题呢，在门与机器人之间加一个弹簧，这个弹簧可以填充理论与现实的误差，所以它能完美的解决这个冲突问题。 只有一个弹簧，又会存在什么问题？当然是振动！而为了避免振动的发生，我们应该引入一个阻尼，去消除这个振动。那么这就是我们柔顺控制的一个思想！ 回过头来看模型，它的动力学方程可以写成：
对它进行拉普拉斯变换，假设输入为位置扰动，输出为力，得到系统传递函数为：
这就是阻抗的定义，而对于导纳来说，它是阻抗的导数，二者本质上其实是相同的，只不过是不同角度的解读方式。 既然是个二阶系统，那么显然参数的不同，将呈现出三种状态，即过阻尼、欠阻尼和无阻尼的情况，后面我们将会通过修改参数去观察一下连杆的反应。
声明：上图摘自《Modern Robotics Mechanics Planning and Control》，版权归原作者所有。
问题又来啦，我们怎么应用这个模型去实现柔顺控制？ 类比这个模型，我们得到二阶误差动力学模型。实际上，柔顺控制就是一个二阶误差动力学应用的结果，我们假想一下，在关节处有一个扭簧和阻尼器（这里我们以关节柔顺控制为例，笛卡尔柔顺控制同理，至于为啥没考虑质量呢，低速情况下我们可以忽略加速度，再者质量m在奇异点以外一般不会太大，当然我们这里的单连杆模型不存在这个问题，所以我们可以直接忽略质量问题），在不受外力（包括重力）的情况下，给它一个外力干扰，由于扭簧的存在，它会呈现出顺从性，而外力消失，在扭簧的作用下它又会慢慢回到平衡点。 那么，接下来我们就要写这个控制律了。 首先是外力问题，我们需要先给机械臂创造一个出外界作用力外的无外力环境，这时候我们就需要去做零力控制，通俗的说就是去补偿重力和摩擦力（当然还有一些其他因素的影响，我们要抓主要矛盾）。 为了简化，这里我们就不考虑关节摩擦力问题了，所以就只对它进行重力补偿。 联想到第2节建的动力学模型，由于处于低速情况下，因此我们可以忽略单连杆动力学模型中与角加速度有关的项，那么也就只剩下重力了，重力引起的力矩为：
所以它的阻抗控制律为：
对你没看错，啰嗦了这么多，实际上就这么一个公式！
现在我们就到webots中仿真一下，看看什么效果。 
嗯，有nei味儿了~ 现在我们再来修改一下阻尼系数，让系统表现出无阻尼和过阻尼的状态： 
具体参数就不给了，大家自己玩玩加深一下印象。
注意：在仿真环境下随便设置参数无所谓，但在实际机器人控制中务必要注意不要随便去设置参数，而应该慢慢去尝试一下，否则可能会导致由于参数设置不当导致的控制器失稳，以免发生危险~
okay，现在呢我们完成了单连杆柔顺控制的仿真，对于多自由度系统的关节柔顺控制，也很容易由单自由度去扩展得到。具体的柔顺参数的调整，大家还需要去回顾一下二阶系统的特性。 今天就先讲到这儿，读万卷书也要行万里路，我是罗伯特祥，下次见！
最后小声BB一句：今年秋招可真是一言难尽，在下太菜，有没有大佬要把我带去做同事？
