双足步行机器人-机器谱-robotway

作者：李海鹏刘垆钰杨立强徐晓斌

单位：杭州师范大学钱江学院

指导老师：孙红梅马宝丽

随着世界第一台工业机器人1962年在美国诞生，机器人已经有了三十多年的发展史。三十多年来，机器人由工业机器人到智能机器人，成为21世纪具有代表性的高新技术之一，其研究涉及的学科涵盖机械、电子、生物、传感器、驱动与控制等多个领域。

本作品设计了一个小型双足步行机器人。足式步行机构有双足、三足、四足、六足、八足等形式，其中双足步行机器人具有最好的适应性，也最接近人类，故也称为类人双足行走机器人。类人双足行走机构是多自由度的控制系统，这种机构除结构复杂外，在静或动状态下的行走性能、稳定性、和高速运动都不是很理想。双足步行机器人行走机构是一空间连杆机构。在行走过程中，行走机构始终满足静力学的静平衡条件，也就是机器人的重心始终落在支持地面的一脚上。双足机器人属于类人机器人，典型特点是机器人的下肢以刚性构件通过转动副联接，模仿人类的腿及髋关节、膝关节和踝关节，并以执行装置代替肌肉，实现对身体的支撑及连续地协调运动，各关节之间可以有一定角度的相对转动。

双足机器人不仅具有广阔的工作空间，而且对步行环境要求很低，能适应各种地面且具有较高的逾越障碍的能力，其步行性能是其它步行结构无法比拟的，研究双足行走机器人具有重要的意义。本作品主体材料采用具有良好抗腐蚀性和韧性，抗氧化效果极佳的铝材7075，利用Arduino控制器使整个机构协同化运作，完成这一整套动作。

1. 引言

1.1 前言

机器人(robot)一词最早出现在捷克作家Karel Capek于1921年所写的戏剧“R.U.R.,”原意为“强迫劳动”之意。根据美国机器人学会（The Robot Instituteof America）于1979年对机器人的定义：“机器人是一个可程序化，多功能的操作者，可经由事先规划好的动作来达到移动材料、工具、零件等目的。”若从此定义来看，广义的机器人很早即出现在人类的历史上，如在一千七百多年前由诸葛亮所设计用来搬运粮草的「木牛流马」，即是中国最早的军事用机器人。然而第一台现代化的工业用机器人Unimates，则在1962年由George Devol和 Joe Engleberger所创造出来，之后机器人便逐渐进入工业界，替代人类接管单调与危险的工作。

根据移动方式的不同，机器人又可以细分为很多种类，包含：轨道式、轮型式、双足式，以及多足式。其中又以双足机器人步行不象轮式机器人那样持续地与地面接触,其支撑脚与地面的接触是离散的，而且可以根据地形情况主动选择最佳支撑点，可以做各类跨越动作，因而受环境制约较少。近年来，组建成为最热门的研究之一。近年来因电子、计算器及控制技术的快速发展，各种仿生物机器人（机器鱼、机器蛇、机器狗、人形机器人等）应运而生。仿人形机器人具有类人形结构，双足行走方式使其更方便于在人类环境中进行活动，因而具有广泛的应用市场，是研究的热点。

仿人双足步行机器人是各国科技综合实力的集中体现，其研究涉及众多学科的交叉融合，如机构学、传感器科学、仿生学、控制理论与工程学、计算机科学、电子工程学等。

逐渐成为机器人研究中的一个热点。研究仿人双足步行机器人，不但具有重要的学术意义，而且剧痛现实的应用价值。

1.2 双足步行机器人研究背景与现状

1.2.1 国外研究现状

双足步行机器人的研发已经有数十年的历史了，其结合了机器人学、控制工程学、机械力学及机构学等学问。因为其高度的非线性系统且无固定支撑底面，步行的运动控制较固定式的机械臂更具困难。日本早稻田大学研制了一系列双足步行机器人，包括：WL1、WL3[1969]、WAP_2~WL_5 、WABOT_1[1973] 、WL_9DR~WL_12R 、WL_RV[1992] 、WABIAN[1999] 、WABIAN_2[ 2004]

作品说明

WL_15、WABIAN_2

日本东京大学Jouhou System Kougaka 实验室先后研制出共有30个自由度的H5、共有30个自由度的H6、以及32个自由度的H7系列仿人形双足步行机器人，至H7时已经可以实现实时步态步行，并在户外成功进行了行走实验。日本Honda机器人的第一阶段机器人由Honda R & D Center（Hirai,1997）发展，这个机器人没有驱干但是有一个很高的骨盆，它是“动力＂12 个自由度的双足机器人，此双足机器人能够上下阶梯、行走坡度低于10%的斜坡、转弯和后退。后来又先后研制了E系列(Ex Perimental Model)试验样机 EO_E6，P系列(protot 即e Model)的样机pl、p2和 p3。又于2000 年，推出著名的仿人形机器人ASIMO(Advanced step in Innovative Mobility) ，ASIMO高1.2m，重43kg，共有26 个自由度，可以无线连接互联网，具有语音识别和说话的功能，可以与人进行交流，实时识别人的姿态与运动，并进行交互，正常行走速度达到2.7km/h，奔跑时可以达到 62.7km/h，并可以迂回行走。

