相对全面的四足机器人驱动规划MATLAB和Simulink实现方式（足端摆线规划，Hopf-CPG，Kimura-CPG）

        许久没更新四足机器人相关的博客文章，由于去年一整年都在干各种各样的~活，终于把硕士毕业论文给写好，才有点时间更新自己的所学和感悟。步态规划和足端规划只是为了在运动学层面获取四足机器人各关节的期望角位移和速度信号，再由底层的关节控制器输出控制律（角加速度或力矩）使得期望和现实信号的偏差在容许范围内，今天将来探讨下四足机器人的三种常见的驱动方式，并用数值仿真和simscape仿真的方式验证所提出方法的有效性，对比其优缺点。

 图1. 四足机器人simscape实时交互仿真

1. 四足机器人步态定义

        依据文献[8]，四足动物的步态是指各个腿之间具有固定相位关系的行走模式，不同的动物由于自身条件的限制，如腿长、腿的位置、神经控制方式等，其步态也会变得不一样。就如双足动物的“行走”、“奔跑”，四足动物的行走（Walk）、对角小跑（Trot）、奔跑（Gallop）、溜步（Pace）、跳跑（Bound）、原地四足跳跃（Pronk）。以 LF(左前腿)、RF(右前腿)、RB(右后腿)、LB(左后腿)分别替代四足动物的四条腿，然后可以根据其步态的特点得出它们各自的相位关系(左前腿作为参考基准，φ_LF=0  ，即相位为0，一个周期为2*pi )，以上步态的相位关系如表1所示。 步态是实现仿生四足机器人运动性能的基础规划，步态的协调性、连续性与可调整性直接决定着仿生四足机器人运动性能的优劣。

表1. 各种步态相位关系

        步态可以由特定的几个参数表征，相位差φ_i 、负载因子β、步态周期T、步长S、抬腿高度h，定义一个步态周期T内，腿部离地的时间被称为摆动相，撑地的时间称为支撑相，具体参数有以下定义：

表2. 机器人步态参数定义

        通过对四足动物的运动观察与分析，总结出了四足动物的步态描述表，其中的节奏可以分为单拍、双拍、准双拍、四拍步态，可以从四足动物足端拍打地面的节拍进行区分，例如Trot是双拍步态，Walk是四拍步态。另外，负载因子（步态占空比）β衡量腿部与地面的接触时间与步态周期的比重，有着重要的意义，因为当β等于0.5时，机器人在任意时刻或者平均约有两条腿支撑地面，而当β大于或者等于0.5的时候，说明机器人在任意时刻或者平均至少有三条腿支撑地面，这对机器人的步态输出有着极其重要的作用。其中，walk 步态β值为 0.75，trot 和其余常规步态的β为0.5。

图1.四足动物行走步态示意

        如图1所示，四足机器人起步时，左前腿LF抬起后以一定的足端轨迹向前跨步，待LF落地后紧跟着右后腿RB跨腿，之后再轮到RF、LB进行相同的动作，行走的过程中，该动作重复循环，步态周期较长并且在走动的过程始终保持三条腿在支撑身体。而小跑步态，先是LF与RB同时抬起并同步抬腿运动，当LF与RB重新落到地面时候，RF与LB跨腿，重复循环，为身体保持平稳，步态周期较短，在小跑过程中的任意时刻都只有对角腿在支撑身体，具体的相位拓扑关系分析见我的另一个文章EzekielMok。

2. 单腿正-逆向运动学计算

        要实现四足机器人的精准控制，需获取精确的关节角度空间下的角位移-角速度-角加速度与笛卡尔坐标下足端的位置-速度-加速度之间的映射关系，不同腿部结构的四足机器人的映射关系不一样，如有些是串联式的多关节机械臂，某些是并联式的机械臂，它们都可依据机器人学[1]的建模方法求解分析。

图2.四足机器人机身坐标定义

2.1 正向运动学分析

        根据机器人学[1]中的连杆上坐标系建模原则进行坐标变换，可以推导出一个包含姿态信息与位置信息的齐次矩阵

         其中，R为旋转矩阵，表征位姿变换；P为位置矩阵，表征位置变换；O是透视矩阵，此处元素全为0；I是比例变换矩阵，没有长度变化的情况下为1；n=[nx，ny，nz]，n=[ox，oy，oz]，n=[ax，ay，az]表示相对坐标系X，Y，Z参考坐标系的方向余弦；在R矩阵中每一步变换，有三种变换方式，即是分别绕X、Y、Z轴旋转相应的角度。现定A为参考坐标系，B为变换目标的相对坐标系，θ表示变换的转角，根据正交原则有如下的变换矩阵：

        有了以上的机器人学建模分析基础后，将图所示常见机构简化为以下图所示的机身腿节坐标分析模型。

图3.四足机器人抽象关节坐标定义 

       定义机身坐标如图3所示，其中机身前进方向为X，左右侧移方向为Y，高度方向为Z，假设侧摆关节之间的距离W，前后髋关节之间的距离L，机器人三个腿节长度L1、L2、L3。并以此为分析基点，分析四足机器人单腿的正-逆向运动学分析。采用机器人学应用最广泛的建模方法DH(Denavit-Hartenberg)方法[1]，重要的两个建模原则是：1.对于笛卡尔坐标系，第i+1旋转关节的轴线方向始终指向Zi轴的正方向；2.对于笛卡尔坐标系，尽量把连杆的方向定为X轴的正向，即是Xi坐标的轴沿着Zi与Zi-1的公垂线，且指向Zi-1轴的负方向。

        四足机器人机身的中心为坐标基点到左前腿LF的过渡矩阵B1的变换矩阵推导如下：

         其中，Trans（·）为坐标系的平移变换矩阵公式，Rot（·）为旋转变换公式，L为机器人机体的长度，W为机体的宽度。同理推导得其他三个腿的过渡变换矩阵为

         定义机身的中心为坐标基点，以左前腿为例，依据机器人学的DH建模法给出连杆参数如下：

