四足机器人摆线规划程序

一、标准摆线公式

{ 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: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π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_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:容器之分类和各种测试

文章目录 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高可用使用

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

Java的关键字volatile保证了有序性和可见性&#xff0c;这里我试着从底层开始讲一下有序性和可见性。 一&#xff0c;一致性 数据如果同时被两个cpu读取了&#xff0c;如何保证数据的一致性&#xff1f;或者换句话说&#xff0c;cpu1改了数据&#xff0c;cpu2的数据就成了无效…

python在Django中切换语言,中英文两种语言怎样切换

在Django中切换语言(比如中英文两种语言)通常涉及以下步骤: 设置语言和本地化 在你的Django项目的settings.py文件中,你需要设置LANGUAGES和LOCALE_PATHS。LANGUAGES是一个包含所有可用语言和它们的本地化的元组列表,而LOCALE_PATHS是包含.mo翻译文件路径的列表。 pyth…

LeetCode 题目 94:五种算法递归|迭代|莫里斯|线索二叉树|栈的迭代二叉树 实现中序遍历

本文详细探讨了五种二叉树中序遍历算法&#xff0c;包括递归、迭代、莫里斯遍历、线索二叉树和栈的迭代&#xff0c;评估了它们的效率和实用性。 题目描述 给定一个二叉树的根节点 root&#xff0c;返回它的中序遍历。 输入格式 root&#xff1a;二叉树的根节点。 输出格式…

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

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

242 基于matlab的3D路径规划

基于matlab的3D路径规划&#xff0c;蚁群算法&#xff08;ACO&#xff09;和天牛须&#xff08;BAS&#xff09;以及两种结合的三种优化方式&#xff0c;对3D路径规划的最短路径进行寻优。程序已调通&#xff0c;可直接运行。 242 3D路径规划 蚁群算法和天牛须 - 小红书 (xiaoh…

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…

ES6类与面向对象编程

ES6类与面向对象编程 ES6&#xff08;ECMAScript 6&#xff09;引入了类的概念&#xff0c;使得面向对象编程更加简洁和直观。ES6的类可以通过class关键词定义&#xff0c;类中可以定义构造函数、属性和方法。 以下是一个使用ES6类来定义一个简单的“人”类的示例&#xff1a…

unity入门学习笔记

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

Python基础学习之os模块

在Python编程的世界中&#xff0c;内置库为我们提供了丰富的功能和工具&#xff0c;使我们能够轻松处理各种任务。其中&#xff0c;os模块是一个极其重要且常用的库&#xff0c;它提供了与操作系统交互的接口。本文将带您一起探索os模块的一些常用功能。 1. os模块简介 os模块…

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

欢迎关注博主 Mindtechnist 或加入【智能科技社区】一起学习和分享Linux、C、C、Python、Matlab&#xff0c;机器人运动控制、多机器人协作&#xff0c;智能优化算法&#xff0c;滤波估计、多传感器信息融合&#xff0c;机器学习&#xff0c;人工智能等相关领域的知识和技术。关…

OceanBase 分布式数据库【信创/国产化】- OceanBase 数据并发性和一致性概述

本心、输入输出、结果 文章目录 OceanBase 分布式数据库【信创/国产化】- OceanBase 数据并发性和一致性概述前言OceanBase 数据更新架构OceanBase 数据并发性和一致性概述数据并发性数据一致性并发控制OceanBase 分布式数据库【信创/国产化】- OceanBase 数据并发性和一致性概…

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

使用双绞线最大的理由是抗干扰。不仅可以防止别人干扰,也可以防止自己干扰别人。这与EMC中的EMS和EMI相对应(参考【EMC专题】电磁兼容--基本概念)。 双绞线是由一对带有绝缘层的铜线(绝缘层使两根线中的金属导体不会因为互碰而导致短路)以螺旋的方式缠绕在一起所构成的。通…

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…

【代码随想录】

代码随想录 数组2. 二分查找3. 移除元素4. 有序数组的平方5. 长度最小的子数组 数组 2. 二分查找 力扣题目链接 前提&#xff1a; 有序数组数组中无重复元素 代码&#xff1a; &#xff08;版本一&#xff09;左闭右闭区间 class Solution {public int search(int[] num…

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

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

PG实例连接访问控制

实例访问控制可以控制来自于不同主机&#xff0c;不同用户是否允许访问指定的数据库&#xff0c;以及验证方式。 与oracle中的连接管理器的功能相同&#xff0c;之前有写过一篇oracleCMAN连接管理器的配置实操&#xff1a; 配置oracle连接管理器&#xff08;cman&#xff09;…