ASIMO

Sony公司研制了用于娱乐的仿人双足机器人SDR--3X (SDR为Sony Dream Robot简称)。之后又推出了SDR4X仿人双足机器人，SDR系列机器人均为SONY公司在消费及娱乐市场推出的仿人双足机器人，该系类机器人使Sony公司在双足机器人领域跻身于世界前列。之后SONY又推出了新一代仿人双足机器人QRIO，能够与人进行语言和动作上的交互，在运动能力上也有进一步提高，可以跑，跳，摔倒之后可以自己爬起来，并且自行躲避障碍物。

QRIO

日本经济产业省的HRP项目更加注重工业生产中的应用，目前已经实现了机器人与人协作抬桌子，开铲车和进行工作间隔板装配等操作的应用。

1.2.2 国内研究现状

我国在仿人双足步行机器人研究方面也取得了许多成果。哈尔滨工业大学先后研制了双足机器人HIT_I,HIT_ II和HIT_ III系列，国防科技大学先后成功研制成功了双足机器人 KDW_I、KDW_II和KDW_III型双足步行机器人，可以前进、后腿、上下台阶，以及准动态步行。北京理工大学黄强教授研制了仿人机器人BHR_1和 BHR_2（汇童），能够通过传感器感知自身的平衡状态和地面高度的变化，实现未知地面的步行，并可以进行太极拳、刀术等表演。上海交通大学研制的仿人机器人SFHR，配有主动视觉系统，可以对目标及环境进行识别。清华大学研制了仿人机器人THBIP_1，南京航空航天大学研制了双足步行机器NAIWAR_1均可以实现静态步行功能，但是在半被动步行方面的研究较少，且成果较少。

汇童

1.3 本项目研究内容

本项目涉及了一个小型双足步行机器人，主要内容分为以下几个部分首先针对机器人下肢结构进行了分析，通过分析取定了一个合理的自由度，在此基础上进行了双足步行机器人机构设计，设计了三维机构模型。

推导了双足步行机器人的数学模型，包括运动学，逆运动学，动力学模型，该控制器采用了反射控制理论，实现对机器人非线性系统的控制。在MATLAB和ADAMS中进行了联合仿真验证，证明了该方法的合理性。整体来讲，目前的双足机器人行走控制主要基于编程机制，步态规划的研究主要集中在轨迹的离线规划，这种规划模式在一定程度上限制了步行的灵活性。

2. 双足步行机器人机构描述

2.1 机构设计简介

将机件（Machine members）以特定的接头与方式组合，使其中一个或数个机件的运动，依照这个组合所形成的限制，强迫其它机件产生确定的相对运动，这个组合称为机构（Mechanism），如变形机器人玩具、机械式钟表等。机构设计（Mechanism Design）的步骤如下：

(1) 确定工作目的

确定所拟设计机器之目的为何、使用条件为何、能量输入形式为何，及最后发生运动的机件应该产生何种运动、克服多大负荷、以何种方式输出多少的有效功等。

(2) 创思拓扑构造

根据机器之机构的特性与设计限制，构想出用以组成这个机构的拓朴构造，以便让这个组合产生拘束运动。

(3) 合成运动尺寸

合成出机构中每一个机件接头之间的几何尺寸，使输入机件在所给予的运动状态﹙位移、速度、加速度﹚下，迫使输出机件产生所需的运动状态。

(4) 分析运动状态

验证机构的输入与输出的运动关系是否合乎所求，并求得机构中机件与重要点的角加速度与线性加速度，以为动力分析的根据。

(5) 分析受力情形

利用静力学原理，在已知的静态负荷下，求得机件的每一个接头在任一位置所受的静态力量，及输入机件之输入力或力矩的大小。

(6) 设计负荷尺寸

利用材料力学原理，在已知静态受力情形下，选用机件的材料，以决定机件的形狀大小，使机件有足够之强度与刚性来安全的承受力量负荷。

(7) 研究动态特性

研究机件之质量与运动所引起的动力问题，包括动态负荷、惯性力、摇撼力与力矩、动平衡、以及动态反应等。整个阶段必须重新确定上一个步骤中所决定的负荷尺寸是否安全。每个设计步骤皆非各自独立；假若任何一个步骤所得到之结果无法满足设计上的功能要求，则必须修正前面步骤的设计结果。除了上述七个步骤外，完整的机器设计还包括动力源选用、控制设计、工业设计、材料选用与处理、热流效应考虑、组装与测试等。如下图所示为机构设计流程图：

机构设计流程图

2.1.1 人体下肢机构分析

人体下肢关节主要有髋关节、膝关节、踝关节和足趾关节。

(1) 髋关节

髋是连接躯干和下肢的关节，属球窝关节，它周围有强韧的韧带包裹。髋关节除了承担身体重要的压力外，还可以做前屈、后伸、外展、内收、回旋等运动，共3个自由度。