表3. 单腿连杆DH参数定义

       其中，ai是沿着xi方向，从zi到zi+1平移的距离；αi是绕着xi方向，从zi到zi+1转过的角度；di 是沿着zi方向，从xi-1到xi平移的距离，θi是绕着zi 方向，从xi-1到xi 转过的角度。Offseti是第i个关节的补偿值，作为初始姿态角。因此，机身中心到左腿足端的坐标变换矩阵定义为

         其中，

         式子中， s1=sin(θ1)，c1=cos(θ1)，s2=sin(θ2)，c2=cos(θ2),s3=sin(θ3)，c3=cos(θ3)。

         单腿正向向运动学计算代码（此处从B1坐标系转换至足端，机身中心至B1加个矩阵就行）：

function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)% 
%输出角度为弧度
% du_trans = 180/pi;
rad_trans = pi/180;
alpha = alpha_du*rad_trans;
beta  = beta_du*rad_trans;
gama  = gama_du*rad_trans;
T_01 = [[cos(alpha) -sin(alpha) 0 0];
         [sin(alpha) cos(alpha) 0 0];
         [0          0          1 0];
         [0          0          0 1]];
T_12 = [[cos(beta) -sin(beta)   0 0];
       [0          0            0 L1];
       [-sin(beta) -cos(beta)   1 0];
       [0          0            0 1]];
   
T_23 = [[cos(gama) -sin(gama)   0 L2];
       [sin(gama)   cos(gama)   0 0];
       [0           0           1 0];
       [0           0           0 1]];
T_34 = [[1  0       0        L3];
       [0   1       0        0];
       [0   0       1        0];
       [0   0       0        1]];
%% 正向运动学矩阵
T = T_01*T_12*T_23*T_34;%% 可以通过替换参数直接求取结果
R = T(1:3,1:3);
P = T(1:3,4);
end

        至此，正向运动学求解完毕，可实现关节空间到笛卡尔空间的映射计算。 

 2.2 逆向运动学分析

         逆向运动学分析为了获取笛卡尔空间至关节空间的映射关系，可以通过期望的足端轨迹给定期望的关节角位移、角速度和角加速度期望控制量，是做四足机器人行为和稳定性控制的重要基础，具体方法包括几何求解法和解析解法。 以下将参考文献[2][3]，利用几何解析法来求解单腿的逆向运动学。

图4.四足机器人侧视和前视图

         如图4所示，θ1是侧摆角，绕X轴旋转，最大旋转角度约束为20度，初始转角为0度（初始站立姿态的转角）；θ2是髋关节转角，最大旋转关节角度约束为 35度（我自己simscape仿真里给定的，根据自己具体工况设计），初始转角为45度；θ3为膝关节转角，最大旋转关节角度约束为 45度，初始转角为135度。        

        给定足端的期望空间位置（以机身坐标原点为起始坐标），以及连杆长度，机体长度和宽度（以腿节计算），以右手定则（图5）判定各坐标的方位和关节旋转角的正负值，利用几何法求解θ1，θ2，θ3。

图5.坐标正负方向定义右手定则（逆时针为正方向）

        首先求θ1，几何关系如图6所示，简单明了，用小学初中三角函数知识便可求解，需要注意的是actan(·)的求解范围要定义好，根据上述的坐标图示可知，此处的y和z坐标均为负，由右手定则可规定θ1为负。给定足端相对侧摆关节坐标的参考坐标，根据图中的几何关系，会有以下等式：

图6. 单腿几何关系分析图示

 

 

        其中，为和的正视投影长度，与末端坐标的具体值相关。

        再者求θ3，如图7所示，腿节关系可以抽象为一个三角形，依据坐标设定，足端x方向的坐标假设为正值，根据余弦定理就可以直接求取θ2如下

        最后求θ2，也依赖于上述的抽象三角形关系，θ2由θ2a和θ2b两部分构成，根据正切三角关系和余弦定理可以直接求取θ2如下

图7. 单腿几何关系分析图示 

        单腿逆向运动学计算代码：

%代码实现方式1
function [alfa, beta, gamma]=xyztoang(x, y, z)
    h_up=49.0;
    h_mid=125.5899;
    h_low=116.0+20;
    dyz=sqrt(y.^2+z.^2);
    lyz=sqrt(dyz.^2-h_up.^2);
    gamma_yz=-atan(y./z);
    gamma_h_offset=-atan(h_up./lyz);
    gamma=gamma_yz-gamma_h_offset;
    lxzp=sqrt(lyz.^2+x.^2);
    n=(lxzp.^2-h_low^2-h_mid.^2)/(2*h_mid*h_low);
    beta=-acos(n);
    alfa_xzp=-atan(x/lyz);
    alfa_off=acos((h_mid+n)/lxzp);
    alfa=(alfa_xzp+alfa_off);
end
%代码实现方式2
function [theta] = Kine_inv(L1,L2,L3,P)% 
theta = zeros(3,1);
du_trans = 180/pi;
rad_trans = pi/180;
True = 1;
phai = atan2(P(2),P(1));%求角度
rlo = sqrt(P(1).^2 + P(2).^2);
rlo1 = sqrt(P(1).^2 + P(2).^2 - L1.^2);
if True
    theta(1) = phai - atan2(L1, rlo1);
    theta(1) = theta(1)*du_trans;
