先上代码：https://github.com/lsk-gith/robot_impedance_control

初衷：

研三马上毕业，2019年入学，当年12月份就开始闹疫情，很多事情都当误了，不过自己以后也不从事这个行业了，并且现在论文也写完了，就把知识点总结一下，以帮助更多的人吧。

动力学模型：

标准动力学模型

M C G项都是我们所熟知的，其求法近期会上传。

阻抗原理：

弹簧阻尼模型这个我们都知道，所以也就不在啰嗦。

弹簧阻尼模型

替代公式

来替代X，毕竟是做轨迹跟踪，下标d是理想位姿，位姿速度和位姿加速度。

阻抗和导纳：

阻抗控制中有人会把它分为阻抗和导纳，其实这些都是阻抗，只不过是实现方式不同而已，一个是基于力矩的一个是基于位置的。

基于力矩的：

控制率

在基于力矩的阻抗控制中，一般通过关节位置编码器、转速表和加速度计用来采集得到关节角度角速度和角加速度变化，与规划的理想轨迹做差得出

差值

代入到阻抗模中，将这部分阻抗模型产生的力矩转换到关节空间的力矩就可以了，不需要末端安装六维力矩传感器。

基于位置的：

推导公式

其中Fext就是六维力矩传感器采集得到的力，基于位置控制的不需要动力学模型，这一点也是很多人喜欢的，毕竟动力学模型的MCG系数很难求，一般需要CAD导出，若要更精确的需要参数辨识等。

怎么在机械臂上实现：

这里不介绍基于关节空间的（单个关节实现阻抗模型，这个很简单不在叙述），全文所述都是基于笛卡尔空间的，目前已经知道了阻抗模型，那么问题来了怎么把阻抗模型使用到机械臂末端的位姿X上呢，以及机械臂末端位姿的△X，△Xd和△Xdd怎么计算？。

我们知道在三自由度以及一下的机械臂中，末端都只是位置变化，不涉及姿态变化，所以X，Xd，Xdd的求解就很简单，毕竟末端就只是xyz三个量表示，对其求导就是Xd，再次求导就是Xdd。

但是对于六轴甚至七轴的就不这么幸运了，末端是由位置和姿态组成，位置的计算还是可以遵循以上规则，但是姿态就不可以了，首先我们知道在一般的机器人运动学计算中末端姿态都是使用旋转矩阵计算的，由9个量组成，我们总不能将旋转矩阵求个导来计算Xd和Xdd吧，当然这是不现实的。

姿态最常用的表示方法就是欧拉角（具体定义，可以wiki或者百度），总的来说就是使用三个变量来表示一个唯一的旋转矩阵，其中还使用了正交矩阵的凯莱公式，推导如下：

正交矩阵的凯莱公式

下面以ZXZ的经典欧拉角为例介绍：

欧拉角按照旋转轴分为经典欧拉角（Proper Euler Angle）和泰特布莱恩角（Tait–Bryan angles），共 12种旋转方式：经典欧拉角-Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)使用两个轴的旋转角度表示，第一个旋转角度和第三个旋转角度都是绕同一个轴。泰特-布莱恩角-Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)使用三个轴的旋转角度表示。

绕X轴Y轴Z轴旋转计算公式：

旋转公式

ZXZ顺序的旋转矩阵R计算

其中红框里的矩阵很重要也是我们在计算分析雅各比矩阵时特别重要的部分。

其实计算红框里矩阵不必这么麻烦，在peter所编写的机器人工具箱中eul2jac()和rpy2jac()函数有关于该矩阵的计算，这里就班门弄斧了一下：