(2) 膝关节

膝关节是连接大腿和小腿的关节，由股骨内、外侧髁和胫骨内、外侧髁以及髌骨构成。属滑车关节，周围坚韧的韧带使股骨与胫骨牢固相连。当膝部弯曲时，关节呈松散结构，腿完全伸直时，关节紧密结合。膝关节只能做屈伸运动，共1个自由度。

(3) 踝关节

踝关节是连接小腿和脚的关节，属于鞍状关节。踝关节可以做足背屈、伸的运动，也可以做较小的内、外翻转动作，共3个自由度。

(4) 趾关节

趾关节是趾骨与脚趾骨所形成的关节。当人体运动时，趾关节是主要的受力部位。而且该部位是运动中活动最频繁，尤其是人体落地一瞬间最先着地的部位，具有较大的弹性和缓冲作用，共 1个自由度。

2.1.2 自由度分析

人体下肢关节结构一共有16个自由度,对其进行简化,取其中6个自由度进行双足步行机器人的机构设计，如下图所示为双足步行机器人（自由度6）：

双足步行机器人机构图

2.1.3 运动能力分析

机器人关节运动采用舵机进行控制，舵机是一种位置（角度）伺服的驱动器，适用于那些需要角度不断变化并可以保持的控制系统。目前在高档遥控玩具，如航模，包括飞机模型，潜艇模型；遥控机器人中已经使用得比较普遍。舵机是一种俗称，其实是一种伺服马达。

(1) 舵机的工作原理

控制信号由接收机的通道进入信号调制芯片，获得直流偏置电压。它内部有一个基准电路，产生周期为 20ms，宽度为 1.5ms 的基准信号，将获得的直流偏置电压与电位器的电压比较，获得电压差输出。最后，电压差的正负输出到电机驱动芯片决定电机的正反转。当电机转速一定时，通过级联减速齿轮带动电位器旋转，使得电压差为 0，电机停止转动。

(2) 舵机的控制

舵机的控制一般需要一个20ms左右的时基脉冲，该脉冲的高电平部分一般为0.5ms~2.5ms 范围内的角度控制脉冲部分。以 180 度角度伺服为例，那么对应的控制关系是这样的：

0.5ms--------------0 度

1.0ms------------45 度

1.5ms------------90 度

2.0ms-----------135 度

2.5ms-----------180 度

舵机控制脉冲示例

小型舵机的工作电压一般为 4.8V 或 6V，转速也不是很快，一般为 0.22/60 度或 0.18/60度，所以假如你更改角度控制脉冲的宽度太快时，舵机可能反应不过来。如果需要更快速的反应，就需要更高的转速了。要精确的控制舵机，其实没有那么容易，很多舵机的位置等级有 1024 个，那么，如果舵机的有效角度范围为 180 度的话，其控制的角度精度是可以达到 180/1024 度约 0.18度了，从时间上看其实要求的脉宽控制精度为 2000/1024us 约 2us。

使用传统单片机控制舵机的方案有很多，多是利用定时器和中断的方式来完成控制的，这样的方式控制 1 个舵机还是相当有效的，但是随着舵机数量的增加，也许控制起来就没有那么方便而且可以达到约 2 微秒的脉宽控制精度了。AVR 的 mega128 共有 5 组 GPIO口,可以控制 32 个以上舵机，而且自带 AD 采样接口。是实现机器人舵机控制板的理想MCU。本文设计的机器人由于控制算法不在采用控制板向舵机输入角度值，舵机进行跟随的控制方法，因而对舵机进行了一定的处理，将舵机内部的 PD 控制电路去掉，直接将电位器电压信号反馈到控制板的 AD 接口，进行 AD 采集，然后通过运算得出角度值，再通过本文提出的新的控制算法进行控制。

2.2 机构组件说明

整个机构的组成是利用国产的探索者套件加以组装完成。以下针对组装时所需的各项参数加以说明。

2.2.1 机械部分组件

机械部分组件图

2.2.2 电控部分组件

电控部分组件图

2.2.3 实际完成图

双足机器人实际图

3. 双足步行机器人数学模型

3.1 双足步行机器人模型

机器人机构设计完成后，需要建立双足机器人的数学模型，在此基础上求出双足步行控制所需的向量位置、逆运动学方程式、规划期步行轨迹方程式、检验其零力矩点是否符合。双足步行机器人数学模型各参数说明如下图所示：

双足机器人数学模型图

其中

L ：第 i 个杆件长度。

Rix ：第 i 个杆件对 X 轴旋转自由度。R iy：第 i 个杆件对 Y 轴旋转自由度。

Wi ：第 i 个杆件质量。

建立双足步行机器人的数学模式时。首先针对所必要的物理条件做如下假设：

(1) 步行时，假设各杆件不变形。

(2) 杆件重心为各杆件质量且集中于杆件的中心。

(3) 不考虑双足步行机器人受地面冲击的各项参数及振动现象。

3.2 向量解析数学算法推导