end
theta(3) = -acos((P(1).^2 + P(2).^2 + P(3).^2 - L1.^2 - L2.^2 - L3.^2)/(2*L2*L3));
theta(3) = theta(3)*du_trans;
a = P(1)*cos(theta(1)*rad_trans) + P(2)*sin(theta(1)*rad_trans);
b = -P(3);
rlo2 = sqrt(a.^2 + b.^2);
theta(2) = pi-asin((L3.^2 - L2.^2 - a.^2 - b.^2)/(-2*L2*rlo2)) - asin(a/rlo2);
theta(2) = theta(2)*du_trans;
end

         至此，运动学逆解求解完毕。

3. 行走足端轨迹约束条件

        通用引用四足机器人相关的文献和资料[2][3][4][7]，要实现足式机器人理想的步态行走，其足端的空间轨迹需要满足：

• 1. 行进平稳和协调，无明显的上下波动、左右摇晃和前后冲击；
• 2. 各关节在摆动相抬腿和落地瞬间无较大冲突，可以无冲击抬腿和平滑落地；
• 3. 摆动相跨腿迅速，足端轨迹平滑，关节速度和加速度平滑连续无畸点；
• 4. 避免足端与地面发生滑动和拖地现象。

         为了满足上述的四个要求，假定此时（为便于分析，与上述机身的整体坐标不一样，整体的坐标）的蹬腿前进方向为X，抬腿高度方向为Y，在笛卡尔空间下对给定足端轨迹状态方程施加约束条件如下：

 图8. 机器人足端坐标定义

 

        其中，分别是X方向和Y方向的笛卡尔坐标位置，为速度，为加速度，S为抬腿，步长H为抬腿高度。

4.足端复合摆线规划

        足端摆线规划是当前四足机器人最常见的驱动方式[2][4]，如波士顿动力学spotdog，MIT，绝影，宇树、淘宝上各种无刷电机狗以及舵机狗都是采用摆线规划，可见其实用性。足端摆线规划是将四足机器人行走时候的足端运动建模成一个空间中运动的摆线，这样能够尽量减少足端在触地瞬间的爆发冲击，更好地与地面摩擦，实现机身的前进驱动。

        摆线，又称旋轮线、圆滚线，在数学中，摆线（Cycloid）被定义为，一个圆沿一条直线运动时，圆边界上一定点所形成的轨迹。它是一般旋轮线的一种。标准摆线的参数方程为

         其中，表示摆线圆弧的半径，为滚动角。摆线的MATLAB数值仿真绘制代码如下：

%Ezekiel
%2023年3月25日
%% 清理变量
clc;
clear;
close all;
n=2000;
r = 1;
for index = 0:1:n
    theta = 5*pi/1000*index;
    x(index+1) = r*(theta - sin(theta));
    y(index+1) = r*(1 - cos(theta));
end
plot (x, y,'-r','linewidth',1);
axis equal
grid on
xlabel('X')
ylabel('Y')

 图9. 摆线数值仿真示意 

         单纯的摆线能够很好地满足足端轨迹状态方程的约束条件，但是与地面接触时候存在着滑动和行为时候出现拖地的现象，因此根据文献[11]对摆线规划进行针对性设计，为复合摆线规划，其参数方程定义为

        其中，S为抬腿步长，H为抬腿高度，  为摆动相时间。具体的MATLAB代码实现如下

function [X, Y] = Gait_cal(S_0,H,Ty,Tst,index1,Point_num_Tt)
%% 改进的复合摆线规划
r = S_0/(2*pi); % 摆线的半径
Tt = Ty + Tst; % 计算周期长度，摆动相和支撑相
Tsa = Tt/(Point_num_Tt-1);%计算所处周期序列
n = 4;
%% 摆动相与支撑相分别规划
% 取模运算
    Trun = (index1 - 1)*Tsa;%时刻 
    if Trun >= 0 && Trun < Ty/2
        Sgn_1 = 1;
    else
        Sgn_1 = -1;
    end
    if Trun >= 0 && Trun < Ty
        X = r*(2*pi*Trun/Ty - sin(2*pi*Trun/Ty)); % x方向
        if Trun  < Ty/2
            Fe = Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));
            Y = 2*H*(Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty)));
        else
            Fe = Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));
            Y = H*(Sgn_1*(2*Fe - 1) + 1);
        end
    else
        X = 2*pi*r - r*(2*pi*(Trun - Ty)/(Tt - Ty) - sin(2*pi*(Trun - Ty)/(Tt - Ty))); % x方向
        Y =0;
    end 
end

        为了便于周期性输入控制接入，利用周期性的方波信号控制摆线的频率（周期），简单来说，就是利用方波的输入控制四足机器人的抬腿和落脚时间，可以通过占空比的形式直接控制摆动相和支撑相的比例，进而控制实现四足机器人各种不同的步态，具体的调制原理可见代码和simulink实现环节。

%Ezekiel
%2023年3月25日
%% 清理变量
clc;
clear;
close all;
n=2000;
t_total = 6*pi/2;
t=linspace(0,t_total,n);%转一圈
ts=t_total/n;
trace_sq = nan+t;%轨迹配置初始化，空值
trace_xyz = nan+[t;t;t];%轨迹配置初始化，空值
trace_abg= nan+[t;t;t];%轨迹配置初始化，空值
trace_triang = nan+t;%轨迹配置初始化，空值
k=0;
omega = 2*pi;% 控制步态周期
siggnal_sq_intergration1 = 0;
siggnal_sq_intergration2 = 0;
S = 50;
H = 40;
Tm = 0.5;
for j=t
    k=k+1;
    singnal_sq1_1 =0.5*square(omega*j)+0.5;
    singnal_sq1_2 =0.5*square(omega*j/2)+0.5;
    singnal_sq2_1 =0.5*square(2*omega*j)+0.5;
    singnal_sq2_2 =0.5*square(omega*j)+0.5;
    siggnal_sq_intergration1 = siggnal_sq_intergration1 + ts*logic_clock(singnal_sq1_1);
    singnal_triang1 = siggnal_sq_intergration1*logic_clock(singnal_sq1_2);
    siggnal_sq_intergration2 = siggnal_sq_intergration2 + ts*logic_clock(singnal_sq2_1);
    singnal_triang2 = siggnal_sq_intergration2*logic_clock(singnal_sq2_2);
    trace_triang(k)=  siggnal_sq_intergration1;
    x_3d = cycloid_x(singnal_triang1,Tm,S);
    z_3d = 150-cycloid_y(singnal_triang2,Tm,H);
    y_3d = 49;
    [alfa_dg, beta_dg, gamma_dg]=xyztoang( x_3d, y_3d,  z_3d);
    trace_xyz(:, k)= [x_3d;y_3d;z_3d];
    trace_abg(:, k)= [alfa_dg; beta_dg; gamma_dg];
    trace_sq(k)= singnal_sq1_1;
