四足机器人|机器狗|仿生机器人|多足机器人|Adams仿真|Simulink仿真|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|_adams四足机器人运动学分析

基于CPG的四足机器人Simulink与Adams虚拟样机系统搭建教程

特别鸣谢Dong、kmjer、Min、Janfly

摘要

针对目前仿生四足机器人控制中存在的稳定性低、控制精度不高、可控性差等问题，本文引入一种基于CPG(中央模式发生器)的步态控制算法模型，CPG 生成的节律运动具有独立性与稳定性，还具有反馈调整功能，能够实现仿生机器人的运动控制。针对仿生机器人研发周期长与成本高的问题，本文利用Simulink与Adams构建虚拟样机对步态控制模型进行联合仿真验证运动控制算法可行性。通过计算得出了精确的足间协调、足内协调相位关系，使仿生机器人能够实现Walk与Trot两种步态与转弯控制。针对克服复杂环境问题还加入了反馈环节，能够实现简单的环境自适应，引入坡度等反馈至CPG网络后，仿生机器人能够与环境进行交互，自调整适应爬坡动作。并在仿真中将四足机器人运动模型嵌入到基于行为控制框架，对多机器人的任务进行规划并综合输出，稳定运动的基础上实现避障功能。结果表明，基于CPG的步态控制方法在控制简便性、机器人环境适应性、运动速度方面优于传统方法，能够有效控制仿生机器人以各种步态稳定行走，使其运动具有较强的适应性，实时性，协调性与准确性。研究节律运动并应用到四足机器人有广阔的前景，深入开展四足机器人理论及研究关键技术有着非常深远的学术与实践意义。而且利用Simulink和Adams联合仿真能够显著降低仿生机器人的开发周期与成本。

1. 具有显著的仿生学特性。本文借鉴生物中枢神经的中央模式发生器(Central pattern generator, CPG)机制，设计出带反馈功能的 HOPF 震荡网络，以此控制12个关节协调运动实现机器人的步态控制。
2. 加入反馈后能够实现简单的环境自适应。引入坡度等反馈至 CPG 网络后，仿生机器人能够与环境进行交互，自调整适应爬坡动作。
3. 能显著降低仿生机器人开发的调试时间与成本。利用 Simulink 和Adams 联合仿真搭建仿生机器人虚拟样机，进行运动学及动力学分析后能规避研发设计中的大部分问题。
4. 多仿生机器人协同控制。利用基于零空间投影(NSB)行为控制方法对多个仿生机器人进行任务分配及避障规划。
   

1. 四足机器人相关资料及文献

知名的四足机器人团体有波士顿动力学公司的Bigdog、Spotdog，MIT的Mini Cheetah robot，国内的则有浙江绝影、宇树科技的laikadog。加上19年MIT的一名硕士生开源了一个四足机器人的源码，使得国内的四足机器人企业如雨后春笋般涌现市场。然而我的观点是四足机器人的技术核心在于电机设计及机身稳定的非线性控制环节，目前聚焦研究如何利用强化学习与MPC做决策控制。最近两年，某宝上也有专为四足机器人设计相关的伺服舵机出售，宇树科技也出售四足机器人电机，但是价格较贵，但为了可控性高，可能需要自己设计或者定制电机。
相关的资料及网址：
波士顿动力学：https://www.bostondynamics.com/
宇树科技：https://www.unitree.com/cn
杭州云深处科技：http://www.deeprobotics.cn/
知乎好文：https://zhuanlan.zhihu.com/p/79391139?utm_source=wechat_session&utm_medium=social&s_r=0
MIT 开源代码及相关资料：MIT Biomimetic Robotics Lab 以 MIT Cheetah 3, MIT Cheetah 2, MIT Cheetah Mini 闻名。lab 成员 Benjamin Katz 在他的硕士论文[1]中开源了 MIT Cheetah Mini 电机驱动器, 连接12个电机与机载电脑的中心板(SPIne)的代码和硬件：
电机控制硬件：https://github.com/bgkatz/3phase_integrated
电机控制固件：https://link.zhihu.com/?target=https%3A//os.mbed.com/users/benkatz/code/Hobbyking_Cheetah_Compact_DRV8323/
SPine硬件：https://github.com/bgkatz/SPIne
https://link.zhihu.com/?target=https%3A//os.mbed.com/users/benkatz/code/SPIne/
SPine固件：https://os.mbed.com/users/benkatz/code/SPIne/
Cheetah-Software-master四足机器人源码:
https://github.com/mit-biomimetics/Cheetah-Software
CSDN转载：
https://download.csdn.net/download/qq_28044715/12281175?utm_medium=distribute.pc_relevant.none-task-download-BlogCommendFromMachineLearnPai2-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-download-BlogCommendFromMachineLearnPai2-1.control

相关文献：
[1] 张秀丽. 四足机器人节律运动及环境适应性的生物控制研究[D].北京:清华大学,2004.
[2] 徐海东，干苏，任杰等.基于 Hopf 振荡器的四足机器人步态 CPG 调节[J].2017,12,29(12)1-6.
[3] 何冬青.JTUWM-III 四足机器人 trot 步态运动特性研究 [D]. 上海：上海交通大学，2006.
[4] 罗庆生，罗霄.仿生四足机器人[M].北京：北京理工大学出版社：2014.

2. 四足机器人结构设计及方案对比

本文以犬类四足哺乳动物为仿生学对象，结构设计参照足式动物的身体特征，进而设计出用足进行移动的机械结构。足式机器人的运动机构是整机系统的基础，直接影响到机器人的步态控制，而其机构设计可以借鉴犬科四足动物的仿生学特征，抽象后进行仿生机器人运动学分析。该结构在外观上符合足式动物，单足关节主要包括有髋关节、膝关节和踝关节。

其中，前腿的肩胛骨、后腿的股骨能够侧摆，在横移和原地旋转过程中起重要作用。肩关节能使整个大腿进行俯仰运动，膝关节则能使大腿下部进行俯仰运动，为行走时提供前进的动力，并且有效调整自身的姿态；踝关节则是在行走落地时提供缓冲的作用。总的来说，犬科动物前后腿结构均是三腿节三关节，四个自由度。

2.1 四足机器人关节布置方式

现有的四足机器人中，北京理工大学的液压机器人与清华大学张秀丽教授的实验样机是采用前肘后膝式，而部分Bigdog采用前膝后肘式布置方式、部分Bigdog、Spotdog、Laikadog、绝影都为全膝性布置方式。
由于三腿节的机械设计复杂程度要远远大于两腿节，而且很难保证仿生机器人的结构稳定性。出于对设计简化与稳定性的考虑，本课题选用两腿节、三自由度的腿部结构进行仿生机器人的腿部方案设计。另外，罗庆生验证了前肘后膝式的静、动态稳定性比其他三种要高，出于对机器人的高稳定性要求，本文选择了前肘后膝式的关节布置形式。

2.2 侧摆、髋、膝关节驱动方式

侧摆与髋关节的驱动目前大多都采用的都是无刷电机+行星齿轮减速器或者永磁同步电机+行星齿轮减速器直接拖动。而驱动方式多采用FOC（Field-Oriented Control），即磁场定向控制，也称矢量变频，是目前无刷直流电机（BLDC）和永磁同步电机（PMSM）高效控制的最佳选择。FOC精确地控制磁场大小与方向，使得电机转矩平稳、噪声小、效率高，并且具有高速的动态响应。由于FOC的优势明显，目前已在很多应用上逐步替代传统的控制方式，在运动控制行业中备受瞩目。

