沧浪网站建设,上海民政网站相关建设情况,手机网站如何开通微信公众号,wordpress 数据库 旧Ip许久没更新四足机器人相关的博客文章#xff0c;由于去年一整年都在干各种各样的~活#xff0c;终于把硕士毕业论文给写好#xff0c;才有点时间更新自己的所学和感悟。步态规划和足端规划只是为了在运动学层面获取四足机器人各关节的期望角位移和速度信号#xff0c;再由底… 许久没更新四足机器人相关的博客文章由于去年一整年都在干各种各样的~活终于把硕士毕业论文给写好才有点时间更新自己的所学和感悟。步态规划和足端规划只是为了在运动学层面获取四足机器人各关节的期望角位移和速度信号再由底层的关节控制器输出控制律角加速度或力矩使得期望和现实信号的偏差在容许范围内今天将来探讨下四足机器人的三种常见的驱动方式并用数值仿真和simscape仿真的方式验证所提出方法的有效性对比其优缺点。 图1. 四足机器人simscape实时交互仿真
1. 四足机器人步态定义 依据文献[8]四足动物的步态是指各个腿之间具有固定相位关系的行走模式不同的动物由于自身条件的限制如腿长、腿的位置、神经控制方式等其步态也会变得不一样。就如双足动物的“行走”、“奔跑”四足动物的行走Walk、对角小跑Trot、奔跑Gallop、溜步Pace、跳跑Bound、原地四足跳跃Pronk。以 LF(左前腿)、RF(右前腿)、RB(右后腿)、LB(左后腿)分别替代四足动物的四条腿然后可以根据其步态的特点得出它们各自的相位关系(左前腿作为参考基准φ_LF0 即相位为0一个周期为2*pi )以上步态的相位关系如表1所示。 步态是实现仿生四足机器人运动性能的基础规划步态的协调性、连续性与可调整性直接决定着仿生四足机器人运动性能的优劣。
表1. 各种步态相位关系 步态可以由特定的几个参数表征相位差φ_i 、负载因子β、步态周期T、步长S、抬腿高度h定义一个步态周期T内腿部离地的时间被称为摆动相撑地的时间称为支撑相具体参数有以下定义
表2. 机器人步态参数定义 通过对四足动物的运动观察与分析总结出了四足动物的步态描述表其中的节奏可以分为单拍、双拍、准双拍、四拍步态可以从四足动物足端拍打地面的节拍进行区分例如Trot是双拍步态Walk是四拍步态。另外负载因子步态占空比β衡量腿部与地面的接触时间与步态周期的比重有着重要的意义因为当β等于0.5时机器人在任意时刻或者平均约有两条腿支撑地面而当β大于或者等于0.5的时候说明机器人在任意时刻或者平均至少有三条腿支撑地面这对机器人的步态输出有着极其重要的作用。其中walk 步态β值为 0.75trot 和其余常规步态的β为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是透视矩阵此处元素全为0I是比例变换矩阵没有长度变化的情况下为1n[nxnynz]n[oxoyoz]n[axayaz]表示相对坐标系XYZ参考坐标系的方向余弦在R矩阵中每一步变换有三种变换方式即是分别绕X、Y、Z轴旋转相应的角度。现定A为参考坐标系B为变换目标的相对坐标系θ表示变换的转角根据正交原则有如下的变换矩阵 有了以上的机器人学建模分析基础后将图所示常见机构简化为以下图所示的机身腿节坐标分析模型。 图3.四足机器人抽象关节坐标定义 定义机身坐标如图3所示其中机身前进方向为X左右侧移方向为Y高度方向为Z假设侧摆关节之间的距离W前后髋关节之间的距离L机器人三个腿节长度L1、L2、L3。并以此为分析基点分析四足机器人单腿的正-逆向运动学分析。采用机器人学应用最广泛的建模方法DH(Denavit-Hartenberg)方法[1]重要的两个建模原则是1.对于笛卡尔坐标系第i1旋转关节的轴线方向始终指向Zi轴的正方向2.对于笛卡尔坐标系尽量把连杆的方向定为X轴的正向即是Xi坐标的轴沿着Zi与Zi-1的公垂线且指向Zi-1轴的负方向。 四足机器人机身的中心为坐标基点到左前腿LF的过渡矩阵B1的变换矩阵推导如下 其中Trans·为坐标系的平移变换矩阵公式Rot·为旋转变换公式L为机器人机体的长度W为机体的宽度。同理推导得其他三个腿的过渡变换矩阵为 定义机身的中心为坐标基点以左前腿为例依据机器人学的DH建模法给出连杆参数如下
表3. 单腿连杆DH参数定义 其中ai是沿着xi方向从zi到zi1平移的距离αi是绕着xi方向从zi到zi1转过的角度di 是沿着zi方向从xi-1到xi平移的距离θi是绕着zi 方向从xi-1到xi 转过的角度。Offseti是第i个关节的补偿值作为初始姿态角。因此机身中心到左腿足端的坐标变换矩阵定义为 其中 式子中 s1sin(θ1)c1cos(θ1)s2sin(θ2)c2cos(θ2),s3sin(θ3)c3cos(θ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_up49.0;h_mid125.5899;h_low116.020;dyzsqrt(y.^2z.^2);lyzsqrt(dyz.^2-h_up.^2);gamma_yz-atan(y./z);gamma_h_offset-atan(h_up./lyz);gammagamma_yz-gamma_h_offset;lxzpsqrt(lyz.^2x.^2);n(lxzp.^2-h_low^2-h_mid.^2)/(2*h_mid*h_low);beta-acos(n);alfa_xzp-atan(x/lyz);alfa_offacos((h_midn)/lxzp);alfa(alfa_xzpalfa_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 Truetheta(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]如波士顿动力学spotdogMIT绝影宇树、淘宝上各种无刷电机狗以及舵机狗都是采用摆线规划可见其实用性。足端摆线规划是将四足机器人行走时候的足端运动建模成一个空间中运动的摆线这样能够尽量减少足端在触地瞬间的爆发冲击更好地与地面摩擦实现机身的前进驱动。 摆线又称旋轮线、圆滚线在数学中摆线Cycloid被定义为一个圆沿一条直线运动时圆边界上一定点所形成的轨迹。它是一般旋轮线的一种。标准摆线的参数方程为 其中表示摆线圆弧的半径为滚动角。摆线的MATLAB数值仿真绘制代码如下
%Ezekiel
%2023年3月25日
%% 清理变量
clc;
clear;
close all;
n2000;
r 1;
for index 0:1:ntheta 5*pi/1000*index;x(index1) r*(theta - sin(theta));y(index1) 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/2Sgn_1 1;elseSgn_1 -1;endif Trun 0 Trun TyX r*(2*pi*Trun/Ty - sin(2*pi*Trun/Ty)); % x方向if Trun Ty/2Fe Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));Y 2*H*(Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty)));elseFe Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));Y H*(Sgn_1*(2*Fe - 1) 1);endelseX 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;
n2000;
t_total 6*pi/2;
tlinspace(0,t_total,n);%转一圈
tst_total/n;
trace_sq nant;%轨迹配置初始化空值
trace_xyz nan[t;t;t];%轨迹配置初始化空值
trace_abg nan[t;t;t];%轨迹配置初始化空值
trace_triang nant;%轨迹配置初始化空值
k0;
omega 2*pi;% 控制步态周期
siggnal_sq_intergration1 0;
siggnal_sq_intergration2 0;
S 50;
H 40;
Tm 0.5;
for jtkk1;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 u0y -1;
elsey1;
end
end
function x cycloid_x(u,Tm,S)if u0x 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)n4;if u0y2*H*(u/Tm-1/(n*pi)*sin(n*pi*u/Tm)).*(u0uTm/2)...2*H*(1-u/Tm1/(n*pi)*sin(n*pi*u/Tm)).*(uTm/2uTm);else y0;end
end
function [alfa, beta, gamma]xyztoang(x, y, z)h_up49.0;h_mid125.5899;h_low116.020;dyzsqrt(y.^2z.^2);lyzsqrt(dyz.^2-h_up.^2);gamma_yz-atan(y./z);gamma_h_offset-atan(h_up./lyz);gammagamma_yz-gamma_h_offset;lxzpsqrt(lyz.^2x.^2);n(lxzp.^2-h_low^2-h_mid.^2)/(2*h_mid);beta-acos(n/h_low);alfa_xzp-atan(x/lyz);alfa_offacos((h_midn)/lxzp);alfa(alfa_xzpalfa_off);
end图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振荡器的状态方程可以利用积分器对该微分方程进行求解: 坐标变换阵或者说是步态权重矩阵这个矩阵利用坐标变换原理使得不同腿、关节之间的信号相位可以按照步态矩阵θiji1,...,4;j1,...,4,表示腿间相位关系给定的相位关系生成。 表4. Hopf-CPG配置参数 %Ezekiel Mok
%2023年3月29日
clc;
clear;
close all
tic
%CPG构建基本参数
Alpha10;%收敛速度
leg_num4;%腿的数量
gait2;%步态选择1walk,2trot,3pace,4gallop
u1;%影响曲线幅值幅值为开根号
a50;%决定w在Wsw和Wst之间变化的速度
Psa1;%关节形式膝式-1肘式1
Wsw2*pi;%摆动相频率,影响信号周期T为2*pi/Wsw
u10; u20;%误差影响x,y的平衡位置%关节参数此处参数需实测试验数据参考书
h0.02; %抬腿高度
v1; %行走速度
T0.4; %步态周期
Sv*T; %步长
l0.4; %腿节长度
theta030/180*pi;%髋关节和膝关节平衡位置与垂直线夹角
L2*l*cos(theta0);%髋关节与足端之间长度
Ah8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度%负载系数对相关参数影响
switch gaitcase 1Beta0.75;%负载系数,walk为0.75,trot,pace,gallop为0.5影响振荡信号上升时间和下降时间Ah8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度Ak5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度case 2Beta0.5;Ah8.3;%髋关节摆动幅度Ak5.3;%膝关节摆动幅度case 3Beta0.5;Ah10;%asin((Beta*S)/(2*L));%髋关节摆动幅度Ak5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度case 4Beta0.5;Ah13.4;%asin((Beta*S)/(2*L));%髋关节摆动幅度Ak10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
endWst((1-Beta)/Beta)*Wsw;%支撑相频率Rhk(1-Beta)*[cos(pi) -sin(pi);sin(pi) cos(pi)];%髋关节和膝关节之间的耦合矩阵
%时间
Pon_num5000;
t_begin0; t_end30; t_step(t_end-t_begin)/(Pon_num-1);
tzeros(Pon_num,1);
for n1:Pon_numt(n)(n-1)*t_stept_begin;
end
%初始值非0即可
leg_xzeros(leg_num,1);
leg_yzeros(leg_num,1);
for i1:leg_numleg_x(i)0.5;leg_y(i)0.5;
end%CPG曲线左前1右前2右后3左后4
leg_h_Point_xzeros(Pon_num,leg_num);%髋关节
leg_h_Point_yzeros(Pon_num,leg_num);
leg_k_Point_xzeros(Pon_num,leg_num);%膝关节
leg_k_Point_yzeros(Pon_num,leg_num);
Foot_end_xzeros(Pon_num,leg_num);%足端
Foot_end_yzeros(Pon_num,leg_num);Phizeros(leg_num,1);
switch gaitcase 1 %walk相位Phi(1)0*2*pi; %Phi_LFPhi(2)0.5*2*pi; %Phi_RFPhi(3)0.25*2*pi; %Phi_RHPhi(4)0.75*2*pi; %Phi_LHcase 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 i1:leg_numfor j1:leg_numR_cell{j,i}[cos(Phi(i)-Phi(j)) -sin(Phi(i)-Phi(j));sin(Phi(i)-Phi(j)) cos(Phi(i)-Phi(j))];endendfor n1:Pon_numfor i1:leg_numr_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)0leg_k_Point_x(n,i)0;elseif(i3)leg_k_Point_x(n,i)-sign(Psa)*(Ak/Ah)*leg_y(i);elseleg_k_Point_x(n,i)sign(Psa)*(Ak/Ah)*leg_y(i);endendFoot_end_x(n,i)sin(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)theta0)*l-sin(theta0leg_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)*lcos(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)theta0)*l);end
end
%画图
for i1:4subplot(4,3,i2*(i-1))plot(leg_h_Point_x(:,i),leg_h_Point_y(:,i))subplot(4,3,i12*(i-1))hold onplot(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 onplot(t,leg_k_Point_x(:,1),red);grid onylabel(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 onplot(t,leg_k_Point_x(:,2),red);grid onylabel(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 onplot(t,leg_k_Point_x(:,3),red);grid onylabel(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 onplot(t,leg_k_Point_x(:,4),red);grid onylabel(LB)xlabel(时间t/s)axis([0,t_end,-1.5,1.5])%XY坐标均衡legend(thetah,thetak);toc
figure(2)
tttemp1630;
tttemp2175;
plot(t(tttemp:tttemptttemp2),leg_h_Point_x(tttemp:tttemptttemp2,1),t(tttemp:tttemptttemp2),leg_k_Point_x(tttemp:tttemptttemp2,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:tttemptttemp2,3), Foot_end_y(tttemp:tttemptttemp2,3))
grid on
xlabel(x方向位移)
ylabel(y方向位移)
figure(4)
for i1:leg_num
hold on
plot(t,leg_h_Point_x(:,i))
grid on
hold off
end
figure(5)
for i1: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_num4;%足的个数
mm8;%m项反馈信息
Tr0.04;%上升时间参数
Ta0.38;
c0.23;%直流输入
b-2.0;%适应系数
a-1.0;%抑制系数
phai1;
%%
gait2;
w_wzeros(leg_num,leg_num);%定义步态矩阵
switch(gait)case 1w_w[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];case 2w_w[0 -1 -1 1;-1 0 1 -1;-1 1 0 -1;1 -1 -1 0];case 3w_w[0 1 -1 -1;1 0 -1 -1;-1 1 0 -1;-1 -1 1 0];case 4w_w[0 -1 -1 -1;-1 0 -1 -1;-1 -1 0 -1;-1 -1 -1 0];otherwisew_w[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];
end
%%
vfzeros(leg_num,1);
vezeros(leg_num,1);
Vfe[vf,ve];
%%
yfzeros(leg_num,1);
yezeros(leg_num,1);
Yfe[yf,ye];
Yef[ye,yf];
Yzeros(leg_num,1);%输出的关节映射矩阵
%%
ufrand(leg_num,1)*c/10;
uerand(leg_num,1)*c/10;
Ufe[uf,ue];
%%
hfzeros(mm,1);
hezeros(mm,1);
Hfe[hf,he];
%%
sszeros(leg_num,mm);
WYfzeros(leg_num,1);
WYezeros(leg_num,1);
SHfzeros(leg_num,1);
SHezeros(leg_num,1);
%%
Ezeros(leg_num,2);
for i1:1:leg_numE(i,1)1;E(i,2)1;
end
%% 关节参数此处参数需实测试验数据参考书
h0.02; %抬腿高度
v1; %行走速度
T0.4; %步态周期
Sv*T; %步长
l0.4; %腿节长度
theta030/180*pi;%髋关节和膝关节平衡位置与垂直线夹角
L2*l*cos(theta0);%髋关节与足端之间长度
Ah8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak5.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;];%初值会影响相序
n2000;
t_total 8*pi/2;
tlinspace(0,t_total,n);%转一圈
tst_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 jtkk1;[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;
Judge1diff(yPlot(:,1));
Judge2diff(yPlot(:,2));
Judge3diff(yPlot(:,3));
Judge4diff(yPlot(:,4));
kplotzeros(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 j1:1:length(yPlot(:,1))-1if Judge1(j)0kplot(j,1)1*(Ak/Ah)*(1-(yPlot(j,1)/thetah(1))^2);elsekplot(j,1)0;endif Judge2(j)0kplot(j,2)1*(Ak/Ah)*(1-(yPlot(j,2)/thetah(2))^2);elsekplot(j,2)0;endif Judge3(j)0kplot(j,3)-1*(Ak/Ah)*(1-(yPlot(j,3)/thetah(3))^2);elsekplot(j,3)0;endif Judge4(j)0kplot(j,4)-1*(Ak/Ah)*(1-(yPlot(j,4)/thetah(4))^2);elsekplot(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:legnumyf(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)*(-ufb*vfa*yewyf_sumshsumc);vf_dot (1/Ta)*(yf-vf);ue_dot (1/Tr)*(-ueb*vea*yfwye_sumshsumc);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:legnumfor j_j1:1:legnumWYf(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:legnumfor j1:1:legnumshsum(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 u0x 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)
n4;
if u0y2*H*(u/Tm-1/(n*pi)*sin(n*pi*u/Tm)).*(u0uTm/2)2*H*(1-u/Tm1/(n*pi)*sin(n*pi*u/Tm)).*(uTm/2uTm);;
else y0;
end
% 运动学逆解函数
function [alfa, beta, gamma]xyztoang(x, y, z)h_up49.0;h_mid125.5899;h_low116.020;dyzsqrt(y.^2z.^2);lyzsqrt(dyz.^2-h_up.^2);gamma_yz-atan(y./z);gamma_h_offset-atan(h_up./lyz);gammagamma_yz-gamma_h_offset;lxzpsqrt(lyz.^2x.^2);n(lxzp.^2-h_low^2-h_mid.^2)/(2*h_mid);beta-acos(n/h_low);alfa_xzp-atan(x/lyz);alfa_offacos((h_midn)/lxzp);alfa(alfa_xzpalfa_off);
end
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);
Alpha10;%收敛速度
u1;%影响曲线幅值幅值为开根号
a50;%决定w在Wsw和Wst之间变化的速度
for i 1:4r_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:4Vx 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));endX_dot(i) Vx;Y_dot(i) Vy;
end7.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)*(-ufb*vfa*yewyf_sumshsumc);vf_dot (1/Ta)*(yf-vf);ue_dot (1/Tr)*(-ueb*vea*yfwye_sumshsumc);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.