双足步行机器人步行时的各杆件位置可由向量解析的方式计算得到位置参数，当三维空间的机器人运动时，将空间中机器人的运动，可分为平移(Translation)和旋转(Rotation)两种。双足步行机器人每个杆件质量假设于杆件中央，且各杆件以向量表示，杆件向量原点为旋转接头处且指向下一个机构接头。利用各杆件与旋转角在空间中的位置关系，求解机器人顺向运动方程式。X、Y、Z 轴转换矩阵分别如下：

将上式代入向量位置，计算出各杆件质量中心位置iP ，如上图所示：各杆件向量，以卡氏坐标方法，垂直方向为Z 轴，前进方向为X 轴，左右方向为Y 轴，且坐标表示法中向上向右为正，向左向下为负，1P为向量基准点，定义如下： D 1，D 2为髋部(Hip)，位置于坐标轴Y 轴的方向，且向左为负，则：

3.3 逆运动学方程式

逆运动学有许多种的算法则，可分为几何解析法、数值分析迭代法、代数法、坐标转换矩阵等来求解。运用轨迹追踪控制时必须先推导出机器人之逆运动学方程式，在分析上又可分为实时的方式与非实时的方式，而实时的方法有代数法、几何解析法、坐标转换矩阵求解。而数值分析迭代法属于非实时的方式。

双足步行机器人的控制上，希望可以达到实时的方式的控制要求，以下针对所建构的机器人加以分析逆运动学。为了降低求解的复杂度，把三维的卡式坐标分解为两个独立的二维坐标，一为前进坐标 (X-Z平面)，二为方向坐标(Y-Z平面)，如下图所示：

前进坐标

3.4 零力矩点平衡方程式

在最开始的双足机器人使用的平衡控制策略是“静态步行”，这种策略的特点是：机器人步行的过程中，重心（COG）的投影始终位于多边形支撑区域内，这种控制策略的好处在于：在整个的行进过程中，机器人可以在行走动作中停止而不摔倒，但代价是行动速度非常迟缓（每一步需要花费10秒甚至更长）（因为需要保持重心的投影始终位于支撑区域，否则将不稳定），因为静态步行和人类的期望相差甚远，于是人类开发出来了另一种步行平衡策略：“动态步行”，在动态步行中机器人的行动速度被提升至了每步不超过1秒。但是弊端也是显而易见的，机器人难以在运动的状态下立即停顿（惯性的作用），从而使得机器人在状态转换的过程中变得不稳定。为了解决惯性带来的影响，零力矩点（ZMP）被引入到了这一控制策略中。在单脚支撑相中，ZMP=COG。引入零力矩点的好处在于，如果零力矩点严格的存在于机器人的支撑区域中，机器人绝对不会摔倒。现在双足平衡的主流是用基于ZMP的动态步行。基于上述的基本内容，双足机器人的一条腿就可以抽象成控制系统中最基本的“倒立摆”模型。

倒立摆模型

可以推导出机器人的一条腿的正向ZMP动力学公式为：

正向ZMP动力学公式

Xzmp代表正向ZMP，Xmc代表质量中心的前进位移，l是倒立摆的长度，g是重力加速度。
由于复杂地形的双足平衡无法由单一的控制器实现，所以多个控制器的切换策略被用于解决平衡问题。在这一个策略中，机器人的行走被设定为一个周期（cycle）每一个周期被分成了不同的行走阶段（stage）如下图所示：

双足机器人行走周期和行走阶段

图中的1-4阶段是一个周期。而在第2和第4阶段可以发现，在这两个阶段中，机器人的SSP与DSP发生了“共存”，即同时存在状态。在一般的双足平衡的算法中包含了三种控制策略：（1）实时平衡控制策略

（2）步行模式控制策略

（3）行动预测控制策略

4. 仿生学控制

4.1 仿生学控制简介

目前基于仿生学的半被动步行实现主要有两种方法，一种是基于CPG（Central Pattern Generators）的，生物中枢模式发生器CPG能够在缺乏高层控制信号和外部反馈的情况下，自发产生稳定的节律性运动，因此人们模拟这种模型建立了基于CPG的被动机器人步行控制策略。例如卡内基梅隆大学Dynamic Biped Project（动态双足步行机器人计划）。一种是基于神经反射的，采用模拟人类神经系统对外界信号的反射来进行机器人行走的控制，例如斯图灵大学的RUNBOT，无论是CPG还是神经反射控制，都是利用了机器人自身的动力学特性，在步行的周期中进行了隐式的计算，将控制算法和机器人自身的动力学公式进行了耦合计算，这样既简化控制系统，又充分利用机器人的动力学特性，使得能量也得到节省。目前国内主要都是采用基于模型的控制策略，而在仿生学控制方面的研究较少，本文采用基于神经反射的控制系统。

4.2 系统描述

