- 1医学护理怎么搜答案？ #知识分享#职场发展
- 2鸿蒙系统会像安卓一样卡顿吗,鸿蒙系统是安卓系统套个壳？鸿蒙系统手机会像安卓一样越来越卡吗？...
- 3解决ADB连接不了问题： daemon not running:staring now at tcp:5037.adb:CreateFileW ‘nul‘ failed:系统找不到指定的文件。_* daemon not running; starting now at tcp:5037
- 4Pytorch：NLP 迁移学习、NLP中的标准数据集、NLP中的常用预训练模型、加载和使用预训练模型、huggingface的transfomers微调脚本文件_eng-french.csv
- 5Instruction Tuning for Large Language Models_ A Survey
- 6Python数据分析基础技术之scikit-learn（史上最全面）_python中 double requirement given: scikit-learn (fr
- 7Redis（二）
- 8【MIMO雷达】基于matlab压缩感知大规模MIMO雷达仿真【含Matlab源码 3858期】_mimo雷达波形设计matlab仿真
- 9代码的c++实现_HTTP中间件机制实现与原理 从零开始写GOAPI框架
- 10YOLOV5-7.0推理代码精简版详解(去除了所有冗余代码只留了必要的)_yolov5精简代码
四足机器人——12自由度舵机狗DIY（二）_四足 zmp
赞
踩
目录
4.3matlab下在hopf模型中加入足内协调关系生成trot步态波形图
4.4matlab下在hopf模型中加入足内协调关系生成walk步态波形图
4.6主控与上位机建立通信在上位机上将数据以波形图的形式展现
上一节详细记述了整个四足狗的机械结构部分的分析设计。这节给大家分享一下我实现四足狗行走的策略及关于四足机器人步态方面的一些知识。
我把自己的matalab代码和stm32F1的keil代码都发出来了，有需要的可自行下载，一些四足相关论文资料由于版权原因发布不了，可私信找我要……
四足机器人 Matalb hopf算法+步态算法 代码
https://download.csdn.net/download/weixin_49585296/85093057
四足机器人Stm32F1控制算法代码 keil代码https://download.csdn.net/download/weixin_49585296/85095491
一.四足机器人步态研究控制的现状
1.1目前的三种控制策略
四足机器人步态控制方法在当下研究中最常用的有三种：
<1>基于静态稳定的控制方法。
通常，在对四足机器人行走下静步态进行控制时，以协调腿部关节之间的运动为目的的步态轨迹规划显得尤为重要，保证机器人运动过程中的稳定通常以 ZMP、ESM 和 SAL 等静态稳定性判断依据来保证。为了提高四足机器人在行走时的动态平衡依据 ZMP 准则对设施的四足机器人稳定性进行控制，为机器人规划足端的运动轨迹。是一种简单却极为繁琐不稳定的控制策略。
突然想起来，之前学习查询资料时在某站上看到过，有兴趣的可以去看看，链接附上：
四足教程之程序篇https://www.bilibili.com/video/BV1xt411g7ws?spm_id_from=333.999.0.0
四足教程之理论篇https://www.bilibili.com/video/BV1zt411V7gB?spm_id_from=333.999.0.0
（这种控制方法他讲的比较详细有兴趣的可以去瞅瞅。这个B站博主视频中还有对运动学分析计算公式的推导过程，思路是没问题的，可惜最后他那个方程没解出来）
<2>基于动力学模型的控制方法。
在当下对工业机器人的控制方法中，基于动力学模型的控制算法具有不可替代的作用，但目前这种方法在运用中主要针对的是机器人阻断接触的地面环境较为光滑，因为四足机器人的应用范围较广，足端接触环境复杂，还受到阻断接触地面时瞬间接触力大小变化的影响，对四足机器人建立精确的动力学模型控制方法难度较大。因此采用这种方法对四足机器人进行控制的控制效果有待改善，不过这种控制方法所还是在四足机器人上有一定的运用效果，需要业界专家学者的贡献。
动力学模型控制方法有以下几种：一是逆动力学控制，这种控制方法下要想得到较高的控制精度，对动力学方程的准确性提出了较高的要求，计算量较大，并且在四足机器人行走过程中四足之间也会产生相互作用的干扰力，使实际控制较复杂；二是虚拟模型控制，这种控制方法是以在机器人腿部添加虚拟部件的方式，建立虚拟部件与关节输出力矩之间的关系，该方法在最初的研究中常被应用于对双足机器人的控制过程中，在后来的发展中这种方法也逐渐应用于四足机器人；三是基于生物动力学特性简化模型的控制方法，这是一种将动物的运动特点总结成原理后应用到机器人的步态控制中，通过大量的观察，将得出的结论应用到实际控制中。
运动学分为运动学正解和运动学逆解。因为在四足机器人中用的是12个舵机，所以运动学正解是已经知道运动关节的各个电机运动参数，也就是此时对于初始位置转动的角度，去求末端执行器的相对参考坐标系的位姿。而运动学逆解恰恰相反，是根据相对参考坐标系的位姿去求出运动关节的各个电机此时对于初始位置转动的角度。
<3>基于生物所具有的神经性调节机制以及行为特性的控制方法
参考 北京理工大学 李华师前辈的《四足机器人仿生运动控制理论与方法的研究》中提到的CPG节律性算法实现的控制方法。
针对基于模型的机器人步态控制方法建模复杂、单周期规划、机器人环境适应性差等缺点，借鉴动物的中枢模式发生器（Cenral Paern Generaor，CPG）运动控制机理，研究了四足机器人步态生成与步态转换的仿生控制方法。针对传统的基于CPG的步态控制策略中CPG模型仅用于实现机器人足间协调，而不考虑单足内部各关节之间的协调，以及CPG输出信号对应的机器人支撑相与摆动相持续时间相等的问题，提出了将CPG同时用于实现四足机器人足间及足内多关节协调控制的方法，以Hopf振荡器为基础，构建了两种可同时实现对四足机器人髋关节和膝关节控制的CPG控制网络，所设计的控制网络很好地整合了膝关节的控制，不仅能够满足四足机器人不同负载因子节律步态的控制需求，而且结构简单，参数易于调整，便于实时控制。
针对在转换点直接替换,离散步态矩阵的步态转换方法会产生断点、锁相、停振、尖点等不利现象，从而造成机器人步态转换不平滑的问题，提出了一种基于CPG的四足机器人连续平滑步态转换方法，通过对机器人各足之间相对相位的连续调整，可使机器人实现任意两种步态之间的平滑转换。在这篇论文中利用ADAMS和MATLAB联合仿真，验证了所提出的步态生成与步态转换算法的正确性和有效性。
1.2选择采用的控制策略及原因
我采用的是第三种，类神经性调节的节律性控制方法。下面简要分析下选择这种方案的原因：
<1>基于静态稳定的控制方法。按我的理解简单来说就是“死调”，所有的参数和数据都是写死的，只是单纯的循环调用已经存储好的那些数据，而且调试周期很长还极其不稳定。实际上算是人为的进行好了轨迹路径规划（但每次都会不一样），将足端坐标转化为舵机或电机的转动角度，即把舵机反复转动的角度存储在一个二维数组里，然后按序一直调用二维数组里的转动值，自己设置舵机固定的递增或递减转动值，过程较为繁琐（之前在B站看到过一位博主就是这么做的，最后他的四足狗确实也走起来了，效果还可以；上面我也附上链接了，有兴趣的可以去尝试运动，我没有试过，所以过程中会遇到一些什么问题还有注意哪些细节，也一无所知)。过程繁琐，不稳定，而且对后续的研究与开发无任何意义，果断放弃。
<2>基于动力学模型的控制方法。这种方法最为广泛使用了，网上各种大佬，开源代码大部分都是这种。基本上就是正运动学或逆运动学实时计算关节摆动输出的角度，会涉及到一些运动学分析，雅可比矩阵，线性代数方面的一些知识，精通高数，线代，运动学的知识。在理论建立的基础上还得熟悉使用一些动力学仿真软件，编写程序代码。主要对个人的理论能力要求较高。如果有学长或导师指导，应该能行。总体来说，普通的个人爱好研究者居多，即使有很多的开源代码，没有人指导，自己去摸索弄明白也很困难。所以也果断放弃。（这里我把一些开源的四足项目的链接附上，有能力感兴趣的可以去研究研究）。
<3>基于生物所具有的神经性调节机制以及行为特性的控制方法。 最终选择这种方法的原因很多。基于自己的学识认知，这种方法当前研究的成熟度，稳定和优越性，对个人能力的评估，目前掌握的论文资料，成功率较高等种种因素。最终确定以这种方式来开展为期将近四个月的四足步态的研究。
1.3CPG的介绍
这里我就着重介绍一下基于第三种控制方法的（Cenral Paern Generaor）CPG的控制方法。以下CPG内容来自北京理工大学 李华师前辈的《四足机器人仿生运动控制理论与方法的研究》
动物的骨骼－肌肉系统是实现躯体运动及姿态的物质基础，包括骨骼、肌肉、肌腱和关节。动物实现运动的关键在于神经系统对肌肉的精确控制。骨骼肌的神经控制系统涉及神经中枢、感受器、效应器，是一个多环伺服系统，如图1所示。
图1 神经-肌肉控制原理
在图1中，肌梭和高尔基腱器官是两种本体感受器，为控制系统的反馈检测元件。肌梭与骨骼肌并联，主要检测肌肉的长度，反映肢体位置，是牵张反射的感受器。位于肌腱中的高尔基腱器官，与骨骼肌串联，主要检测肌肉张力，是腱反射的感受器。高层神经中枢通过α运动神经元发出运动指令，直接引起梭外肌收缩。同时，通过γ神经元，调节肌梭适应肌肉长度，保持肌梭的敏感性。当肌肉受到牵拉时，肌梭的输出增大，通过正反馈，使梭外肌收缩加大，维持适当的肌张力，抵抗外力或保持姿式。当肌肉受到强力牵拉时，高尔基腱器官产生反应，通过负反馈使肌肉松弛下来，起到保护作用。动物的运动控制系统利用肌梭和高尔基腱器官构成位置和力双参数闭环控制，准确把握肌肉收缩程度，使运动符合大脑意识或适应外界环境。
高等动物的运动控制系统是一个复杂的网络，涉及高级神经中枢、低级神经中枢、骨骼－肌肉执行系统、感受器以及感觉器官，如图2所示。
图2 动物的运动控制网络
图2中，高层神经中枢由一系列运动中枢组成，包括大脑皮层、基底神经节、脑干和小脑，主要功能是发出运动指令，控制节律运动的起始，选择运动模式，综合CPG的中枢反馈信息、本体感受信息、视觉信息等对运动进行调控，实现复杂环境中的避障、路径规划等高级运动功能。中枢模式发生器（CPG）是节律运动的中心控制单元，主要功能是产生节律信号，控制效应器实现运动。CPG是由中间神经元构成的具有多个振荡中心的分布网络，通过神经元之间的相互抑制实现自激振荡，产生具有稳定相位互锁关系的多路或者单路周期信号，控制肢体或者躯体相关部位的节律运动。CPG中各神经元之间的突触连接具有可塑性，因而表现出多种输出行为，控制动物实现多种运动模式。效应器即骨骼－肌肉系统，负责实现肢体的运动。整个控制系统中还存在一个复杂的反馈网络，包括中枢反馈、反射反馈、环境反馈等，这些反馈回路用于实现生物反射功能。中枢反馈是CPG输出的复制，高层中枢通过中枢反馈了解CPG的控制过程。反射反馈利用各种本体感受器为上层提供躯体运动状态或位置信息，通过反射机制，对CPG的输出进行调节，协调CPG与本体及环境的关系。环境反馈主要通过视觉、触觉等感受器或感觉器官监控环境与本体之间的关系，保证上层控制系统能够根据环境状况发出适当的命令或产生适应性的反应。
CPG是动物节律运动的中心控制单元，负责产生节律运动信号。采用CPG控制方法对机器人进行步态控制的首要步骤是建立CPG模型，即构造一个能够产生稳定节律输出的信号发生器，作为机器人各自由度的运动控制器。多个振荡单元通过相位耦合构成CPG网络，改变网络的拓扑结构可以改变振荡信号的输出模式，从而实现多种步态。
目前，用于机器人运动控制的比较典型的CPG模型可划分为两大类：基于神经元的模型和基于非线性振荡器的模型。应用最为广泛的Matsuoka神经元振荡器模型和Kimura模型属于第一类，该类CPG模型生物学意义比较明确，但参数较多，且方程具有非线性、强耦合和高维数等特点，参数整定和模型的动态特性分析比较复杂。基于非线性振荡器的CPG模型有Kuramoto相位振荡器、Hopf谐波振荡器、Van der Pol（VDP）松弛振荡器等，该类模型的优点是需要整定的
参数相对较少，模型也较成熟。
CPG模型的主要任务是产生稳定的周期性振荡信号，为了易于工程应用，在保证能够输出稳定的周期性振荡信号的前提下，形式简单、参数较少、计算量小、便于分析、易于实现CPG的模型无疑是较好的选择。
常见的四足动物都是进行节律运动的。节律运动是指空间和时间对称的周期性运动，譬如走、跑、跳等 。因此， 要实现四足机器人的稳定运动就需要控制器能够产生具有周期性、对称性的节律信号， 来控制 各个关节 (执行器 )实现机体的节律运动 。这种控制方式就是 (CPG)中枢神经控制模式。CPG是节律运动的中心控制器，不仅需要产生节律信号，控制执行器进行运动，还需要根据反馈信号进行识别，及时修改生成新的信号，使四足机器人能够稳定行走。
二.四足机器人步态介绍
2.1步态的概念
参考 清华大学 张秀丽等前辈的 《四足机器人节律运动及环境适应性的生物控制研究》，足式动物的运动模式可以用“步态”来表示，步态指的是各腿在行走时具有固定相位关系的行走模式。 四足动物的步态是指各个腿之间具有固定相位关系的行走模式，不同的动物由于自身条件的限制，如腿长、腿的位置、神经控制方式等，其步态也会变得不一样；简单来说，就是行走起来的一种运动规律，就如双足动物的“行走”、“奔跑”，四足动物的行走。如下动图3展示：
图3 四足多种步态行走动图
比如：人走起路来无非是先左脚——>再右脚，跳走，单跳，滑步等等，这些都是人的步态；狗狗，小猫，马这些四足动物走路的时候也有他们的走路规律，比如左前——>右前 ——>右后 ——>左后 依次循环的walk步态；左前右后 ——>右前左后 这样依次循环的trot步态；一侧两条腿同时走动的 侧溜步态 ；还有跳跃，奔跑等等多种步态。
2.2步态参数概念
步态是实现仿生四足机器人运动性能的基础规划，步态的协调性、连续性与可调整性直接决定着仿生四足机器人运动性能的优劣。步态可以由特定的几个参数表征，相位差φi、负载因子β、步态周期T、步长S、抬腿高度h，步态表征参数的具体定义如下表1所示。
表1 步态参数
2.3步态足间关系分析
通过对四足动物的运动观察与分析，总结出了四足动物的步态描述表，其中的节奏可以分为单拍、双拍、准双拍、四拍步态，可以从四足动物足端拍打地面的节拍进行区分，例如Trot是双拍步态，Walk是四拍步态。
另外，负载因子（步态占空比）β衡量腿部与地面的接触时间与步态周期的比重，有着重要的意义，因为当β等于0.5时，机器人在任意时刻或者平均约有两条腿支撑地面，而当β大于或者等于0.5的时候，说明机器人在任意时刻或者平均至少有三条腿支撑地面，这对机器人的步态输出有着极其重要的作用。其中walk步态β值为0.75，trot和其余常规步态的β为0.5。（这些参数的得出在清华大学 张秀丽等前辈的《四足机器人节律运动及环境适应性的生物控制研究》这篇论文中都有提到）
现在我们以LF(左前腿)、RF(右前腿)、RB(右后腿)、LB(左后腿)分别替代四足动物的四条腿，然后可以根据其步态的特点得出它们各自的相位关系(左前腿作为参考基准，φ（LF）=0,即，相位为0，一个周期为2π）。Walk步态行走起步时，左前腿LF抬起后以一定的足端轨迹向前跨步，待LF落地后紧跟着右后腿RB跨腿，之后再轮到RF、LB进行相同的动作，行走的过程中，该动作重复循环，步态周期较长并且在走动的过程始终保持三条腿在支撑身体。而Trot步态，先是LF与RB同时抬起并同步抬腿运动，当LF与RB重新落到地面时候，RF与LB跨腿，重复循环，为身体保持平稳，步态周期较短，在小跑过程中的任意时刻都只有对角腿在支撑身体。分析后可以总结出四足动物步行walk步态与小跑trot步态的时序描述如下图4所示，其中白色条纹表示支撑相，黑色条纹表示摆动相，每四个格子一个步态周期。
图4 walk和trot步态时序图
对四足动物的步态分析进行总结，可以得出四足动物walk与trot步态的四腿的相位关系拓扑网络，可以作为四足机器人步态规划与行走稳定控制的基础框架。如下图5所示：
图5 步态相位关系
关于四足动物行走的动态图，相位关系想详细研究的看下面这两个网页，很详细，满满干货
四足步态动图（二）https://www.cgjoy.com/forum.php?mod=viewthread&tid=196465
2.4步态层级
关于步态层级这方面，是我个人通过做这次四足项目的认识和了解以及对代码理解后，个人总结出的一个概念。首先层级，就是肯定有基础奠定和衍生，有框架，自上而下或者自下而上，有先后顺序的。
我把四足步态分为三种大层，分别是 Trot（对角） ，Walk（一次迈一只脚） ，pace（同侧），gallup（奔跑）.这四大步态分别是四足步态四大层的顶层，在这四个步态的基础上往下衍生又会衍生出不同运动轨迹的“衍生小步态”。划分为四个层次的依据是根据他们步态的相位：
关于这些步态层级和衍生小步态附上我自己整理的框架图，便于大家理解：
这里我只 Trot ，Walk进行研究与实物效果验证，实现了原地踏步，左右横移，左右转弯，小跑行走后退，慢步行走后退。至于pace，gallup还没进行研究，我自己的推断基于pace，gallup又能衍生出跳跃，滑行，后空翻等多样步态。
既然说到这里了，就把我对于四足机器人实现行走控制的一个思路列个框架图出来，这里就以trot为例，walk基本思路跟这个是一样的：
这部分步态层级完全是我个人理解创作的术语，其中的“衍生小步态”等的这种名词也都是我自己的术语。如果有什么地方叙述不当，欢迎大家评论区指出讨论。
在这里给一些刚入门四足没有方向的小伙伴一点建议，实现四足机器人的行走，先从trot步态的行走做起，然后再实现基于trot的原地踏步，进行调试。之后再进行walk以及后面一些衍生步态的实现。这个顺序必须是这样的，步态与步态之间都是环环相扣的。
三.hopf振荡器
3.1hopf振荡器的概念
hopf振荡器是一种CPG单元模型（上面CPG的介绍中也有提到过），它是一种较简单的谐波振荡器，输出信号的幅值和频率易于控制。它初始数学表达式如下：
具体了解请直接参阅北京理工大学 李华师前辈的《四足机器人仿生运动控制理论与方法的研究》的这篇论文
3.2hopf模型及参数含义
利用Hopf 振荡器建立CPG模型其实就是建立振荡单元的过程，单个CPG振荡单元的数学模型如下：
x y———————为两个相互耦合的变量
α————————固定参数
μ————————控制振荡器收敛到极限环的速度，为固定参数，用于控制振荡器的幅值
u1 u2——————外部反馈, 给ω发为固定参数，用于控制振荡器的频率
β————————占空比，是指每条腿与底面接触的时间和一个周期时间的比值
ωst ωsw———— 分别为摆动相和支撑相的频率
a————————一个正定的常数
运用Hopf振荡器建立起CPG控制中心，为满足四足机器人稳定行走的目的，需要控制中心产生至少8路节律信号，分别用来驱动四条腿的髋关节和膝关节。
单个CPG振荡单元可以控制一条腿的运动，采用四个CPG振荡单元即可实现对四条腿的控制；从生物神经机制而言，这四个振荡网络不管是在时间域还是空间域都是相互耦合的，这就意味着四足机器人腿与腿之间的协调可以通过建立相位耦合来实现。
根据上述总结，单腿膝、髋关节的输出需要对单个CPG振荡单元的输出值进行变换，协调四条腿的运动需要对四个CPG振荡单元进行相位变换，因此通过对式(3-9)进行改进，得到完整的CPG网络数学模型如下：
R(θij)旋转矩阵表征不同振荡器之间的相位耦合关系，其表达式如下：
θ(i,j)是指振荡器i和振荡器j之间的相对相位差,通过改变θ(i,j)的值就能改变旋转矩阵的值，这样就能使到不同振荡器之间的耦合相位发生变化，从而输出符合不同步态需求的控制信号，从而实现多种步态之间的变换;
θhi、θki分别表示第i条腿髋关节和膝关节的输出信号;
Ak、Ah分别表示髋关节和膝关节的摆动幅值;
φ表示腿部关节的布置形式，当腿的关节布置形式为肘式时φ=1,腿的关节布置形式为膝式时φ=-1。在我们的设计中，采用的是膝式，所以，在公式中的φ取-1。
3.3hopf数学模型生成的理论波形
这个hopf波形图是从论文中截下来的，下面有我自己在matlab中自己生成的波形图，可与论文中的作对比，下面还有部分代码，有图有真相。
四. 步态生成
4.1matlab建立hopf模型
- function dydt = f( t,x)
- %F 此处显示有关此函数的摘要
- % 此处显示详细说明
- persistent xo;
- if isempty(xo)
- x=[0.1;0;0;0;0;0;0;0];
- xo=1;
- end
- a=10000;
- B=0.5; %0.75
- u=0.5;
- Wsw=4*pi;
- A=100;
-
- QLF=0;%1
- QRH=0;%3 0.25
-
- QRF=0.5;%2
- QLH=0.5;%4
-
- W1=(((1-B)/B)*Wsw)/(exp(-1*A*x(2))+1)+(Wsw/(exp(A*x(2))+1));
- W2=(((1-B)/B)*Wsw)/(exp(-1*A*x(4))+1)+(Wsw/(exp(A*x(4))+1));
- W3=(((1-B)/B)*Wsw)/(exp(-1*A*x(6))+1)+(Wsw/(exp(A*x(6))+1));
- W4=(((1-B)/B)*Wsw)/(exp(-1*A*x(8))+1)+(Wsw/(exp(A*x(8))+1));
-
- r1=x(1)^2+x(2)^2;
- r2=x(3)^2+x(4)^2;
- r3=x(5)^2+x(6)^2;
- r4=x(7)^2+x(8)^2;
-
-
-
- th11=2*pi*(QLF-QLF);
- R11 = [cos(th11), -sin(th11);
- sin(th11), cos(th11)];
-
- %th21=-pi;
- th21=2*pi*(QLF-QRF);
- R21 = [cos(th21), -sin(th21);
- sin(th21), cos(th21)];
-
- %th31=-pi/2;
- th31=2*pi*(QLF-QRH);
- R31 = [cos(th31), -sin(th31);
- sin(th31), cos(th31)];
-
- % th41=-3*pi/2;
- th41=2*pi*(QLF-QLH);
- R41 = [cos(th41), -sin(th41);
- sin(th41), cos(th41)];
-
- %2行
- % th12=pi;
- th12=2*pi*(QRF-QLF);
- R12 = [cos(th12), -sin(th12);
- sin(th12), cos(th12)];
-
- % th22=0;
- th22=2*pi*(QRF-QRF);
- R22 = [cos(th22), -sin(th22);
- sin(th22), cos(th22)];
-
- % th32=pi/2;
- th32=2*pi*(QRF-QRH);
- R32 = [cos(th32), -sin(th32);
- sin(th32), cos(th32)];
-
- % th42=-pi/2;
- th42=2*pi*(QRF-QLH);
- R42 = [cos(th42), -sin(th42);
- sin(th42), cos(th42)];
-
- %3行
- % th13=pi/2;
- th13=2*pi*(QRH-QLF);
- R13 = [cos(th13), -sin(th13);
- sin(th13), cos(th13)];
-
- % th23=-pi/2;
- th23=2*pi*(QRH-QRF);
- R23 = [cos(th23), -sin(th23);
- sin(th23), cos(th23)];
-
- % th33=0;
- th33=2*pi*(QRH-QRH);
- R33 = [cos(th33), -sin(th33);
- sin(th33), cos(th33)];
-
- % th43=-pi;
- th43=2*pi*(QRH-QLH);
- R43 = [cos(th43), -sin(th43);
- sin(th43), cos(th43)];
- %4行
- % th14=3*pi/2;
- th14=2*pi*(QLH-QLF);
- R14 = [cos(th14), -sin(th14);
- sin(th14), cos(th14)];
-
- % th24=pi/2;
- th24=2*pi*(QLH-QRF);
- R24 = [cos(th24), -sin(th24);
- sin(th24), cos(th24)];
-
- % th34=pi;
- th34=2*pi*(QLH-QRH);
- R34 = [cos(th34), -sin(th34);
- sin(th34), cos(th34)];
-
- % th44=0;
- th44=2*pi*(QLH-QLH);
- R44 = [cos(th44), -sin(th44);
- sin(th44), cos(th44)];
-
-
- RR=[R11,R21,R31,R41;
- R12,R22,R32,R42
- R13,R23,R33,R43
- R14,R24,R34,R44];
-
- FF=[a*(u-r1)*x(1)-W1*x(2);
- a*(u-r1)*x(2)+W1*x(1);
- a*(u-r2)*x(3)-W2*x(4);
- a*(u-r2)*x(4)+W2*x(3);
- a*(u-r3)*x(5)-W3*x(6);
- a*(u-r3)*x(6)+W3*x(5);
- a*(u-r4)*x(7)-W4*x(8);
- a*(u-r4)*x(8)+W4*x(7);
- ];
- RR(abs(RR)-0<0.00001)=0;
- dydt=FF+RR*x;
-
- out=[];%输出变量
- count=1;
- temp=f([],[0.1;0;0;0;0;0;0;0]);%后面参数为初值
- y=zeros(8,1);
- y=y+temp*0.0001;%0.0001为步长
- for i=0:0.0001:5
- temp=f([],y);
- y=y+temp*0.0001;
-
-
- out(count,1)=y(1);
- out(count,2)=y(2);
- out(count,3)=y(3);
- out(count,4)=y(4);
- out(count,5)=y(5);
- out(count,6)=y(6);
- out(count,7)=y(7);
- out(count,8)=y(8);
- count=count+1;
- end
- t=0:0.0001:5;
- %绘图
- subplot(1,1,1)
- plot(t,out(:,1),t,out(:,2));
- title('hopf')
- axis([0,5,-1,1])%XY坐标均衡
- grid;
4.2足内关系协调（生成步态的逻辑）
trot
- for i=0:0.0001:5
- temp=f([],y);
- y=y+temp*0.0001;
- %处理膝关节
- if y(2)<=0
- out(count,2)=-1*sign(-1)*0.7067*y(2);
- else
- out(count,2)=0;
- end
- if y(4)<=0
- out(count,4)=-1*sign(-1)*0.7067*y(4);
- else
- out(count,4)=0;
- end
- if y(6)<=0
- out(count,6)=-1*sign(-1)*0.7067*y(6);
- else
- out(count,6)=0;
- end
- if y(8)<=0
- out(count,8)=-1*sign(-1)*0.7067*y(8);
- else
- out(count,8)=0;
- end
- out(count,1)=y(1);
- % out(count,2)=y(2);
- out(count,3)=y(3);
- % out(count,4)=y(4);
- out(count,5)=y(5);
- % out(count,6)=y(6);
- out(count,7)=y(7);
- % out(count,8)=y(8);
- count=count+1;
- end
walk
- R_cell={leg_num,leg_num};
- for i=1:leg_num
- for j=1:leg_num
- R_cell{j,i}=[cos(Phi(i)-Phi(j)) -sin(Phi(i)-Phi(j));sin(Phi(i)-Phi(j)) cos(Phi(i)-Phi(j))];
- end
- end
-
- for n=1:Pon_num
- for i=1:leg_num
- r_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)>0
- leg_k_Point_x(n,i)=0;
- else
- if(i<3)
- leg_k_Point_x(n,i)=-sign(Psa)*(Ak/Ah)*leg_y(i);
- else
- leg_k_Point_x(n,i)=sign(Psa)*(Ak/Ah)*leg_y(i);
- end
- end
- Foot_end_x(n,i)=sin(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l-sin(theta0+leg_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)*l+cos(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l);
- end
- end
- Phi(1)=0*2*pi; %Phi_LF
- Phi(2)=0.5*2*pi; %Phi_RF
- Phi(3)=0.25*2*pi; %Phi_RH
- Phi(4)=0.75*2*pi; %Phi_LH
-
- Ah=15;%asin((Beta*S)/(2*L));%髋关节摆动幅度
- Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
- %时间
- Pon_num=2000;
- t_begin=0; t_end=20; t_step=(t_end-t_begin)/(Pon_num-1);
- t=zeros(Pon_num,1);
- for n=1:Pon_num
- t(n)=(n-1)*t_step+t_begin;
- end
-
- %初始值，非0即可
- leg_x=zeros(leg_num,1);
- leg_y=zeros(leg_num,1);
- for i=1:leg_num
- leg_x(i)=0.5;
- leg_y(i)=0.5;
- end
-
- %CPG曲线，左前1，右前2，右后3，左后4
- leg_h_Point_x=zeros(Pon_num,leg_num);%髋关节
- leg_h_Point_y=zeros(Pon_num,leg_num);
- leg_k_Point_x=zeros(Pon_num,leg_num);%膝关节
- leg_k_Point_y=zeros(Pon_num,leg_num);
- Foot_end_x=zeros(Pon_num,leg_num);%足端
- Foot_end_y=zeros(Pon_num,leg_num);
-
- Phi=zeros(leg_num,1);
4.3matlab下在hopf模型中加入足内协调关系生成trot步态波形图
4.4matlab下在hopf模型中加入足内协调关系生成walk步态波形图
4.5将matlab下的算法代码移植到主控板中
（主控我用的是Stm32F103ZGT6）
hopf.c
- void dydt(hopf_type *parameter)
- {
- static unsigned char first_int_flag = 0;/* 第一次进入中断标志位 */
-
- /* 首次上电后进入的步态 */
- if(first_int_flag == 0)
- {
-
- if( gait_change.now_gait_mode == trot || gait_change.now_gait_mode == back )
- {
- Q3=0;
-
- }
- if( gait_change.now_gait_mode == walk || gait_change.now_gait_mode==sides_way || gait_change.now_gait_mode==turn )
- {
- Q3=0.25;
- }
- }
-
- /* 从trot步态到walk步态的平滑步态转换 */
- if(gait_change.target_gait_mode == walk || gait_change.target_gait_mode == sides_way || gait_change.target_gait_mode == turn )
- {
- if(Q3<0.25)
- {
- Q3+=0.05; //相位以每0.05连续变化 可自行修改
- }
- }
- /* 从walk步态到trot步态的平滑步态转换 */
- if(gait_change.target_gait_mode == trot || gait_change.target_gait_mode == back)
- {
- if(Q3>0)
- {
- Q3-=0.05; //相位以每0.05连续变化 可自行修改
- }
- }
-
- parameter->QRH = Q3;
- parameter->QLH = 0.5 + Q3;
-
-
- if(gait_change.target_gait_mode == sides_way || gait_change.target_gait_mode == turn)
- {
- /* 左右侧移时负载因子参数的变化 */
- if(res=='l') /* 左移B=0.43 */
- {
- parameter->B=0.43;
- }
- if(res=='r') /* 右移B=0.56 */
- {
- parameter->B=0.53;
- }
- }
-
- /* 振荡器频率 */
- parameter->W1 = ((1 - parameter->B) / parameter->B) * parameter->Wsw / (exp(-1 * parameter->A * parameter->y2) + 1) + (parameter->Wsw / (exp(parameter->A * parameter->y2) + 1));
- parameter->W2 = ((1 - parameter->B) / parameter->B) * parameter->Wsw / (exp(-1 * parameter->A * parameter->y4) + 1) + (parameter->Wsw / (exp(parameter->A * parameter->y4) + 1));
- parameter->W3 = ((1 - parameter->B) / parameter->B) * parameter->Wsw / (exp(-1 * parameter->A * parameter->y6) + 1) + (parameter->Wsw / (exp(parameter->A * parameter->y6) + 1));
- parameter->W4 = ((1 - parameter->B) / parameter->B) * parameter->Wsw / (exp(-1 * parameter->A * parameter->y8) + 1) + (parameter->Wsw / (exp(parameter->A * parameter->y8) + 1));
-
- /* 状态量 */
- parameter->r1 = powf(parameter->y1, 2) + powf(parameter->y2, 2);
- parameter->r2 = powf(parameter->y3, 2) + powf(parameter->y4, 2);
- parameter->r3 = powf(parameter->y5, 2) + powf(parameter->y6, 2);
- parameter->r4 = powf(parameter->y7, 2) + powf(parameter->y8, 2);
-
- /* 振荡器的相对相位 */
- parameter->th11 = 2 * pi * (parameter->QLF - parameter->QLF);
- parameter->th21 = 2 * pi * (parameter->QLF - parameter->QRF);
- parameter->th31 = 2 * pi * (parameter->QLF - parameter->QRH);
- parameter->th41 = 2 * pi * (parameter->QLF - parameter->QLH);
-
- parameter->th12 = 2 * pi * (parameter->QRF - parameter->QLF);
- parameter->th22 = 2 * pi * (parameter->QRF - parameter->QRF);
- parameter->th32 = 2 * pi * (parameter->QRF - parameter->QRH);
- parameter->th42 = 2 * pi * (parameter->QRF - parameter->QLH);
-
- parameter->th13 = 2 * pi * (parameter->QRH - parameter->QLF);
- parameter->th23 = 2 * pi * (parameter->QRH - parameter->QRF);
- parameter->th33 = 2 * pi * (parameter->QRH - parameter->QRH);
- parameter->th43 = 2 * pi * (parameter->QRH - parameter->QLH);
-
- parameter->th14 = 2 * pi * (parameter->QLH - parameter->QLF);
- parameter->th24 = 2 * pi * (parameter->QLH - parameter->QRF);
- parameter->th34 = 2 * pi * (parameter->QLH - parameter->QRH);
- parameter->th44 = 2 * pi * (parameter->QLH - parameter->QLH);
-
- /* 振荡器输出量 */
- parameter->dydt1 = parameter->a * (parameter->u - parameter->r1) * parameter->y1 - parameter->W1 * parameter->y2 +
- cos(parameter->th11) * parameter->y1 - sin(parameter->th11) * parameter->y2 +
- cos(parameter->th21) * parameter->y3 - sin(parameter->th21) * parameter->y4 +
- cos(parameter->th31) * parameter->y5 - sin(parameter->th31) * parameter->y6 +
- cos(parameter->th41) * parameter->y7 - sin(parameter->th41) * parameter->y8;
-
- parameter->dydt2 = parameter->a * (parameter->u - parameter->r1) * parameter->y2 + parameter->W1 * parameter->y1 +
- sin(parameter->th11) * parameter->y1 + cos(parameter->th11) * parameter->y2 +
- sin(parameter->th21) * parameter->y3 + cos(parameter->th21) * parameter->y4 +
- sin(parameter->th31) * parameter->y5 + cos(parameter->th31) * parameter->y6 +
- sin(parameter->th41) * parameter->y7 + cos(parameter->th41) * parameter->y8;
-
- parameter->dydt3 = parameter->a * (parameter->u - parameter->r2) * parameter->y3 - parameter->W2 * parameter->y4 +
- cos(parameter->th12) * parameter->y1 - sin(parameter->th12) * parameter->y2 +
- cos(parameter->th22) * parameter->y3 - sin(parameter->th22) * parameter->y4 +
- cos(parameter->th32) * parameter->y5 - sin(parameter->th32) * parameter->y6 +
- cos(parameter->th42) * parameter->y7 - sin(parameter->th42) * parameter->y8;
-
- parameter->dydt4 = parameter->a * (parameter->u - parameter->r2) * parameter->y4 + parameter->W2 * parameter->y3 +
- sin(parameter->th12) * parameter->y1 + cos(parameter->th12) * parameter->y2 +
- sin(parameter->th22) * parameter->y3 + cos(parameter->th22) * parameter->y4 +
- sin(parameter->th32) * parameter->y5 + cos(parameter->th32) * parameter->y6 +
- sin(parameter->th42) * parameter->y7 + cos(parameter->th42) * parameter->y8;
-
- parameter->dydt5 = parameter->a * (parameter->u - parameter->r3) * parameter->y5 - parameter->W3 * parameter->y6 +
- cos(parameter->th13) * parameter->y1 - sin(parameter->th13) * parameter->y2 +
- cos(parameter->th23) * parameter->y3 - sin(parameter->th23) * parameter->y4 +
- cos(parameter->th33) * parameter->y5 - sin(parameter->th33) * parameter->y6 +
- cos(parameter->th43) * parameter->y7 - sin(parameter->th43) * parameter->y8;
-
- parameter->dydt6 = parameter->a * (parameter->u - parameter->r3) * parameter->y6 + parameter->W3 * parameter->y5 +
- sin(parameter->th13) * parameter->y1 + cos(parameter->th13) * parameter->y2 +
- sin(parameter->th23) * parameter->y3 + cos(parameter->th23) * parameter->y4 +
- sin(parameter->th33) * parameter->y5 + cos(parameter->th33) * parameter->y6 +
- sin(parameter->th43) * parameter->y7 + cos(parameter->th43) * parameter->y8;
-
- parameter->dydt7 = parameter->a * (parameter->u - parameter->r4) * parameter->y7 - parameter->W4 * parameter->y8 +
- cos(parameter->th14) * parameter->y1 - sin(parameter->th14) * parameter->y2 +
- cos(parameter->th24) * parameter->y3 - sin(parameter->th24) * parameter->y4 +
- cos(parameter->th34) * parameter->y5 - sin(parameter->th34) * parameter->y6 +
- cos(parameter->th44) * parameter->y7 - sin(parameter->th44) * parameter->y8;
-
- parameter->dydt8 = parameter->a * (parameter->u - parameter->r4) * parameter->y8 + parameter->W4 * parameter->y7 +
- sin(parameter->th14) * parameter->y1 + cos(parameter->th14) * parameter->y2 +
- sin(parameter->th24) * parameter->y3 + cos(parameter->th24) * parameter->y4 +
- sin(parameter->th34) * parameter->y5 + cos(parameter->th34) * parameter->y6 +
- sin(parameter->th44) * parameter->y7 + cos(parameter->th44) * parameter->y8;
-
- /* 输出量积分 */
- if (first_int_flag == 0)
- {
- parameter->y1 = 0.0f + parameter->dydt1 * 0.0001f;
- first_int_flag = 1;
- }
- else
- {
- parameter->y1 = parameter->y1 + parameter->dydt1 * 0.0001f;
- }
- parameter->y2 = parameter->y2 + parameter->dydt2 * 0.0001f;
- parameter->y3 = parameter->y3 + parameter->dydt3 * 0.0001f;
- parameter->y4 = parameter->y4 + parameter->dydt4 * 0.0001f;
- parameter->y5 = parameter->y5 + parameter->dydt5 * 0.0001f;
- parameter->y6 = parameter->y6 + parameter->dydt6 * 0.0001f;
- parameter->y7 = parameter->y7 + parameter->dydt7 * 0.0001f;
- parameter->y8 = parameter->y8 + parameter->dydt8 * 0.0001f;
-
- /* 模拟起步虚拟时间 */
- // if (gait_change.gait_init_flag == 1)
- // {
- // starting.time++;
- // }
- // else
- // {
- // starting.time = starting.time;
- // }
- /*--------------------------------------------------------------------------trot步态走 ----------------------------------------------------------------------*/
- /* 膝关节处理 */
- if( gait_change.target_gait_mode == trot || gait_change.target_gait_mode == walk || gait_change.target_gait_mode == back || gait_change.target_gait_mode == mark_time )
- {
-
- if (parameter->y2 <= 0) /* 改变与髋关节的耦合关系 */ /* 2，4，6，8 分别对应每条腿的膝关节的振荡器 */
- {
- out2 = (-1) * parameter->y2 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time); /* 改变膝肘式 */
- }
- else
- {
- out2 = 0.0f;
- }
-
- if (parameter->y4 <= 0)
- {
- out4 = (-1) * parameter->y4 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
- else
- {
- out4 = 0.0f;
- }
-
- if (parameter->y6 <= 0)
- {
- out6 = (-1) * parameter->y6 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
- else
- {
- out6 = 0.0f;
- }
-
- if (parameter->y8 <= 0)
- {
- out8 = (-1) * parameter->y8 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
- else
- {
- out8 = 0.0f;
- }
- out1 = parameter->y1 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- out3 = parameter->y3 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- out5 = parameter->y5 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- out7 = parameter->y7 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
-
- // steering_move(); /* 舵机控制 */
- }
-
-
- if( gait_change.target_gait_mode == sides_way )
- {
- /* 根据walk步态相位生成的髋关节和膝关节函数曲线 */
- if (parameter->y1 >= 0)
- {
- out1 = (-1)*parameter->y1 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time); // out1 与 out2 踏步逻辑
- out2 = parameter->y1 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
-
- if (parameter->y3 >= 0)
- {
- out3 = (-1)*parameter->y3 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- out4 = parameter->y3 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
-
- if (parameter->y5 >= 0)
- {
- out5 = (-1)*parameter->y5 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time); // out1 与 out2 踏步逻辑
- out6 = parameter->y5 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
-
- if (parameter->y7 >= 0)
- {
- out7 = (-1)*parameter->y7 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- out8 = parameter->y7 * sigmoid(0, starting.a, starting.interval, starting.time, starting.start_time);
- }
-
- /* 生成侧移曲线 */
- /*——————————————————生成 LF RH 侧移曲线逻辑————————————————————__*/
- if(derivation_out2(date,out2)==1)
- {
- out11=(-1)*out1; /* out11——LF */
- out15=out11; /* out15——RH */
- }
- else if(derivation_out4(date,out4)==1)
- {
- out11=(-1)*out7;
- out15=out11;
- }
- else
- {
- if(out11>0.95)
- {
- out11=1;
- }
- else
- {
- out11=(-1)*out1;
- }
- out15=out11;
- }
hopf.h
- #define pi 3.1415926f /* 圆周率 */
- #define l 0.115f /* 膝关节长度长度(m) */ /*0.115f*/
- #define m 0.1f /*髋关节长度(m)*/ //m*cos(theat_Ah)+l*cos(theat_Ak)
-
-
- #define theat_Ak 30*(pi/180) /* 膝关节平衡位置（腿节与垂直方向的夹角） */ //15 /*弧度制*/ //10
- #define theat_Ah 60*(pi/180) /* 髋关节平衡位置（腿节与垂直方向的夹角） */ /*弧度制*/ //40
-
- /* 步态模式 */
- typedef enum
- {
- temp, /* 初始时为空 */
- mark_time, /* 踏步 */
- walk, /* 单步行走 四拍步态 左前--右后--右前--左后->顺序 稳定行走 但速度慢 B_i = 0.75 */
- trot, /* 对角小跑 双拍步态 左后右前--右后左前->顺序 */
- pace, /* 同侧溜步 双拍步态 左后左前--右后右前->顺序 */
- gallop, /* 奔跑 准双拍步态 左前--右前--左后--右后->顺序 */
- bound, /* 跳跑 双拍步态 左前右前--左后右后->顺序 */
- pronk, /* 四足跳跃 单拍步态 0--0--0--0 */
- turn, /*自想侧移步态 相位同trot步态 */
- sides_way, /*侧移步态 相位关系同walk步态 */
- back,
- }gait_mode;
-
- /* sigmoid过渡 */
- typedef struct
- {
- float a; /* 参数0~1，越大跟随越快 */
- float interval; /* 过渡期间 */
- float start_time; /* 开始过渡时刻 */
- int time; /* 当前时刻 */
- }sigmoid_type;
-
- /* hopf振荡器的基本参数 */
- typedef struct
- {
- /* 基本参数 */
- float a;
- float B; /* 上升段和下降段时间调整 */
- float u; /* 改变幅值 */
- float Wsw; /* 摆动相频率 （改变周期） */
- float A;
-
- /* 步态参数 */
- float v; /* 速度 m/s */
- float T; /* 步态周期 s */
- float h; /* 最大足端高度 m */
- float s; /* 步长 v*T m */
-
- float Ah; /* 髋关节摆动幅值 */
- float Ak; /* 膝关节摆动幅值 */
- float L; /* 髋关节与足端的距离 */
-
- float QLF; /* 左前 */
- float QRF; /* 右前 */
- float QRH; /* 右后 */
- float QLH; /* 左后 */
-
- float W1;
- float W2;
- float W3;
- float W4;
-
- float y1, y2, y3, y4, y5, y6, y7, y8;
-
- float r1, r2, r3, r4;
-
- float th11, R11,
- th21, R21,
- th31, R31,
- th41, R41,
-
- th12, R12,
- th22, R22,
- th32, R32,
- th42, R42,
-
- th13, R13,
- th23, R23,
- th33, R33,
- th43, R43,
-
- th14, R14,
- th24, R24,
- th34, R34,
- th44, R44;
-
- /* 输出量 */
- float dydt1, dydt2, dydt3, dydt4, dydt5, dydt6, dydt7, dydt8;
-
- }hopf_type;
参数展示
- /* 振荡器最终的输出量 */
- float out1, out2, out3, out4, out5, out6, out7, out8; /* 1:LF 3:RF 5:RH 7:LH */
- float out11, out13, out15, out17;/* 横滚髋关节的输出量 */
-
- /*----------------------------------------------------------基本步态--------------------------------------------------------*/
-
- /* 侧移 右移 */
- hopf_type sides_way_parameter =
- {
- .a = 5000, /* 振荡器基本参数 */
- .B = 0.53, /* 0.56 */ /* 负载因子（支撑相的占比） */ /*右移B=0.56 左移B=0.43*/
- .u = 1.0, /* 改变振荡器的幅值 */
- .Wsw =110* pi, /*100*/ /* 摆动相频率（改变振荡器周期） */
- .A = 100, /* 振荡器基本参数 */
-
- .v = 0.25, /* 速度 m/s */
- .T = 0.25, //0.25
- .h = 0.008, //0.008 /* 最大足端高度 m */ /* 同时决定侧关节的抬高的幅值 */
-
- .QLF = 0, /* 左前腿相位 */
- .QRF = 0.5, /* 右前腿相位 */
- // .QRH = Q3, /* 右后腿相位 */
- // .QLH = 0.5 + Q3, /* 左后腿相位 */
-
- .y1 = 0.1, /* 初始值输入 */
- };
-
- hopf_type trot_parameter =
- {
- /* 振荡器固定参数 */
- .a = 5000, //振荡器的震荡(振荡器收敛到极限环的速度)
- .A = 100, //不可太大（50~300）（决定了支撑相和摆动相之间过渡变化的速度）
-
- .B = 0.48, /* 0.48 */ // 支撑相占比
- .u = 1.0, // 振荡器输出幅值
- .Wsw = 230 * pi, /*200*/ /* 210 */ /* 振荡器输出速率 函数输出随时间变化的速率 此处其实是模型中的 W */
-
- /* 决定Ah幅值的参数 */
- .T = 0.3, /* 此处的周期是单独赋予的一个值 只决定Ah的大小 s=vt */
- .v = 0.2, /* 此处的速度是单独赋予的一个值 只决定Ah的大小 s=vt */
-
- /* 决定Ak幅值的参数 */
- .h = 0.02, /* 膝关节抬高的高度 决定Ak的大小 */
-
- /* 各腿之间的相位 */
- /* hopf振荡器各腿输出存在的固定相位 */
- .QLF = 0,
- .QRF = 0.5,
- // .QRH = 0,
- // .QLH = 0.5,
-
- .y1 = 0.1, /* 振荡器输出的第一个输出值的初值 不为0 即可 */
- };
- /* 转弯（ turn ） */
- hopf_type turn_parameter =
- {
- .a = 4000, /* 振荡器基本参数 */
- .B = 0.63, /* 负载因子（支撑相的占比） */
- .u = 1.0, /* 改变振荡器的幅值 */
- .Wsw = 260* pi, /* 摆动相频率（改变振荡器周期） */
- .A = 100, /* 振荡器基本参数 */
-
- .v = 0.3, /* 速度 m/s */
- .T = 0.01, /* 步态周期 s */
- .h = 0.014, //0.013 /* 最大足端高度 m */ //0.015
-
- .QLF = 0, /* 左前腿相位 */
- .QRF = 0.5, /* 右前腿相位 */
- // .QRH = 0.25, /* 右后腿相位 */
- // .QLH = 0.75, /* 左后腿相位 */
-
- .y1 = 0.1, /* 初始值输入 */
- };
-
- /* 踏步（mark_time） */
- hopf_type mark_time_parameter =
- {
- .a = 10000, /* 振荡器基本参数 */
- .B = 0.5, /* 负载因子（支撑相的占比） */
- .u = 1.0, /* 改变振荡器的幅值 */
- .Wsw = 200* pi, /* 摆动相频率（改变振荡器周期） */
- .A = 100, /* 振荡器基本参数 */
-
- .v = 0.3, /* 速度 m/s */
- .T = 0.2, /* 步态周期 s */
- .h = 0.02, /* 最大足端高度 m */
-
- .QLF = 0,
- .QRF = 0.5,
- // .QRH = 0,
- // .QLH = 0.5,
-
- .y1 = 0.1, /* 初始值输入 */
- };
4.6主控与上位机建立通信在上位机上将数据以波形图的形式展现
Trot
walk
Turn
以上贴出来的只是部分代码，完整代码可在以下链接自行下载，或者私我
四足机器人 Matalb hopf算法+步态算法 代码https://download.csdn.net/download/weixin_49585296/85093057四足机器人Stm32F1控制算法代码 keil代码https://download.csdn.net/download/weixin_49585296/85095491
还有一些四足相关论文资料由于版权原因发布不了，可私信找我要……^^
有什么代码和控制思路方面的问题大家一起学习交流，持续更新中……（下一篇可能会详细讲解分析下代码）
