3.3 柔顺控制代码实现

        本文隶属于工程机械臂末端柔顺控制（Ros+Gazebo仿真实现）

0 前言

        之前写了一个UR5的运动学库，参考3.2 写一个UR机器人运动学库，发现该运动学库需要补充一部分内容，主要是关于雅各比矩阵的，后面能不能用到雅各比不知道，但是运动学库里面怎么能不包含雅各比矩阵呢？所以先写上去再说。

        于是乎添加了一个获取当前雅各比矩阵的函数，代码如下：

bool Kinematics::GetJacobian(const JointsDisplacement &angles, JacobianMat &mat)
{
    try
    {
        TransMat T12 = FT::rotz(angles[0]) * FT::trans(0, 0, this->d[0]) * FT::rotx(this->alpha[0]) 
                        * FT::trans(this->a[0], 0, 0);
        TransMat T23 = FT::rotz(angles[1]) * FT::trans(0, 0, this->d[1]) * FT::rotx(this->alpha[1]) 
                        * FT::trans(this->a[1], 0, 0);
        TransMat T34 = FT::rotz(angles[2]) * FT::trans(0, 0, this->d[2]) * FT::rotx(this->alpha[2]) 
                        * FT::trans(this->a[2], 0, 0);
        TransMat T45 = FT::rotz(angles[3]) * FT::trans(0, 0, this->d[3]) * FT::rotx(this->alpha[3]) 
                        * FT::trans(this->a[3], 0, 0);
        TransMat T56 = FT::rotz(angles[4]) * FT::trans(0, 0, this->d[4]) * FT::rotx(this->alpha[4]) 
                        * FT::trans(this->a[4], 0, 0);
        TransMat T67 = FT::rotz(angles[5]) * FT::trans(0, 0, this->d[5]) * FT::rotx(this->alpha[5]) 
                        * FT::trans(this->a[5], 0, 0);

        TransMat T13 = T12 * T23;
        TransMat T14 = T13 * T34;
        TransMat T15 = T14 * T45;
        TransMat T16 = T15 * T56;
        TransMat T17 = T16 * T67;

        Direction o1, o2, o3, o4, o5, o6, o7;
        o1 << 0.0, 0.0, 0.0;
        o2 << T12(0, 3), T12(1, 3), T12(2, 3);
        o3 << T13(0, 3), T13(1, 3), T13(2, 3);
        o4 << T14(0, 3), T14(1, 3), T14(2, 3);
        o5 << T15(0, 3), T15(1, 3), T15(2, 3);
        o6 << T16(0, 3), T16(1, 3), T16(2, 3);
        o7 << T17(0, 3), T17(1, 3), T17(2, 3);

        Direction z1, z2, z3, z4, z5, z6, z7;
        z1 << 0.0, 0.0, 1.0;
        z2 << T12(0, 2), T12(1, 2), T12(2, 2);
        z3 << T13(0, 2), T13(1, 2), T13(2, 2);
        z4 << T14(0, 2), T14(1, 2), T14(2, 2);
        z5 << T15(0, 2), T15(1, 2), T15(2, 2);
        z6 << T16(0, 2), T16(1, 2), T16(2, 2);
        z7 << T17(0, 2), T17(1, 2), T17(2, 2);

        auto Jv1 = z1.cross(o7 - o1);
        auto Jv2 = z2.cross(o7 - o2);
        auto Jv3 = z3.cross(o7 - o3);
        auto Jv4 = z4.cross(o7 - o4);
        auto Jv5 = z5.cross(o7 - o5);
        auto Jv6 = z6.cross(o7 - o6);

        mat << Jv1(0), Jv2(0), Jv3(0), Jv4(0), Jv5(0), Jv6(0),
            Jv1(1), Jv2(1), Jv3(1), Jv4(1), Jv5(1), Jv6(1),
            Jv1(2), Jv2(2), Jv3(2), Jv4(2), Jv5(2), Jv6(2),
            z1(0), z2(0), z3(0), z4(0), z5(0), z6(0),
            z1(1), z2(1), z3(1), z4(1), z5(1), z6(1),
            z1(2), z2(2), z3(2), z4(2), z5(2), z6(2);

        return true;
    }
    catch(...)
    {
        std::cerr << "Can not get Jacobian matrix"
                  << "\n";
        return false;
    }
}

        雅各比矩阵的具体算法清自行查阅机器人教程，这里不做过多讲解。其中，代码中使用了一些自定义的数据类型，其实都是使用Eigen库定义的一些矩阵，具体可参考一下代码：