end
subplot(4,1,1)
plot(t,trace_sq(:),'red')
% title('Kimura振荡器输出')
ylabel('pulse')
axis([0,t_total ,-0.5,1.5])%XY坐标均衡
grid on;
subplot(4,1,2)
plot(t,trace_triang(:),'blue')
ylabel('trangpulse')
axis([0,t_total ,0,1])%XY坐标均衡
grid on;
subplot(4,1,3)
plot(trace_xyz(1, :),trace_xyz(3, :),'red')
ylabel('XY-plot')
axis([0,50 ,100,160])%XY坐标均衡
grid on;
subplot(4,1,4)
plot(t, trace_abg(1, :), 'blue', t, trace_abg(2, :), 'red',t, trace_abg(3, :), 'green')
ylabel('theta')
axis([0,t_total ,-3,2])%XY坐标均衡
grid on;
function y = logic_clock(u)
if u==0
    y = -1;
else
    y=1;
end
end
function x = cycloid_x(u,Tm,S)
    if u>0
        x =S*(u/Tm-1/(2*pi)*sin(2*pi*u/Tm));
    else 
        u=-u;
         x =S*(u/Tm-1/(2*pi)*sin(2*pi*u/Tm));
    end
end
function y = cycloid_y(u,Tm,H)
    n=4;
    if u>0
        y=2*H*(u/Tm-1/(n*pi)*sin(n*pi*u/Tm)).*(u>=0&u=Tm/2&u

 图10. 摆线规划数值仿真示意  

        至此，摆线规划设计完毕。

5. Hopf-CPG振荡器驱动

        生物的节律步态[7]是生物神经节律控制机理产生的一种自激振荡、相位互锁的运动模式，由生物低级神经中枢的中枢模式发生器 ( Central pattern Generator, CPG)产生的节律信号控制。常见的四足动物都是进行节律运动的。节律运动[1]是指空间和时间对称的周期性运动，譬如走、跑、跳等。因此，要实现四足机器人的稳定运动，就需要控制器能够产生具有周期性、对称性的节律信号，来控制各个关节(执行器)实现机体的节律运动。这种控制方式就是(CPG)中枢神经控制模式。
CPG是节律运动的中心控制器，不仅需要产生节律信号，控制执行器进行运动，还需要根据反馈信号进行识别，及时修改生成新的信号，使四足机器人能够稳定行走。

     根据第三节中行走足端轨迹约束条件，设计足间相位关系如下（参考文献[7][8][9]）

        其中， 为髋关节的角位移状态，为膝关节位移状态，为髋关节幅值，膝关节幅值，为控制腿节布置关系的标志位。其中，和的具体取值可以依据步长和抬腿高度，利用第2.2节中所设计的运动学逆解关系求解，如抬腿高度是H，步长为S，逆向运动学求出最大的关节摆动幅度即可。

       根据前两节足间与足内的拓扑关系可以构建出四足机器人的分层拓扑关系图（注：一般图论中箭头表示信号传递，但此处箭头表征指向方向与起始点腿的相位差）

 图11. 各腿关节的相位拓扑关系

        作为仿生机器人控制方式，CPG模型可以划分为两种，第一种是基于神经元的模型，十分贴切生物，但是模型复杂，如Kimura[5][6]；第二种是基于非线性振荡器的模型[10]，模型相对比较简单，模型运用相当成熟。数学上，常用的非线性振荡器有Hopf、Rayleigh、Matsuoka等振荡器，其中Hopf振荡器是一种谐波振荡器，结构简单，控制实现容易，因此本文采用 Hopf 振荡器来建立。

        单个Hopf振荡器的数学表达式：

        根据上节给定的拓扑关系构建的8路（髋关节4路、膝关节4路）Hopf振荡器的状态方程，可以利用积分器对该微分方程进行求解:

        坐标变换阵或者说是步态权重矩阵，这个矩阵利用坐标变换原理，使得不同腿、关节之间的信号相位可以按照步态矩阵θij（i=1,...,4;j=1,...,4,表示腿间相位关系）给定的相位关系生成。 

表4. Hopf-CPG配置参数

%Ezekiel Mok
%2023年3月29日
clc;
clear;
close all
tic
%CPG构建基本参数
Alpha=10;%收敛速度
leg_num=4;%腿的数量
gait=2;%步态选择，1walk,2trot,3pace,4gallop
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
Psa=1;%关节形式，膝式-1，肘式1
Wsw=2*pi;%摆动相频率,影响信号周期，T为2*pi/Wsw
u1=0; u2=0;%误差，影响x,y的平衡位置

%关节参数，此处参数需实测，试验数据参考书
h=0.02;  %抬腿高度
v=1;   %行走速度
T=0.4; %步态周期
S=v*T; %步长
l=0.4;   %腿节长度
theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 
L=2*l*cos(theta0);%髋关节与足端之间长度
Ah=8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度

%负载系数对相关参数影响
switch gait
    case 1
        Beta=0.75;%负载系数,walk为0.75,trot,pace,gallop为0.5，影响振荡信号上升时间和下降时间
        Ah=8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 2
        Beta=0.5;
        Ah=8.3;%髋关节摆动幅度
        Ak=5.3;%膝关节摆动幅度
    case 3
        Beta=0.5;
        Ah=10;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 4
        Beta=0.5;
        Ah=13.4;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
end
        Wst=((1-Beta)/Beta)*Wsw;%支撑相频率
        Rhk=(1-Beta)*[cos(pi) -sin(pi);sin(pi) cos(pi)];%髋关节和膝关节之间的耦合矩阵
%时间
Pon_num=5000;
t_begin=0; t_end=30; t_step=(t_end-t_begin)/(Pon_num-1);
t=zeros(Pon_num,1);
for  n=1:Pon_num
     t(n)=(n-1)*t_step+t_begin;
end
%初始值，非0即可
leg_x=zeros(leg_num,1);
leg_y=zeros(leg_num,1);
for i=1:leg_num
   leg_x(i)=0.5;
   leg_y(i)=0.5;
end

%CPG曲线，左前1，右前2，右后3，左后4
leg_h_Point_x=zeros(Pon_num,leg_num);%髋关节
leg_h_Point_y=zeros(Pon_num,leg_num);
leg_k_Point_x=zeros(Pon_num,leg_num);%膝关节
leg_k_Point_y=zeros(Pon_num,leg_num);
Foot_end_x=zeros(Pon_num,leg_num);%足端
Foot_end_y=zeros(Pon_num,leg_num);
  
Phi=zeros(leg_num,1);
switch gait
    case 1   
        %walk相位
        Phi(1)=0*2*pi;    %Phi_LF
        Phi(2)=0.5*2*pi;  %Phi_RF
        Phi(3)=0.25*2*pi; %Phi_RH
        Phi(4)=0.75*2*pi; %Phi_LH
   case 2
        %trot相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0*2*pi;
        Phi(4)=0.5*2*pi;
   case 3
        %pace相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0*2*pi;
   case 4
        %gallop相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0.5*2*pi;    
end 

    %相对相位矩阵
    R_cell={leg_num,leg_num};
    for i=1:leg_num
        for j=1:leg_num
            R_cell{j,i}=[cos(Phi(i)-Phi(j)) -sin(Phi(i)-Phi(j));sin(Phi(i)-Phi(j)) cos(Phi(i)-Phi(j))];
        end
    end

for  n=1:Pon_num
       for i=1:leg_num
        r_seq=(leg_x(i)-u1)^2+(leg_y(i)-u2)^2;
        W=(Wst/(exp(-a*leg_y(i))+1))+(Wsw/(exp(a*leg_y(i))+1));
        V=[Alpha*(u-r_seq) -W;W Alpha*(u-r_seq)]*[leg_x(i)-u1;leg_y(i)-u2]+R_cell{1,i}*[leg_x(1)-u1;leg_y(1)-u1]+R_cell{2,i}*[leg_x(2)-u2;leg_y(2)-u2]+R_cell{3,i}*[leg_x(3)-u1;leg_y(3)-u2]+R_cell{4,i}*[leg_x(4)-u1;leg_y(4)-u2];
        leg_x(i)=leg_x(i)+V(1,1)*t_step;
        leg_y(i)=leg_y(i)+V(2,1)*t_step;
        leg_h_Point_x(n,i)=leg_x(i);
        leg_h_Point_y(n,i)=leg_y(i);
        if leg_y(i)>0
            leg_k_Point_x(n,i)=0;
        else
            if(i<3)
                 leg_k_Point_x(n,i)=-sign(Psa)*(Ak/Ah)*leg_y(i);
            else
                leg_k_Point_x(n,i)=sign(Psa)*(Ak/Ah)*leg_y(i);
            end
        end
        Foot_end_x(n,i)=sin(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l-sin(theta0+leg_h_Point_x(n,i)/sqrt(u)*Ah*(pi/180))*l;
        Foot_end_y(n,i)=L-(cos(leg_h_Point_x(n,i)/sqrt(u)*Ah*(pi/180)+theta0)*l+cos(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l);
      end
end  
%画图
for i=1:4
    subplot(4,3,i+2*(i-1))
    plot(leg_h_Point_x(:,i),leg_h_Point_y(:,i))
    subplot(4,3,i+1+2*(i-1))
    hold on
    plot(t,leg_h_Point_x(:,i))
    plot(t,leg_k_Point_x(:,i))
    hold off
end
hold on
subplot(4,1,1)
    plot(t,leg_h_Point_x(:,1),'b');
    hold on
    plot(t,leg_k_Point_x(:,1),'red');
    grid on
    ylabel('LF')
    axis([0,t_end,-1.5,1.5])%XY坐标均衡
    %title('Hopf振荡器的输出');
    legend('thetah','thetak');
 grid on;
subplot(4,1,2)  
    plot(t,leg_h_Point_x(:,2),'b');
    hold on
    plot(t,leg_k_Point_x(:,2),'red');
    grid on
    ylabel('RF')
    axis([0,t_end,-1.5,1.5])%XY坐标均衡
        legend('thetah','thetak');
subplot(4,1,3)
    plot(t,leg_h_Point_x(:,3),'b');
    hold on
    plot(t,leg_k_Point_x(:,3),'red');
    grid on
    ylabel('RB')
    axis([0,t_end,-1.5,1.5])%XY坐标均衡
        legend('thetah','thetak');
subplot(4,1,4)
    plot(t,leg_h_Point_x(:,4),'b');
    hold on
    plot(t,leg_k_Point_x(:,4),'red');
    grid on
    ylabel('LB')
    xlabel('时间t/s')
    axis([0,t_end,-1.5,1.5])%XY坐标均衡
        legend('thetah','thetak');
    toc
figure(2)
tttemp=1630;
tttemp2=175;
plot(t(tttemp:tttemp+tttemp2),leg_h_Point_x(tttemp:tttemp+tttemp2,1),t(tttemp:tttemp+tttemp2),leg_k_Point_x(tttemp:tttemp+tttemp2,1));
grid on
xlabel('时间t/s')
ylabel('一个周期的信号')
axis([9.5,11,-1.5,1.5])%XY坐标均衡
    legend('thetah','thetak');
figure(3)
plot(Foot_end_x(tttemp:tttemp+tttemp2,3), Foot_end_y(tttemp:tttemp+tttemp2,3))
grid on
xlabel('x方向位移')
ylabel('y方向位移')
figure(4)
for i=1:leg_num
hold on
plot(t,leg_h_Point_x(:,i))
grid on
hold off
end
figure(5)
for i=1:leg_num
hold on
plot(t,leg_h_Point_y(:,i))
grid on
hold off
end

 

 图12. 各腿关节的角度变化曲线

  图13. 某腿关节的单个周期的角度变化曲线

 图14. 某腿关节的单个周期的足端轨迹曲线 

 图15. 振荡器的输出波形

 6. Kimura-CPG振荡器驱动

       Kimura神经振荡器的生物学意义明显，在控制层面模仿神经细胞突触连接中的神经兴奋和抑制控制，具有较强的理论研究意义。图X是Kimura神经振荡器的原理图[5][6]，Kimura振荡器原作者与张秀丽教授的论文中均有呈现，图中连接方式为控制论的搭建形式，白色圆圈代表抑制（负效应），白色圆圈代表兴奋（正效应）。

图16. Kimura振荡器的物理含义示意图[5] 

        该相位的拓扑关系利用权重矩阵的形式表征腿间的相位关系，具体的步态权重矩阵取法可以参考文献[5][6][7]。

图17 Kimura振荡器的腿节相位关系拓扑[5]  

         式中，i、e、j代表第i个振荡器、第e个伸肌屈肌神经元和第j屈肌个神经元；Tr表示兴奋的上升时间常数、Ta代表自抑制的适应时间常数；ui为神经元内部的兴奋状态；vi是神经元的自抑制状态；a是神经元间的互相抑制系数；b是适应系数；yfi、yei分别表示屈肌与伸肌神经元的输出；wij代表i与j振荡器之间的连接权值；sik为反射系数；c为高层神经中枢的直流激励输入；yi为第i个神经元的输出。

        给定模型参数 ，状态方程积分可求得期望控制信号，足间协调的方式同HopfCPG中的设计， 再次强调的是，和的具体取值可以依据步长和抬腿高度，利用第2.2节中所设计的运动学逆解关系求解，如抬腿高度是H，步长为S，逆向运动学求出最大的关节摆动幅度即可。具体参数给定如下

表5. Kimura-CPG配置参数 

%Ezekiel
%2023年3月25日
%% 清理变量
% clc;
% clear;
% close all;
%% 参数设置
leg_num=4;%足的个数
mm=8;%m项反馈信息
Tr=0.04;%上升时间参数
Ta=0.38;
c=0.23;%直流输入
b=-2.0;%适应系数
a=-1.0;%抑制系数
phai=1;
%%
gait=2;
w_w=zeros(leg_num,leg_num);%定义步态矩阵
switch(gait)
    case 1
       w_w=[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];
    case 2
        w_w=[0 -1 -1 1;-1 0 1 -1;-1 1 0 -1;1 -1 -1 0];
    case 3
        w_w=[0 1 -1 -1;1 0 -1 -1;-1 1 0 -1;-1 -1 1 0];
    case 4
        w_w=[0 -1 -1 -1;-1 0 -1 -1;-1 -1 0 -1;-1 -1 -1 0];
    otherwise
        w_w=[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];   
end
%%
vf=zeros(leg_num,1);
ve=zeros(leg_num,1);
Vfe=[vf,ve];
%%
yf=zeros(leg_num,1);
ye=zeros(leg_num,1);
Yfe=[yf,ye];
Yef=[ye,yf];
Y=zeros(leg_num,1);%输出的关节映射矩阵
%%
uf=rand(leg_num,1)*c/10;
ue=rand(leg_num,1)*c/10;
Ufe=[uf,ue];
%%
hf=zeros(mm,1);
he=zeros(mm,1);
Hfe=[hf,he];
%%
ss=zeros(leg_num,mm);
WYf=zeros(leg_num,1);
WYe=zeros(leg_num,1);
SHf=zeros(leg_num,1);
SHe=zeros(leg_num,1);
%%
E=zeros(leg_num,2);
for i=1:1:leg_num
    E(i,1)=1;
    E(i,2)=1;
end
%% 关节参数，此处参数需实测，试验数据参考书
h=0.02;  %抬腿高度
v=1;   %行走速度
T=0.4; %步态周期
S=v*T; %步长
l=0.4;   %腿节长度
theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 
L=2*l*cos(theta0);%髋关节与足端之间长度
Ah=8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
%% 时间，分辨率设置
%对时间进行算法积分
u0=[0.009 0.0021 0.0134 0.0022;
       0.0272 0.0003 0.0206 0.0057;
       0.0199 0.0067 0.0171 0.0111;
       0.0022 0.0151 0.0149 0.0081;];%初值会影响相序
n=2000;
t_total = 8*pi/2;
t=linspace(0,t_total,n);%转一圈
ts=t_total/n;
% 积分计算
 uf = u0(:,1);
 vf = u0(:,2);
 ue = u0(:,3);
 ve = u0(:,4);
 shsum = zeros(leg_num,1);
 k = 0;
 trace_Y_Y = nan+[t;t;t;t];%轨迹配置初始化，空值
for j=t
    k=k+1;
    [WYf, WYe] = wye_sumeq(w_w, leg_num,  yf, ye);
    [uf_dot, vf_dot,ue_dot, ve_dot] = kimura_stateq(Tr, Ta, uf, vf, ue, ve, yf, ye,a, b, c,WYf, WYe, shsum);
    uf =uf + ts.*uf_dot;
    vf =vf + ts.*vf_dot;
    ue =ue + ts.*ue_dot;
    ve =ve + ts.*ve_dot;
    Y_Y = uf - ue;
    trace_Y_Y(:,k)= Y_Y ;
    [yf, ye] = returnyfe(uf, ue, leg_num);
end
yPlot = trace_Y_Y';
Judge1=diff(yPlot(:,1));
Judge2=diff(yPlot(:,2));
Judge3=diff(yPlot(:,3));
Judge4=diff(yPlot(:,4));
kplot=zeros(length(yPlot(:,1)),4);
thetah(1)=max(yPlot(:,1));
thetah(2)=max(yPlot(:,2));
thetah(3)=max(yPlot(:,3));
thetah(4)=max(yPlot(:,4));
for j=1:1:length(yPlot(:,1))-1
    if Judge1(j)>0
       kplot(j,1)=1*(Ak/Ah)*(1-(yPlot(j,1)/thetah(1))^2);
    else
       kplot(j,1)=0;
    end
    if Judge2(j)>0
       kplot(j,2)=1*(Ak/Ah)*(1-(yPlot(j,2)/thetah(2))^2);
    else
       kplot(j,2)=0;
    end
    if Judge3(j)>0
       kplot(j,3)=-1*(Ak/Ah)*(1-(yPlot(j,3)/thetah(3))^2);
    else
       kplot(j,3)=0;
    end
    if Judge4(j)>0
       kplot(j,4)=-1*(Ak/Ah)*(1-(yPlot(j,4)/thetah(4))^2);
    else
       kplot(j,4)=0;
    end
end
figure(2)
subplot(4,1,1)
plot(t,yPlot(:,1),t,kplot(:,1),'green')
% title('Kimura振荡器输出')
ylabel('LF')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,2)
plot(t,yPlot(:,2),t,kplot(:,2),'green')
ylabel('RF')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,3)
plot(t,yPlot(:,3),t,kplot(:,3),'green')
ylabel('RB')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,4)
plot(t,yPlot(:,4),t,kplot(:,4),'green')
xlabel('时间（t/s）')
ylabel('LB')
axis([0,10,-2,2])%XY坐标均衡
grid on;
function [yf, ye] = returnyfe(uf, ue,legnum)
    yf = zeros(legnum,1);
    ye= zeros(legnum,1);
    for i= 1:1:legnum
        yf(i) = max(uf(i), 0);
        ye(i) = max(ue(i), 0);
    end