而膝关节的驱动对控制效果的影响十分巨大，早期人们研究（2000-2010年）的电驱动型四足机器人利用伺服电机+减速器直接带动膝关节，这会使到机器人的足端重量过大，臃肿，基本无法实现比较精准灵活的控制。近几年灵活的一些电驱四足机器人的膝关节驱动方案主要有两种：

1. 皮带轮驱动
2. 四连杆或者推杆驱动（要求有连杆运动学设计）
   据我调研，Spotdog、Laikadog、绝影都采用四连杆或者推杆的形式驱动，某宝上面的一个仿MIT的四足机器人（DOGOTIX-019B-BD）采用的是皮带轮拖动。

采用连杆驱动会有一个传动关系，根据机械原理相关知识对四连杆进行驱动分析，进而获得膝关节转动角度与减速器输出角度的映射关系，具体求法如下：

2.3 仿生四足机器人的机械设计模型

机器人具有四个足，每个足有三个关节，对应三个自由度，每个自由度由一个电机驱动。以此为基础，根据机械设计与传动相关知识，可以在SolidWorks软 件中设计出仿生四足机器人的如图三维模型如图。三个电机布置于大腿及髋关节处，髋关节电机通过减速器直接连接，而膝关节则通过连杆由电机间接驱动。该模型建立为Adams仿真环境搭建提供一个模型基础。机器人模型有放沐风网也有百度云链接：https://my.mfcad.com/Tuzhi/Sou/index?cc=1&keyword=%BB%F9%D3%DACPG&srid=160787080219726769952201https://my.mfcad.com/Tuzhi/Sou/index?cc=1&keyword=%BB%F9%D3%DACPG&srid=160787080219726769952201

2.4 简要的运动学分析与姿态角分析

由于仿生四足机器人的足采用是串联的三连杆机械臂，因此可以利用机器人学（Robot model and control）中D-H建模法对之进行建模，根据关节布置方式可得相关的D-H参数（具体建模可以参考熊有伦机器人学这本书），进而建立MATLAB姿态仿真调试模型如图。
在该模型中，可以通过对12个关节的角度调整，协调控制机身的欧拉角，进而调整姿态。反之，可以根据姿态欧拉角及上述运动学逆解公式解算出每个关节的角度，这个地方就可以为机身稳定性控制提供基础，能够利用十二个电机的配合控制机身姿态，并利用位姿反馈传感器（如加速度传感器）做姿态的闭环控制等。

单腿的运动学正解公式：
（B、T为D-H参数决定的坐标变换矩阵。）
运动学逆解（本文利用几何法+余弦定理求解三连杆机械臂关节角度），具体见我另一篇博客。

机身姿态的精确描述：在MATLAB机器人工具箱里的位姿调试（机器人模型搭建可见第八章附件代码）。
机身欧拉角的定义：

3. Adams虚拟样机配置

3.1 Adams环境搭建

利用上述所绘制的三维模型，可以通过三维模型导入的形式得到仿生四足机器人Adams虚拟样机模型。对主要的参数进行配置，主要包括机器人零部件的材料属性、约束、驱动、接触力等约束，并设置实验测量指标，收集数据验证实验。具体Adams模型各项参数在图中均有显示。

1. 三维模型导入。由于Adams软件的三维建模操作不友好且仿真度不高，本课题将设计软件的三维模型导入Adams软件，再进行相应的运动学及动力学配置。具体的细节可参考Solidworks文件导入ADAMS的相关博客，主体思路是将SolidWorks或者其他三维设计的模型转格式为X_T文件再导入Adams进行实体配置。本文除机器人外还配置了一个长红色矩形作为地面部件。

2. 材料属性配置。该步骤主要是对虚拟样机平台的各个零部件进行质量、密度、材质相关的配置，能够让机械系统有较高的现实仿真度。

3. 关节连接约束配置。该步骤主要是对仿生机器人虚拟样机的关节连接以及某些刚性连接进行配置，限制机器人的冗余自由度，只留下能控的电机、关节可供控制的自由度。

4. 力矩驱动配置。该步骤主要是对上述配置好的铰链约束添加驱动信号，添加后可以通过控制节点输入控制信号（如力矩、角速度、角度等），实现虚拟样机的运转。

5. 接触力设置。仿生机器人通过利用足端对地面的摩擦力作为行走的驱动力，该步骤根据该环境要求对虚拟样机系统中的足端及地面设置相应的接触力与摩擦力系数。注：接触力设置参数参考现实世界中橡胶与水泥地面的力指数与摩擦因数。
   

6. 系统单元配置。仿生机器人需要通过API接口与MATLAB软件或者Simulink建立通讯，该步骤设置每个关节控制输入、输出系统变量，以此为数据接口展开虚拟样机的联合仿真实验。

7. 实验测量指标配置。该步骤配置如关节角度、位移、力矩、欧拉角等实验测量指标，用于衡量、评估仿生机器人虚拟样机的的运动仿真性能。

8. 机械控制系统输出。该步骤可以生成一个MATLAB文件与一个Simulink文件，用于配置Simulink与Adams联合仿真的通讯连接环境。
   

10. 斜坡环境配置。本步骤配置一个机器人需要翻越的坡面（ 15度），验证仿生机器人虚拟样机的爬坡性能。

11. 多智能体避障环境配置。该步骤配置三个机器人与三个障碍物，验证零空间投影行为控制（NSBC）方法在仿生四足机器人编队控制的可行性。
    

至此，Simulink与Adams联合仿真的环境配置完成。

4. 基于CPG足机器人的机器人的步态设计

4.1 步态定义

四足动物的步态是指各个腿之间具有固定相位关系的行走模式，不同的动物由于自身条件的限制，如腿长、腿的位置、神经控制方式等，其步态也会变得不一样。就如双足动物的“行走”、“奔跑”，四足动物的行走（Walk）、对角小跑（Trot）、奔跑（Gallop）、溜步（Pace）、跳跑（Bound）、原地四足跳跃。以 LF(左前腿)、RF(右前腿)、RB(右后腿)、LB(左后腿)分别替代四足动物的四条腿，然后可以根据其步态的特点得出它们各自的相位关系(左前腿作为参考基准，φ_LF=0 ，即相位为0，一个周期为2*pi )。
步态是实现仿生四足机器人运动性能的基础规划，步态的协调性、连续性与可调整性直接决定着仿生四足机器人运动性能的优劣。
步态可以由特定的几个参数表征，相位差φ_i 、负载因子β、步态周期T、步长S、抬腿高度h，步态表征参数的具体定义如下：

4.2 四足动物的足间步态分析

通过对四足动物的运动观察与分析，总结出了四足动物的步态描述表，其中的节奏可以分为单拍、双拍、准双拍、四拍步态，可以从四足动物足端拍打地面的节拍进行区分，例如Trot是双拍步态，Walk是四拍步态。
另外，负载因子（步态占空比）β衡量腿部与地面的接触时间与步态周期的比重，有着重要的意义，因为当β等于0.5时，机器人在任意时刻或者平均约有两条腿支撑地面，而当β大于或者等于0.5的时候，说明机器人在任意时刻或者平均至少有三条腿支撑地面，这对机器人的步态输出有着极其重要的作用。其中 walk 步态β值为 0.75，trot 和其余常规步态的β为0.5。
如图所示，起步时，左前腿LF抬起后以一定的足端轨迹向前跨步，待LF落地后紧跟着右后腿RB跨腿，之后再轮到RF、LB进行相同的动作，行走的过程中，该动作重复循环，步态周期较长并且在走动的过程始终保持三条腿在支撑身体。
而小跑步态，先是LF与RB同时抬起并同步抬腿运动，当LF与RB重新落到地面时候，RF与LB跨腿，重复循环，为身体保持平稳，步态周期较短，在小跑过程中的任意时刻都只有对角腿在支撑身体。