#include 

typedef Eigen::Matrix JointsDisplacement;
typedef Eigen::Matrix JointsVelocity;
typedef Eigen::Matrix Direction;
typedef Eigen::Matrix TransMat;
typedef Eigen::Matrix RotMat;
typedef Eigen::Matrix JacobianMat;

1 代码核心算法

        柔顺控制框图如下，本文采用的是导纳控制方法。

        本质思想很简单。

        红色框里的东西不用我们实现，机器人厂商早就实现好了。为方便理解，我们姑且可以认为厂商的位置控制非常准确，我们任意发送一个期望位置，机器人的实际位置都能准确无误差地到达。

        现在问题就来了，假想一个情况，假如机器人在向我们给出的期望位置的运动路径上出现一堵坚不可摧的墙，机器人如果还按照我们给出的期望位置运动的话，势必会撞墙进而产生较大的冲击力导致机器人损坏，因此我们希望机器人受到外部的作用力的期望力为0。

        那么我们的做法就很简单，可以通过力传感器测得实际力和期望力的偏差，通过这个偏差计算出一个位置的修正量来修正我们给出的理想位置，得到一个修正后的理想位置，简而言之就是告诉机器人你已经受到外部的力了，不要再执行原来的位置指令了，我告诉你一个新的位置指令，你只要去这个位置就不会受到外部的力。

        所以代码的核心就是怎么通过力的偏差计算位置修正量。

        导纳控制应运而生。

导纳控制核心算法如下式。

        其中，如图所示，

        本质思想就是把机器人和外界的接触力当成一个弹簧阻尼模型，那么当受到外力时，就可以根据这个模型反向计算位置信息从而修正期望位置。

        由于上文假设了位置控制及其精确，因此，。因此导纳控制需要依赖于精确的位置控制，在进行导纳控制之前，必须先保证位置控制的精确。

        这个式子是连续的，而在我们编程的时候需要进行离散化，可参考基于阻抗控制的工业机器人轨迹跟踪系统 Simulink/Simscape 仿真（PS：这个链接很重要，后续代码都是根据这个写的）

2 开始写代码

        工程目录如下：

ImpedanceControl:
    bin(存放可执行文件)
    include(存放头文件)
        ImpedanceControl.h
        ImpedanceParam.h
        ThreeDimensionalVector.h
    src(存放源代码)
        ImpedanceControl.cpp
        test_ImpedanceControl.cpp
    lib(存放生成的库文件)
    build(编译生成的中间文件)
    CMakeLists.txt

2.1 头文件

        ImpedanceParam.h和ThreeDimensionalVector.h分别定义了一些自定义的数据类型，前者主要是将导纳控制控制所需的三个参数封装为了一个结构体，代码如下：

#pragma once
#include "Eigen/Core"

struct ImpedanceParam
{
    Eigen::Matrix K;
    Eigen::Matrix B;
    Eigen::Matrix M;
};

实际上就是将导纳控制的三个参数定义为了3*3的矩阵。

后者是定义了一些常用的三维空间的向量，代码如下：

#pragma once
#include "Eigen/Dense"

typedef Eigen::Matrix Force;
typedef Eigen::Matrix Displacement;
typedef Eigen::Matrix Velocity;
typedef Eigen::Matrix Acceleration;

实际上就是定义了力、位移、速度、加速度四个向量，本质上就是3*1的矩阵。

敲重点：ImpedanceControl.h

代码如下：

#pragma once
#include "ImpedanceParam.h"
#include "ThreeDimensionalVector.h"
#include 

class ImpedanceControl
{
public:
    //A struct to retain the position of the previous time
    struct Position
    {
        Displacement dis_pre;
        Velocity vel_pre;
    };
    ImpedanceControl(ImpedanceParam param);
    ImpedanceControl(const ImpedanceControl&) = delete;
    ImpedanceControl &operator=(const ImpedanceControl &) = delete;

public:
    ImpedanceParam GetImpedanceParam(void);
    void SetImpedanceParam(ImpedanceParam param);