双足步行系统由机器人本体和控制系统组成，机器人本体由躯干，大腿和小腿组成，髋关节和膝关节处可以转动，踝关节和小腿固定，脚底装有压力传感器，用来检测触地情况，大腿和小腿还装有倾角传感器，用来检测倾斜角度，这些传感器得到的数值反馈给神经反射控制器，再由神经反射控制器输出髋关节和膝关节的转动力矩，从而实现机器人的动态稳定步行，整个机器人实体由动力学仿真软件ADAMS模拟，关节采用旋转副连接，脚与地面之间施加了接触力和摩擦力。

步行系统图

4.3 步行原理

如下图所示，A 点时，右脚触地，此时右脚膝关节保持角度不变，髋关节收缩，左脚膝关节收缩，髋关节伸展，运动到图中第二个动作B 时刻时，左脚髋关节达到最大伸展位置，激发左脚膝关节向前伸展，直到 C 时刻，左脚触地，然后左脚开始髋关节收缩，膝关节保持角度不变，重复之前的动作，关节伸展和收缩方向如下图所示：

步行原理图

运动方向和参数示例图

步行的复杂程度直接控制了机构的难易程度，用纯机械的机构来实现的人体行走过程，必然会有很大误差。步行主要有七个过程，由左右腿相互交替迈腿，收腿，抬脚，收脚，达到向前走的目的。如下图所示是人行走的几个主要过程：

步立行走的几个主要过程

5. 双足步行机器人控制系统介绍

5.1 控制板介绍

如下图所示为Arduino UNO单片机控制板和扩展板：

Arduino是一款便捷灵活、方便上手的开源电子原型平台，包含硬件（各种型号的Arduino板）和软件（Arduino IDE）。由一个欧洲开发团队于2005年冬季开发，其成员包括Massimo Banzi、David Cuartielles、Tom Igoe、Gianluca Martino、David Mellis和Nicholas Zambetti等。

Arduino能通过各种各样的传感器来感知环境，通过控制灯光、马达和其他的装置来反馈、影响环境。板子上的微控制器可以通过Arduino的编程语言来编写程序，编译成二进制文件，烧录进微控制器。对Arduino的编程是通过 Arduino编程语言 (基于 Wiring)和Arduino开发环境(基于 Processing)来实现的。基于Arduino的项目，可以只包含Arduino，也可以包含Arduino和其他一些在PC上运行的软件，他们之间进行通信 (比如 Flash, Processing, MaxMSP)来实现。

5.2 控制系统方案

本作品通过Arduino UNO控制板控制6个探索者舵机来实现步立平衡行走，控制方案如下图所示：

控制方案图

6. 算法之实现与仿真

6.1 MATLAB 和 ADAMS 联合仿真

为了验证控制方法的可行性，首先分析前进平面，使用ADAMS和MATLAB联合仿真，对其髋关节和膝关节施加了驱动力，脚与地面之间施加了接触力和摩擦力，接触力采用Restitution方式，摩擦力采用 Coulomb方式，采用ode45求解。如下图所示：

联合仿真系统图

各关节驱动力矩和关节角度如下图：

驱动力矩和关节角度图（左腿）

由上图可以看出，在红色直线内部分，关节驱动力矩几乎为零，但是机器人仍然依靠自身的动力学特性运动，这样就极大地节省了能量，而且关节的运动周期是和触地信号周期一致，这说明该机器人行走并不需要对外界环境进行建模，而是依靠触地信号来自动调节关节运动，这使得该步行系统具有对环境的适应性。联合仿真效果图如下所示：

步行效果图

6.2 三维位置向量静态模拟

6.2.1 双脚直立时

双脚直立时，由位置向量方程式计算，此姿态为机器人双脚直立且两脚对齐，并以MATLAB模拟，得左脚脚底板位置为(0,0)，右脚脚底板位置为(0,-6)，重心位置为(0,-3)，重心位于两脚中间，则机器人不倾倒。

双足步行机器人双脚直立示意图

6.2.2 右脚跨步时

右脚在前时，由位置向量方程式计算，此姿态为机器人右脚于左脚之前，且左脚及右脚舵机偏转角度为对称角度，并以MATLAB 仿真，得左脚脚底板位置为(0,0)，右脚脚底板位置为(4.2614,-6)，重心位置为(2.1307,-3)，重心位于前后两脚中间，则机器人不倾倒，如下图所示：

双足步行机器人右脚跨步意图

6.2.3 左脚跨步时

左脚在前时，由位置向量方程式计算，此姿态为机器人左脚于右脚之前，且左脚及右脚舵机的偏转角度为对称角度，并以MATLAB 仿真，得左脚脚底板位置为(0,0)，右脚脚底板位置为(-4.2614,-6)，重心位置为(-2.1307,-3)，重心位于前后两脚中间，则机器人不倾倒，如下图所示：

双足步行机器人左脚跨步示意图

6.3 双足步行机器人重心轨迹与零力矩点轨迹

零力矩点与重心位置是作为机器人稳定不倾倒之依据，因此必须落于如下图所示的双足步行机器人零力矩点路径轨迹上(直线区域内部分)，重心轨迹与零力矩点实际运动路径轨迹，得知重心轨迹与零力矩点轨迹两者皆落于规划的轨迹路径范围内，因此双足步行机器人稳定不倾倒。

