四足机器人摆线规划程序

一、标准摆线公式

{ 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.mzph.cn/pingmian/6309.shtml

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

相关文章

3：容器之分类和各种测试

3：容器之分类和各种测试

文章目录 array测试array容器bsearch之前用qsort 可以极大提高搜索效率 测试vector容器list容器forward_list容器测试slistdeque容器stack容器quque为什么stack 和 queue这样的容器 没有迭代器的相关内容 multiset代码里面有使用 stl自带的find 和 容器自带的find 一般容器自带…
阅读更多...
计算机视觉科普到实践

计算机视觉科普到实践

第一部分&#xff1a;计算机视觉基础 引言&#xff1a; 计算机视觉作为人工智能领域的一个重要分支&#xff0c;近年来取得了显著的进展。本文将带领读者深入了解计算机视觉的基础知识&#xff0c;并通过实践案例展示其应用。让我们一同探索这个令人着迷的领域吧&#xff01;…
阅读更多...
docker compose mysql主从复制及orchestrator高可用使用

docker compose mysql主从复制及orchestrator高可用使用

1.orchestrator 功能演示&#xff1a; 1.1 多级级联&#xff1a; 1.2 主从切换&#xff1a; 切换成功后&#xff0c;原来的主库是红色的&#xff0c;需要在主库的配置页面点击“start replication ”&#xff0c;重新连接上新的主库。 1.3 主从故障&#xff0c;从库自动切换新…
阅读更多...
高精度（加减乘除）

高精度（加减乘除）

1.加法 我们第一位存低位&#xff08;倒着存方便&#xff09; 下面是AC代码&#xff1a; #include<bits/stdc.h> using namespace std; const int N1e610; vector<int> add(vector<int> &A,vector<int> &B) {vector<int> c;int t0;for…
阅读更多...
【JVM】从硬件层面和应用层面的有序性和可见性，到Java的volatile和synchronized

【JVM】从硬件层面和应用层面的有序性和可见性，到Java的volatile和synchronized

Java的关键字volatile保证了有序性和可见性&#xff0c;这里我试着从底层开始讲一下有序性和可见性。 一&#xff0c;一致性 数据如果同时被两个cpu读取了&#xff0c;如何保证数据的一致性&#xff1f;或者换句话说&#xff0c;cpu1改了数据&#xff0c;cpu2的数据就成了无效…
阅读更多...
基于AT89C52单片机的智能热水器控制系统

基于AT89C52单片机的智能热水器控制系统

点击链接获取Keil源码与Project Backups仿真图&#xff1a; https://download.csdn.net/download/qq_64505944/89242443?spm1001.2014.3001.5503 C 源码仿真图毕业设计实物制作步骤05 题 目 基于单片机的智能热水器系统 学 院 专 业 班 级 学 号 学生姓名 指导教师 完成日期…
阅读更多...
242 基于matlab的3D路径规划

242 基于matlab的3D路径规划

基于matlab的3D路径规划&#xff0c;蚁群算法&#xff08;ACO&#xff09;和天牛须&#xff08;BAS&#xff09;以及两种结合的三种优化方式&#xff0c;对3D路径规划的最短路径进行寻优。程序已调通&#xff0c;可直接运行。 242 3D路径规划 蚁群算法和天牛须 - 小红书 (xiaoh…
阅读更多...
ant-design中的穿梭框提示文字修改

ant-design中的穿梭框提示文字修改

ant-design中的穿梭框提示文字修改 1.ant-design中的穿梭框提示文字修改 <a-transferv-model:target-keys"targetKeys":data-source"transform.list":filter-option"filterOption":list-style"{width: 100%,height: 500px,}":rowK…
阅读更多...
unity入门学习笔记

unity入门学习笔记

文章目录 unity学习笔记熟悉界面窗口页面快捷键视图特点移动、旋转、缩放快捷键聚焦和隐藏 一些基本概念模型模型的导入一些补充 资源文件资源包的导出资源包的导入 轴心物体的父子关系空物体Global与localpivot与center 组件脚本基础我的第一个脚本 获取脚本组件本地坐标播放模…
阅读更多...
顶顶顶顶顶顶顶顶顶顶顶顶

顶顶顶顶顶顶顶顶顶顶顶顶

欢迎关注博主 Mindtechnist 或加入【智能科技社区】一起学习和分享Linux、C、C、Python、Matlab&#xff0c;机器人运动控制、多机器人协作&#xff0c;智能优化算法&#xff0c;滤波估计、多传感器信息融合&#xff0c;机器学习&#xff0c;人工智能等相关领域的知识和技术。关…
阅读更多...
【电子通识】为什么用双绞线？双绞线抗干扰的原理是什么？

【电子通识】为什么用双绞线？双绞线抗干扰的原理是什么？

使用双绞线最大的理由是抗干扰。不仅可以防止别人干扰，也可以防止自己干扰别人。这与EMC中的EMS和EMI相对应（参考【EMC专题】电磁兼容--基本概念）。 双绞线是由一对带有绝缘层的铜线（绝缘层使两根线中的金属导体不会因为互碰而导致短路）以螺旋的方式缠绕在一起所构成的。通…
阅读更多...
ElementUI从unpkg.com完整下载到本地的方法 - 解决unpkg.com不稳定的问题 - 自建镜像站 - 不想打包只想cdn一下

ElementUI从unpkg.com完整下载到本地的方法 - 解决unpkg.com不稳定的问题 - 自建镜像站 - 不想打包只想cdn一下

方法 方法1&#xff09;随便弄个文件夹&#xff0c;根据官网npm方法下载包&#xff0c;提取即可 npm i element-ui -S cd /node_modules/element-ui/ ls src 安装npm方法&#xff1a;https://nodejs.org/en 方法2&#xff09;不推荐 - 在github中搜索对应的库zip包&#xff0…
阅读更多...
【网络原理】HTTP 协议的基本格式和 fiddler 抓包工具的用法

【网络原理】HTTP 协议的基本格式和 fiddler 抓包工具的用法

系列文章目录 【网络通信基础】网络中的常见基本概念 【网络编程】网络编程中的基本概念及Java实现UDP、TCP客户端服务器程序&#xff08;万字博文&#xff09; 【网络原理】UDP协议的报文结构 及 校验和字段的错误检测机制&#xff08;CRC算法、MD5算法&#xff09; 【网络…
阅读更多...
PG实例连接访问控制

PG实例连接访问控制

实例访问控制可以控制来自于不同主机&#xff0c;不同用户是否允许访问指定的数据库&#xff0c;以及验证方式。 与oracle中的连接管理器的功能相同&#xff0c;之前有写过一篇oracleCMAN连接管理器的配置实操&#xff1a; 配置oracle连接管理器&#xff08;cman&#xff09;…
阅读更多...
深度学习每周学习总结P7（咖啡豆识别）

深度学习每周学习总结P7（咖啡豆识别）

&#x1f368; 本文为&#x1f517;365天深度学习训练营 中的学习记录博客&#x1f356; 原作者&#xff1a;K同学啊 | 接辅导、项目定制 –来自百度网盘超级会员V5的分享 数据链接 提取码&#xff1a;7zt2 –来自百度网盘超级会员V5的分享 目录 0. 总结1. 数据导入及处理部分…
阅读更多...
文章解读与仿真程序复现思路——电力自动化设备EI\CSCD\北大核心《电-氢-混氢天然气耦合的城市综合能源系统低碳优化调度》

文章解读与仿真程序复现思路——电力自动化设备EI\CSCD\北大核心《电-氢-混氢天然气耦合的城市综合能源系统低碳优化调度》

本专栏栏目提供文章与程序复现思路&#xff0c;具体已有的论文与论文源程序可翻阅本博主免费的专栏栏目《论文与完整程序》 论文与完整源程序_电网论文源程序的博客-CSDN博客https://blog.csdn.net/liang674027206/category_12531414.html 电网论文源程序-CSDN博客电网论文源…
阅读更多...
Java | Leetcode Java题解之第68题文本左右对齐

Java | Leetcode Java题解之第68题文本左右对齐

题目&#xff1a; 题解&#xff1a; class Solution {private String line(List<String> list,int maxWidth,int totalLength,boolean isLast){StringBuilder sb new StringBuilder();sb.append(list.get(0));if(list.size() 1){String ap " ".repeat(maxW…
阅读更多...
二维数组的鞍点（C语言）

二维数组的鞍点（C语言）

一、鞍点解释&#xff1b; 鞍点就是该位置上的元素在该行上最大、在该列上最小&#xff1b; 二、N-S流程图&#xff1b; 三、运行结果&#xff1b; 四、源代码&#xff1b; # define _CRT_SECURE_NO_WARNINGS # include <stdio.h>int main() {//初始化变量值&#xff…
阅读更多...
定制开发AI名片商城AI导购系统：引领营销自动化的新时代

定制开发AI名片商城AI导购系统：引领营销自动化的新时代

在数字营销日新月异的今天&#xff0c;一个革命性的工具——定制开发AI名片商城AI导购系统&#xff0c;正逐渐崭露头角&#xff0c;成为企业私域运营中的得力助手。它不仅仅是一个营销工具&#xff0c;更是一个拥有强大营销自动化能力和先进算法技术的在线助理&#xff0c;为企…
阅读更多...
【neteq】tgcall的调用

【neteq】tgcall的调用

G:\CDN\P2P-DEV\Libraries\tg_owt\src\call\call.cc基本是按照原生webrtc的来的：G:\CDN\P2P-DEV\tdesktop-offical\Telegram\ThirdParty\tgcalls\tgcalls\group\GroupInstanceCustomImpl.cpptg对neteq的使用 worker 线程创建call Call的config需要neteqfactory Call::CreateAu…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023