    /*
    @brief  Compute the ideal displacement of the next point by impedance control algorithm
    @params  Dis_exp: Expected displacement
             Vel_exp: Expected velocity
             Acc_exp: Expected acceleration
             delta_force: The variation of force, expected force minus current force
             delta_t: Time interval between two points
    @retrun  The next point displacement
    */
    Displacement ComputeImpedance(const Displacement &dis_exp, const Velocity &vel_exp, const Acceleration &acc_exp, Force delta_force, double delta_t);

private:
    ImpedanceParam param;
    Position pos;
};

        主要就是一个成员变量param加上一个成员函数ComputeImpedance。还有两个成员函数就是获取和改变导纳参数的，不细讲。

        但注意，里面还有一个成员变量pos，它的类型为Position，这个结构体被定义在了这个类最开头的位置，内部保存了一个位移和一个速度信息。因为根据上文离散化中给的那个链接中的写法，我们是需要一个变量来记录上一时刻的位移和速度信息的。实际上也可以定义一个全局变量来记录，但个人认为放在类当中作为成员变量更好。

        ComputeImpedance函数：同样的，根据链接中的写法，需要传入的是期望的位移、速度和加速度以及期望的力的改变量和时间间隔，返回的实际就是上文控制框图中的。

2.2 源代码

ImpedanceControl.cpp，代码如下：

#include "ImpedanceControl.h"
#include 

ImpedanceControl::ImpedanceControl(ImpedanceParam param)
{
    this->param = param;
    this->pos.dis_pre << 0, 0, 0;
    this->pos.vel_pre << 0, 0, 0;
}

ImpedanceParam ImpedanceControl::GetImpedanceParam(void)
{
    return this->param;
}

void ImpedanceControl::SetImpedanceParam(ImpedanceParam param)
{
    this->param = param;
}

Displacement ImpedanceControl::ComputeImpedance(const Displacement &dis_exp, const Velocity &vel_exp, const Acceleration &acc_exp, Force delta_force, double delta_t)
{
    Acceleration acc_pre = acc_exp + param.M.inverse() * (-1 * delta_force + param.B * (vel_exp - pos.vel_pre) + param.K * (dis_exp - pos.dis_pre));
    Displacement dis = pos.dis_pre + pos.vel_pre * delta_t;
    Velocity vel = pos.vel_pre + acc_pre * delta_t;
    pos.vel_pre = vel;
    pos.dis_pre = dis;
    return dis;
}

上文链接中的代码复现。

注意：构造函数中需要将成员pos中的所有变量初始化为0，也也是链接中给出的写法。

test_ImpedanceControl.cpp，代码如下：

#include "ImpedanceControl.h"
#include 

int main()
{
    ImpedanceParam param;
    param.B << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    param.K << 0.05, 0, 0, 0, 0, 0, 0, 0, 0;
    param.M << 0.05, 0, 0, 0, 0.05, 0, 0, 0, 0.05;

    ImpedanceControl IC(param);

    //Simulate a force

    //The displacement of spring
    Displacement spring_dis;
    spring_dis << 1, 0.0, 0.0;

    //The current displacement of robot
    Displacement robot_dis;
    robot_dis << 0.0, 0.0, 0.0;

    //The expected displacement of robot
    Displacement robot_dis_exp;
    robot_dis_exp << 10, 0.0, 0.0;

    //The stiffness of spring
    double K = 10;

    //Compute the force from spring
    auto GetSpringForce = [&](Displacement &robot_dis) -> Force
    {
        Force F;
        F << 0, 0, 0;
        if(robot_dis.norm() > spring_dis.norm())
        {
            F = K * (robot_dis - spring_dis);
            return F;
        }
        return F;
    };

    while(true)
    {
        auto f = GetSpringForce(robot_dis);
        auto delta_f = f - Eigen::Matrix{10, 0, 0};

        //The expected vel and acc of robot
        Velocity robot_vel_exp;
        Acceleration robot_acc_exp;
        robot_vel_exp << 0.0, 0.0, 0.0;
        robot_acc_exp << 0.0, 0.0, 0.0;

        std::cout << "Force: " << f << std::endl;
        std::cout << "current dis: " << robot_dis << std::endl;
        std::cout << "--------------------" << std::endl;

        robot_dis = IC.ComputeImpedance(robot_dis_exp, robot_vel_exp, robot_acc_exp, delta_f, 0.05);
    }
    return 0;
}

        这里的测试代码要稍微复杂一点。

        具体来说就是我们模拟这样一个场景，在位置spring_dis=[1, 0, 0]处放置一根弹簧，弹簧的刚度K=10，假设有一个机器人在初始位置robot_dis=[0, 0, 0]处，机器人和弹簧之间的；相互作用力f可以通过lambda表达式GetSpringForce进行计算。我们的期望位置robot_dis_exp为[10, 0, 0]，我们期望机器人受到的外部力为10，即delta_f=f-10。

        为了简便，我们将期望速度和加速度始终设为0，这里我们同时将机器人的位置控制考虑为及其精确，即可以认为我们通过导纳方程计算出的robot_dis即是最开始图中的也是。

        那么机器人这个计算结果的最为理想的状态就是机器人一开始受力为0，期望位置为[10, 0, 0]，机器人开始向期望位置运动，即robot_dis逐渐增大，当robot_dis=1时开始受到弹簧的作用力，但还没达到我们期望的f=10，因此robot_dis继续增大，当robot_dis=2的时候，f=10，机器人停止运动，robot_dis=2维持不变。

