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

这篇具有很好参考价值的文章主要介绍了相对全面的四足机器人驱动规划MATLAB和Simulink实现方式(足端摆线规划,Hopf-CPG,Kimura-CPG)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

1. 四足机器人步态定义

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

表1. 各种步态相位关系

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

2.1 正向运动学分析

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

         其中,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为变换目标的相对坐标系,θ表示变换的转角,根据正交原则有如下的变换矩阵:

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

图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的变换矩阵推导如下:

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

         其中,

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

         式子中, 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],利用几何解析法来求解单腿的逆向运动学。

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

 四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

 

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

图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,在笛卡尔空间下对给定足端轨迹状态方程施加约束条件如下:

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

 图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')

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

 图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)+...
            2*H*(1-u/Tm+1/(n*pi)*sin(n*pi*u/Tm)).*(u>=Tm/2&u<Tm);
    else 
        y=0;
    end
end
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);
    beta=-acos(n/h_low);
    alfa_xzp=-atan(x/lyz);
    alfa_off=acos((h_mid+n)/lxzp);
    alfa=(alfa_xzp+alfa_off);
end

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

5. Hopf-CPG振荡器驱动

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

表4. Hopf-CPG配置参数

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

%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

 四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

 图15. 振荡器的输出波形

 6. Kimura-CPG振荡器驱动

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

表5. Kimura-CPG配置参数 

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

%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 

 四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

 四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

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

7.1 足端摆线规划Simulink搭建

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

图20. 复合摆线规划simulink

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink 图21. 复合摆线规划simulink2

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

  图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)+2*H*(1-u/Tm+1/(n*pi)*sin(n*pi*u/Tm)).*(u>=Tm/2&u<Tm);;
else 
    y=0;
end
% 运动学逆解函数
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);
    beta=-acos(n/h_low);
    alfa_xzp=-atan(x/lyz);
    alfa_off=acos((h_mid+n)/lxzp);
    alfa=(alfa_xzp+alfa_off);
end

7.2 Hopf-CPG Simulink搭建

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,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搭建

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,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。

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

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

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

四足机器人的抬腿高度和步长怎么设计,四足机器人,机器人,电机控制,机器人,算法,MATLAB,matlab,simulink

  图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.文章来源地址https://www.toymoban.com/news/detail-778597.html

到了这里,关于相对全面的四足机器人驱动规划MATLAB和Simulink实现方式(足端摆线规划,Hopf-CPG,Kimura-CPG)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 云深处绝影四足机器人协议学习解析

    本学习文档介绍了云深处 绝影X20 四足机器人的通信协议,并对相关的通信机制和命令格式进行了简单的解析。该协议在机器人系统和上位机(例如外部板卡或系统)之间进行TCP通信时使用。 在此协议中, 机器人本体 作为 TCP服务端 , 上位机 作为 TCP客户端 。协议的TCP服务

    2024年02月13日
    浏览(40)
  • 一款基于ESP32的迷你四足机器人

    增加自定义动作模式,可以在小程序中自定义一个最多10个步骤的动作。 附件中:带自定模式固件bin.zip esp32c3固件文件 烧录下图设置 无串口版本esp32c3开发板烧录前先按住BOOT键再插线进入烧录模式,LoadMode选择USB。 微信小程序蓝牙遥控app审核通过了,扫这个码就能用: 视频

    2024年02月04日
    浏览(52)
  • 论文阅读:四足机器人对抗运动先验学习稳健和敏捷的行走

    论文:Learning Robust and Agile Legged Locomotion Using Adversarial Motion Priors 进一步学习:AMP,baseline方法,TO 介绍了一种新颖的系统,通过使用对抗性运动先验 (AMP) 使四足机器人在复杂地形上实现稳健和敏捷的行走。主要贡献包括为机器人生成AMP数据集,并提出一种教师-学生训练框架

    2024年02月21日
    浏览(53)
  • 【全网首发开源教程】【Labview机器人仿真与控制】Labview与Solidworks多路支配关系-四足爬行机器人仿真与控制

    🎉欢迎来到Labview专栏~四足爬行机器人仿真与控制 ☆* o(≧▽≦)o *☆ 嗨 ~我是 小夏与酒 🍹 ✨ 博客主页: 小夏与酒的博客 🎈该系列 文章专栏: Labview-3D虚拟平台 文章作者技术和水平有限,如果文中出现错误,希望大家能指正🙏 📜 欢迎大家关注! ❤️ 🔸 四足机器人整

    2024年02月03日
    浏览(49)
  • 基于STM32与PCA9685制作四足机器人(代码开源)

            前言: 本文为手把手教学 基于STM32的四足机器人项目 —— JDY-31蓝牙控制 ,特别地,本次项目 采用的是 STM32 作为 MCU 。 四足机器人的支架为 3D打印件 , SG90舵机 驱动机器人实现姿态运动。借助 PCA9685舵机驱动板 实现 12路PWM波 控制, 更多的舵机 可以实现机器人

    2024年02月03日
    浏览(73)
  • 关于四足行走机器人步态分析STM32+SG90舵机控制

    最近逛某站看了很多国赛大佬的赛车,下面评论区一堆大佬在感叹“老师看到赛车都摇头。”作为一个初入STM32坑的萌新,我在某些站上查阅了相关资料后,确定给我自己的机器人搞成四足行走(ps,其实是L298N烧了)。 目前手上的板子是C8T6,学习的视频是江科协,主要代码

    2024年01月21日
    浏览(72)
  • 【标准DH法和改进DH法介绍及三自由度四足或双足机器人腿建模——MATLAB机器人工具箱使用】

    DH参数是机器人学习过程中常用的一种建模方法,通常情况下每一次坐标变换需要6个独立参数来描述坐标系i相对坐标系i-1的关系,即3个用来描述位置另外3个用来描述姿态。而DH参数法只需要4个参数,列出DH参数表并带入变换矩阵中可以轻易的得到机械手末端和基

    2024年02月02日
    浏览(64)
  • 未来的机器人:全面接管人类的工作和生活

             1972 年,波士顿一个实验室里发生了一场可怕的机器人“叛乱”。 当时,马文·明斯基是 MIT 人工智能实验室的创始人,他想让医生能远程操控机器人手臂做手术。于是,他给斯坦福大学的朋友约翰·麦卡锡打电话,借了一个研究助理维克托·沙因曼。沙因曼是个机

    2024年01月23日
    浏览(49)
  • 移动机器人农田机器人全覆盖路径规划

    鉴于目前网上对于全覆盖路径规划方面的资料比较少,本次博客内容主要分享下拖拉机在农田里面作业的路径规划,以及轨迹优化。 目录 1. 什么是全覆盖路径规划 2. 实用案例 3. 农田作业机器人 如何获取地图 如何规划出全覆盖的路径 如何确保规划出来的路径是符合车辆动力

    2024年01月25日
    浏览(56)
  • 返利机器人的实现原理:从技术到收益的全面解析

    返利机器人的实现原理:从技术到收益的全面解析 大家好,我是免费搭建查券返利机器人赚佣金就用微赚淘客系统3.0的小编,也是冬天不穿秋裤,天冷也要风度的程序猿!在电商时代,许多消费者对返利机器人并不陌生。这种自动化的工具能帮助用户在购物时获取额外的优惠

    2024年02月03日
    浏览(45)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包