四足机器人摆线规划程序

news2024/11/18 15:43:10

一、标准摆线公式

{ 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(1cos(θ))
这里的r表示摆线的圆的半径, θ \theta θ是圆的半径所经过的弧度(滚动角)。

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

二、摆动相的摆线公式

{ 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πTytsin(2πTyt))2πS0(1cos(2πTyt))
此处的 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(21Ty2π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_num
    index_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 minor


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
%% 

六、摆线轨迹

在这里插入图片描述

七、walk 步态

在这里插入图片描述

八、trot 步态

在这里插入图片描述

九、参考文献

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

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

codeforces round944(div4)A~E题解

文章目录 [A. My First Sorting Problem](https://codeforces.com/contest/1971/problem/A)[B. Different String](https://codeforces.com/contest/1971/problem/B)[C. Clock and Strings](https://codeforces.com/contest/1971/problem/C)[D. Binary Cut](https://codeforces…

性能测试 --概念

什么是性能测试 性能测试和功能测试都是在系统测试阶段运行, 两者有什么区别呢? 案例:豌豆射手和三线射手都是射手, 它们的功能都是向前发射豌豆进行攻击, 能够攻击到地面的僵尸. 但是从性能上来讲, 豌豆射手只能攻击到一路的僵尸, 而三线射手能同时攻击三路(注:放在边路实际…

读天才与算法:人脑与AI的数学思维笔记24_预测性文本生成器

1. 起源 1.1. 人类讲故事可能起源于“假如……”这种问答结构 1.2. 讲故事是人类做安全试验的一种方式 1.2.1. 如果你问一个人“假如……”&#xff0c;其实是在探索你的行为对他可能带来的影响 1.3. 最早出现的故事极有可能就源自我们对在周遭混乱的环境中寻找某种秩序的渴…

基于vgg16和efficientnet卷积神经网络的天气识别系统(pytorch框架)全网首发【图像识别-天气分类】

一个能够从给定的环境图像中自动识别并分类天气&#xff08;如晴天、多云、雨天、雪天闪电等&#xff09;的系统。 技术栈&#xff1a; 深度学习框架&#xff1a;PyTorch基础模型&#xff1a;VGG16与EfficientNet任务类型&#xff1a;计算机视觉中的图像分类 模型选择 VGG16 …

螺栓扭矩如何设计?——SunTorque智能扭矩系统

螺栓扭矩设计的大小是一个涉及工程实践的重要问题&#xff0c;它直接关系到螺栓连接的紧固质量和安全性。螺栓扭矩是工程领域中常用的一个概念&#xff0c;用来描述螺栓在连接过程中所需的旋转力矩。正确的螺栓扭矩可以确保螺栓和螺母之间的紧密连接&#xff0c;避免由于松动而…

JINGWHALE 虚拟现实物质与空间理论 —— 全息世界

JINGWHALE 对此论文相关未知以及已知概念、定理、公式、图片等内容的感悟、分析、创新、创造等拥有作品著作权。未经 JINGWHALE 授权&#xff0c;禁止转载与商业使用。 一、虚拟现实物质与空间理论 物质是由离散的奇点JING粒子&#xff0c;依据不同的维度粒度&#xff0c;通过…

《解锁高效合同管理系统:优化业务流程,提升管理效率》

随着企业规模的扩大和业务复杂性的增加&#xff0c;合同管理变得愈发重要。合同是企业与客户、供应商、合作伙伴之间的法律约束和商业承诺&#xff0c;而有效的合同管理系统则成为企业提高运营效率、降低风险的关键工具。本文将探讨合同管理系统的重要性以及如何利用合同管理系…

网络编程——Socket——模拟用户登录

功能一&#xff1a;模拟用户登录 功能二&#xff1a;实现客户发送登录用户信息&#xff0c;服务器端显示登录信息并响应给客户端登录成功 这里设置的用户登录信息为&#xff1a;admin&#xff0c;123456 实现&#xff1a; 1.首先&#xff0c;服务端创建并启动服务器&#x…

selenium爬取TapTap评论

上一篇写的beautifulsoup和request爬取出的结果有误。首先&#xff0c;TapTap网页以JS格式解析&#xff0c;且评论并没有“下一页”&#xff0c;而是每次加载到底部就要进行等待重新加载。我们需要做的&#xff0c;是模仿浏览器的行为&#xff0c;所以这里我们用Selenium的方式…

STM32_HAL_RTC_中断实现闹钟

1STM32设置 在STM32Cude中设置RTC//具体设置看先前发的文章 再打开闹钟中断&#xff08;如下图&#xff09; 2代码思路 2.1启动闹钟&#xff08;HAL_RTC_SetAlarm_IT(&hrtc,&sAlarm,FORMAT_BCD)&#xff09; 2.2设置回调函数&#xff08;void HAL_RTC_AlarmAEventC…

C++ VScode: launch: program ...... dose not exist

VScode: launch: program … dose not exist 介绍 参考VS Code 配置 C/C 编程运行环境&#xff08;保姆级教程&#xff09;教程配置了VSCode。在配置launch.json适用多个.c 文件编译时&#xff0c;弹出下面错误。 原因和解决方法 是task.json 默认配置的问题。 默认的 cwd参…

内网远程软件哪个好用

内网远程软件哪个好用 在现代化的办公环境中&#xff0c;内网远程软件已成为提高工作效率、实现灵活办公的重要工具。然而&#xff0c;市场上内网远程软件众多&#xff0c;究竟哪个好用呢&#xff1f;本文将为您推荐几款优秀的内网远程软件&#xff0c;并分析其特点&#xff0…

将来会是Python、Java、Golang三足鼎立吗?

在开始前我有一些资料&#xff0c;是我根据网友给的问题精心整理了一份「 Java的资料从专业入门到高级教程」&#xff0c; 点个关注在评论区回复“888”之后私信回复“888”&#xff0c;全部无偿共享给大家&#xff01;&#xff01;&#xff01; 软件工程里没有银弹&#xff…

【Mac】Indesign 2023 Mac(ID2023) v18.5中文版安装教程

软件介绍 Adobe InDesign是一款由Adobe Systems开发的桌面排版软件&#xff0c;旨在用于创建、编辑和格式化印刷和数字出版物&#xff0c;如书籍、杂志、报纸、传单等。以下是一些关于Adobe InDesign的主要特点和功能&#xff1a; 1.强大的排版工具&#xff1a;InDesign提供了…

【Java难点】多线程-终极【更新中...】

Java内存模型之JMM 为什么需要JMM 计算机存储结构&#xff1a;从本地磁盘到主存到CPU缓存&#xff0c;也就是从硬盘到内存&#xff0c;到CPU。一般对应的程序的操作就是从数据库查数据到内存然后到CPU进行计算。 CPU和物理主内存的速度不一致&#xff0c;所以设置多级缓存&am…

JAVA 双亲委派之一

JAVA 双亲委派之一 JVM类加载流程 java语言系统内置了众多类加载器&#xff0c;从一定程度上讲&#xff0c;只存在两种不同的类加载器&#xff1a;一种是启动类加载器&#xff0c;此类加载由C实现&#xff0c;是JVM的一部分&#xff1b;另一种就是所有其他的类加载器&#xf…

《软件方法(下)》8.3.2.2 警惕拼凑泛化(202405更新)

DDD领域驱动设计批评文集 做强化自测题获得“软件方法建模师”称号 《软件方法》各章合集 8.3 建模步骤C-2 识别类的关系 8.3.2 识别泛化关系 8.3.2.1 识别泛化的思路 &#xff08;3&#xff09;自上而下&#xff08;从一般到特殊&#xff09; 如图8-92所示&#xff0c;这…

西湖大学英语听力考试音频无线发射系统-英语听力发射系统浅析

西湖大学英语听力考试音频无线发射系统-英语听力发射系统浅析 由北京海特伟业科技任洪卓发布于2024年5月10日 西湖大学&#xff0c;这所矗立于时代前沿的高等学府&#xff0c;始终秉持着创新精神和追求卓越的坚定信念&#xff0c;不断致力于教学质量的提升与学术研究的深化。其…

verilog中含有无关项的序列检测

编写一个序列检测模块&#xff0c;检测输入信号a是否满足011XXX110序列&#xff08;长度为9位数据&#xff0c;前三位是011&#xff0c;后三位是110&#xff0c;中间三位不做要求&#xff09;&#xff0c;当信号满足该序列&#xff0c;给出指示信号match。 程序的接口信号图如…

能聚合各站热点的DailyHot

什么是 DailyHot ? 今日热榜&#xff08;DailyHot&#xff09;是一个获取各大热门网站热门头条的聚合网站&#xff0c;能追踪全网热点、实现简单高效阅读。项目分为前、后端&#xff0c;其中后端提供了一个聚合热门数据的 API 接口。 &#x1f6a9; 后端 API 特性 极快响应&a…