嗨伙计们，我又来啦~ 上次说以后要写点儿控制的东西，思前想后纠结了好久到底要写个啥，最终决定还是先从参数辨识开始，至于结合控制理论的小demo，容我梳理一下思路再来搞吧！进入正题之前，请先允许我……
史上最通俗易懂的参数辨识教程！我还是进入正题吧！说起参数辨识，当初刚入坑的时候可算是被折磨的体无完肤，其实思路很简单，就是坑稍微多一点儿，论文都搞的太复杂，包括网上一些网友写的教程也是没有那么容易理解，所以想写个科普文，希望能让后来者更容易入门吧！先来讲下我们这次的目标，这次我们要建立一个单杆模型，然后通过参数辨识把连杆参数辨识出来！再就是文章的安排，首先我们要在webots中建立一个连杆模型，然后我们需要对它进行动力学建模；接下来我们要对动力学方程进行一个线性化，把方程写成Ax=b的形式；然后我们需要通过一个激励轨迹，去充分的激励出连杆的特性，那完成这一些，剩下的就是数据采集与处理啦，我们后续再看吧！

1. webots建模

笔者比较懒嘛，就用上次我们建的模型好了，具体参考聊聊协作机器人中柔顺控制那点事儿这篇文章(ps：上次的文章没有仔细检查，把阻抗的倒数写成了阻抗的导数，罪过罪过，希望大家能够谅解^_^) 但是这次我们为了能够得到关节力矩的反馈值，需要在上一次模型的基础上进行一个小修改，你可能会问为什么要进行修改？在webots软件中Motor节点实时力矩的采集，需要它的父节点设置物理属性，否则是得不到反馈值的，也就是说Physics的参数不能为空，而上一次的模型，为了让机器人能够浮在空中，所以关节的父节点我们是没有对此进行设置的。话不多说，直接上修改方案：
史上最通俗易懂的参数辨识教程！关于各个节点的关系图，如下图所示：
具体参数，请见文末。

2. 动力学模型怎样线性化？

先来回顾一下上次的内容，我们之前说过，一个刚体的质量分布，由质量(1个参数)、质心(3个参数)以及惯性张量(6个参数)这10个参数来决定，所以这就是我们要辨识的内容，即：
史上最通俗易懂的参数辨识教程！我们上次建的模型中，连杆在质心坐标系下的惯性张量矩阵为：
动力学方程为：
所以第一步，我们要把它线性化成Ax=b的形式，并且求出最小参数集的形式（ps：所谓的最小参数集，就是剔除这10个动力学参数中对力矩无影响或者影响较小的元素后所剩余的几个参数）。对比上面的10个参数组成的参数集，显然我们这里只能辨识出于Izz和mX相关的参数（假设质心到坐标原点的距离为X的话），所以这就构成了该模型动力学方程的最小参数集。那么就有：
史上最通俗易懂的参数辨识教程！所以我们要辨识的参数为：
式子前面的系数呢，我们通常又称其为观测矩阵，也就是：
如果是多连杆系统呢，这里的动力学方程线性化也是很大的一个坑，通常有QR分解的数值法和基于DH参数直接推导法，由于是科普文，我们就不过多介绍了，真正要用的时候再去翻文献吧老铁！

注意，这里的Izz实际上对应的是我们在webots中建立的模型的Ixx

3. 激励轨迹是个啥？

首先我们要明确激励轨迹是个啥，它的作用又是啥？其实呢，激励轨迹还是一条轨迹，我们去设计它的目的就是为了能够激发出连杆的动力学特性，这样才能去辨识出我们要得到的参数。再通俗点讲，就是我们用来采数据的实验轨迹，让机械臂去执行这条轨迹，然后得到力矩和观测矩阵的值，通过辨识方法去计算出要辨识的值。激励轨迹有很多种，其中最常用的是五次多项式轨迹和周期性轨迹，它会直接影响辨识结果。同样的，为了容易理解呢，我们就直接去执行一条正弦曲线。这条曲线的方程如下：
史上最通俗易懂的参数辨识教程！

这里要注意，在实际辨识中，对于刚性模型下，机械臂的辨识不应该有太大的速度，防止激发出机械臂关节的柔性，从而降低我们的参数辨识精度！

4. 数据采集与处理

先来说采集，通过线性化后的动力学方程，我们可以知道，需要采集的数据有：关节力矩、关节角度以及关节角加速度。在webots中，关于关节力矩的采集，请大家查看Webots文档中关于Motor节点的描述部分，需要注意的点我们在第一部分已经说过了。关节角度的采集，上次的模型中我们通过PositionSensor实现了角度值的采样反馈，而关节角加速度，这里我们采用的是先通过Gyro采集角速度，然后通过差分计算得到关节的角加速度值。在实际过程中，采集的数据我们需要进行滤波处理，一般通过一个低通滤波器将噪声剔除掉，同样的，这里为了简便，我们省略了滤波的处理。以上就是数据在webots中的一个采集工作，关于具体的实现请查看后面的代码部分。这里我们先假设采集了n组数据，也就是n个力矩值和n个观测矩阵，那么怎么去求我们估计的参数值呢？也是有好多种办法，工程上最常用的是最小二乘法和极大似然估计法，具体的区别我就不讲了，感兴趣的可以继续查阅相关文献和教程。实际上我们采集的这n组数据，构成了一个超静定的线性方程组，理想情况下，无论从哪一组数据求出的估计值应该都是相等的，而现实情况由于各种误差的存在，使得求出的值并不相等，所以我们就需要求出一个折中的值，能满足绝大多数采集的观测矩阵和观测值呈线性关系。我们这里采用的是最小二乘法辨识，假设我们现在有观测矩阵A和观测值b，那么通过最小二乘法去求待估计参数时应该这样做：
史上最通俗易懂的参数辨识教程！好，问题又来啦！很多人知道这么求，但是不知道具体应该怎么求！通过我们线性化的方程可以得知，矩阵行列数应该满足1×1=(1×2)*(2×1)，我们已经采集了n组数据，所以方程左侧应该变成了nx1，那我们怎么确定超静定方程下的观测矩阵呢，首先要明确一点，我们辨识的参数是2个，也就是2×1的一个矩阵，通过矩阵相乘的关系呢，我们就可以得知，这个超静定方程下的观测矩阵应该是一个nx2的矩阵，所以我们就应该把每次采集的1×2的观测矩阵去组合成一个nx2的大的观测矩阵！然后通过最小二乘法得到要辨识的结果，这就是整个辨识过程！下面我们来看下在webots下进行的模拟实验：关节执行过程如下：
史上最通俗易懂的参数辨识教程！执行过程中，反馈的关节数据如下：
辨识得到的结果为：