步态示意图中，白色格子代表该腿部处于支撑相，黑色格子处表示该腿部处于摆动相，LF、LB、RF、RB分别是腿部的命名编号。

对四足动物的步态分析进行总结，可以得出四足动物walk与trot步态的四腿的相位关系拓扑网络（注：一般图论中箭头表示信号传递，但此处箭头表征指向方向与起始点腿的相位差），可以作为四足机器人步态规划与行走稳定控制的基础控制拓扑结构。

Walk步态每个相邻腿间相位相差0.25个步态周期，而Trot步态对角腿相位相同，相邻腿间的步态相差0.5个步态周期。

4.3 足内协调分析

由于足内关系也是步态的一部分，在研究完四足动物的足间协调规律之后，接下来需要去研究同一条腿之中髋关节与膝关节之间的相位关系。为了简化控制难度，需要提出以下的必要性假设：膝关节的运动不影响四足机器人的步长，步长只由髋关节决定，并且膝关节运动的主要目的是提高抬腿的足末端高度，使得步态圆滑，机器人抬腿、落地流畅，不会出现搓地、绊脚的问题。
张秀丽[1]通过高速摄像机观察四足动物的行走协调，总结出了具有普遍意义的四足机器人足内髋关节与膝关节的协调规律：①在四足动物在正常的行走过程中，同一条腿的膝关节与髋关节存在着固定的相位函数关系；②在步态处于摆动相时，膝关节与髋关节同步运动，髋关节摆动的前阶段，膝关节往身体内屈腿，当运动到摆动中点时，膝关节屈腿角度到最大，运动到摆动后期时，膝关节开始伸腿展开，当运动到摆动终点时，膝关节恢复原位；③处于支撑相时，髋关节往后摆动推动躯体，而膝关节保持原状态不动。
基于上述髋关节与膝关节的运动规律，本课题引入半波函数（膝关节处腿部摆动相时，按波形运动，支撑相时不运动）可以得知膝关节的控制曲线表示为:

式子中Ah与Ak分别代表髋关节与膝关节驱动波形的幅值，即使运动周期中的最大转动角度； θ（t）表关节的驱动信号关于时间的函数可计算得出髋关节与膝关节的波形图如。sgn()是一个标志函数。假设 是一个正弦函数 ，半波函数膝关节规划函数符合四足动物的步态规律，而且曲线圆滑、可调节性高，可以有效解决绊脚、搓地等问题。

4.3. 基于中央模式发生器（CPG）的步态控制系统建模

生物的节律步态是生物神经节律控制机理产生的一种自激振荡、相位互锁的运动模式，由生物低级神经中枢的中枢模式发生器 ( Central pattern Generator, CPG)产生的节律信号控制。常见的四足动物都是进行节律运动的。节律运动[1]是指空间和时间对称的周期性运动，譬如走、跑、跳等。因此，要实现四足机器人的稳定运动，就需要控制器能够产生具有周期性、对称性的节律信号，来控制各个关节(执行器)实现机体的节律运动。这种控制方式就是(CPG)中枢神经控制模式。
CPG是节律运动的中心控制器，不仅需要产生节律信号，控制执行器进行运动，还需要根据反馈信号进行识别，及时修改生成新的信号，使四足机器人能够稳定行走。
作为主流的机器人控制方式，CPG模型可以划分为两种，第一种是基于神经元的模型，十分贴切生物，但是模型复杂，如Kimura；第二种是基于非线性振荡器的模型[1]，模型相对比较简单，模型运用相当成熟。数学上，常用的非线性振荡器有Hopf、Rayleigh、Matsuoka等振荡器，其中Hopf振荡器是一种谐波振荡器，结构简单，控制实现容易，因此本文采用 Hopf 振荡器来建立四足机器人的CPG模型。本文会有两个数值仿真：hopf &Kimura。
根据前两节足间与足内的拓扑关系可以构建出四足机器人的分层拓扑关系图（注：一般图论中箭头表示信号传递，但此处箭头表征指向方向与起始点腿的相位差）。

单个Hopf振荡器的数学表达式：

根据上节给定的拓扑关系构建的8路（髋关节4路、膝关节4路）Hopf振荡器的状态方程，可以利用积分器对该微分方程进行求解，不需像神经振荡器那样利用龙格库塔数值解法或者是牛顿法求解微分方程。

坐标变换阵或者说是步态权重矩阵，这个矩阵利用坐标变换原理，使得不同腿、关节之间的信号相位可以按照步态矩阵θij（i=j=4,表示腿间相位关系）给定的相位关系生成。

该图是Kimura神经振荡器的原理图，Kimura振荡器原作者与张秀丽教授的论文中均有呈现，图中连接方式为控制论的搭建形式，白色圆圈代表抑制（负效应），白色圆圈代表兴奋（正效应）。

该相位的拓扑关系利用权重矩阵的形式表征腿间的相位关系，具体的步态权重矩阵取法可以参考张秀丽博士论文。

Kimura神经振荡器的微分方程组如下：

5. 基于Simulink的机器人控制信号输出仿真

5.1 单个振荡器仿真

5.2 Hopf-CPG震荡网络仿真

Walk与Trot步态的髋、膝关节输出信号如图所示，从上到下分别是LF、RF、RB、LB腿的电机控制信号，黄色曲线为髋关节，紫色曲线为膝关节。可见曲线满足Trot要求的相位关系，且步态周期为1s，负载因子为0.5，波形输出无误，可以作为仿生四足机器人的关节输入信号。

5.2 利用fcn代替Simulink复杂模块

由于利用Simulink的基本模块运算速度慢，移植性差，不能有效地进行开发步态与Adams联合仿真，本课题利用Simulink的m-function模块对上述模型进行改进，将模型中的大多数运算都替换成成移植性与兼容性更高的fcn模块，实验表明，更换后能显著提高运行速度与控制的实时性。

将每个波形产生与调制的运算都改成fcn模块如图：

每个fcn模块源码如下，需要Simulink仿真源文件可以访问以下链接：

参数定义:

function [Ak,Ah,Wst] = fcn(gait,Wsw)
%#codegen
if gait == 1
    Beta =  0.75;
    Ah = 12.2;
    Ak = 7.6;
elseif gait == 2
    Beta =  0.5;
    Ah = 12.2;
    Ak = 7.6;
else
    Beta =  0.5;
    Ah = 10;
    Ak = 5;