end
function [uf_dot, vf_dot,ue_dot, ve_dot] = kimura_stateq(Tr, Ta, uf, vf, ue, ve, yf, ye,a, b, c,wyf_sum, wye_sum, shsum)
    uf_dot =(1/Tr)*(-uf+b*vf+a*ye+wyf_sum+shsum+c);
    vf_dot = (1/Ta)*(yf-vf);
    ue_dot = (1/Tr)*(-ue+b*ve+a*yf+wye_sum+shsum+c);
    ve_dot =  (1/Ta)*(ye-ve);
end
function [WYf, WYe] = wye_sumeq(w, legnum,  yfaugment, yeaugment)
WYf= zeros(legnum,1);
WYe= zeros(legnum,1);
for i_i =1:1:legnum
    for j_j=1:1:legnum
        WYf(i_i)=WYf(i_i)+w(i_i, j_j)*yfaugment( j_j);
        WYe(i_i)=WYe(i_i)+w(i_i, j_j)*yeaugment( j_j);
    end
end
end
function shsum = shsumeq(s, legnum, haugment)
shsum = zeros(legnum,1);
for i =1:1:legnum
    for j=1:1:legnum
        shsum(i)=shsum(i)+s(i,j)*haugment(j);
    end
end
end 

 

图18. Kimura振荡器的各腿节角度变化曲线拓扑

 

