四足机器人摆线规划程序

一、标准摆线公式

{ x = r ∗ ( θ − sin ⁡ ( θ ) ) y = r ∗ ( 1 − cos ⁡ ( θ ) ) \left\{\begin{array}{l} x=r *(\theta-\sin (\theta)) \\ y=r *(1-\cos (\theta)) \end{array}\right. {x=r∗(θ−sin(θ))y=r∗(1−cos(θ))​
这里的r表示摆线的圆的半径， θ \theta θ是圆的半径所经过的弧度（滚动角）。

for index = 0:1:1000theta = 2*pi/1000*indexx(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')

二、摆动相的摆线公式

{ x ( t ) = S 0 2 π ( 2 π t T y − sin ⁡ ( 2 π t T y ) ) y ( t ) = S 0 2 π ( 1 − cos ⁡ ( 2 π t T y ) ) \left\{\begin{array}{c} x(t)&=&\frac{S_{0}}{2 \pi}\left(2 \pi \frac{t}{T_{y}}-\sin \left(2 \pi \frac{t}{T_{y}}\right)\right) \\ y(t) &=&\frac{S_{0}}{2 \pi}\left(1-\cos \left(2 \pi \frac{t}{T_{y}}\right)\right) \end{array}\right. ⎩ ⎨ ⎧​x(t)y(t)​==​2πS0​​(2πTy​t​−sin(2πTy​t​))2πS0​​(1−cos(2πTy​t​))​
此处的 T y T_y Ty​表示摆动相时间，t是

三、足端轨迹约束

z ( t ) = a t + b sin ⁡ ( 2 π 1 2 T y t ) + c z(t)=a t+b \sin \left(\frac{2 \pi}{\frac{1}{2} T_{y}} t\right)+c z(t)=at+bsin(21​Ty​2π​t)+c

四、摆动相完整描述

$$

五、步态周期摆线规划完整描述

% clc;
% clear;
close all
%% 改进的摆线规划
S_0 = 0.4; % 步长给定150mm
H = 0.2;
r = S_0/(2*pi); % 摆线的半径
Ty = 2;      %运行时间2s
Tst = 2;
Tt = Ty + Tst; % 
Point_num_Tt = 1000;% 这些点是一个步态周期的点数
Point_num = 8*Point_num_Tt;% 这些点是一个步态周期的点数Point_num_dv = round((Ty/Tt)*Point_num);
Tsa = Tt/(Point_num_Tt-1);
LF = zeros(Point_num,2);
RF = zeros(Point_num,2);
LB = zeros(Point_num,2);
RB = zeros(Point_num,2);
Trun = zeros(Point_num_Tt,1);
T = zeros(Point_num,1);
n = 4;
Sgn_1 = 0;
T_gait = [0, 0.25, 0.5, 0.75;0,  0.5, 0.5, 0    ];%步态相位矩阵
gait_mode = 2; % 1表示walk，2表示trot
%% 运动学部分
du_trans = 180/pi;
rad_trans = pi/180;
True = 1;
Flase = 0;
% syms L1 L2 L3 alpha beta gama alpha_du beta_du gama_du xx yy zz
L1 = 0.5;
L2 = 1;
L3 = 1;
% for i = 1:1:Point_num
alpha_du = 0;
beta_du  = 45;%%限定初始角度
gama_du  = -90;  
%% 运动学正解
[Trans_LF,Rot_LF,Pos_LF_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_RF,Rot_RF,Pos_RF_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_RB,Rot_RB,Pos_RB_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_LB,Rot_LB,Pos_LB_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
%% 运动学逆解
theta_LF = Kine_inv(L1,L2,L3,Pos_LF_init);% 
theta_RF = Kine_inv(L1,L2,L3,Pos_RF_init);% 
theta_RB = Kine_inv(L1,L2,L3,Pos_RB_init);% 
theta_LB = Kine_inv(L1,L2,L3,Pos_LB_init)% 
Pos_LF = zeros(Point_num,3);
Pos_RF = zeros(Point_num,3);
Pos_RB = zeros(Point_num,3);
Pos_LB = zeros(Point_num,3);
Theta_LF = zeros(Point_num,3);
Theta_RF = zeros(Point_num,3);
Theta_RB = zeros(Point_num,3);
Theta_LB = zeros(Point_num,3);
%% 摆动相与支撑相分别规划
% 取模运算
for index = 1:1:Point_numindex_LF = mod(index + round(T_gait(gait_mode,1).*Point_num_Tt),Point_num_Tt) + 1;  %index_RF = mod(index + round(T_gait(gait_mode,2).*Point_num_Tt),Point_num_Tt) + 1;  %index_RB = mod(index + round(T_gait(gait_mode,3).*Point_num_Tt),Point_num_Tt) + 1;  %index_LB = mod(index + round(T_gait(gait_mode,4).*Point_num_Tt),Point_num_Tt) + 1;  %T(index) = (index - 1)*Tsa ;[LF(index,1), LF(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_LF,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)Pos_LF(index,1) = Pos_LF_init(1) - LF(index,2);Pos_LF(index,2) = Pos_LF_init(2);Pos_LF(index,3) = Pos_LF_init(3) + LF(index,1); [theta] = Kine_inv(L1,L2,L3,Pos_LF(index,:));% Theta_LF(index,:) = theta';[RF(index,1), RF(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_RF,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)Pos_RF(index,1) = Pos_RF_init(1) - RF(index,2);Pos_RF(index,2) = Pos_RF_init(2);Pos_RF(index,3) = Pos_RF_init(3) + RF(index,1); [theta] = Kine_inv(L1,L2,L3,Pos_RF(index,:));% Theta_RF(index,:) = theta';[RB(index,1), RB(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_RB,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)Pos_RB(index,1) = Pos_RB_init(1) - RB(index,2);Pos_RB(index,2) = Pos_RB_init(2);Pos_RB(index,3) = Pos_RB_init(3) + RB(index,1); [theta] = Kine_inv(L1,L2,L3,Pos_RB(index,:));% Theta_RB(index,:) = theta';[LB(index,1), LB(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_LB,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)Pos_LB(index,1) = Pos_LB_init(1) - LB(index,2);Pos_LB(index,2) = Pos_LB_init(2);Pos_LB(index,3) = Pos_LB_init(3) + LB(index,1); [theta] = Kine_inv(L1,L2,L3,Pos_LB(index,:));% Theta_LB(index,:) = theta';
end
figure(1)
plot (LF(:,1), LF(:,2),'-r','linewidth',1);
hold on
plot (RF(:,1), RF(:,2),'-r','linewidth',1);
hold on
plot (LB(:,1), LB(:,2),'-r','linewidth',1);
hold on
plot (RB(:,1), RB(:,2),'-r','linewidth',1);
axis equal
xlabel('X')
ylabel('Y')
grid minor
hold on
pic_index = 1;%记录图像编号for i = 1:end
% for k = 1:10:Point_num
%     cla;
%     plot (LF(:,1), LF(:,2),'-r','linewidth',1);
%     hold on;
%     plot (LF(k,1), LF(k,2),'g*','linewidth',2);
%     [A,map] = rgb2ind(frame2im(getframe),256);
%     if pic_index == 1
%         imwrite(A,map,'test.gif', 'gif','Loopcount',inf,'DelayTime',0.2);
%     else
%         imwrite(A,map,'test.gif','gif','writeMode','append','DelayTime',0.2);
%     end
%     pic_index = pic_index + 1;
% end
% plot(X(k), Y(k), 'g*','markersize',5, 'linewidth',1.5 );
legend('quadruped robot control');
figure(2)
subplot(4,1,1)
plot (T, LF(:,1),'-r',T,LF(:,2),'-b','linewidth',1);
% axis([0,32,-0.3,0.3])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,2)
plot (T, RF(:,1),'-r',T,RF(:,2),'-b','linewidth',1);
% axis([0,32,-0.2,0.2])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,3)
plot (T, LB(:,1),'-r',T,LB(:,2),'-b','linewidth',1);
% axis([0,32,-0.2,0.2])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,4)
plot (T, RB(:,1),'-r',T,RB(:,2),'-b','linewidth',1);
% axis([0,32,-0.3,0.3])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
figure(3)
subplot(2,1,1)
plot (T,Theta_LF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_LF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(4)
subplot(2,1,1)
plot (T,Theta_RF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_RF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(5)
subplot(2,1,1)
plot (T,Theta_RB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_RB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(6)
subplot(2,1,1)
plot (T,Theta_LB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_LB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(7)
subplot(4,1,1)
plot (T,Theta_LF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,2)
plot (T,Theta_RF(:,2),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,3)
plot (T,Theta_RB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,4)
plot (T,Theta_LB(:,2),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
figure(8)
subplot(4,1,1)
plot (T,Theta_LF(:,3),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,2)
plot (T,Theta_RF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,3)
plot (T,Theta_RB(:,3),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,4)
plot (T,Theta_LB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minorfunction [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
%%

六、摆线轨迹

在这里插入图片描述

七、walk 步态

在这里插入图片描述

八、trot 步态

在这里插入图片描述

九、参考文献

[1]陈光荣. 四足机器人静动步态行走控制策略研究[D].北京理工大学,2018.
[2]郭晖晖. 四足机器人步态规划与运动控制研究[D].南京航空航天大学,2017.
[3]张千伟. 基于虚拟样机的四足机器人设计与步态研究[D].南京理工大学,2017.
[4]张志宇. 基于ADAMS的四足机器人虚拟样机仿真及刚柔耦合分析[D].哈尔滨工业大学,2016.

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.hqwc.cn/news/687165.html

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

nmap使用教程

nmap使用教程

nmap使用教程 一、nmap简介二、nmap常用命令2.1、target specification&#xff08;目标规范&#xff09;2.1.1、用法2.1.2、详情 2.2、HOST DISCOVERY&#xff08;主机发现&#xff09;2.2.1、用法2.2.2、详情 2.3、SCAN TECHNIQUES&#xff08;扫描技术&#xff09;2.4、PORT…
阅读更多...
【全开源】酷柚易汛ERP 源码部署/售后更新/上线维护

【全开源】酷柚易汛ERP 源码部署/售后更新/上线维护

一款基于FastAdminThinkPHPLayui开发的ERP管理系统&#xff0c;帮助中小企业实现ERP管理规范化&#xff0c;此系统能为你解决五大方面的经营问题&#xff1a;1.采购管理 2.销售管理 3.仓库管理 4.资金管理 5.生产管理&#xff0c;适用于&#xff1a;服装鞋帽、化妆品、机械机电…
阅读更多...
JINGWHALE 量子能量意识进化理论 —— 全息世界

JINGWHALE 量子能量意识进化理论 —— 全息世界

JINGWHALE 对此论文相关未知以及已知概念、定理、公式、图片等内容的感悟、分析、创新、创造等拥有作品著作权。未经 JINGWHALE 授权&#xff0c;禁止转载与商业使用。 人类对于自身的来源充满了好奇心和求知欲望&#xff0c;探索人类起源是人类科学研究和探索的重要领域之一。…
阅读更多...
FMEA助力医疗设备研发制造：领跑未来，实现弯道超车！

FMEA助力医疗设备研发制造：领跑未来，实现弯道超车！

医疗设备作为保障人类健康的重要工具&#xff0c;其研发与制造水平直接关系到医疗技术的进步。然而&#xff0c;在激烈的市场竞争中&#xff0c;如何能够让自家医疗设备研发制造实现弯道超车&#xff0c;成为行业佼佼者&#xff1f;答案就在于——FMEA&#xff08;失效模式与影…
阅读更多...
uniapp 版本检查更新

uniapp 版本检查更新

总体来说uniapp的跨平台还是很不错的&#xff0c;虽然里面各种坑要去踩&#xff0c;但是踩坑也是开发人员的必修课和成长路。 这不&#xff0c;今天就来研究了一下版本检查更新就踩到坑了。。。先来看看检查更新及下载、安装的实现。 先来看看页面&#xff1a; 从左到右依次为…
阅读更多...
JAVA排序相关习题7

JAVA排序相关习题7

1.插入排序 1.1 基本思想 直接插入排序是一种简单的插入排序法&#xff0c;其基本思想是&#xff1a; 把待排序的记录按其关键码值的大小逐个插入到一个已经排好序的有序序列中&#xff0c;直到所有的记录插入完为止&#xff0c;得到一个新的有序序列 。 /*** 时间复杂度&…
阅读更多...
JAVA 项目＜果园之窗＞_完结

JAVA 项目＜果园之窗＞_完结

目录 1、前言&#xff1a;2、视频展示&#xff1a;3、环境配置&#xff1a;4、工程代码&#xff1a;5、原理&#xff1a;6、原理补充&#xff1a;7、综上&#xff1a; 1、前言&#xff1a; 因为没有足够的时间这个项目用的是别人搭好的框架&#xff0c;在此基础上做调整并根据前…
阅读更多...
机器学习算法 - 逻辑回归

机器学习算法 - 逻辑回归

逻辑回归是一种广泛应用于统计学和机器学习领域的回归分析方法&#xff0c;主要用于处理二分类问题。它的目的是找到一个最佳拟合模型来预测一个事件的发生概率。以下是逻辑回归的一些核心要点&#xff1a; 基本概念 输出&#xff1a;逻辑回归模型的输出是一个介于0和1之间的…
阅读更多...
[ 视频号]代替用户发布视频api

[ 视频号]代替用户发布视频api

使用接口&#xff0c;替代用户使用设备直接发布视频api 接口地址&#xff1a; http://接口地址/api/v2 先调用登录接口&#xff0c;进行账号登录 登录二维码接口入参&#xff1a; {"appId": "","proxyIp": "","regionId"…
阅读更多...
debian testing （预计13版本）wps字体无法正常显示

debian testing （预计13版本）wps字体无法正常显示

背 景 本人使用debian办公&#xff0c;原来使用的是debian 12,由于“生命不息&#xff0c;折腾不止“&#xff0c;终于将稳定版的debian 12升级为testing. 结果发现&#xff0c;debian 12能够正常使用的wps存在部分字体无法正常显示&#xff0c;经研究发现&#xff0c;原来是w…
阅读更多...
使用com.google.common.collect依赖包中的Lists.transform()方法转换集合对象之后，修改集合中的对象属性，发现不生效

使用com.google.common.collect依赖包中的Lists.transform()方法转换集合对象之后，修改集合中的对象属性，发现不生效

目录 1.1、错误描述 &#xff08;1&#xff09;引入依赖 &#xff08;2&#xff09;模拟代码 &#xff08;3&#xff09;运行结果 1.2、解决方案 1.1、错误描述 最近在开发过程中&#xff0c;使用到了com.google.common.collect依赖包&#xff0c;通过这个依赖包中提供的…
阅读更多...
C语言 [力扣]详解环形链表和环形链表II

C语言 [力扣]详解环形链表和环形链表II

各位友友们&#xff0c;好久不见呀&#xff01;又到了我们相遇的时候&#xff0c;每次相遇都是一种缘分。但我更加希望我的文章可以帮助到大家。下面就来具体看看今天所要讲的题目。 文章目录 1.环形链表2.环形链表II 1.环形链表 题目描述:https://leetcode.cn/problems/link…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023