end
Wst=((1-Beta)/Beta)*Wsw;%支撑相频率`
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

被积分环境（状态方程）：

function [X_dot,Y_dot] = fcn(Wsw,Wst,Phi,leg_x,leg_y,u1,u2)
%#codegen
X_dot = zeros(1,4);
Y_dot = zeros(1,4);
Alpha=10;%收敛速度
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
for i = 1:4
   r_seq = (leg_x(i)-u1(i))^2+(leg_y(i)-u2(i))^2;
   W = (Wst/(exp(-a*leg_y(i))+1))+(Wsw/(exp(a*leg_y(i))+1));
   Vx = Alpha * (u - r_seq)* (leg_x(i) - u1(i))  -  W * (leg_y(i) - u2(i));
   Vy = Alpha * (u - r_seq) * (leg_y(i) - u2(i)) + W * (leg_x(i) - u1(i));
   for j = 1:4
   Vx = Vx + cos(Phi(i)-Phi(j))*(leg_x(j) - u1(i)) - sin(Phi(i)-Phi(j))*(leg_y(j) - u2(i));
   Vy = Vy + cos(Phi(i)-Phi(j))*(leg_y(j) - u2(i)) + sin(Phi(i)-Phi(j))*(leg_x(j) - u1(i));
   end
   X_dot(i) = Vx;
   Y_dot(i) = Vy;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

足内协调：

function leg_k = fcn(leg_y,Ak,Ah)
%#codegen
Psa = -1;
leg_k = zeros(1,4);
for i=1:1:4
    if leg_y(i) < 0
        leg_k(i) = 0;
    else
        if(i<3)
            leg_k(i)=sign(Psa)*(Ak/Ah)*leg_y(i);
        else
            leg_k(i)=-sign(Psa)*(Ak/Ah)*leg_y(i);
        end
    end
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

原地踏步：

function [leg_h_spot,leg_k_spot] = fcn(leg_x,Ak,Ah)
leg_h_spot = zeros(4,1);
leg_k_spot = zeros(4,1);
Psa=-1;
for i=1:1:4 
    if i == 1 || i==2
           if leg_x(i)>=0
                 leg_h_spot(i) = -leg_x(i);   
           else
                 leg_h_spot(i) = 0;  
           end
       else
           if leg_x(i)>=0
                 leg_h_spot(i) = leg_x(i); 
                 
           else
                 leg_h_spot(i) = 0;
           end 
    end
        if leg_x(i)<0
                leg_k_spot(i)=0;
        else
            if(i<3)
                leg_k_spot(i)=-sign(Psa)*(Ak/Ah)*leg_x(i);
            else
                leg_k_spot(i)=sign(Psa)*(Ak/Ah)*leg_x(i);
            end
        end
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

转弯、侧移运动：

function [leg_p,Pin_return] = fcn(diff_h,leg_x,leg_x_delay,P)
leg_p = zeros(4,1);
P_int = zeros(4,1);
Pin_return = zeros(4,1);
%转弯操作的截波
for i=1:1:4
    P_int(i)= P(i);
end
    if diff_h(1) < 0
       P_int(1) = leg_x(1);
    end 
    leg_p(1) = P_int(1); 
    leg_p(3) = -P_int(1); 
    if diff_h(2) < 0
       P_int(1) = leg_x_delay(1);   
    end
    leg_p(1) = P_int(1);
    leg_p(3) = -P_int(1);
    
    if diff_h(2) < 0
       P_int(2) = leg_x(2);
    end
    leg_p(2) = P_int(2);
    leg_p(4) = P_int(2);
    if diff_h(1) < 0
       P_int(2) = leg_x_delay(2);
    end
    leg_p(2) = P_int(2);
    leg_p(4) = P_int(2);
for i=1:1:4
     Pin_return(i)= P_int(i);
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

5.3 原地踏步、转弯、侧移爬坡等运动的控制信号调制

5.3.1 原地踏步设计

原地踏步的控制信号需要从髋关节与膝关节的控制信号里面截取波形，因为原地踏步的足端没有往后方蹭地，不需要腿部蹬地的动作，所以把一半的波形截除，腿间相位关系与Trot或者Walk步态一致，定义t时刻，第i条腿髋关节信号值为 ，膝关节的驱动信号值为 ，原地踏步的控制信号 具体的数学描述如下：

机器人单腿如图连杆机械臂，需利用连杆机械臂的逆运动学公式计算出垂直提腿的髋关节与膝关节比例关系，设抬腿高度为0.2m，可计算可得原地踏步的Ah=10、Ak=5。在Simulink与Adams中的对原地踏步进行联合仿真的得到以下的测试结果。在仿真中，机器人原地踏步，只见腿来回有规律性摆动，但是不产生位移。

5.3.2 转弯及侧移设计

转弯与侧移的控制策略类似，都是在机器人腿侧摆关节处于摆动相的时候跨腿，在支撑相的时候回复初始位置。以Trot为步态信号为调制基础，定义t时刻，第i条腿髋关节信号值为 ，膝关节的驱动信号值为 ，步态的摆动相为 ，可以得出侧移侧摆关节的控制信号 的精确数学描述如下：

而转弯只需要将髋关节的对角腿方向取负即可实现，设 为侧摆关节输出信号，转弯的关节输出信号 的精确数学描述为如下：

5.3.3 爬坡设计

当四足机器人采用前肘后膝的关节布置方式，爬坡的示意图如图所示，虚线表示机器人在平地行走的正常平衡位置，实线表示在一定坡度下的调整姿态后的平衡位置。

机器人通过调整身体的姿态角，来改变重心在支撑平面投影的位置，从而来减轻打滑与搓脚、失稳等问题。上坡时，前腿髋关节后摆，后腿髋关节后

摆，膝关节平衡位置摆动则与同腿髋关节作反向等角度变化。上坡调整完成后，身体姿态变得前低后高，俯仰角小于零（俯角），下坡则相反。
由前面章节可知道，本课题使用的Hopf振荡器带反馈调整功能，所以可调整波形的偏移量来对爬坡动作进行调整。假设四足机器人实现坡面运动的躯体姿态角Δα与坡度α呈线性关系，即Δα=k1α，k1=0.24，上坡时α>0，下坡时α<0。对上坡的姿态调整过程腿部动作进行几何分析可得：

通过坡度反馈调节，如图所示，四足机器人各关节控制信号根据计算反馈产生偏置，最终实现了图4-24的爬坡动作。仿真结果数据如图4-25，Distance曲线表示机器人的前行位移随时间均速变化，Height曲线表明机器人的高度变化情况。综上，机器人在爬坡过程中能够稳定攀爬行走，没有产生打滑、绊脚等情况。

5.3.4 多机器人避障设计

6. Simulink与Adams联合仿真测试

可见，虚拟样机中机器人能按照控制信号的输入进行关节角的控制，进而实现机器人的电机协调控制，进而实现行走。



由图可见，实验中欧拉角pitch、roll、yaw，表示机器人在运行过程中机身的姿态变化，表征机器人运动的平稳性，可见仿生机器人的欧拉角程规律性变化，机器人通过跨腿、挪动身体实现行走动作，而且角度变化值较小（<6度），可见运行平稳度高。
从图中位移曲线（z方向为前进方向）可以看出，仿生四足机器人的走直线度较高，速度达到0.11ms(曲线斜率)，机器人的行走控制策略得以验证。
如图所示，髋关节的受力最为集中，承受了机身重量以及行走时候足端传来的冲击，可以根据该曲线对电机及相关执行机构进行缓冲结构设计。从曲线可看出，启动的时候由于有角度突变值，而且电机无旋转惯性，关节承受的力矩达到峰值，之后程周期性变化。总结得出，在行走信号接入前，设计一个平滑 启动的步态设计能够解决该问题。

需要以下资料可以联系我1096474659：

7. 控制信号C语言实现（搭载单片机、树莓派）

微机实现的关键是将以下环节放进定时器中断中定时刷新求积分，输出关节控制信号。
以下是MATLAB代码，需转C语言或者python。

function [X_dot,Y_dot] = fcn(Wsw,Wst,Phi,leg_x,leg_y,u1,u2)
%#codegen
X_dot = zeros(1,4);
Y_dot = zeros(1,4);
Alpha=10;%收敛速度
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
for i = 1:4
   r_seq = (leg_x(i)-u1(i))^2+(leg_y(i)-u2(i))^2;
   W = (Wst/(exp(-a*leg_y(i))+1))+(Wsw/(exp(a*leg_y(i))+1));
   Vx = Alpha * (u - r_seq)* (leg_x(i) - u1(i))  -  W * (leg_y(i) - u2(i));
   Vy = Alpha * (u - r_seq) * (leg_y(i) - u2(i)) + W * (leg_x(i) - u1(i));
   for j = 1:4
   Vx = Vx + cos(Phi(i)-Phi(j))*(leg_x(j) - u1(i)) - sin(Phi(i)-Phi(j))*(leg_y(j) - u2(i));
   Vy = Vy + cos(Phi(i)-Phi(j))*(leg_y(j) - u2(i)) + sin(Phi(i)-Phi(j))*(leg_x(j) - u1(i));
   end
   X_dot(i) = Vx;
   Y_dot(i) = Vy;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

C语言案例：

#include "stdlib.h"
#include "math.h"
#include "timer.h"
#include "led.h"
#include "usart.h"
#include "delay.h"
#include "net_work.h"
/*****************************
	Edited by Zombie&Yangson in shantou University according to 'robots and control'
*****************************/
//定义相差矩阵，不同步态不同 
  int Theta_Goal[4][3]={0};
	extern int Theta_start[4][3];
	u8 i,j,n=0;
	double beta;
	double LF_fai;    //Phi_LF
	double LB_fai;    //Phi_LB
	double RF_fai;    //Phi_RF
	double RB_fai;    //Phi_RB
//CPG参数
	double alpha=100; 
	double a=50;//支撑相与摆动相的转换速度
  double w_st; //支撑相
	/********************************************
	float theta0=pi*6/180;%腿与垂直方向的夹角为theta0
	float h=0.02;     %抬腿高度
	float l=1;        %髋关节到踝关节的长度
	float L=2*l*cos(theta0); %髋关节到地面的距离
	float v=1;        %向前运行的速度
	float S=v*T;
	********************************************/
	const double Ah=13.4; //asin((beta*S)/(2*L));髋关节摆动幅度
	const double Ak=10;//acos((l*cos(theta0)-h)/l)-theta0 膝关节摆动幅度
	const double Ahset=15;
	const double Akset=15;
	double temp_t=0; // 取点的密度 
	double temp_x,temp_y,Vx,Vy;
  double limbs_x[limbs_num]={0};
	double limbs_y[limbs_num]={0};
	double limbs_hip_point[limbs_num]={0};
	double limbs_y_point[limbs_num]={0};
	double limbs_knee_point[limbs_num]={0};
	double Fbx[limbs_num]={0}, Fby[limbs_num]={0};
	double w_sw=3*pi;  //摆动相，影响T的大小，即使运动周期
  double u=1.0;      //u值,输出振荡网络的幅值的平方
	double fai[4];
	double r_squre;
	double W;
//初始化CPG初始步态选择，步态参数
void CPG_net_init(u8 gait)
{
	switch(gait)
		  {
			  case 1:
		          {  
								beta=0.75;//定义默认负载因子
			          //定义相对相位矩阵
			          LF_fai=   0*2*pi;
			          RF_fai=0.5*2*pi;
			          RB_fai=0.25*2*pi;
			          LB_fai=0.75*2*pi;
								w_st=((1-beta)/beta) * w_sw;
			          break;
		          } 
		    case 2:
		          { 
						    beta=0.5;//定义默认负载因子
			          LF_fai=  0*2*pi;
			          RF_fai=0.5*2*pi;
			          RB_fai=  0*2*pi;
			          LB_fai=0.5*2*pi;
							  w_st=((1-beta)/beta) * w_sw;
			          break;
		          }
		    case 3:
		          {
						    beta=0.5;//定义默认负载因子    
			          LF_fai=  0*2*pi;
			          RF_fai=0.5*2*pi;
			          RB_fai=0.5*2*pi;
			          LB_fai=  0*2*pi;
							  w_st=((1-beta)/beta) * w_sw;
			          break;
			        }
		    case 4:
		          {
                beta=0.5;//定义默认负载因子
			          LF_fai=0.5*2*pi;
			          RF_fai=0.5*2*pi;
			          RB_fai=  0*2*pi;
			          LB_fai=  0*2*pi;
							  w_st=((1-beta)/beta) * w_sw;
			          break;
		          }
		   }			
			fai[0]=LF_fai;
			fai[1]=RF_fai;
			fai[2]=RB_fai;
			fai[3]=LB_fai;
	  // 算法的初值
			for(i=0;i<limbs_num;i++)
				{
					limbs_x[i]=0.5;
					limbs_y[i]=0.5;
				}
}
void net_work_producing(double t_step)
{
	 for(i=0;i<limbs_num;i++)
				{
					r_squre=pow((limbs_x[i]-Fbx[i]),2)+pow((limbs_y[i]-Fby[i]),2);
					W= (w_st / (exp(-a * (limbs_y[i]-Fby[i])) + 1)) + (w_sw / (exp(a * (limbs_y[i]-Fby[i])) + 1));	
					temp_x=alpha*(u-r_squre)*(limbs_x[i]-Fbx[i])-W*(limbs_y[i]-Fby[i]);
					temp_y=alpha*(u-r_squre)*(limbs_y[i]-Fby[i])+W*(limbs_x[i]-Fbx[i]);
					for(j=0;j<limbs_num;j++)
						{
							temp_x=temp_x+cos(fai[i]-fai[j])*(limbs_x[j]) - sin(fai[i]-fai[j])*(limbs_y[j]);
							temp_y=temp_y+cos(fai[i]-fai[j])*(limbs_y[j]) + sin(fai[i]-fai[j])*(limbs_x[j]);
						}
					limbs_x[i]=	limbs_x[i]+temp_x*t_step;
					limbs_y[i]= limbs_y[i]+temp_y*t_step;
					limbs_hip_point[i]=limbs_x[i];
					limbs_y_point[i]=limbs_y[i];
					if (limbs_y[i]>0)
						{
			        limbs_knee_point[i]=-Fby[i];
			      }
	        else
			      {
							if(i<2) limbs_knee_point[i] = (phai) * (limbs_y[i]-Fby[i])* (Ak / Ah);
							else    limbs_knee_point[i] = -(phai) * (limbs_y[i]-Fby[i])* (Ak / Ah);
			      }       	
				}     
}

void goal_reflsh(void)
{
	Theta_Goal[0][0]=Theta_start[0][0]+(int)(Ahset*4096*limbs_hip_point[0]/360);
	Theta_Goal[0][1]=Theta_start[0][1]+(int)(Akset*4096*limbs_knee_point[0]/360);
	Theta_Goal[0][2]=Theta_start[0][2];
	
	Theta_Goal[1][0]=Theta_start[1][0]+(int)(Ahset*4096*limbs_hip_point[1]/360);
	Theta_Goal[1][1]=Theta_start[1][1]+(int)(Akset*4096*limbs_knee_point[1]/360);
	Theta_Goal[1][2]=Theta_start[1][2];
	
	Theta_Goal[2][0]=Theta_start[2][0]+(int)(Ahset*4096*limbs_hip_point[2]/360);
	Theta_Goal[2][1]=Theta_start[2][1]+(int)(Akset*4096*limbs_knee_point[2]/360);
	Theta_Goal[2][2]=Theta_start[2][2];
	
	Theta_Goal[3][0]=Theta_start[3][0]-100+(int)(Ahset*4096*limbs_hip_point[3]/360);
	Theta_Goal[3][1]=Theta_start[3][1]+100+(int)(Akset*4096*limbs_knee_point[3]/360);
	Theta_Goal[3][2]=Theta_start[3][2];
	
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155

8. 数值仿真源代码及Simulink模型（可直接运行）

附件1 Hopf

%Ezekiel
%2018年4月22日
%%% edited by Ezekiel /2017/12/14
%  参考仿生四足机器人技术&Min程序
%  振荡器模型
%四足机器人CPG控制网络二四足
%8.29改进：用循环代替大部分数组赋值
%8.30改进：增加足端轨迹
%基本概念
%Alpha收敛速度，Beta占空比,Wsw摆动相频率，Wst支撑相频率
clc;
clear;
%CPG构建基本参数
Alpha=10;%收敛速度
leg_num=4;%腿的数量
gait=2;%步态选择，1walk,2trot,3pace,4gallop
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
Psa=1;%关节形式，膝式-1，肘式1
Wsw=2*pi;%摆动相频率,影响信号周期，T为2*pi/Wsw
u1=0; u2=0;%误差，影响x,y的平衡位置
%关节参数，此处参数需实测，试验数据参考书
h=0.02;  %抬腿高度
v=1;   %行走速度
T=0.4; %步态周期
S=v*T; %步长
l=0.4;   %腿节长度
theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 
L=2*l*cos(theta0);%髋关节与足端之间长度
Ah=15;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
%负载系数对相关参数影响
switch gait
    case 1
        Beta=0.75;%负载系数,walk为0.75,trot,pace,gallop为0.5，影响振荡信号上升时间和下降时间
        Ah=7.5;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 2
        Beta=0.5;
        Ah=asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 3
        Beta=0.5;
        Ah=10;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 4
        Beta=0.5;
        Ah=13.4;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
end
        Wst=((1-Beta)/Beta)*Wsw;%支撑相频率
        Rhk=(1-Beta)*[cos(pi) -sin(pi);sin(pi) cos(pi)];%髋关节和膝关节之间的耦合矩阵
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);
switch gait
    case 1   
        %walk相位
        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
   case 2
        %trot相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0*2*pi;
        Phi(4)=0.5*2*pi;
   case 3
        %pace相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0*2*pi;
   case 4
        %gallop相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0.5*2*pi;    
end 
 
    %相对相位矩阵
    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  
%画图
figure(1)
% for i=1:4
%     subplot(4,3,i+2*(i-1))
%     plot(leg_h_Point_x(:,i),leg_h_Point_y(:,i))
%     subplot(4,3,i+1+2*(i-1))
%     hold on
%     plot(t,leg_h_Point_x(:,i))
%     plot(t,leg_k_Point_x(:,i))
%     hold off
% end
figure(1)
subplot(4,1,1)
    plot(t,leg_h_Point_x(:,1));
    hold on
    plot(t,leg_k_Point_x(:,1),'green');
    grid on
    ylabel('LF')
    axis([5,15,-1.5,1.5])%XY坐标均衡
    %title('Hopf振荡器的输出');
 grid on;
subplot(4,1,2)  
    plot(t,leg_h_Point_x(:,2));
    hold on
    plot(t,leg_k_Point_x(:,2),'green');
    grid on
    ylabel('RF')
    axis([5,15,-1.5,1.5])%XY坐标均衡
subplot(4,1,3)
    plot(t,leg_h_Point_x(:,3));
    hold on
    plot(t,leg_k_Point_x(:,3),'green');
    grid on
    ylabel('RB')
    axis([5,15,-1.5,1.5])%XY坐标均衡
subplot(4,1,4)
    plot(t,leg_h_Point_x(:,4));
    hold on
    plot(t,leg_k_Point_x(:,4),'green');
    grid on
    ylabel('LB')
    xlabel('时间t/s')
    axis([5,15,-1.5,1.5])%XY坐标均衡
% subplot(4,3,3)
% plot(Foot_end_x(:,3), Foot_end_y(:,3))
% subplot(4,3,6)
% for i=1:leg_num
%     hold on
%     plot(t,leg_h_Point_x(:,i))
%     hold off
% end
% 
% subplot(4,3,9)
% for i=1:leg_num
%     hold on
%     plot(t,leg_h_Point_y(:,i))
%     hold off
% end附录
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193

附件2 Kimura

该源码是基于Kimura-CPG实现的MATLAB脚本，命名为Kimura，需要调用四节龙格-库塔法EQ2文件，详情见附2。
附1. Kimura CPG振荡网络

%% 根据张秀丽《四足机器人节律运动》编写的CPG模型
%Ezekiel
%2018年4月22日
%% 清理变量
clc;
clear all;
%% 参数设置
leg_num=4;%足的个数
mm=8;%m项反馈信息
Tr=0.04;%上升时间参数
Ta=0.38;
c=0.23;%直流输入
b=-2.0;%适应系数
a=-1.0;%抑制系数
u0=rand(leg_num,1)*c/10;%初值矩阵
Ah=8.3;
Ak=5.3;
phai=1;
%%
gait=2;
w=zeros(leg_num,leg_num);%定义步态矩阵
switch(gait)
    case 1
       w=[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];
    case 2
        w=[0 -1 -1 1;-1 0 1 -1;-1 1 0 -1;1 -1 -1 0];
    case 3
        w=[0 1 -1 -1;1 0 -1 -1;-1 1 0 -1;-1 -1 1 0];
    case 4
        W=[0 -1 -1 -1;-1 0 -1 -1;-1 -1 0 -1;-1 -1 -1 0];
    otherwise
        w=[0 -1 1 -1;-1 0 -1 1;1 -1 0 -1;-1 1 -1 0];   
end
%%
vf=zeros(leg_num,1);
ve=zeros(leg_num,1);
Vfe=[vf,ve];
%%
yf=zeros(leg_num,1);
ye=zeros(leg_num,1);
Yfe=[yf,ye];
Yef=[ye,yf];
Y=zeros(leg_num,1);%输出的关节映射矩阵
%%
uf=rand(leg_num,1)*c/10;
ue=rand(leg_num,1)*c/10;
Ufe=[uf,ue];
%%
hf=zeros(mm,1);
he=zeros(mm,1);
Hfe=[hf,he];
%%
ss=zeros(leg_num,mm);
WYf=zeros(leg_num,1);
WYe=zeros(leg_num,1);
SHf=zeros(leg_num,1);
SHe=zeros(leg_num,1);
%%
E=zeros(leg_num,2);
for i=1:1:leg_num
    E(i,1)=1;
    E(i,2)=1;
end
%% 关节参数，此处参数需实测，试验数据参考书
h=0.02;  %抬腿高度
v=1;   %行走速度
T=0.4; %步态周期
S=v*T; %步长
l=0.4;   %腿节长度
theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 
L=2*l*cos(theta0);%髋关节与足端之间长度
Ah=8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
%% %时间，分辨率设置
Pon_num=1000;
t_begin=0; t_end=10; t_step=(t_end-t_begin)/(Pon_num-1);
t=zeros(Pon_num,1);
y=zeros(Pon_num,leg_num);
%对时间进行算法积分
 %% vdot=EQ3(t2,v,flag,Ta,yf,ye)
 %% udot=EQ2(t,u,flag,WYf,WYe,ye,yf,w)
u0=[0.009 0.0021 0.0134 0.0022
    0.0272 0.0003 0.0206 0.0057
    0.0199 0.0067 0.0171 0.0111
    0.0022 0.0151 0.0149 0.0081];%初值会影响相序
t_set=0:0.01:20;
[t1,u]=ode45('EQ2',t_set,u0,[],WYf,WYe,ye,yf,w);  
yPlot(:,1)=u(:,1)-u(:,3);
yPlot(:,2)=u(:,5)-u(:,7);
yPlot(:,3)=u(:,9)-u(:,11);
yPlot(:,4)=u(:,13)-u(:,15);
Judge1=diff(yPlot(:,1));
Judge2=diff(yPlot(:,2));
Judge3=diff(yPlot(:,3));
Judge4=diff(yPlot(:,4));
kplot=zeros(length(yPlot(:,1)),4);
thetah(1)=max(yPlot(:,1));
thetah(2)=max(yPlot(:,2));
thetah(3)=max(yPlot(:,3));
thetah(4)=max(yPlot(:,4));
for j=1:1:length(yPlot(:,1))-1
    if Judge1(j)>0
       kplot(j,1)=1*(Ak/Ah)*(1-(yPlot(j,1)/thetah(1))^2);
    else
       kplot(j,1)=0;
    end
    if Judge2(j)>0
       kplot(j,2)=1*(Ak/Ah)*(1-(yPlot(j,2)/thetah(2))^2);
    else
       kplot(j,2)=0;
    end
    if Judge3(j)>0
       kplot(j,3)=-1*(Ak/Ah)*(1-(yPlot(j,3)/thetah(3))^2);
    else
       kplot(j,3)=0;
    end
    if Judge4(j)>0
       kplot(j,4)=-1*(Ak/Ah)*(1-(yPlot(j,4)/thetah(4))^2);
    else
       kplot(j,4)=0;
    end
 end
subplot(4,1,1)
plot(t1,yPlot(:,1),t1,kplot(:,1),'green')
% title('Kimura振荡器输出')
ylabel('LF')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,2)
plot(t1,yPlot(:,2),t1,kplot(:,2),'green')
ylabel('RF')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,3)
plot(t1,yPlot(:,3),t1,kplot(:,3),'green')
ylabel('RB')
axis([0,10,-2,2])%XY坐标均衡
grid on;
subplot(4,1,4)
plot(t1,yPlot(:,4),t1,kplot(:,4),'green')
xlabel('时间（t/s）')
ylabel('LB')
axis([0,10,-2,2])%XY坐标均衡
grid on;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144

辅助函数函数-四五阶龙格库塔法待求微分方程组

function udot=EQ2(t,u,flag,WYf,WYe,ye,yf,w)
Tr=0.04;%上升时间参数
Ta=0.38;
c=0.23;%直流输入
b=-2.0;%适应系数
a=-1.0;%抑制系数
leg_num=4;%足的个数
uf=[u(1);u(5);u(9);u(13)];
ue=[u(3);u(7);u(11);u(15)];
yf(1)=max(uf(1),0);
ye(1)=max(ue(1),0);
yf(2)=max(uf(2),0);
ye(2)=max(ue(2),0);
yf(3)=max(uf(3),0);
ye(3)=max(ue(3),0);
yf(4)=max(uf(4),0);
ye(4)=max(ue(4),0);
for j=1:1:leg_num
    WYf(1)=WYf(1)+w(1,j)*yf(j);
    WYe(1)=WYe(1)+w(1,j)*ye(j);
    WYf(2)=WYf(2)+w(2,j)*yf(j);
    WYe(2)=WYe(2)+w(2,j)*ye(j);
    WYf(3)=WYf(3)+w(3,j)*yf(j);
    WYe(3)=WYe(3)+w(3,j)*ye(j);
    WYf(4)=WYf(4)+w(4,j)*yf(j);
    WYe(4)=WYe(4)+w(4,j)*ye(j);
end 
udot=[(-u(1)+b*u(2)+a*ye(1)+WYf(1)+c)/Tr;
     (yf(1)-u(2))/Ta;
     (-u(3)+b*u(4)+a*yf(1)+WYe(1)+c)/Tr
     (ye(1)-u(4))/Ta;
     (-u(5)+b*u(6)+a*ye(2)+WYf(2)+c)/Tr;
     (yf(2)-u(6))/Ta;
     (-u(7)+b*u(8)+a*yf(2)+WYe(2)+c)/Tr
     (ye(2)-u(8))/Ta;
     (-u(9)+b*u(10)+a*ye(3)+WYf(3)+c)/Tr;
     (yf(3)-u(10))/Ta;
     (-u(11)+b*u(12)+a*yf(3)+WYe(3)+c)/Tr
     (ye(3)-u(12))/Ta;
     (-u(13)+b*u(14)+a*ye(4)+WYf(4)+c)/Tr;
     (yf(4)-u(14))/Ta;
     (-u(15)+b*u(16)+a*yf(4)+WYe(4)+c)/Tr
     (ye(4)-u(16))/Ta;];
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

附件3 机器人DH模型

RVC工具箱（需配置机器人工具箱环境）的四足机器人模型（虚拟样机）

%% compiled by Ezekiel according to robot modeling and control
%% 2017/12/3
%莫智斌
%2018年4月22日d
% clear;
% clc;
% clear L
figure(2)
Len_tool=0;
du=pi/180;
L1 = 0.08; L2 = 0.2; L3 = 0.2;L = 0.53; W = 0.35; C=0.05;
P=[
L/2 -L/2 L/2 -L/2;
W/2 W/2 -W/2 -W/2;
-0.30 -0.30 -0.30 -0.30];
F=[
L/2 -L/2 L/2 -L/2;
W/2 W/2 -W/2 -W/2];
Q=zeros(3,4);
plotopt = {'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes', 'delay', 0};
s = 'Rx(q1).Tz(L1).Ry(q2).Tx(L2).Ry(q3).Tx(L3)';
dh = DHFactor(s);
dh.command('leg') ;
leg = eval( dh.command('leg') );
leg.offset=[0;pi-pi/6;+pi/3];
legs(1) = SerialLink(leg, 'name', 'leg1','base', transl(L/2, W/2, 0)*trotx(pi)*troty(pi/2)); 
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L/2, W/2, 0)*trotx(pi)*trotz(pi)*troty(pi/2));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(L/2, -W/2, 0)*trotx(pi)*troty(pi/2));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(-L/2, -W/2, 0)*trotx(pi)*trotz(pi)*troty(pi/2));
%建立四足机器人模型
%模型
hold on
% 身体
xdata=[L/2;
       -L/2;
      -L/2;
       L/2];
ydata=[W/2;
       W/2;
       -W/2;
       -W/2];
zdata=[0;
       0;
       0;
       0];
zdata1=zdata+[C,C,C,C]';
xdata2=[-L/2,-L/2,-L/2,-L/2]';
ydata2=[-W/2,-W/2,W/2,W/2]';
zdata2=[0,C,C,0]';
xdata4=[-L/2,-L/2,L/2,L/2]';
ydata4=[-W/2,-W/2,-W/2,-W/2]';
zdata4=[0,C,C,0]';
xdata3=-xdata2;
ydata5=-ydata4;
patch(xdata,ydata,zdata,'blue');
patch(xdata,ydata,zdata1,'blue');
patch(xdata2,ydata2,zdata2,'blue');
patch(xdata3,ydata2,zdata2,'blue');
patch(xdata4,ydata4,zdata4,'blue');
patch(xdata4,ydata5,zdata4,'blue');
% %%初始位置
legs(4).plot([0*du,(0)*du,0*du], plotopt{:});
legs(2).plot([0*du,(0)*du,0*du], plotopt{:});
legs(1).plot([0*du,(0)*du,0*du], plotopt{:});
legs(3).plot([0*du,(0)*du,0*du], plotopt{:});
% %%
for A=90:-5:35
pause(0.2);
view(-A,0+A);
end
%设置髋关节与膝关节幅值，侧摆关节动作
Ahip=15*du;
Akne=10*du;
draw_LF=zeros(Pon_num,3);
draw_RF=zeros(Pon_num,3);
draw_RB=zeros(Pon_num,3);
draw_LB=zeros(Pon_num,3);
for i=1:1:Pon_num
 %LF关节映射函数
 draw_LF(i,2)=Ahip*leg_h_Point_x(i,1);
 draw_LF(i,3)=Akne*leg_k_Point_x(i,1);
 %RF关节映射函数
 draw_RF(i,2)=Ahip*leg_h_Point_x(i,2);
 draw_RF(i,3)=Akne*leg_k_Point_x(i,2);
 %RB关节映射函数
 draw_RB(i,2)=-Ahip*leg_h_Point_x(i,3);
 draw_RB(i,3)=-Akne*leg_k_Point_x(i,3);
 %LB关节映射函数
 draw_LB(i,2)=-Ahip*leg_h_Point_x(i,4);
 draw_LB(i,3)=-Akne*leg_k_Point_x(i,4);
end
%% 启动行走动画
        for i=1:1:Pon_num
            legs(1).animate( draw_LF(i,:));
            legs(2).animate( draw_RF(i,:));
            legs(3).animate( draw_RB(i,:));
            legs(4).animate( draw_LB(i,:));
            drawnow
        end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

附件4 带反馈的Hopf

%基本概念
%Alpha收敛速度，Beta占空比,Wsw摆动相频率，Wst支撑相频率
% clc;
% clear;

%CPG构建基本参数
Alpha=25;%收敛速度
leg_num=4;%腿的数量
gait=2;%步态选择，1walk,2trot,3pace,4gallop
u=1;%影响曲线幅值，幅值为开根号
a=50;%决定w在Wsw和Wst之间变化的速度
Psa=1;%关节形式，膝式-1，肘式1
Wsw=2*pi;%摆动相频率,影响信号周期，T为2*pi/Wsw
u1=0; u2=0;%误差，影响x,y的平衡位置

%关节参数，此处参数需实测，试验数据参考书
h=0.02;  %抬腿高度
v=1;   %行走速度
T=0.4; %步态周期
S=v*T; %步长
l=0.4;   %腿节长度
theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 
L=2*l*cos(theta0);%髋关节与足端之间长度
Ah=8.3;%asin((Beta*S)/(2*L));%髋关节摆动幅度
Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度

%负载系数对相关参数影响
switch gait
    case 1
        Beta=0.75;%负载系数,walk为0.75,trot,pace,gallop为0.5，影响振荡信号上升时间和下降时间
        Ah=7.5;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 2
        Beta=0.5;
        Ah=asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 3
        Beta=0.5;
        Ah=10;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
    case 4
        Beta=0.5;
        Ah=13.4;%asin((Beta*S)/(2*L));%髋关节摆动幅度
        Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度
end
        Wst=((1-Beta)/Beta)*Wsw;%支撑相频率
        Rhk=(1-Beta)*[cos(pi) -sin(pi);sin(pi) cos(pi)];%髋关节和膝关节之间的耦合矩阵
        
%时间
Pon_num=1000;
t_begin=0; t_end=15; 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
feed_dx=zeros(leg_num,1);feed_dy=zeros(leg_num,1);%反馈值，CPG振荡网络的反馈调整值
%初始值，非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);
switch gait
    case 1   
        %walk相位
        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
   case 2
        %trot相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0*2*pi;
        Phi(4)=0.5*2*pi;
   case 3
        %pace相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0.5*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0*2*pi;
   case 4
        %gallop相位矩阵
        Phi(1)=0*2*pi;
        Phi(2)=0*2*pi;
        Phi(3)=0.5*2*pi;
        Phi(4)=0.5*2*pi;    
end 

    %相对相位矩阵
    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
K1=1;K2=2.26;Ap=0.5;
podu=zeros(Pon_num,1);
for  n=1:Pon_num
       for i=1:leg_num
        r_seq=(leg_x(i)-feed_dx(i))^2+(leg_y(i)-feed_dy(i))^2;
        W=(Wst/(exp(-a*leg_y(i)-feed_dy(i))+1))+(Wsw/(exp(a*leg_y(i)-feed_dy(i))+1));
        V=[Alpha*(u-r_seq) -W;W Alpha*(u-r_seq)]*[leg_x(i)-feed_dx(i);leg_y(i)-feed_dy(i)]+R_cell{1,i}*[leg_x(1);leg_y(1)]+R_cell{2,i}*[leg_x(2);leg_y(2)]+R_cell{3,i}*[leg_x(3);leg_y(3)]+R_cell{4,i}*[leg_x(4);leg_y(4)];
        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)>feed_dy(i)
            leg_k_Point_x(n,i)=-feed_dy(i);
        else
            if(i<3)
                 leg_k_Point_x(n,i)=-sign(Psa)*(Ak/Ah)*(leg_y(i)-feed_dy(i))-feed_dy(i);
            else
                leg_k_Point_x(n,i)=sign(Psa)*(Ak/Ah)*(leg_y(i)-feed_dy(i))-feed_dy(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
       if n<375&&n>=0
       feed_dx(1)=-0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(2)=-0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(3)=-0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(4)=-0.5;%-K1*(K2/2-1)*Ap
       
       feed_dy(1)=-0.4%K1*K2*Ap;
       feed_dy(2)=-0.4%K1*K2*Ap;
       feed_dy(3)=-0.4%K1*K2*Ap;
       feed_dy(4)=-0.4%K1*K2*Ap
       podu(n)=0.5;
       elseif n>=375&&n<750
       feed_dx(1)=0;%-K1*(K2/2-1)*Ap;
       feed_dx(2)=0;%-K1*(K2/2-1)*Ap;
       feed_dx(3)=0;%-K1*(K2/2-1)*Ap;
       feed_dx(4)=0;%-K1*(K2/2-1)*Ap
       
       feed_dy(1)=0;%K1*K2*Ap;
       feed_dy(2)=0;%K1*K2*Ap;
       feed_dy(3)=0;%K1*K2*Ap;
       feed_dy(4)=0;%K1*K2*Ap
       podu(n)=0;
        elseif n>=750&&n<1125
       feed_dx(1)=0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(2)=0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(3)=0.5;%-K1*(K2/2-1)*Ap;
       feed_dx(4)=0.5;%-K1*(K2/2-1)*Ap
       
       feed_dy(1)=0.4;%K1*K2*Ap;
       feed_dy(2)=0.4;%K1*K2*Ap;
       feed_dy(3)=0.4;%K1*K2*Ap;
       feed_dy(4)=0.4;%K1*K2*Ap
       podu(n)=-0.5;
       end
end  
%画图
% for i=1:4
%     subplot(4,3,i+2*(i-1))
%     plot(leg_h_Point_x(:,i),leg_h_Point_y(:,i))
%     subplot(4,3,i+1+2*(i-1))
%     hold on
%     plot(t,leg_h_Point_x(:,i))
%     plot(t,leg_k_Point_x(:,i))
%     hold off
% end
subplot(5,1,2)
    plot(t,30*leg_h_Point_x(:,1));
    hold on
    plot(t,20*leg_k_Point_x(:,1),'red');
    grid on
    ylabel('LF')
    axis([0,15,-60,60])%XY坐标均衡
    %title('Hopf振荡器的输出');
 grid on;
subplot(5,1,3)  
    plot(t,30*leg_h_Point_x(:,2));
    hold on
    plot(t,20*leg_k_Point_x(:,2),'red');
    grid on
    ylabel('RF')
   axis([0,15,-60,60])%XY坐标均衡
subplot(5,1,4)
    plot(t,30*leg_h_Point_x(:,3));
    hold on
    plot(t,20*leg_k_Point_x(:,3),'red');
    grid on
    ylabel('RB')
    axis([0,15,-60,60])%XY坐标均衡
subplot(5,1,5)
    plot(t,30*leg_h_Point_x(:,4));
    hold on
    plot(t,20*leg_k_Point_x(:,4),'red');
    grid on
    ylabel('LB')
    xlabel('时间t/s')
    axis([0,15,-60,60])%XY坐标均衡
    
    subplot(5,1,1)
    plot(t,30*podu);
    ylabel('坡度值')
    axis([0,15,-20,20])%XY坐标均衡
    grid on
    hold off
 
figure(2)
plot(Foot_end_x(:,3), Foot_end_y(:,3))
grid on
xlabel('x方向位移')
ylabel('z方向位移')

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220