声明：笔者MATLAB已得到正版授权。

接下来我们验证一下，看看辨识的结果与我们预期的结果是否一致，从webots中读到我们的模型参数为：m=2kg，g=9.81m/( s^2)，Izz=1，l=0.5m。那么由此得到待辨识参数的理想值应为：
史上最通俗易懂的参数辨识教程！可以看到，通过仿真数据辨识出的参数基本符合我们的预期！那么接下来，我们通过辨识的参数带入观测数据，看下这个过程中的拟合情况以及误差曲线：
可以看到，误差在10e-3级，基本满足使用要求！再提一句，实际辨识过程中，我们验证的时候一般是再设计一条轨迹执行，采集实际的关节力矩，然后对比通过辨识参数计算的关节力矩来看辨识精度的。

5. 辨识Demo例程

这次我们用的是MATLAB进行的仿真，所以直接上MATLAB代码（具体的使用与python等语言没啥区别，只不过是换了个API），代码如下：

% MATLAB controller for Webots
% File:          IdentificationDemo.m
% Date:             2020-10-19
% Description:   Identification Demo
% Author:        Robert H.X.S
% License：      MIT License

% MATLAB's desktop to interact with the controller:
desktop;
keyboard;

TIME_STEP = wb_robot_get_basic_time_step();

% get and enable devices, e.g.:
motor = wb_robot_get_device('rotational_motor');
ps = wb_robot_get_device('position_sensor');
gyro = wb_robot_get_device('my_gyro');

wb_position_sensor_enable(ps, TIME_STEP);
wb_gyro_enable(gyro, TIME_STEP);
wb_motor_enable_torque_feedback(motor, TIME_STEP)

% set motor position
wb_motor_set_position(motor, 0)


tau = [];
A = [];
numFlag = 1;
time = wb_robot_get_time();
gyro_val = wb_gyro_get_values(gyro);
angular_vel = gyro_val(1);
figure(1)

% main loop:
while wb_robot_step(TIME_STEP) ~= -1
  if time >= 10
    break;
  end
  if time < 1 % 抛弃前1s的数据，防止坏数据的影响
    numFlag = 1;
  end
  lastTime = time;
  last_vel = angular_vel;
  time = wb_robot_get_time();

  % 激励轨迹的执行
  wb_motor_set_position(motor, sin(0.5*time));

  % 关节反馈数据的获取
  ps_value = wb_position_sensor_get_value(ps);
  gyro_val = wb_gyro_get_values(gyro);
  angular_vel = gyro_val(1);
  angular_acc = (angular_vel - last_vel)/(time - lastTime);

  t(numFlag) = time;
  pos(numFlag) = ps_value;
  vel(numFlag) = angular_vel;
  acc(numFlag) = angular_acc;

  % 关节力矩的获取
  tau(numFlag) = wb_motor_get_torque_feedback(motor);

  % 观测数据的整理
  b = tau';
  A(numFlag,1) = angular_acc;
  A(numFlag,2)   = -9.81 * sin(ps_value);

  numFlag = numFlag + 1;
  plot(t,pos,'r','LineWidth',2)
  hold on
  plot(t,vel,'g','LineWidth',2)
  plot(t,acc,'b','LineWidth',2)
  xlabel('time/s');
  drawnow
end

% 最小二乘法求解辨识参数
x = pinv(A' * A) * A' * b

% 结果验证
estTau = A * x;
err = estTau - b;

figure(2)
subplot(2,1,1)
plot(t,b,'g','LineWidth',2)
hold on
plot(t,estTau,'b','LineWidth',2)
xlabel('time/s');
ylabel('tau/(Nm)');

subplot(2,1,2)
plot(t,err,'r','LineWidth',2)
xlabel('time/s');
ylabel('tau_{err}/(Nm)');

小结

参数辨识流程为：①建立动力学模型；②方程线性化；③设计激励轨迹；④采集数据；⑤通过观测数据辨识参数；
动力学方程线性化的方法：直接推导法、QR数值法等；
最优激励轨迹的设计：常基于有限傅里叶级数实现，结合关节电机参数的限制，使用优化方法去优化；
参数辨识方法：最常用的有最小二乘法及其变种、极大似然法等。关于Webots与MATLAB的联合仿真，我们私下再交流，欢迎大家留言讨论！那我们今天就先讲到这儿，读万卷书也要行万里路，我是罗伯特祥，下次见！

史上最通俗易懂的参数辨识教程！