图19. Kimura振荡器的某腿节足端轨迹曲线拓扑 

 7. 三种驱动方式的Simulink搭建

        数值仿真阶段已经实现各驱动方式的信号输出，在simulink的实时仿真环境里，只需把数值仿真中的循环改成时钟信号或方波信号激励输入，把关键运算更改为Matlab-function或S-function即可。注：simulink的搭建思路见数值仿真，函数封装和信号关系均有体现。

7.1 足端摆线规划Simulink搭建

图20. 复合摆线规划simulink

 图21. 复合摆线规划simulink2

  图22. 复合摆线规划simulink2仿真的足端曲线

        核心环节的函数如下： 

% 函数1
function x = fcn(u,Tm,S)

if u>0
    x =S*(u/Tm-1/(2*pi)*sin(2*pi*u/Tm));
else 
    u=-u;
     x =S*(u/Tm-1/(2*pi)*sin(2*pi*u/Tm));
end
% 函数2
function y = fcn(u,S,Tm,H)
n=4;
if u>0
    y=2*H*(u/Tm-1/(n*pi)*sin(n*pi*u/Tm)).*(u>=0&u=Tm/2&u

7.2 Hopf-CPG Simulink搭建

 图23. Hopf-CPG Simulink仿真

        核心积分环节的函数如下： 

function [X_dot,Y_dot] = fcn(Wsw,Wst,Phi,leg_x,leg_y,u1,u2)
%#codegen
X_dot = zeros(1,4);
Y_dot = zeros(1,4);
Alpha=10;%收敛速度
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
for i = 1:4
   r_seq = (leg_x(i)-u1(i))^2+(leg_y(i)-u2(i))^2;
   W = (Wst/(exp(-a*leg_y(i))+1))+(Wsw/(exp(a*leg_y(i))+1));
   Vx = Alpha * (u - r_seq)* (leg_x(i) - u1(i))  -  W * (leg_y(i) - u2(i));
   Vy = Alpha * (u - r_seq) * (leg_y(i) - u2(i)) + W * (leg_x(i) - u1(i));
   for j = 1:4
   Vx = Vx + cos(Phi(i)-Phi(j))*(leg_x(j) - u1(i)) - sin(Phi(i)-Phi(j))*(leg_y(j) - u2(i));
   Vy = Vy + cos(Phi(i)-Phi(j))*(leg_y(j) - u2(i)) + sin(Phi(i)-Phi(j))*(leg_x(j) - u1(i));
   end
   X_dot(i) = Vx;
   Y_dot(i) = Vy;
end

7.2 Kimura-CPG Simulink搭建

 图24. Kimura-CPG Simulink仿真示意

        关键积分环节的函数如下：

function [uf_dot, vf_dot,ue_dot, ve_dot] = kimura_stateq(Tr,...
              Ta, uf, vf, ue, ve, yf, ye,a, b, c,wyf_sum, wye_sum, shsum)
    uf_dot = zeros(4, 1);
    ue_dot = zeros(4, 1);
    vf_dot = zeros(4, 1);
    ve_dot = zeros(4, 1);
    uf_dot =(1/Tr)*(-uf+b*vf+a*ye+wyf_sum+shsum+c);
    vf_dot = (1/Ta)*(yf-vf);
    ue_dot = (1/Tr)*(-ue+b*ve+a*yf+wye_sum+shsum+c);
    ve_dot =  (1/Ta)*(ye-ve);
end

8. simscape四足机器人仿真验证

        Simscape模型参考CSDN（四足机器人Simscape搭建）博客构建，单腿simscape模型和整机可视化模型示意如图24与图25。

 图24. 单腿  Simscape模型simulink建模示意

  图25. 整机  Simscape模型可视化示意

         经过的引入关节角位移和角速度期望信号至各关节的PID控制器中，三种步态规划方法均能实现机器人的多种步态行走，转弯设计，原地踏步以及阻抗控制器我之后有空再更新（画个饼先）。 

  图26. 四足机器人simcape模型实时仿真示意

9. 总结

        这三种方法的优缺点如下：复合摆线规划能很好地结合运动学逆解输出关节角位移和角速度的期望信号，十分实用且简单，很好地添加力反馈或姿态反馈控制，缺点是仿生学意义不突出。而CPG模型的优点是仿生学意义突出，可以不需要阶跃信号输入调制信号周期，能够通过在状态方程模型加入反馈项实现姿态反馈和力反馈，缺点是模型复杂，不能直接结合运动学逆解，在应用到工程上时，参数和初值对输出的性能影响较大，调参困难。

        感谢各位的仔细观看，我之后还会做一些强化学习和模型预测控制相关的四足机器人稳定控制策略，以及四足机器人关节阻抗控制器（正在学）设计相关的文章（之前阶段都是用步态规划给出期望的关节角位移和角速度，底层用PI控制去跟踪期望信号），假如有疑惑和问题的朋友，可以联系我，一起探讨交流。 

        在过去的一段日子里，我深刻体会到陈奕迅《苦瓜》歌词里面的“珍惜淡定的心境，苦过后更加清，万般过去亦无味但有领会留下”，生活的本质似乎是痛苦和挣扎，只有当逃离舒适区去体会某些艰辛才能领会苦后的甘甜。人生将迎来下个全新阶段，我将如往常一样义无反顾，秉承“拒绝平庸，敢于不同，有梦最美”的信仰，于雨夜中拥抱着星火，挣扎着前行，同时警醒自己别忘了来时的路。 

10. 参考文献

[1]Spong M W, Hutchinson S, Vidyasagar M. Robot modeling and control[M]. New York: Wiley, 2006.

[2] Bledt G, Powell M J, Katz B, et al. Mit cheetah 3: Design and control of a robust, dynamic quadruped robot[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 2245-2252.

[3] Di Carlo J. Software and control design for the MIT Cheetah quadruped robots[D]. Massachusetts Institute of Technology, 2020.

[4] Seo K, Chung S J, Slotine J J E. CPG-based control of a turtle-like underwater vehicle[J]. Autonomous Robots, 2010, 28: 247-269.

[5] Kimura H, Fukuoka Y, Cohen A H. Biologically inspired adaptive walking of a quadruped robot[J]. Philosophical Transactions of the Royal Society A: Mathematical, Physical and Engineering Sciences, 2007, 365(1850): 153-170.

[6] Kimura H, Fukuoka Y, Nakamura H. Biologically inspired adaptive dynamic walking of the quadruped on irregular terrain[A]. Robotics Research[C].MIT press,2000,9,260-278.

[7] 张秀丽，郑浩峻，段广洪，动物运动控制机理对机器人控制的启示[J].机器人技术与应用,2002(5):24-26.

[8] 李华师.四足机器人仿生运动控制理论与方法的研究[D].北京:北京理工大学,2014:57-69

[9] 张秀丽. 四足机器人节律运动及环境适应性的生物控制研究[D].北京:清华大学,2004.

[10] 徐海东，干苏，任杰等.基于Hopf振荡器的四足机器人步态CPG调节[J].2017,12,29(12)1-6.

[11] 黄照翔,颉潭成,徐彦伟,王亚锋.基于Simulink/SimMechanics的四足机器人足端轨迹规划及动态仿真分析[J].制造业自动化,2022,44(04):77-82.