第4卷第5期 智能系统学报 VoL.4 No.5 2009年10月 CAAI Transactions on Intelligent Systems 0ct.2009 doi:10.3969/j.issn.16734785.2009.05.013 二足机器人之动态平衡研究 王仲淳，倪世铨，黄加庆，李明哲，郑晖腾，郭怡均 (元智大学电机系，台湾中扬32003) 摘要：主要探讨二足机器人(biped robots)行走或受到外力干扰时，通过动态平衡控制使机器人行走更趋稳定，并 增强站立时稳定性.二足机器人动态平衡之实现，主要是将动态平衡控制程序撰写于N0sⅡ发展环境中，当二足机 器人行走时，利用脚底压力传感器取得压力值，运算及判断二足机器人实际重心是否落在二足机器人支撑多边形范 围内，并计算实际重心与期望重心之误差，以模糊控制器将二足机器人重心控制于支撑多边形范围内，使二足机器 人行走时能够更加稳定，实验结果表明该方法是有效的， 关键词：二足机器人；动态平衡控制；模糊控制器 中图分类号：TP242文献标识码：A文章编号：16734785(2009)05046308 Dynamic balance for bipedal robots WANG Jong-chwen,NI Shih-chiuan,HUANG Chia-ching,LI Ming-che,CHENG Hui-teng,KUO Yi-chun (Department of Electrical Engineering,Yuan Ze University,Chung-Li 32003,China) Abstract:The main purpose of this paper was to study how to enhance the dynamic balance of bipedal robots dur- ing walking or when interacting with external forces.Their stability while standing was also examined.The dynamic equilibrium of the tested biped robots resulted from built-in dynamic control programs which were written in the Nios development environment.To get pressure values from the pressure sensors of biped robots,we used control algo- rithms to compute the position of the actual center of gravity (CoG)and also to determine whether the CoG fell in- side the polygon defining the area of balance for the robot.It was found possible to use a fuzzy controller to compen- sate for errors due to differences between the actual CoG and the theoretical one.The objective was to keep the CoG within the supporting polygon area.This helped the bipedal robots walk more steadily and also improved their over- all stability when standing.The experimental results show that this method is effective. Keywords:biped robot;dynamic state control;fuzzy control 随着科技的进步，世界各国渐渐开始投入机器 本文主要研究二足机器人的动态平衡，当机器 人领域的研究.机器人有很多不同类型，像是水下机 人运行时，经由传感器取值并通过控制器补偿机器 器人、飞行机器人、地下机器人等.不论是何种类型 人重心稳定位置，分析比较不同控制器的性能差异， 机器人大致上都可以分为轮型、多足型和轨道式等， 让机器人不论在静态或动态状态下都能够具有抗干 其中又以最像人类的二足机器人(biped robots)最 扰性能，使得机器人在姿态及运作效能上有所改善. 受世人瞩目 1二足机器人硬件架构 目前二足机器人步行还无法像人类一样，来去 自如行走于不同的地形上，而且人类步行时本身因1.1硬件架构 为拥有平衡功能，所以步行时才不至于跌倒，目前二 如图1所示为二足机器人正面及侧面图，该二 足机器人步行时最需克服的就是平衡问题.二足机 足机器人是由双足10个自由度、头部1个自由度、 器人克服地形限制后，即可代替人类于危险环境或 FPGA发展实验板、伺服机驱动模块、无线模块、超 是人类无法到达的地方执行各种任务. 音波传感器、电子罗盘、加速度计以及8颗压力传感 收稿日期：200903-12. 器所架构出的.二足机器人站立时高为320mm,宽 通信作者：郭怡均.E-mail:goldber6@yahoo.com.tw. 为230mm,重量约1.5kg,脚底是A/D转换电路及
·464 智能系统学报 第4卷 8颗压力传感器组合，大小为6cm×9cm. (Microsoft foundation classes)架构建立了监控画面， 时 以利能够在桌上型计算机(desktop)上监控二足机 器人各个传感器取值的情况.如图4所示为压力监 控画面。 图1二足机器人正面及侧面图 Fig.1 The biped robot 该二足机器人主要是藉由脚底压力传感器取值 图4脚底压力监控画面 并运算出实际重心位置后，并控制伺服机以达到行 Fig.4 Foot pressure monitoring screen 走平衡的目的.本文所使用的压力传感器为 1.2.1平衡架构 FSR400,如图2所示.SR400是一种整体体积小且 二足机器人步行时，平衡控制重心的偏移分为 能够连续检测压力强弱的电阻式压力传感器. 双脚与单脚2种情况，如图5所示为二足机器人 双脚控制流程， 机器人一 单與粹制 双翠 控制 单解控制 图5二足机器人双脚控制流程 Fig.5 Biped robot legs control flow 图2SR400压力传感器实体图 如图6所示为二足机器人脚底坐标系统，二足 Fig.2 FSR400 pressure sensor entity chart 机器人重心位置偏移分为X、Y2种不同方向，经由 由于压力传感器输出为模拟信号，为了使得 此2种不同方向判断出实际重心位置，当二足机器 FPGA发展实验板取得压力值，故透过IC设计出AV 人步行时往前为-X方向，往后为+X方向，而往左 D转换电路.为了节省空间，故将A/D转换电路制 倾时为一Y方向，往右倾则为+Y方向。 作于二足机器人脚底，如图3所示为二足机器人脚 桑X灯后) 底实体图 -Y左} (右) 压力感测器 -x灯前) 图3二足机器人脚底实体图 图6二足机器人脚底坐标系统 Fig.3 The biped robot sole physical map Fig.6 Biped robot foot coordinate system 1.2软件架构 本文平衡架构分为压力传感器初始化、压力传 本研究的目的是实现二足机器人的动态平衡， 感器取得压力值、压力值滤波、重心误差、平衡控制 主要通过程序撰写于FPGA发展实验板中.为了能 及伺服机补偿等6个架构，通过这6部分规划出整 够方便测试与监控整个机器人状态，使用VC6.0 个二足机器人步行平衡控制流程，如图7所示. (Microsoft visual C+6.0)开发环境，并利用MFC
第5期 王仲淳，等：二足机器人之动态平衡研究 ·465· 府动 压力感测器 初始化 压力感测发 读取压力值 压力值滤波 计算压力重心 -1 期望重心 重心误差 判断重心位置 Error=0 Y方向 X方向 Error=0 t上rror≠0 rror≠0 平衡控制器 平衡控制器 单脚支持 单脚支持 相位 相位 单或双脚 单或双脚 者地 若地 双脚支持 双脚支持 左 神位 左脚或右脚 左 左脚或h脚 相位 者地 着地 五 补偿左脚腹新 补偿右脚腹部 补偿双脚腹部 补偿左脚 补偿右脚 补偿双脚 及脚躁角度 及脚踩角度 及脚蹀角度 膝盖角度 膝盖角度 膝盖角度 主控端足不停止 Y 结束 图7步态平衡流程图 Fig.7 The flow chart of walk-state balance 1.2.2压力传感器初始化 1.2.3计算压力值 图8所示为压力值初始化流程图.二足机器人 二足机器人运行时，压力传感器受力后其电阻 站立时的重量对地面有一定作用力，所以二足机器 值随之改变，经由AVD电路转换成PWM信号，透过 人启动前会将压力传感器初始化，其用意为取得二 FPGA实验板运算PWM由低电位到高电位脉波数 足机器人站立时初始状态压力值，并排除机器人本 量，从而取得各个压力传感器的实际压力值，如图9 身震动之干扰，避免二足机器人平衡系统误判。 所示为取得压力值流程图.PCA实验板运算后各 个压力传感器压力值的范围为0~4000个脉冲
·466· 智能系统学报 第4卷 开始 d() 0.2 二足机器人运行 Input- -du-1) 爪力感测器取爪力值一 -2→ 压力值加总求基准俏 图10数字滤波器 Fig.10 Digital filter 取值是否达到10还 N 1.2.5计算重心及平衡控制 Y 二足机器人移动时会有一个实际重心位置，实 一压力感测器取压力值 际重心分为X、Y2个不同方向，二足机器人运动时 每一个动作都会对应一个理论重心位置，将实际重 平衡系统 心与期望重心相减后得到一组重心误差值，此误差 值是平衡控制的主要依据.将重心的误差值通过控 主端是否停止一 制器运算，运算出伺服机补偿角度，藉由此角度补偿 Y 所对应的伺服机，来补偿误差的影响，使二足机器人 结束) 行走时能更加顺畅稳定，并让机器人能够拥有抗干 图8压力传感器初始化流程图 扰的功能，如图11所示. Fig.8 Pressure sensor initialization flowchart X方向误差值 X方向补偿伯 开始 控制器 二足机器人运行 方向误差值 Y方向补偿值 压力感测器取初始化 图11补偿值运算流程图 Fig.11 The flow chart of compensation value operation 压力感测器感受压力+ 2平衡控制理论 AD电路产生Pulse 2.1零力矩点 PGA计算ls数量 零力矩点(zero moment point,ZMP)是由Vuko bratvic于1968年提出[3].如图12所示为脚底反作 取得压力值 用力情形，二足机器人接触地面时，若将地面的反作 用力集中一点P,会有集中力(counterforce)N及力 半衡控制系统 矩M产生，此时P点周围的惯性力及重力产生的净 力矩和为零，即称P点为零力矩点. 主控端是停止 Y 结束 ZMP 图9取得压力值流程图 Fig.9 The flow chart of pressure value obtaining 1.2.4压力值滤波 二足机器人运行时机身晃动将产生突波，因此 藉由滤波器降低突波干扰.本文采用的滤波方法是 图12脚底反作用力情形 将各个压力传感器当次取得的压力值与前1次及前 Fig.12 Foot reaction force 2次所取得的压力值分别乘一定得倍率后加总而 二足机器人运动时本身各连杆会产生加速度而 成.如图10所示为数字滤波器，d(t)为当前所取到 具有惯性力.假设机器人本身全部连杆、力矩与地面 的压力值、d(t-1)为前1次所取到的压力值、d(t- 反作用力相互抵消，即二足机器人所有杆件合力作 2)为前2次所取到的压力值 用于二足机器人支撑多边形（脚掌）内，此时二足机
第5期 王仲淳，等：二足机器人之动态平衡研究 ·467· 器人则保持稳定不倾倒；反之若二足机器人合力作 用于支撑多边形外，二足机器人则会有产生倾倒状 况发生，如图13所示， {xny）,⊙ (x2ys () 左脚单脚支撑面 . 图14压力传感器位置图 Fig.14 Pressure sensor location 右脚单脚支撑向 2.2模糊控制器 本文还要设计一模糊控制器，并由模糊控制器 图13二足机器人步行时ZMP的稳定范围 运算出各伺服机所需的补偿角度.如图15所示为模 Fig.13 Biped robot walking stability of the scope of ZMP 糊控制器补偿流程，将二足机器人期望重心h。与实 际重心位置e,相加减会得到位置误差e,误差e通 通过机器人机身各连杆的关系可求得零力矩点 过模糊控制器运算得到各个伺服机补偿角度9，经 位置，其公式如式(1)所示[3)： 由所对应伺服机将补偿角度日作补偿，让二足机器 ∑(mx8-(m:(元-x)+0y）） 人在站立或步行时达到平衡， h,e+模糊制器 二足机器人 饲服机 mg ∑m: 实际重心、压力感测器 ,(my8-(m:(-y:)+ù)) 图15模糊控制器补偿流程 Fig.15 Fuzzy controller compensation process 补偿方向分为X、Y2个方向，所以X方向及Y (1) 方向的前件部分别为重心误差△x、△y,重心误差变 式中：(xmym,0)为零力矩点的坐标位置；m为总 化量△、△y,后件部为二足机器人双脚伺服机补偿 质量；g为重心的加速度；m:为第i个连杆的质量； 角度大小“、出，·X方向补偿角度向机器人前方“ (x:,y:,)为第i个连杆的质心位置；（无，少，）为第 为负，向后4.为正，Y方向补偿角度向右“，为正，向 i个连杆的质心位置加速度；o、⊙为第i连杆X、Y 左“，为负.前件部模糊集合为5个，后件部模糊集 轴角加速度， 合为7个.△x、△y可以取EM(误差负大)、ES(误差 Vukobratvic也说明若二足机器人运动时所有杆 负小)、EZ(误差零)、EL(误差正小)、EB(误差正 件合力作用于支撑多边形内，压力中心与零力矩点 大)，△、△y可以取DS(差量负大)、DY(差量负 的位置是同一个点，则零力矩点与重心关系可以通 小)、DZ(差量零)、DB(差量正小)、DL(差量正大)， 过二足机器人脚底压力传感器与地面反用力反馈计 山“，其值为ML(补偿负大)MM(补偿负中)、MS 算出二足机器人实际零力矩点位置[45].实际重心 (补偿负小)、Z0(补偿零)、PS(补偿正小)、PM(补 位置可由式(2)求得： 偿正中)和L(补偿正大) ∑xf 经由前件部与后件部的对应并定义出X、Y方 X gp 向的模糊集合如图16~21所示 ∑f =1 (2) -40 -20 0 20 40 式中：f为地面的反作用力；(x:,少：)为压力传感器坐 图16△x归属函数 标位置.图14所示为压力传感器位置图. Fig.16 Ax membership functions