双足步行机器人零力矩点路径轨迹

双足步行机器人COG、ZMP实际运动路径轨迹

6. 机器人运行程序

#include<Servo.h>

Servo servo1, servo3, servo2, servo4 , servo5, servo6;

int pos1,pos2,pos3,pos4,pos5,pos6;

int DSD=30;

void setup()

{

servo1.attach(4); // servo1 伺服舵机连接引脚4 舵机代号'1'

delay(200); // 稳定性等待

servo2.attach(7); // servo2 伺服舵机连接引脚7 舵机代号'2'

delay(200); // 稳定性等待

servo3.attach(11); // servo3 伺服舵机连接引脚11 舵机代号'3'

delay(200); // 稳定性等待

servo4.attach(3); // servo4 伺服舵机连接引脚3 舵机代号'4'

delay(200); // 稳定性等待

servo5.attach(8); // servo5 伺服舵机连接引脚8 舵机代号'5'

delay(200); // 稳定性等待

servo6.attach(12); // servo6 伺服舵机连接引脚12 舵机代号'6'

delay(200);

servo1.write(58);

delay(10);

servo2.write(63);

delay(10);

servo3.write(99);

delay(10);

servo4.write(100);

delay(10);

servo5.write(131);

delay(10);

servo6.write(58);

delay(10);

}

void loop()