clear
clc
syms phi theta psi phid thetad psid A real
syms phit(t) thetat(t) psit(t) 
order = 'zxz';
R = eul2r_my(phi, theta, psi, order)
Rt = eul2r_my(phit, thetat, psit, order);
dRdt = diff(Rt, t);
dRdt = subs(dRdt, {diff(phit(t),t), diff(thetat(t),t), diff(psit(t),t),}, {phid,thetad,psid});
dRdt = subs(dRdt, {phit(t),thetat(t),psit(t)}, {phi,theta,psi});
dRdt = formula(dRdt);
w = vex(dRdt * R');
w = simplify(w);
rpyd = [phid thetad psid];

    for i=1:3
        for j=1:3
            C = coeffs(w(i), rpyd(j));
            if length(C) == 1
                A(i,j) = 0;
            else
                A(i,j) = C(2);
            end
        end
    end
A

function rotmat = rotz(gamma)
rotmat = [cos(gamma) -sin(gamma) 0; sin(gamma) cos(gamma) 0; 0 0 1]
end
function rotmat = roty(beta)
rotmat = [cos(beta) 0 sin(beta); 0 1 0; -sin(beta) 0 cos(beta)]
end
function rotmat = rotx(alpha)
rotmat = [1 0 0;0 cos(alpha) -sin(alpha); 0 sin(alpha) cos(alpha)]
end

clear
clc
syms phi theta psi phid thetad psid A real
syms phit(t) thetat(t) psit(t) 
order = 'zxz';
R = eul2r_my(phi, theta, psi, order)
Rt = eul2r_my(phit, thetat, psit, order);
dRdt = diff(Rt, t);
dRdt = subs(dRdt, {diff(phit(t),t), diff(thetat(t),t), diff(psit(t),t),}, {phid,thetad,psid});
dRdt = subs(dRdt, {phit(t),thetat(t),psit(t)}, {phi,theta,psi});
dRdt = formula(dRdt);
w = vex(dRdt * R');
w = simplify(w);
rpyd = [phid thetad psid];

    for i=1:3
        for j=1:3
            C = coeffs(w(i), rpyd(j));
            if length(C) == 1
                A(i,j) = 0;
            else
                A(i,j) = C(2);
            end
        end
    end
A

function rotmat = rotz(gamma)
rotmat = [cos(gamma) -sin(gamma) 0; sin(gamma) cos(gamma) 0; 0 0 1]
end
function rotmat = roty(beta)
rotmat = [cos(beta) 0 sin(beta); 0 1 0; -sin(beta) 0 cos(beta)]
end
function rotmat = rotx(alpha)
rotmat = [1 0 0;0 cos(alpha) -sin(alpha); 0 sin(alpha) cos(alpha)]
end

计算出来的转换矩阵：

欧拉角转换矩阵

这样就可以计算分析雅各比矩阵来计算基于欧拉角的Xd了

分析雅各比计算：

几何雅各比计算：

几何雅各比为矩阵为6X6的矩阵，分为两部分前三行代表位置，后三行代表姿态。
对于雅各比矩阵的计算：

方法一：

方法一

方法二：

使用peter机器人工具箱jacobian()函数，不在详细讲解。

分析雅各比计算：

将几何雅各比的两部分分为：

几何雅各比矩阵

分析雅各比计算

其中R(ZXZ)就是我们上面计算的红框矩阵。

计算X，Xd，Xdd:

X 计算：
X的位置部分使用P

计算X中姿态部分

旋转部分使用上述计算的。

计算Xd，Xdd

dJ_ana两部分分开求，前三行和dJ_o求解一样，后三行是使用做差的形式，J_anaold（下三行）-J_ananew（下三行）组合一下就是dJ_ana

基于位置的阻抗控制实现：

代码地址：https://github.com/lsk-gith/robot_impedance_control

仿真代码：

使用simscape仿真

程序

上图中蓝色模块子程序

上图左上角模块子程序

先看仿真结果：

基于位置的阻抗控制仿真

图中红色线条是实际运行轨迹，蓝色时理论轨迹。出现位置偏离是因为末端施加了一个外力，这个外力只维持了一段时间就撤销了。

位姿变化图

关节角度变化图

讨论其他表示位姿的方法：

欧拉角只是表示末端姿态的方法之一，可以表示末端姿态的方法有很多，我们先看看其他机械臂厂家如何表示末端姿态吧：目前常见的机械臂公司例如史陶比（Staubli）公司使用绕XYZ的泰特布莱恩角，爱得普（Adept）使用绕ZYZ的欧拉角，库卡（KUKA）使用绕ZYX的泰特布莱恩角，发那科（Fanuc）与安川（Motoman）则使用绕XYZ的泰特布莱恩角，ABB机器人使用四元数表达旋转，优傲（UR）机器人使用轴角。

四元数：

我们先来观摩Franka机器人使用的四元数代码（https://frankaemika.github.io/libfranka/cartesian_impedance_control_8cpp-example.html）。

Franka基于力矩的笛卡尔空间阻抗

四元数

性质

四元数误差求解

轴角：（定义自行百度或者Wiki）

轴角误差计算

轴角误差计算

基于力矩的，

基于力矩

我所理解的基于力矩的阻抗控制思路和上述图片中描述的一致，

说明：基于力矩的仿真，实现的效果并不好，不知道是simscape仿真模型设置问题还是其他的，就是用力矩控制的时候，没发让机械臂跑一个空间圆轨迹，但这里还是将代码贴出来，以供他人参考，也真诚希望某些大佬能够完善这个。

这里需要设置关节的质量转动惯量，

设置关节惯性参数

首先采集力矩用来替代动力学模型的力矩，虽然和动力学模型实时计算的还有差异，但是可以勉强替换，将这个采集的力矩作为阻抗控制器中的机械臂动力学模型的补偿部分。

力矩采集

首先将simscape模型设置成力矩输入，并且将传感器设置成可以采集角度角速度和角加速度模式

关节设置

整体搭建的模型

模型

但以上模型在仿真的时候会出现问题，猜测是simscape仿真模型设置问题，但博主现在没有解决，这个只是把代码分析，仅供参考。

以上只是个人见解，如有错误也请多多保函，也不必联系我修改，毕竟之后不从事这个。

补充一下雅各比矩阵导数（Janadot）的求解方法，以及P导数（Pdot ）的求解方法，：

%% load data
syms q_vec q1 q2 q3 q4 q5 q6 g real;
syms t t1(t) t2(t) t3(t) t4(t) t5(t) t6(t);
syms q1dot q2dot q3dot q4dot q5dot q6dot real;
syms t1dot(t) t2dot(t) t3dot(t) t4dot(t) t5dot(t) t6dot(t);
syms q1dotdot q2dotdot q3dotdot q4dotdot q5dotdot q6dotdot real;
syms t1dotdot(t) t2dotdot(t) t3dotdot(t) t4dotdot(t) t5dotdot(t) t6dotdot(t);
q_vec = [q1 q2 q3 q4 q5 q6].';
t_vec = [t1 t2 t3 t4 t5 t6].';
tdot_vec = [diff(t1(t),t) diff(t2(t),t) diff(t3(t),t) diff(t4(t),t) diff(t5(t),t) diff(t6(t),t)].';
tdotdot_vec = [diff(t1dot(t),t) diff(t2dot(t),t) diff(t3dot(t),t) diff(t4dot(t),t) diff(t5dot(t),t) diff(t6dot(t),t)].';
tdotsh_vec = [t1dot(t) t2dot(t) t3dot(t) t4dot(t) t5dot(t) t6dot(t)].';
tdotdotsh_vec = [t1dotdot(t) t2dotdot(t) t3dotdot(t) t4dotdot(t) t5dotdot(t) t6dotdot(t)].';
qdot_vec = [q1dot q2dot q3dot q4dot q5dot q6dot].';
qdotdot_vec = [q1dotdot q2dotdot q3dotdot q4dotdot q5dotdot q6dotdot].';
load(('.\InverseDynamic\MCG.mat'));
load(('.\kinematic\PR.mat'));
%% Forward Kinematics
P_fin = P; %Extracting only the x-y-z position from T matrix 
P_fint = subs(P_fin,q_vec,t_vec); % substitute time dependence 
Pdottemp = diff(P_fint,t); % Pdot differentiate w.r.t. time
Pdot = subs(Pdottemp,[t_vec,tdot_vec],[q_vec,qdot_vec]); % substitute time independence
R0tip = R;% rotation matrix of end point
% Initialize the robot at some position as home [0.5 0.5 0.2 0.2 0.2 0.2]';
% qHome = [pi/2.5 pi/6 1.5 1.2 pi/5 pi/7]';

load('.\Green2dai\data.mat');
thetalist = data(1:10:end,1:6);%N*6
dthetalist = data(1:10:end,7:12);
ddthetalist = data(1:10:end,13:18);
qHome = thetalist(1,:)';
% Calculate numerical transformation matrix
T_fin=[[R;0,0,0],[P;1]];
matlabFunction(T_fin,'File','transion');
THome = subs(T_fin,q_vec,qHome);
%% Geometric and Analytical Jacobian Calculation Calculate the first three rows of Jacobian (common to Analytical and Geometric Jacobians) J3 = jacobian(P_fin,q_vec)'Janasym','Janadot','Jgeom'
J3=J(1:3,1:6);
% omega = last three rows of geometric Jacobian as defined by axis of rotation with respect to the base frame.
omega1 = double(subs(J,q_vec,qHome));
omega=omega1(4:6,1:6);
% Combine to form the Geometric Jacobian Jgeom = [J3;omega];
Jgeom = J;
Jge = J;
matlabFunction(Jge,'File','Jge');
% Calculate rpy Angles from Transformation Matrix using Peter Corke's tr2eul function
eul = tr2rpy(THome);% 
% roll-pitch-yaw, i am lazy, so i dont want to change the name!!!!
phi = eul(1);theta = eul(2);xi = eul(3);
% EUL2JAC Euler angle rate Jacobian, this can reference the eul2jac.m function.
om2eulmat = [cos(theta)*cos(xi), -sin(xi), 0;cos(theta)*sin(xi),  cos(xi), 0;-sin(theta),       0, 1];
euldot = om2eulmat\omega;
% Symbolic Analytical Jacobian
Janasym = [J3;euldot];
% Symbolic First derivative of Analytical Jacobian
Janadot = subs(diff(subs(Janasym,q_vec,t_vec),t),[t_vec,tdot_vec],[q_vec,qdot_vec]);
Jdot = subs(diff(subs(J,q_vec,t_vec),t),[t_vec,tdot_vec],[q_vec,qdot_vec]);
JanadotUp3 = Jdot(1:3,:);
matlabFunction(JanadotUp3,'File','JanadotUp3');
%% Initialize variables for iterations
j=0;
% iter = 0:dt:time;
dt = 0.02;
iter = size(thetalist,1);
qnew=qHome;
q=qnew;
% qdotnew = [0 0 0 0 0 0]';
qdotnew = dthetalist(1,:)';
euldot_old = euldot;
% traj_des_theta = [qHome(1).*ones(size(iter,2),1),qHome(2).*ones(size(iter,2),1),qHome(3).*ones(size(iter,2),1),qHome(4).*ones(size(iter,2),1),qHome(5).*ones(size(iter,2),1),qHome(6).*ones(size(iter,2),1)];
traj_des_theta = thetalist;
traj_des_XYZeul = geteul(traj_des_theta);
%test 
Janaold = double(subs(Janasym,q_vec,thetalist(1,:)'));
[Xdd, Xd, X] = getDesiredX(thetalist(2,:)',dthetalist(2,:)',ddthetalist(2,:)',Janaold);
Janaold_e = double(subs(Janasym, q_vec, thetalist(1,:)'));
%% Initialize parameters for iterations
% Desired Mass of end effector
Md = 1*eye(6); 
% Desired spring of end effector
Kp = 5*eye(6);
% Desired Damping coefficient of end effector
Kd = 3*eye(6);
% End effector initial force vector
he = [0 0 0 0 0 0]';
% Initial end effector actual cartesian position
xe = [THome(1:3,4);phi;theta;xi];
% Initial end effector desired cartesian acceleration. ddx = dJ_ana * dq + J_ana * ddq
xddoubled = [0 0 0 0 0 0]';
% Initial end effector desired cartesian velocity.dx = J_ana * dq 
xddot = [0 0 0 0 0 0]';
% Initial end effector actual cartesian velocity
xedot = [subs(Pdot,[q_vec,qdot_vec],[qnew,qdotnew]);0;0;0];
% Initial difference in end effector desired and actual cartesian velocities
xtildedot = xddot - xedot;    
%% Pre-allocating size of matrices for simulation iterations
save('vars_for_cb','iter','M','C','G','R0tip','Janasym','Janadot','Jgeom','Md','Kp','Kd','P_fin','q_vec','qdot_vec','xe','xddoubled','xtildedot','j','qnew','qdotnew','g','omega','euldot_old','Pdot','xddot','J','Janaold_e','dt');
% F_ext =1.* [5,-6,5,0,0,0].';
F_ext =2.* [2,-4,-3,0,0,0].';
% F_ext =1.* [0,0,0,0,0,0].';
[F,xehist,q_record,dq_record,ddq_record] = control_law(F_ext,traj_des_XYZeul,thetalist,dthetalist,ddthetalist);
figure(1)
subplot(2,1,1);
plot(F,'DisplayName','F','LineWidth',1.2)
ylabel('F');
xlabel('time');
title('F(ext)');
subplot(2,1,2);
plot(q_record,'DisplayName','q_record','LineWidth',1.2);
ylabel('theta');
xlabel('time');
title('theta');
figure(2)
subplot(2,1,1);
plot(F,'DisplayName','F','LineWidth',1.2)
ylabel('F');
xlabel('time');
title('F(ext)');
subplot(2,1,2);
plot(xehist,'DisplayName','xehist','LineWidth',1.2)
ylabel('X');
xlabel('time');
title('X');
figure(3)
subplot(2,1,1);
plot(q_record-thetalist,'DisplayName','xehist','LineWidth',1.2)
subplot(2,1,2);
plot(xehist-traj_des_XYZeul,'DisplayName','xehist','LineWidth',1.2)
% figure(4);
% plot3(traj_des_XYZeul(1:end,1),traj_des_XYZeul(1:end,2),traj_des_XYZeul(1:end,3),'b');
% for i = 1 : size(xehist,1)
%     hold on
%     plot3(xehist(i,1),xehist(i,2),xehist(i,3),'-o','Color','r','MarkerSize',10,'MarkerFaceColor','#D9FFFF');
%     pause(0.001);
% end
figure(5);
plot3(traj_des_XYZeul(1:end,1),traj_des_XYZeul(1:end,2),traj_des_XYZeul(1:end,3),'b');
hold on
plot3(xehist(1:end,1),xehist(1:end,2),xehist(1:end,3),'Color','r');

figure(4);
plot3(traj_des_XYZeul(1:end,1),traj_des_XYZeul(1:end,2),traj_des_XYZeul(1:end,3),'b');
n = size(xehist,1) / 10;
for i = 1 : n
    hold on
    plot3(xehist(10 * i,1),xehist(10 * i,2),xehist(10 * i,3),'-o','Color','r','MarkerSize',10,'MarkerFaceColor','#D9FFFF');
    pause(0.001);
end