2.3 CMakeLists.txt

比较简单，如下：

cmake_minimum_required(VERSION 3.5.0)

project(ImpedanceControl)

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

file(GLOB ${PROJECT_NAME}_SOURCES "./src/*.cpp")
file(GLOB ${PROJECT_NAME}_HEADERS "./include/*")

# Header file path
include_directories(${CMAKE_SOURCE_DIR}/include)
include_directories(/home/wujinshan/MyProject/ThirdPartyLibrary include)

add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES})

add_executable(test_ImpedanceControl
                ${${PROJECT_NAME}_SOURCES}
                ${${PROJECT_NAME}_HEADERS}
            )

3 运行测试

运行bin文件夹下的可执行文件，便有如下结果：

Force: 0
0
0
current dis: 0
0
0
--------------------
Force: 0
0
0
current dis: 0
0
0
--------------------
Force: 0
0
0
current dis: 0.525
    0
    0
--------------------
Force: 0.5
  0
  0
current dis: 1.05
   0
   0
--------------------
Force: 5.73688
      0
      0
current dis: 1.57369
      0
      0
--------------------
Force: 10.7106
      0
      0
current dis: 2.07106
      0
      0
--------------------
Force: 13.0528
      0
      0
current dis: 2.30528
      0
      0
--------------------
Force: 12.8958
      0
      0
current dis: 2.28958
      0
      0
--------------------
Force: 11.5617
      0
      0
current dis: 2.15617
      0
      0
--------------------
Force: 10.3066
      0
      0
current dis: 2.03066
      0
      0
--------------------
Force: 9.72183
      0
      0
current dis: 1.97218
      0
      0
--------------------
Force: 9.76777
      0
      0
current dis: 1.97678
      0
      0
--------------------
Force: 10.1076
      0
      0
current dis: 2.01076
      0
      0
--------------------
Force: 10.4242
      0
      0
current dis: 2.04242
      0
      0
--------------------
Force: 10.5702
      0
      0
current dis: 2.05702
      0
      0
--------------------
Force: 10.557
     0
     0
current dis: 2.0557
     0
     0
--------------------
Force: 10.4705
      0
      0
current dis: 2.04705
      0
      0
--------------------
Force: 10.3906
      0
      0
current dis: 2.03906
      0
      0
--------------------
Force: 10.3542
      0
      0
current dis: 2.03542
      0
      0
--------------------
Force: 10.3579
      0
      0
current dis: 2.03579
      0
      0
--------------------
Force: 10.3799
      0
      0
current dis: 2.03799
      0
      0
--------------------
Force: 10.4001
      0
      0
current dis: 2.04001
      0
      0
--------------------
Force: 10.4092
      0
      0
current dis: 2.04092
      0
      0
--------------------
Force: 10.4081
      0
      0
current dis: 2.04081
      0
      0
--------------------
Force: 10.4025
      0
      0
current dis: 2.04025
      0
      0
--------------------
Force: 10.3974
      0
      0
current dis: 2.03974
      0
      0

可以看到当机器人达到[2, 0, 0]位置后的数据基本都是在2前后波动，与上文的最理想结果近似，说明导纳控制是有效的。