{

for(pos2=63,pos3=99,pos5=131,pos6=58;pos5<166;pos5++)

{

if(pos2>58)

{

pos2--;

servo2.write(pos2);

}

if(pos3<106)

{

pos3++;

servo3.write(pos3);

}

if(pos6>39)

{

pos6--;

servo6.write(pos6);

}

servo5.write(pos5);

delay(DSD);

}

for(pos1=58,pos2=58,pos3=106,pos4=100,pos5=166,pos6=39;pos4<144;pos4++)

{

if(pos1<75)

{

pos1++;

servo1.write(pos1);

}

if(pos2<67)

{

pos2++;

servo2.write(pos2);

}

if(pos3<114)

{

pos3++;

servo3.write(pos3);

}

if(pos5<179)

{

pos5++;

servo5.write(pos5);

}

if(pos6<52)

{

pos6++;

servo6.write(pos6);

}

servo4.write(pos4);

delay(DSD);

}

for(pos1=75,pos4=144,pos5=179,pos6=52;pos6<79;pos6++)

{

if(pos1<78)

{

pos1++;

servo1.write(pos1);

}

if(pos4>127)

{

pos4--;

servo4.write(pos4);

}

if(pos5>129)

{

pos5-=2;

servo5.write(pos5);

}

servo6.write(pos6);

delay(DSD);

}

for(pos1=78,pos2=67,pos3=114,pos5=129;pos2>19;pos2-=2)

{

if(pos1>60)

{

pos1--;

servo1.write(pos1);

}

if(pos3<126)

{

pos3++;

servo3.write(pos3);

}

if(pos5>117)

{

pos5--;

servo5.write(pos5);

}

servo2.write(pos2);

delay(DSD);

}

for(pos1=60,pos2=19,pos4=127,pos5=117,pos6=79;pos4>81;pos4-=2)

{

if(pos1>40)

{

pos1--;

servo1.write(pos1);

}

if(pos2<30)

{

pos2++;

servo2.write(pos2);

}

if(pos5>103)

{

pos5--;

servo5.write(pos5);

}

if(pos6>72)

{

pos6--;

servo6.write(pos6);

}

servo4.write(pos4);

delay(DSD);

}

for(pos1=40,pos2=30,pos3=126,pos4=81,pos5=103,pos6=72;pos2<63;pos2++)

{

if(pos1<58)

{

pos1++;

servo1.write(pos1);

}

if(pos3>99)

{

pos3--;

servo3.write(pos3);

}

if(pos4<100)

{

pos4++;

servo4.write(pos4);

}

if(pos5<131)

{

pos5++;

servo5.write(pos5);

}

if(pos6>58)

{

pos6--;

servo6.write(pos6);

}

servo2.write(pos2);

delay(DSD);

}

7.2 机器人调试程序

#include <Servo.h> //使用servo库

Servo servo1, servo3, servo2, servo4 , servo5, servo6; //创建6个servo对象

//存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变)

const int servo1Min = 0;

const int servo1Max = 180;

const int servo2Min = 0;

const int servo2Max = 180;

const int servo3Min = 0;

const int servo3Max = 180;

const int servo4Min = 0;

const int servo4Max = 180;

const int servo5Min = 0;

const int servo5Max = 180;

const int servo6Min = 0;

const int servo6Max = 180;

int DSD = 15; //Default Servo Delay (默认电机运动延迟时间)

//此变量用于控制电机运行速度.增大此变量数值将

//降低电机运行速度从而控制机械臂动作速度。

bool mode; //mode = 1: 指令模式， mode = 0: 手柄模式

int moveStep = 1; // 每一次按下手柄按键，舵机移动量（仅适用于手柄模式）

void setup(){

servo1.attach(4); // servo1 伺服舵机连接引脚2 舵机代号'1'

delay(200); // 稳定性等待

servo2.attach(7); // servo2 伺服舵机连接引脚3 舵机代号'2'

delay(200); // 稳定性等待

servo3.attach(11); // servo3 伺服舵机连接引脚4 舵机代号'3'

delay(200); // 稳定性等待

servo4.attach(3); // servo4 伺服舵机连接引脚5 舵机代号'4'

delay(200); // 稳定性等待

servo5.attach(8); // servo5 伺服舵机连接引脚6 舵机代号'5'

delay(200); // 稳定性等待

servo6.attach(12); // servo6 伺服舵机连接引脚7 舵机代号'6'

delay(200); // 稳定性等待

servo1.write(168);

delay(10);

servo2.write(19);

delay(10);

servo3.write(118);

delay(10);

servo4.write(168);

delay(10);

servo5.write(19);

delay(10);

servo6.write(66);

delay(10);

Serial.begin(9600);

Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial");

}

void loop(){

if (Serial.available()>0) {

char serialCmd = Serial.read();

if( mode == 1 ){

armDataCmd(serialCmd); //指令模式

} else {

armJoyCmd(serialCmd); //手柄模式

}

void armDataCmd(char serialCmd){ //Arduino根据串行指令执行相应操作

//指令示例：b45 底盘转到45度角度位置

// o 输出机械臂舵机状态信息

//判断人类是否因搞错模式而输入错误的指令信息（指令模式下输入手柄按键信息）

if ( serialCmd == 'w' || serialCmd == 's' || serialCmd == 'a' || serialCmd == 'd'

|| serialCmd == '5' || serialCmd == '4' || serialCmd == '6' || serialCmd == '8'

|| serialCmd == 'y' || serialCmd == 'h' || serialCmd == 'g' || serialCmd == 'j' ){

Serial.println("+Warning: Robot in Instruction Mode...");

delay(100);

while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令

return;

}

if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'|| serialCmd == 'q' || serialCmd == 'e'){

int servoData = Serial.parseInt();

servoCmd(serialCmd, servoData, DSD); // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）

} else {

switch(serialCmd){

case 'm' : //切换至手柄模式

mode = 0;

Serial.println("Command: Switch to Joy-Stick Mode.");

break;

case 'o': // 输出舵机状态信息

reportStatus();

break;

case 'i':

armIniPos();

break;

default: //未知指令反馈

Serial.println("Unknown Command.");

}

void armJoyCmd(char serialCmd){ //Arduino根据手柄按键执行相应操作

//判断人类是否因搞错模式而输入错误的指令信息（手柄模式下输入舵机指令）

if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'|| serialCmd == 'q' || serialCmd == 'e'){

Serial.println("+Warning: Robot in Joy-Stick Mode...");

delay(100);

while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令

return;

}

int servo1JoyPos;

int servo2JoyPos;

int servo3JoyPos;

int servo4JoyPos;

int servo5JoyPos;

int servo6JoyPos;

switch(serialCmd){

case 'a': // servo1向左

Serial.println("Received Command: servo1 Turn Left");

servo1JoyPos = servo1.read() - moveStep;

servoCmd('b', servo1JoyPos, DSD);

break;

case 'd': // servo1向右

Serial.println("Received Command: servo1 Turn Right");

servo1JoyPos = servo1.read() + moveStep;

servoCmd('b', servo1JoyPos, DSD);

break;

case 's': // servo2向下

Serial.println("Received Command: servo2 Down");

servo2JoyPos = servo2.read() + moveStep;

servoCmd('r', servo2JoyPos, DSD);

break;

case 'w': // servo2向上

Serial.println("Received Command: servo2 Up");

servo2JoyPos = servo2.read() - moveStep;

servoCmd('r', servo2JoyPos, DSD);

break;

case '8': // servo3向上

Serial.println("Received Command: servo3 Up");

servo3JoyPos = servo3.read() + moveStep;

servoCmd('f', servo3JoyPos, DSD);

break;

case '5': // servo3向下

Serial.println("Received Command: servp3 Down");

servo3JoyPos = servo3.read() - moveStep;

servoCmd('f', servo3JoyPos, DSD);

break;

case '6': // servo4关闭

Serial.println("Received Command: servo4 Close Down");

servo4JoyPos = servo4.read() + moveStep;

servoCmd('c', servo4JoyPos, DSD);

break;

case '4': // servo4打开

Serial.println("Received Command: servo4 Open Up");

servo4JoyPos = servo4.read() - moveStep;

servoCmd('c', servo4JoyPos, DSD);

break;

case 'y': // servo5向上

Serial.println("Received Command: servo5 Up");

servo5JoyPos = servo5.read() + moveStep;

servoCmd('q', servo5JoyPos, DSD);

break;

case 'h': // servo5向下

Serial.println("Received Command: servo5 Down");

servo5JoyPos = servo5.read() - moveStep;

servoCmd('q', servo5JoyPos, DSD);

break;

case 'j': // servo6关闭

Serial.println("Received Command: servo6 Close Down");

servo6JoyPos = servo6.read() + moveStep;

servoCmd('e', servo6JoyPos, DSD);

break;

case 'g': // servo6打开

Serial.println("Received Command: servo6 Open Up");

servo6JoyPos = servo6.read() - moveStep;

servoCmd('e', servo6JoyPos, DSD);

break;

case 'm' : //切换至指令模式

mode = 1;

Serial.println("Command: Switch to Instruction Mode.");

break;

case 'o':

reportStatus();

break;

case 'i':

armIniPos();

break;

default:

Serial.println("Unknown Command.");

return;

}

void servoCmd(char servoName, int toPos, int servoDelay){

Servo servo2go; //创建servo对象

//串口监视器输出接收指令信息

Serial.println("");

Serial.print("+Command: Servo ");

Serial.print(servoName);

Serial.print(" to ");

Serial.print(toPos);

Serial.print(" at servoDelay value ");

Serial.print(servoDelay);

Serial.println(".");

Serial.println("");

int fromPos; //建立变量，存储电机起始运动角度值

switch(servoName){

case 'b':

if(toPos >= servo1Min && toPos <= servo1Max){

servo2go = servo1;

fromPos = servo1.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo1 Servo Value Out Of Limit!");

return;

}

case 'r':

if(toPos >= servo2Min && toPos <= servo2Max){

servo2go = servo2;

fromPos = servo2.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo2 Servo Value Out Of Limit!");

return;

}

case 'f':

if(toPos >= servo3Min && toPos <= servo3Max){

servo2go = servo3;

fromPos = servo3.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo3 Servo Value Out Of Limit!");

return;

}

case 'c':

if(toPos >= servo4Min && toPos <= servo4Max){

servo2go = servo4;

fromPos = servo4.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo4 Servo Value Out Of Limit!");

return;

}

case 'q':

if(toPos >= servo5Min && toPos <= servo5Max){

servo2go = servo5;

fromPos = servo5.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo5 Servo Value Out Of Limit!");

return;

}

case 'e':

if(toPos >= servo6Min && toPos <= servo6Max){

servo2go = servo6;

fromPos = servo6.read(); // 获取当前电机角度值用于“电机运动起始角度值”

break;

} else {

Serial.println("+Warning: servo6 Servo Value Out Of Limit!");

return;

}

//指挥电机运行

if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”

for (int i=fromPos; i<=toPos; i++){

servo2go.write(i);

delay (servoDelay);

}

} else { //否则“起始角度值”大于“目标角度值”

for (int i=fromPos; i>=toPos; i--){

servo2go.write(i);

delay (servoDelay);

}

void reportStatus(){ //舵机状态信息

Serial.println("");

Serial.println("+ Robot-Arm Status Report +");

Serial.print("servo4 Position: "); Serial.println(servo4.read());

Serial.print("servo1 Position: "); Serial.println(servo1.read());

Serial.print("Rear Arm Position:"); Serial.println(servo2.read());

Serial.print("Front Arm Position:"); Serial.println(servo3.read());

Serial.print("servo5 Position: "); Serial.println(servo5.read());

Serial.print("servo1 Position: "); Serial.println(servo6.read());

Serial.println("++++++++++++++++++++++++++");

Serial.println("");

}

void armIniPos(){

Serial.println("+Command: Restore Initial Position.");

int robotIniPosArray[6][3] = {

{'b', 180, DSD},

{'r', 160, DSD},

{'f', 0, DSD},

{'c', 120, DSD},

{'q', 90, DSD},

{'e', 120, DSD},

};

for (int i = 0; i < 6; i++){

servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);

}

8. 结论与未来研究方向

本项目首先分析了人体下肢结构特点，设计了小型双足步行机器人的结构，分析整个机构的最大运动空间及机构的限制条件。设计了双足机器人的整体机构。推导了双足步行机器人的数学模型，包括运动学，逆运动学，求解各位置向量坐标，并利用位置向量求得重心位置（COG）及零力矩点（ZMP）位置，以作为小型双足步行机器人是否倾倒之依据，接着分析了双足机器人的动力学模型，指出了该系统的非线性特征，分析了传统控制器的缺陷，提出了新的仿生学控制策略。最后设计基于神经反射的控制器，该控制器通过控制舵机，使其达到各杆件机构运动之目标与目的，以实现小型双足步行机器人步行轨迹之控制。最后在 MATLAB 和 ADAMS 环境中进行了仿真分析，验证了该控制器的合理性，成功设计了一个小型双足步行机器人。本文尚有不足之处，今后将在以下方面改进：

① 设计了实体机构，并提出了控制策略，但没有进行实体验证，只进行了仿真验证及分析。

② 仿真验证只做了平面环境的分析，并没有进行斜面以及非平坦地面分析。

③ 将双足步行机器人下半身机构再增加上半身机构，模拟人类行走之实际平衡及步态，进而上下楼梯，左转及右转等。

* 本项目未获得作者开源授权，无法提供资料下载。