Robotis OP仿人机器人权威指南

版权信息

书名：Robotis OP仿人机器人权威指南

ISBN：978-7-115-47302-8

本书由人民邮电出版社发行数字版。版权所有，侵权必究。

您购买的人民邮电出版社电子书仅供您个人使用，未经授权，不得以任何方式复制和传播本书内容。

我们愿意相信读者具有这样的良知和觉悟，与我们共同保护知识产权。

如果购买者有侵权行为，我们可能对该用户实施包括但不限于关闭该帐号等维权措施，并可能追究法律责任。

•　著　　　　[韩] 金钟熠（Jong-Wook Kim）

　 译　　　　武传海

　 责任编辑　 胡俊英

•　人民邮电出版社出版发行　　北京市丰台区成寿寺路11号

　 邮编　100164 　电子邮件　315@ptpress.com.cn

　 网址　http://www.ptpress.com.cn

•　读者服务热线：(010)81055410

　 反盗版热线：(010)81055315

版权声明

How to Manage Humanoid Robot Together : Robotis OP

by Jong-Wook Kim

copyright © 2015

All rights reserved by the proprietor, HongReung Science Publishing Company

No part of this book may be used or reproduced in any manner whatever without written permission except in the case of brief quotations embodied in critical articles or reviews.

中文译本：版权所有 © 2017 人民邮电出版社

中文版本依据 HongReung Science Publishing Company 的安排通过 Media Solutions（东京）和 Agency One（首尔）出版。

内容提要

随着科技的不断发展，机器人逐渐成为现今和未来世界值得期待的又一科技产物。仿人机器人模拟人类的身体结构，可以与人类协同工作或者代替人类进行工作，全世界范围内都在积极地研究并开发它。

本书通过6章内容，全面介绍了Robotis公司的ROBOTIS OP机器人。通过对基础理论、仿人机器人运动学、仿人机器人动力学、为仿人机器人创建动作及动作生成等章节的学习，读者将深度了解仿人机器人的各种知识点和关键技术点。

本书适合机器人爱好者阅读，也适合机器人研发和竞赛等相关领域的专业人士参考学习。

序言

仿人机器人模拟人类的身体结构，是一种由上体与下体组成的机器人平台。它可以与人类协同工作或者代替人类进行工作，全世界范围内都在积极地研究并开发它。在2011年3 月福岛核电站核泄漏事故发生之前，仿人机器人的应用重点放在智能生活的服务上，比如在家庭或公司中充当向导，代人提物，或者按时给老人拿药等；事故之后，人们进一步改进了仿人机器人，使其可以提供某种特殊的服务，如代替人类进行危险作业，或者在灾难现场保护人类等。

2000年，日本开发了ASIMO机器人。2004年，韩国向大众公开了HUBO机器人。2015年6月，在美国举行的DRC决赛中出现的仿人机器人可以两腿直立行走，可以开门，用手工作，上下楼梯，与人类对话。像这样，仿人机器人不只存在于科幻电影中，现在它们正在一点点地步入我们的现实生活中。

仿人机器人的目标是实现人类可以做的所有行为，并进一步具有人类的智能与感性，与人类真正进行沟通。其应用领域是无穷无尽的。但是相比于其他领域而言，仿人机器人相关技术发展相对缓慢，原因很多，如机器人零件（各种传感器、执行机构、嵌入式系统）昂贵、多种复杂信号处理及动作生成技术的困难、与人类相比对情势的判断能力明显低下，智能水平低等。为了解决这些问题，除了不断进行研究开发之外，笔者认为目前最迫切的是培养专业人才，让他们掌握仿人机器人的相关理论，同时要培养他们的实际动手能力。

令人遗憾的是，目前讲解仿人机器人的专业书籍在全世界范围内都很难找到。这里说的专业书籍系统讲解有关仿人机器人的机械原理与动力学理论，并收录有机器人实用技术的示例代码。有感于此，我在2012年8月出版了《仿人机器人》一书。它以Robotis公司的BIOLOID为对象，介绍机器人有关理论与实用技术，以及动作创建等示例。转眼之间，3年多过去了，Robotis公司推出了ROBITIS OP（原名叫DARwIn OP）机器人，它比之前的BIOLOID在硬件与软件方面有很大的升级。于是，我以ROBITIS OP为对象，对之前出版的图书在理论内容与示例方面做了大量修改补充，并将其作为专业教材重新出版。

MATLAB是全世界范围内广泛使用的仿真软件，一般理工科学生、研究生、工程技术人员都会使用它。掌握有关仿人机器人的基础理论后，使用MATLAB对各种运动与直立行走等进行模拟，再编写为C++代码，然后在基于Windows或Linux OS的ROBOTIS OP上进行测试、应用。本书会对这一过程进行详细讲解，并提供相关源代码供各位使用。换言之，本书系统讲解了仿人机器人相关理论以及基本的实用技术。通过阅读本书，各位将会对仿人机器人产生兴趣，有助于提升在这方面的能力与自信。

由于本人水平有限，以及受到篇幅的限制，本书并不涵盖各种技术的方方面面，相关内容将在后续版本中不断添加。如同制造一个像人类感知、思考与行动的仿人机器人一样，希望各位读者朋友给予关心、支持与帮助，相信将来会越来越好。

在本书写作与出版过程中，我得到了许多人的帮助与支持，借此机会向他们表达深深的谢意。他们分别是洪陵科学出版社社长及员工，感谢他们的辛勤付出，还有给予大力支持的Robotis公司的金炳洙代表、为我提供技术支持的金熙日，以及开发ROBOTIS OP的许多国内外研究人员。

最后，特别感谢我的爱人与两个女儿（珪丽与智芸），谢谢她们在本书写作期间对我的忍耐与鼓励。在此把本书献给家人，一起分享喜悦。

Jong-Wook Kim

2015年8月

第1章　绪论

1.1　机器人历史

仿人机器人拥有人类外形，经常出现在《铁臂阿童木》《魔神Z》《跆拳V》等动画中，以及《机械战警》《终结者》《机械公敌》《变形金刚》等科幻电影中，一直存在于人们的想象之中（见图1.1）。机器人的始祖是工业机器人，从20世纪中期开始商业化，大量投入到工业生产线。iRobot公司的扫地机器人Roomba从2003年开始在全世界范围内进行销售，一跃成为服务型机器人的著名品牌。随着技术进步与融合时代的到来，预计到21世纪20年代中期就会进入一人一机器人的时代。许多机器人专家达成共识，一致认为每人拥有一台以上伴侣机器人（companion robot）的时代很快就会到来。

图1.1　以机器人为主角或配角的电影

现代人对“robot”（机器人）一词已经非常熟悉，它初次出现在捷克剧作家卡雷尔·恰佩克在1920年所写的戏剧《罗素姆万能机器人》（Rossum’s Universal Robot）中。剧中，罗素姆父子建造了某种人造人“Robota”，它们只是努力工作，最后它们有了自主意识，意识到自身一直被利用，决定铲除人类。顺便说一下，robota一词在捷克斯洛伐克语中带有“工作”与“强制劳动”的含义。

机器人用来代替人类从事简单重复作业，从事要求高精度的作业，或者具有危险性的作业。全世界有很多大学与研究所在进行相关研究。在工业领域中，工业机器人最先开发成功并应用到工业现场中。1954年，美国人George Devol申请了工业机器人专利，当时名称为“可编程装置”。1961年，Joseph Engelberger开发出了工业制造机器人Unimate（见图1.2），并将其投入到General Motors公司的组装生产线上，这在世界上还是首次。此后，从1961年福特汽车首先把模具铸造机械的脱吸机器人投入到工作现场后，多种工业机器人被开发并制造出来。

图1.2　最早的工业机器人Unimate

工业机器人是第一代机器人，被定义为“可重复编程的通用操作机械”。它们被设计与制造出来，主要用来从事一些重复性的工作，有些工作需要保持高精密度与高可靠性，如焊接、组装、涂装（painting）、转送等。工业机器人由电力驱动，只要不发生电气与机械故障，它们就可以24小时不间断地工作，逐渐取代从事简单工作的人力。中国台湾富士康（Foxconn）是世界上最大的电子产品生产企业，为了降低人力成本，计划用3年时间把100万台机器人投入到生产线中（2011年7月30日，新华通讯社）。

2012年全世界机器人市场规模达到133.2亿美元，其中工业（用于制造）机器人约为86.8亿美元（占65.2%），专业服务型机器人约为34.2亿美元（占25.7%），个人服务型机器人约为12.2亿美元（占9.2%）。工业机器人所占份额超过半数以上。然而，从年平均成长率来看，个人服务型机器人占17.7%，专业服务型机器人占15.1%，制造型机器人占8.2%，从中可以看出工业机器人成长速度逐渐放缓。最近，工业机器人融入了更多先进技术，如尖端信息通信技术、用户友好型软件、嵌入式系统、人工智能、物联网（Internet of Things）技术等。全世界的研究人员都在积极研究探索在Smart Factory中机器人如何实现与人类有效地进行合作。

工业机器人在固定场合根据程序预先编写好的指令反复执行指定的任务。美国NASA制造的探测机器人（rover）与此不同，它被开发用来执行高难度工作。Sojourner在1997年被发射到火星，拥有6个特殊驱动轮、太阳能电池板、摄像机、数据传送天线，并且带有嗅觉设备，可以感知气味、分析土壤或岩石成分，利用这些设备，对火星表面岩石组成元素进行检测，并把数百张照片发送回地球。当时，Sojourner成功地完成了指定的任务。接着，为了调查火星表面岩石与土壤的样本，NASA在2004年又向火星发射了Spirit Rover与Opportunity Rover。它们应用了智能技术，不仅可以实时感知周围环境，还可以根据周围环境自主运行。在2012年着陆火星的Curiosity Rover（见图1.3）也属于这种探索型机器人。它一直活跃在火星上，不断向NASA发送探测信息，从这些信息可以推断火星地表下很有可能存在液态水。

图1.3　火星探测机器人Curiosity Rover

像这样，科学技术越发达，就越需要机器人拥有智能（intelligence），要求机器人具备自主判断、运行，以及从事复杂作业的能力。我们把拥有这种智能的机器人称为智能机器人。

一般地，智能机器人被定义为可以自动感知（perception）外部环境，对周围情况做出判断（cognition），并自主做出相应动作（mobility）的机械装置（韩国在2008年2月出台了智能机器人开发与普及促进法案）。更具体地说，智能机器人是机械、电子、信息、生物工程技术的融合产物，综合运用了环境识别、信息获取、智能判断、自主行为等人工智能技术。它可以向人类提供帮助，并代替人类在恶劣环境中工作，或者从事特殊作业。当今世界趋势表现为快速老龄化、低出产率、个体化、社交网络、技术融合、追求健康生活等，这些趋势进一步增强了对智能机器人的需求。

智能机器人大致可分为个人服务型机器人、专业服务型机器人、工业机器人。

• 个人服务型机器人：以普通人为服务对象，向他们提供服务的机器人，如家用机器人（扫地机器人等）、教育机器人、娱乐机器人、个人秘书机器人、看护机器人、伴侣机器人等。
• 专业服务型机器人：协助专家，向社会提供所需服务的机器人，如医疗机器人、国防机器人、建设机器人、社会安全机器人等。
• 工业机器人：应用于各种工业生产活动的机器人，如生产制造机器人、农业机器人、渔业机器人等。

智能机器人常驻于家庭或办公室中，为人类提供智能服务，必须能够与人类相互进行智力、情绪交流。人类居住空间中的家具把手、厨房、家电产品全部依据人类自身的身体条件制造，并且门槛与楼梯有高低。考虑到这些，再加上要有亲切感的外形，最合适的机器人不是轮式机器人，而是拥有类似人类身体结构的仿人机器人（人形机器人）。

1.2　仿人机器人的历史

仿人机器人在设计时模仿了人类身体骨骼与肌肉结构，拥有20~40个自由度的复杂结构，并且两足行走技术难度较大，所以经过很长时间的努力才出现在大众面前。

对于机器人人类外形的研究最早可以追溯到绘画天才莱昂纳多·达芬奇（Leonardo da Vinci）。莱昂纳多·达芬奇在1495年制作了机器人骑士（见图1.4），并在宫廷宴席中进行展示。据说通过操纵滑轮与绞绳，机器人可以做出坐下与站起的动作，也可以移动胳膊，以及把盔甲的面罩（visor）向上抬起。

图1.4　莱昂纳多·达芬奇的机器人骑士（robot knight）

19世纪，John Brainerd开发出了依靠蒸汽机驱动运行的Stream Man，Frank Reade Junior开发出了Electric Man，引起了人们对仿人机器人的关注。

20世纪初，美国Westinghouse公司研发出了名叫Elektro的仿人机器人。到1970年左右，日本正式开始发表关于有足机器人的研究。同时，俄罗斯与美国也在积极研究多足机器人（四足或六足机器人）。美国在1980—1983年间成功开发了名叫cmU的六足机器人（hexapod），最高行走速度为0.11 m/s，相比以前，相关技术得到了很大的提高。此外，MIT从20世纪80年开始积极研究跳跃机器人，并推出了名叫Spring、Flamingo、Uniroo的三维两足行走机器人。通过静态步行与动态步行，它们可以做出行走与跑跳动作。

在四足机器人中，在姿态稳定性与性能方面表现最优秀的机器人当数美国Boston Dynamics公司在2005年推出的BigDog。它在美国DARPA（Defense Advanced Research Projects Agency，国防部高级研究计划署）资助下被开发出来，主要任务是帮助军人运载装备，整机重量达110 kg，时速为6.4 km，可以运送150 kg的物资，攀越35°斜坡。BigDog的执行机构不是电动机，而是液压缸执行机构，可以提供更大承载力，内置大约50个传感器，借助机载计算机对姿态与运动进行准确控制。Boston Dynamics在2008年进一步改进了BigDog，推出了AlphaDog。它可以自由地在光滑冰面上行走。即使从一旁猛踹，它也能轻松地保持平衡。到2013年10月，该公司又推出了WildCat，奔跑时速达到26 km，集中体现了四足步行机器人的尖端技术（见图1.5）。

图1.5　Boston Dynamics公司的BigDog（左侧）与WildCat（右侧）

日本是最早开发可稳定行走的仿人机器人的国家。在仿人机器人的研究领域，他们一直保持着领先。日本早稻田大学从1966年就开始研究两足行走机器人（WL系列），并在1968年开发出了世界上第一个步行机器人WL-3。此外，在1983年推出的WL-10R使用10个伺服电机驱动关节，实现前进、后退以及改变方向等操作。早稻田大学最新的仿人机器人是在2009年推出的WABIAN（WAseda Bipedal humANoid）-2R（见图1.6），共有41个关节，身高为1.53 m，体重为64.5 kg。该机器人的平均步行速度为0.36m/s，每一步的周期是0.96 s。

图1.6　早稻田大学的WABIAN-2R

日本汽车制造公司Honda借鉴了WL系列机器人的优秀技术，在1986年到1993年间开发出了E0~E6，在1993年到2000年间开发出了P1~P3系列仿人机器人。最终，Honda公司在2000年11月第一次面向普通人推出了仿人机器人ASIMO（Advanced Step in Innovative MObility）。这让全世界的机器人研究人员大为吃惊，之前他们一直认为仿人机器人是不可能实现的。

此后，Honda公司不断对ASIMO进行升级，在2002年推出一代ASIMO，它具备与人沟通的能力，可以理解人类的姿势与身体动作；接着在2004年推出二代ASIMO，其动作性能得到极大改善，移动速度达到3 km/h；在2005年推出三代ASIMO，它可以完成“8”字形行走与人牵手行走、推着手推车走，或者递盘子等动作；2007年推出的新一代机器人可以与多台ASIMO一起协同工作，或者在同一个空间中进行单独作业；2011年推出的新一代机器人可以与周围环境或周围的人进行互动，通过物体的位置信息，可以自主决定下一个行动，其运动能力也得到大幅增强，最大时速达9 km；在2014年推出新一代ASIMO，其手指得到大大改进，拥有13个自由度，可以精巧、温柔地抓握物体，甚至可以做手语动作。Honda公司不断改进ASIMO，不断书写着仿人机器人的新历史。目前，ASIMO研究人员把主要精力放在增强机器人的人工智能上，让它能够在无人工操控的情形下可以自主行走，对人类的特定行为进行观察，并对可能出现的结果进行预测。此外，ASIMO可以通过影像从人群中识别出某些人。当多个人同时说话时，它也可以识别出多个语音。图1.7中显示的ASIMO高度为130 cm，质量为48 kg，拥有57个自由度（头部3、胳膊7×2、手部13×2、上身2、腿部6×2）。

图1.7　Honda公司的ASIMO

与ASIMO相比，普通人对HRP（Humanoid Robotics Project）系列机器人知道的并不多，但是它在仿人机器人研究人员之间更为出名。日本的AIST（国立产业技术研究所）在P3（ASIMO之前的原型）基础上经过不断改进研发，开发出了HRP系列机器人。HRP项目的目标是研究协助人类和与人类互助的机器人系统。他们从1998年开始不断进行研究，在2002年开发出了HRP2仿人机器人（见图1.8）。HRP2的高度为154 cm，质量为58 kg，拥有30个自由度的人造关节，其后续机型HRP3可以使用4根手指抓握小的物体。在2010年推出的HRP4C（见图1.8），拥有少女的脸庞与形体，行走起来就像人类一样自然。它是一款更接近人类的仿人机器人。

图1.8　HRP2（左侧，能够跟着人抬着木板行走）与HRP4C（右侧，拥有少女形体）

在ASIMO成功的刺激下，韩国KAIST吴俊镐教授（机械工程专业）的研究小组在2004年12月成功开发出了HUBO（由Humanoid与Robot两个词合成）仿人机器人（见图1.9）。HUBO身高为120 cm，体重为55 kg，步幅长为35 cm，行走时速为1.25 km。

HUBO仿人机器人拥有声音识别与合成功能，两只眼睛可以分别移动，具备完美视野，5根手指可以分别动作，能做出剪子、包袱、锤动作。吴俊镐教授研究小组从2002年1月开始开发人形机器人，经过KHR-1、KHR-2几代机器人，最终开发出HUBO，历经3年时间，投入数百亿（韩元）研究费，最后获得成功。这充分证明了韩国的机器人技术达到世界一流水平。与此相比，Honda研发ASIMO耗费了15年时间，据估算投入的研发资金高达数千亿（韩元）。

KAIST在2008年到2009年之间开发出了HUBO2（见图1.9），其设计更加精简，采用铝合金材料，整身重量轻了20%（45 kg），行走时可以像人类一样舒展开腿部，只需很少的能量就能实现自然行走。并且其行走速度得到提升，时速达到1.4 km，也可以以3.6 km时速进行奔跑。

图1.9　KAIST开发的HUBO（左侧）与HUBO2（中间）、DRC-HUBO（右侧）

DRC-HUBO（见图1.9）由HUBO改造而来。它是为了参加2013年12月在美国迈阿密举行的DRC（DARPA Robotics Challenge）预选赛而特意改造的，身高增加到145 cm，两只胳膊长度加长，以便执行灾难救助任务，方便清除灾难现场的障碍物以及在墙上打洞等作业。DRC-HUBO拥有变形（transform）功能，能够以稳定姿态通过满是障碍物与残骸的危险场所。它变形时，两只胳膊向后转，慢慢躺倒，四肢触到地面，然后进行四足行走。

DRC是美国DARPA（Defense Advanced Research Projects Agency，国防部高级研究计划署）主办的世界上规模最大的机器人挑战赛，其目标在于开发可用于大规模灾难现场（如2011年3月发生的福岛核电站核泄漏事故）进行救助的机器人技术。2015年6月，DRC决赛在美国加利福尼亚举行，共有7个国家的25支队伍参赛，韩国有KAIST小组、Robotis小组、首尔大学小组参加，KAIST小组荣获第一名。

最近木浦大学的俞永才（音译）教授团队开发出了世界上最轻的成年人大小的仿人机器人，命名为CHARLES（Cognitive Humanoid Robot with Learning and Evolutionary System），含义为“具备学习与进化系统，拥有认知能力的人形机器人”。从2012年到2014年，木浦大学分别推出了CHARLES-1（2012年）、CHARLES-2（2013年）、CHARLES-3（2014年），身高为110 cm，体重为10 kg以内，全部采用轻量化设计，这样当机器人与同等身高的儿童（平均体重为20 kg）发生碰撞时，不会伤到儿童。

CHARLES（见图1.10）全身共有21个自由度，装配有摄像机、扬声器、话筒，可以与人类进行交互，通过摄像机可以识别足球，并可以进行带球与踢球动作。特别值得一提的是，其电池续航能力达到2小时以上，可以做200米以上的“长距离马拉松”。它在2014年参加FIRA RoboWorld Cup，在HuroCup马拉松比赛中获得第2名，显示出稳定的行走能力。

图1.10　木浦大学的CHARLES

在日本与韩国大获成功的刺激下，一些欧洲国家也开始积极研究机器人。西班牙的Pal Robotics公司先后开发出了REEM B（2008年）与REEM C（2013年）。REEM C（见图1.11）实现了多种功能，可以在复杂环境中自主行走，能够识别出人类，并与人类进行交互，可以进行物体抓握、影像识别、多语言声音识别与合成，行走时速达到1.4 km。

图1.11　Pal Robotics公司的REEM C

特别地，REEM C开发的着眼点放在机器人相关技术的研究上，所以可以实现与ROS（Robot Operating System）百分之百地进行联动，创建动作，并且被成功应用到人工智能与机械学习（machine learning）相关领域的研究中。REEM C的身高为165 cm，体重为80 kg，拥有44个自由度，两只手能够提起10 kg重的物体。

在德国，Institute of Robotics and Mechatronics隶属的German Aerospace Center（DLR）开发出了名叫Justin与TORO（TOrque controlled humanoid RObot）的仿人机器人。Justin有一个四轮驱动的基座，其上安装有人形上体，而TORO可以进行双足行走。DLR以开发强大的遥控机器人（telerobotic）与人机界面（man-machine-interface，MMI）为目标，一直致力于把Justin应用到太空机器人（space robot，可从地球上远程操控）领域的相关研究。

如图1.12所示，Justin运动起来像人类一样灵活，拥有4根精巧的手指，可以精确灵敏地抓握物体。研究人员正在积极研究，以便让它可以判断事物，并依据判断结果提供相应的智能服务。德国IRM在2012年推出了Agile Justin，它的胳膊由碳纤维制成，通过移动胳膊，可以准确抓住飞来的物体，成功率高达80%。这得益于其头部内置的两台高精密摄像机与强大的物体追踪软件。因而Agile Justin的抓握（grasping）能力大大得到增强。Justin高170 cm（把轮子计算在内），重45 kg，可以提起15 kg的重物，总体自由度为42，其中上体自由度为5，各个胳膊自由度为7，每只手自由度为12。

图1.12　德国IRM的Agile Justin（左侧）与TORO（右侧）

TORO仿人机器人在2013年推出。它是以2009年DLR推出的DLR-Biped为基础，向上体添加了新的头部、小型手、轻量级的胳膊，制造出的运动敏捷且可稳定进行双足行走的机器人。与其他仿人机器人相比，TORO的独特之处是它的足部较小，有助于行走时避开障碍物，或攀爬梯子，但是这需要有高扭矩的执行机构与行走控制技术作为支撑。

美国着重于机器人基础技术的研究，所开发的机器人主要应用于太空探索、军事、医疗、再生、服务领域，在仿人机器人的研究方面落后于其他国家。Dennis Hong教授在弗吉尼亚理工大学工作期间，带领RoMeLa团队于2011年正式推出了第一款仿人机器人——CHARLI（Cognitive Humanoid Autonomous Robot with Learning Intelligence）（见图1.13）。设计CHARLI时，研究人员将人类移动胳膊与腿部时肌肉的涨缩原理应用其中，采用线性执行机构取代旋转电动机，并装配了充当筋腱的柔性材料，对冲击有很好的缓冲作用，并且两足行走效果更理想。CHARLI-2身高140 cm，体重12 kg，会踢足球，会模仿人，还能做出其他各种手势。

图1.13　弗吉尼亚理工大学的CHARLI机器人

为了参加DRC挑战赛，韩国的Robotis公司（http://www.robotis.com）制造出了仿人机器人THOR-MANG1（参加预选赛，也叫THOR-OP）与THOR-MANG2（参加决赛）。THOR-MANG2（见图1.14）仿人机器人平台拥有30个自由度，身高150 cm，体重55 kg，全身由模块化执行机构（Dynamixel-Pro）组成，制造时间短，硬件易于维护，不仅可以用于灾难救助，也可以用在研究中。

图1.14　Robotis公司的THOR-MANG2机器人

在参加DRC决赛的队伍中，有4支队伍选择THOR-MANG2作为硬件平台，充分说明它是一款在硬件与软件方面成功度很高的仿人机器人。

有一款仿人机器人借助DRC挑战赛赢得了世界最棒仿人机器人的称号，那就是Boston Dynamics公司制造的ATLAS仿人机器人（见图1.15）。在2013年DRC预选赛中，ATLAS1作为正式仿人机器人平台出现。它只是个大块头，身高为180 cm，体重为150 kg，四肢（28个自由度）由液压执行机构组成，是专为灾难救助现场定制的仿人机器人平台。其头部内置激光测距器（range finder）与立体摄像机，拥有精巧的手部，由电动机驱动，是一款兼具力量与技术的仿人机器人。在DRC预选赛前8支队伍中，有5支队伍选用ATLAS1硬件平台，并最终取得了好成绩。

图1.15　Boston Dynamics 公司的ATLAS1（左侧）和ATLAS2（右侧）

ATLAS2是为DRC决赛而准备的，在ATLAS1的基础上改造而成。其个头更大，身高188 cm，体重157 kg，除了腿部与脚部之外，整体结构改造比例高达75%。特别地，它配备有3.7 kW·h的锂电池组，续航时间超过1h，这使得ATLAS2可以单独执行多种任务，而不必担心电力供给不足的问题。此外，为了提高作业性能，特意降低了肩部与胳膊位置，下部胳膊关节中应用了电动机，可以实施更精确与更高强度的作业，油压泵更安静。在DRC决赛中，ATLAS2被作为硬件平台，提供给7支参赛队伍使用，最终美国的IHMC获得了第2名。

到目前为止，我们所介绍的仿人机器人都是成人级别的，身高超过1 m，所采用的电动机与传感器十分昂贵，制造一台花费超过1亿韩元，主要作为科研机器人使用。除此之外，有一些小型仿人机器人，价格低廉，一般人就可以购买，如Robotis公司在售的ROBOTIS PREMIUM、GP系列。此外，他们还在开发与销售更高级的仿人机器人平台——ROBOTIS OP系列（OP1、OP2）。

DARwIn-OP（ROBOTIS OP1）是ROBOTIS OP2的前一代机器人，是一个稳定且有着高性能的仿人机器人平台，曾经在世界机器人足球比赛（RoboCup）中连续4年摘得桂冠。特别值得一提的是，DARwIn-OP的所有硬件与软件都已经公开在网上，这对于许多大学与研究所研究多种领域的算法非常有用，相关研究领域涵盖影像识别、两足行走、机械理论、仿真模拟等。

ROBOTIS OP2在ROBOTIS OP1基础上改进而成，主控PC的CPU由Intel Atom 1.6GHz单核升级到双核，RAM也由1GB扩展到4GB，其运算速度更快，内存容量更大，这样就可以实现更智能的实时运动控制算法。此外，它还提供支持Windows OS的环境，提高了与ROS、LabVIEW、MATLAB等商业软件的联系，进一步增强了其作为机器人教育设备的可用性。ROBOTIS OP2身高45 cm，体重3 kg，自由度为20。如图1.16所示，OP1全身为黑色，OP2全身涂成漂亮的象牙白色。关于ROBOTIS OP2更多详细内容，将在第4章中介绍。

图1.16　ROBOTIS OP1（右侧）与OP2（左侧）

法国Aldebaran Robotics公司研发的NAO机器人（见图1.17）于2007年首次在RoboCup亮相，标志着他们在小型仿人机器人研发方面获得了成功。

图1.17　Aldebaran Robotics公司的NAO Next Gen

NAO仿人机器人既可以两足行走，也可以自主运动，能够识别人脸与声音，并与人类进行交互。在2011年又推出了NAO Next Gen，身高58 cm，体重4.3 kg，拥有21~24个自由度，最大移动时速达0.54 km。NAO Next Gen采用Intel Atom 1.6 GHz的CPU，默认OS为Linux，可使用C++、Python、Matlab等编程语言为其编写代码。

1.3　韩国及国外机器人产业动向

从2001年到2013年期间，世界机器人产量持续增长，年平均增幅约为7.2%。2012年，专业服务型机器人市场规模达到34.2亿美元，而个人服务型机器人只有12.2亿美元，但相比上年，增长势头强劲，增幅超过90.6%（World Robotics，IFR，2010—2013）。

据预测，至2020年，全世界机器人市场规模将达到229.8亿美元，年平均增长率分别为工业机器人8.2%，专业服务机器人4.8%，个人服务机器人24.3%（IFR 2013）。仿人机器人包括专业服务机器人与个人服务机器人两部分，其增长率约为30%。

鉴于此，世界各国积极地投入大量人力、物力到机器人研究中，大力发展机器人相关技术，谋求占得市场先机。

在工业机器人领域，日本居世界第一（至2013年市场占有率为50.2%），继续加大对服务型机器人（如老人看护机器人、医疗机器人等）的研究投入，致力于制定并推出机器人相关的安全认证的国际标志。日本的NEDO（新能源产业技术综合开发机构）通过“下一代机器人智能化技术开发项目”（2008—2011）与“生活辅助机器人实用化项目”（2009—2013），走在了日本服务型机器人技术的前列。并且，日本在2015年5月成立了旨在促进机器人普及与研发的产学官机构——机器人革命行动委员会，并以此为中心，至2020年的5年期间，通过多方位政策扶持（包括政府制度环境），以及增加机器人研发的民间投资规模，推动1 000亿日元规模的机器人项目。

韩国于2009年5月推出“新成长动力综合促进计划”，总共三大领域17个业种。机器人应用位列其中，重点涵盖生活护理机器人、清洁生产尖端制造机器人系统、可持续的社会稳定机器人系统、创意娱乐教学机器人、高附加值医疗服务机器人。2015年，产业通商资源部一直在推行机器人研发工作，总共涉及6个领域，包括工业机器人、老人护理机器人、手术机器人、灾难救助机器人等。其中，与仿人机器人直接相关的领域有用于老年人护理的“看护机器人系统开发”、“基于老龄化大数据的服务机器人开发”、灾难救助中的DRC机器人，以及其他领域中的“人机共存安全技术”等。

1986年3月，中国启动了国家863重点项目，涉及机器人领域，主要有厨房辅助机器人、体育运动机器人、娱乐机器人等。中国机器人销售量在2013年为3.7万台，2014年为5万台。现在中国已经成长为世界上最大的机器人销售市场，中国从事机器人制造的企业有400多个。并且，中国通过了“十三五”规划，计划用5年时间从制造大国发展为制造强国。为此，中国工业和信息化部称将把未来10年作为中国“机器人工业高速发展期”。

像这样，世界各个国家不断投入大量财力、物力、人力，大力发展机器人相关技术，以抢占机器人技术先机。在整个世界范围内，机器人课程正在逐渐成为正式教学科目。美国与日本等先进技术国家从小学开始就注重加入机器人教育课程，并不遗余力地投入大量资金。中国从2008年开始把机器人课程义务化。目前，韩国在小学中一直把机器人相关课程作为选学课或者课外教学的一部分。韩国教育部从2015年开始把机器人相关内容加入6年制小学常规课程中，使之正式成为教学课程。对于韩国机器人生产企业而言，机器人教育成为正式教学课程后，不仅可以刺激教育机器人的生产、销售，还为机器人产业的发展打下坚实的基础。

在培养仿人机器人专业研发人才方面，我衷心希望本书能够发挥一点作用，贡献一份力量。

第2章　仿人机器人运动学

机器人身体大致由基座（base frame）、连杆（link）、关节（joint）、末端执行器（end effector）几部分组成。从基座是否可以移动来说，机器人可划分为基座固定型机器人（如工业机器人）与基座可移动型机器人（移动机器人）。仿人机器人很容易被划入移动机器人类别中，但是当做某个动作或进行行走时，它的一只脚必须撑在地上，在这一短暂时间内，我们可以把它看作基座固定的机器人平台。连杆是用来连接基座与关节或关节与关节的刚体（rigid body），相当于人类的骨骼。关节用来连接两个或两个以上的连杆，装有执行机构（actuator），可以进行旋转（rotation）或平移运动（translation），使得机器人可以自由移动。末端执行器（end effector）位于机器人最末端的连杆上，用于抓持物体，或者执行组装或搬运等多种作业。

就仿人机器人而言，和人类一样，它所有的关节都是旋转型关节，所有运动都由各个关节电动机的旋转运动组合而成。当关节中的电动机旋转时，角度、角速度、角加速度值都会发生改变，角度值决定机器人的姿态，角速度与角加速度值会对机器人发出的力（force）与转矩（torque）产生影响。通过所有关节的角度值，计算各个连杆坐标系的位置与方向，以及由此确定机器人姿态，这一计算过程称为运动学（kinematics）。借助关节的角速度与角加速度值，计算机器人特定部位或末端执行器所具有的力量与能力，这叫动力学（dynamics）。运动学在计算机器人在三维空间中所做的多种姿态与运动时是必须的。由于从运动学关系式可以导出动力学关系式，本书首先讲解机器人运动学知识。

运动学大致可分为正向运动学（forward kinematics）与逆向运动学（inverse kinematics）两部分。正向运动学是一门系统的数学学问，它通过各个关节电动机的当前角度值来计算机器人的特定位置或者末端执行器的二维或三维坐标。与此相反，逆向运动学提供了一种计算方法，从机器人末端执行器或特定位置坐标计算出相关关节的角度值。相同的坐标值，存在多个关节角的解，这是其一大特征。

在可应用于大部分机器人的一般运动学方法中，Denavit-Hartenberg（DH）方法最为常用。在DH方法中，使用4个参数来系统化定义两个相邻连杆之间的相互关系，用矩阵来描述连杆坐标系的相对旋转与移动（这由关节的旋转与平移运动引起）。只要不违背关节-连杆间的相关规则，就可以把DH方法应用到所有类型的关节中。因此，不论多么复杂的多关节机器人，都可以使用DH方法计算末端执行器坐标系的方向与位置。

DH方法可以应用到拥有多种结构的机器人上，而投影法（projection-based method）更适合仿人机器人的结构特点（目前我正在研究这一方法）。人类骨骼与关节结构和工业机器人（比如SCARA，Selective Compliant Articulated Robot for Assembly）的结构有所不同，人类的踝关节与肩关节拥有两个自由度，股关节与手关节、颈关节有3个自由度。得益于这种多自由度关节，人类可以前后、左右自由抬起腿部，也可以通过旋转股关节改变前进方向。当向前（旁边）抬起一条腿时，从正面（侧面）看，腿的长度看上去变短了，这是由投影（projection）效果引起的。使用投影法时，要把从正面看机器人与从侧面看机器人区分开来，先计算在各个平面中二维坐标系的位置，再组合起来计算三维连杆坐标系的位置。与DH方法相比，投影法中的关系式简单，计算量也少。

在2.1节中，我们将介绍齐次坐标变换，它是运动学的数学基础。在2.2节中讲解DH方法的原理，并介绍将其应用到仿人机器人的正向运动学与逆向运动学中的方法，同时介绍有关MATLAB仿真模拟的内容。在2.3节中将介绍投影法的原理，讲解如何将其应用到仿人机器人的正向运动学与逆向运动学中，并给出使用MATLAB进行模拟所得到的结果。

2.1　齐次坐标变换

齐次坐标变换（homogeneous transformation）就是把两个相邻连杆坐标系之间的变换关系用矩阵表示出来。在学习齐次坐标变换之前，先了解一下有关多关节机器人的关节、连杆的概念，以及向连杆坐标系标注索引（index）的方法。

图2.1　连杆与关节的关系

对于一个拥有n个关节的机器人操作臂，共有n+1个连杆，其中包含基座连杆。各个关节编号从1到n，各个连杆编号为0（基座连杆）到n。依据这一规则，第i个关节（joint i）把第i-1个连杆（link i-1）与第i个连杆（link i）连接在一起，并负责移动第i个连杆，如图2.1所示。

根据执行机构做旋转运动还是往复运动，关节（joint）大致可划分为旋转关节（revolute joint）与移动关节（prismatic joint）两类，如图2.2所示。若把第i个关节的关节变量记为q_i，当关节为旋转关节时，q_i就是转角θ_i；而当关节为移动关节时，q_i是指关节位移d_i；另外，圆柱关节（cylindrical joint）是二自由度关节，可以同时做旋转与移动两种操作。

图2.2　关节种类

为了便于对各个连杆做运动学分析，需要分别为它们设置坐标系。若为第i个连杆设置坐标系x_iy_iz_i（见图2.1），则当第i个关节做旋转或平移运动时，第i个连杆与x_iy_iz_i坐标系也会随之一起运动。第i个连杆是刚体，其上某个点在坐标系中拥有一个确定的坐标。特别地，x₀y₀z₀坐标系固定于基座，它为机器人运动提供惯性反作用力，因而也被称为惯性坐标系（inertial frame），同时为各个连杆的相对坐标系（relative coordinate frame）提供绝对基准，因此也被称为参考坐标系（reference frame）。

要理解齐次坐标变换，需要先具备有关坐标系、点、向量、旋转矩阵的基础知识。坐标系由原点与互相垂直（orthogonal）的坐标轴组成，在二维空间中，坐标轴为x轴、y轴两条，而在三维空间中则需要3条坐标轴（x轴、y轴、z轴）。假设空间中存在一个点，要确定点的位置，必须设置参考坐标系。参考坐标系可以任意选定。所选的参考坐标系不同，同一个点的坐标值也不同。比如，在图2.3中，根据所选的参考坐标系是x₀y₀还是，二维空间中的点p有如下两种坐标表示：

图2.3　两个二维坐标系中点的相对位置与向量

（2.1）

上式中，p⁰是点在x₀y₀坐标系中的二维坐标，p¹是点在x₁y₁坐标系中的坐标。

坐标系中，向量（vector）是一个与点的位置同等重要的概念。向量是具有方向（direction）与大小（magnitude）的物理量，分为自由向量与固定向量，其中自由向量（free vector）只确定于方向与大小，而不在意位置，可以在空间中自由移动。若两个向量方向与大小相同，则不论它们位置如何，都被视作相等的向量。在图2.3中，与皆为自由向量，其向量值是固定不变的，与坐标系无关。然而，根据所选的参考坐标系是x₀y₀还是x₁y₁，它们彼此又变得不同起来。当以x₀y₀坐标系为参考坐标系时，把自由向量称为向量；当以x₁y₁坐标系为参考坐标系时，把自由向量称为向量，则有如下两种不同表示形式：

（2.2）

同样，对于自由向量，当以坐标系为参考坐标系时，称作向量；当以x₁y₁坐标系为参考坐标系时，称为向量，则其在各个参考坐标系中的表示如下：

（2.3）

试想下面一种情形，即把一个坐标系进行旋转，得到一个新的坐标系。对于新坐标系中的一个点，若指定原坐标系为参考坐标系，那么应该如何计算该点的坐标呢？在图2.4中，坐标系是由坐标系沿着逆时针方向旋转θ而得到的。设坐标系中点p的坐标为（α,β），则点p可以表示为如下形式：

图2.4　把二维坐标系旋转θ得到坐标系

（2.4）

上式中，与分别为坐标轴方向与坐标轴方向上的单位向量（大小为1的向量），也就是说，上式是点p的向量表示。

在坐标系中确定向量p的坐标，并把相应向量记为，则根据投影法可以得到如下式子：

（2.5）

上式中，符号“·”表示矩阵内积（dot product），如图2.5所示。两个向量x与y的内积为一个向量在另一个向量上做垂直投影时的大小与另一个向量大小的乘积，计算公式如下：

（2.6）

式（2.5）中，、、、皆为单位向量，大小为1。因此，图2.4中的相互关系可以使用如下关系式表达：

图2.5　两个向量x与y的内积

（2.7）

上式中，乘在点p坐标值之前的矩阵，被称为旋转矩阵R；是坐标系中的点p在坐标系中的表示。在R上加上标0（参考坐标系）与下标1（目标坐标系），上式可做如下简化：

　（2.8）

下面讲解使用旋转矩阵R表示三维空间中坐标系旋转变换的方法。图2.6描述了两个三维坐标系之间的关系，即把三维坐标系沿着轴（中心轴）旋转θ角后得到坐标系。根据投影原理，可以对关系式（2.5）做如下扩展，把坐标系中的向量p转换为坐标系中的向量。

（2.9）

图2.6　把三维坐标系沿着轴（中心轴）旋转θ角后得到坐标系

在图2.6中，x-y轴的旋转与图2.4是一样的，所以在关系式（2.9）的三维旋转矩阵中，只需再计算与z轴之间的向量内积即可。由于x-z、y-z坐标轴彼此垂直，所以在旋转矩阵的第3列与第3行的元素中，除了、项之外，其他皆为0。、向量彼此相等，均为大小为1的单位向量，最终关系式（2.9）计算得到如下结果：

（2.10）

上式也可以绕着z轴沿着逆时针方向（右手定则正方向）旋转θ角而得到。结合关系式（2.8），可得到旋转矩阵定义，如下所示：

（2.11）

同理，以x轴为中心轴沿逆时针方向旋转角时的旋转矩阵、以y轴为中心轴沿逆时针方向旋转角时的旋转矩阵有如下定义：

（2.12）

（2.13）

从上式可以看出，对于，的符号与其他旋转矩阵不同。参考图2.7，代入关系式（2.9）即可求得。

图2.7　把三维坐标系沿着轴（中心轴）旋转角后得到坐标系

3个旋转矩阵、、属于特殊正交群（Special Orthogonal，SO）。某个矩阵R属于特殊正交群必须具备如下4种性质：

• （是矩阵R的转置矩阵）。
• 矩阵R的所有列相互正交（orthogonal），且所有行也相互正交。
• 矩阵R的所有列全为单位向量，且所有行也是单位向量。
• det R=1。

若某个旋转矩阵R可以只用一个变量表示，且属于特殊正交组，则根据性质1有如下关系式成立：

（2.14）

下面以为例证明该关系式。首先求转置矩阵（transpose matrix），如下：

（2.15）

因此，与相等，表示某个坐标系围绕z轴旋转角。把该关系式代入式（2.14），代表一种复杂旋转，即先围绕z轴旋转角（），而后再旋转角（）。这表示某个坐标系转回到原来的位置。从最终结果看，等同于没有发生任何变换，如下使用单位向量表示转换矩阵，由此证明关系式（2.14）成立。

（2.16）

由上式得知，的逆矩阵与相等，如下：

（2.17）

同理，也可以证明旋转矩阵与拥有一样的性质，如下：

（2.18）

迄今为止，我们考虑的只是旋转关系中有两个坐标系的情形。然而，在多关节机器人中，坐标系的个数与关节数一样多。当多个关节同时旋转或平移时，必须使用数学式子把任意连杆或末端执行器的坐标系的位置与方向表示出来。

首先考察一下两个以上的坐标系做复合旋转（composite rotation）的情形。假设在三维空间中，把坐标系旋转（旋转矩阵），得到坐标系，再将其旋转（旋转矩阵），得到坐标系。那么，在坐标系中表示位于坐标系中点p的坐标时，首先使用如下关系式求出从坐标系看点p时的位置。

（2.19）

经过以上变换得到点后，必须再乘一次旋转矩阵，才能得到其在坐标系中的表示形式，如下所示：

（2.20）

把式（2.19）代入式（2.20），得到如下关系式：

（2.21）

若使用表示从坐标系到坐标系的旋转矩阵，则以上关系式可简化成如下形式：

（2.22）

由上述关系式可知，当对某个坐标系进行旋转得到新坐标系时，通过把该旋转矩阵乘到原有旋转矩阵之后，即可计算出总的旋转矩阵。对此，一般化的描述是，依次使用从到的旋转矩阵对坐标系进行旋转，最终得到坐标系，则该坐标系总的旋转矩阵可表示如下：

（2.23）

像这样，对于复合旋转的情形，我们将这种把末端坐标系的旋转矩阵乘到最后面的法则称为“后乘法”（postmultiply）。

迄今为止，我们只学习了坐标系之间的旋转变换。而对于使用线性电动机的关节，还会有平移变换，因此在进行坐标变换时还要把它考虑进去。如图2.8所示，以轴为中心，沿着逆时针方向把坐标系旋转θ角，而后再依据平移变换向量移动坐标系，最终得到坐标系。此时，坐标系中的点p的位置在坐标系中可用如下关系式（该关系式由关系式（2.8）扩展而来）表示：

图2.8　在三维空间中，以轴为中心，把坐标系沿逆时针旋转θ角，再平移，得到坐标系

（2.24）

接下来，考察一个更一般化的情形，即关节的旋转与平移运动引起两个以上的坐标系做出复合性的旋转与移动。首先，用矩阵对坐标系进行旋转，而后再依据向量移动坐标系，得到坐标系，继续用矩阵旋转该坐标系，然后依据向量移动坐标系，得到坐标系。为了在坐标系中把坐标系中的向量p表示出来，首先使用如下关系式将其在坐标系中的向量计算出来。

（2.25）

然后，利用如下关系式，把向量在坐标系中表示出来。

（2.26）

把式子（2.25）代入式（2.26），得到如下关系式：

（2.27）

通过比较上式与式（2.21）可知，平移运动使得关系式右边新增了、这两项。利用向量可做进一步简化，如下：

（2.28）

有些机器人有n个关节，每个关节在做旋转或平移运动时，若把末端坐标系中的点表示为式（2.28）的形式，关系式就会变得很长，并且很复杂。然而，如果像前面那样，把平移变换向量d添加到三维旋转矩阵R，对矩阵进行扩展，再利用扩展后的矩阵就可以表示得更加简单，如下所示：

（2.29）

上述矩阵称为“齐次变换矩阵”（homogeneous transformation matrix），R为三维方阵（square matrix），d是3×1向量，0是1×3零向量（元素全为0的向量），所以H是四维方阵。式子（2.28）中的与可以用各齐次变换矩阵的积来表示。

（2.30）

若想利用齐次变换矩阵表示坐标系中某个点的位置，三维向量p=（）必须扩展为如下4×1的向量P。

（2.31）

接下来，利用齐次变换矩阵H与扩展向量P，式（2.27）可以简化如下：

（2.32）

并且，与旋转矩阵一样，在复合齐次变换中连续发生n次齐次变换矩阵时，应用式子（2.23）所示的“后乘法”，得到如下关系式：

（2.33）

下面的矩阵分别是关节跟随各个轴做平移运动时的齐次变换矩阵（Trans）以及以各个轴为中心做旋转运动时的齐次变换矩阵（Rot）。比如，表示某个坐标系跟随x轴移动a，表示某个坐标系以y轴为中心旋转β角度。

（2.34）

在逆向运动学中，有时需要用到齐次变换矩阵的逆矩阵。但是一般来说，计算4×4矩阵的逆矩阵并非易事。然而，如果使用如下逆矩阵公式（利用了子矩阵），计算就会变得容易得多。

（2.35）

上式中，A、B、C、D分别是组成原来矩阵的子矩阵。

首先，计算（沿着x轴平移a时的齐次变换矩阵）的逆矩阵，为此需要把它分解为A、B、C、D子矩阵，如下所示：

（2.36）

把这些子矩阵代入式（2.35）中，经整理，得到的逆矩阵，如下所示：

（2.37）

观察上式可以发现，在的逆矩阵中，除了第1行第4列的元素由a变为-a之外，其他元素与原矩阵完全一样。这与前面介绍的旋转变换坐标系的情形类似，即先做平移变换，把某个坐标系沿x轴移动a，而后再沿x轴移动-a，就变回了原来的坐标系。只要想想这个原理，理解起来就容易了。同理，可以求得与的逆矩阵，如下：

（2.38）

对于旋转齐次变换矩阵的逆矩阵，把式（2.11）~式（2.13）代入式（2.35）的A矩阵，并代入B=[1 0 0]^T、C=[0 0 0]、D=[1]，经整理，得到如下逆矩阵：

（2.39）

这些逆矩阵关系式很有用，在下一节中会用到。

2.2　利用DH方法为仿人机器人建模

2.2.1　DH（Denavit-Hartenberg）方法

为了描述相邻坐标系的旋转与移动关系，Denavit与Hartenberg在1955年提出了DH方法。该方法使用带有4个参数的4×4齐次变换矩阵的积，依据前一个连杆坐标系系统构建附着在机器人连杆或躯体上的坐标系。只要机器人的连杆彼此相连，大部分时候都可以应用DH方法，以基座坐标系（）为基准，即可依次求出任意连杆坐标系（）或末端执行器坐标系（）的原点与方向。

为了帮助读者理解DH方法，作如下假设：如图2.1所示，第i个关节把第i1个连杆与第i个连杆连接起来，并且两个连杆上分别固定着坐标系与。如前所述，第i个关节的执行机构做旋转与移动时，后面连接的第i个连杆与坐标系也会随之发生移动。

图2.9　利用DH方法描述相邻坐标系间的运动学关系

在图2.9中，使用DH方法规定的方式定义了两个坐标系间的相对变换关系，4个DH参数、、、分别表示关节转角（joint angle）、连杆扭角（link twist）、连杆距离（link offset）、连杆长度（link length）。

DH方法使用4个参数表示4×4齐次变换矩阵，因此两个相邻坐标系必须满足如下两个约定，才可应用DH方法。

图2.10　由右手法则确定的正角度方向

约定1：轴与轴垂直。

约定2：轴与轴相交。

接下来，逐步讲解利用DH方法以坐标系为基准建立坐标系的原理。

1）为了设置连杆坐标系，先把z轴指定为基准轴。在旋转关节中，把旋转中心轴设为z轴；在平移关节中，把往复方向轴设为z轴。当z轴为旋转轴时，旋转角θ要遵守右手法则，如图2.10所示，当z轴（大拇指所指的方向）朝上时，沿着逆时针（其余4个手指所指方向）方向，角度增加的方向为正方向（+）。

2）把轴作为旋转轴，通过将轴旋转角度，建立坐标系的轴。

3）沿着轴把坐标系移动。这样一来，在轴与轴之间就产生了距离（offset）。

4）沿着新建立的轴，把坐标系移动。在仿人机器人中，由于不存在平移关节，因此就是连杆长度。

5）把轴作为旋转轴，将轴旋转，建立轴。根据右手法则，图中轴是垂直于地面向上的。

上面各个过程可以使用关系式（2.29）中的齐次变换矩阵作为基本单位进行表示，把前一个坐标系依次做旋转与平移变换即可得到新坐标系。因此应用前面介绍的后乘法，得到如下DH变换矩阵B的关系式：

（2.40）

上式中，大写字母S与C分别代表sin函数与cos函数，上下标分别表示DH参数与索引编号（index），比如表示cos，表示sin。下面的矩阵表示的3×3子矩阵（submatrix），用来以坐标系为参考系计算坐标系的方向。

图2.11　两连杆平面机械臂与局部坐标系

（2.41）

并且，中列向量表示坐标系原点的相对位移。

下面举个利用DH方法求各个连杆坐标系方向与位移的例子。如图2.11所示，假如有一个拥有两个连杆的平面机械臂，该机器人只能在由x轴与y轴组成的二维平面中运动，z轴垂直于地面向上。参考坐标系（基准坐标系）为，应用DH方法，建立坐标系。轴由轴绕着轴旋转得到，故。又由于在轴与轴之间沿轴方向没有位移，因而。并且，在轴与轴之间，沿着轴方向的位移为，故。最后以轴为中心，在轴与轴之间无旋转，故有关系式成立。

第二个连杆的末端执行器坐标系与坐标系之间的4个DH参数也可以采用相同方式求得，所有参数如表2.1所示。

表2.1　两连杆平面机械臂与DH参数

利用2-连杆平面机械臂的DH参数与变换矩阵，如何把连杆坐标系中的一点在参考坐标系中表示出来？首先，设点p¹在坐标系中的坐标为（）。从坐标系看，其坐标为（）。把表2.1中的参数代入式（2.40）中的DH变换矩阵关系式，得到如下计算结果：

（2.42）

如果想用坐标系中的坐标表示坐标系中的点（），只要向关系式（2.42）中的矩阵应用后乘法即可，如下所示：

（2.43）

观察关系式（2.42）与式（2.43），可以发现每当新添加一个关节时，就要在齐次变换矩阵末尾乘上新的矩阵。因此，当有两个以上关节时，引入如下复合变换矩阵（该矩阵用来刻画从第i个关节到第j个关节的复合坐标系变换），这样表示起来会更简洁。

（2.44）

此时，若新增第j+1个关节与连杆，利用复合变换矩阵关系式，得到如下后乘形表示形式：

（2.45）

下面介绍利用关系式（2.42）与式（2.43）计算坐标系与坐标系的原点在坐标系中的坐标。在2.2.2小节中，该计算方法将用于求仿人机器人的关节坐标。首先，计算坐标系的坐标原点在参考坐标系中的坐标，把（）=（0,0,0）代入关系式（2.42）中，得到（）=（）。接着求坐标系的坐标原点在参考坐标系中的坐标，把（）= （0,0,0）代入关系式（2.43）中，得到（）=（）。

接下来，应用前面内容，使用MATLAB模拟两连杆平面机械臂的简单动作。设、代表连杆长度，分别为0.1 m、0.2 m，为从20°到50°每秒旋转1°，为从0°到30°每秒旋转1°。MATLAB代码包含在twp_link_planar_robot.m文件中，该文件位于http://office.ucloud.com的My ucloud之下的code/matlab code目录中，图2.12显示的是代码在计算机中的运行画面。代码2.1中，p1_0与p2_0分别是坐标系与坐标系的原点在坐标系中的坐标。DHMat函数是MATLAB所提供的众多函数之一，它实现了关系式（2.40），如代码2.2所示。关于MATLAB语言的详细讲解，请阅读参考文献[3]。

图2.12　双连杆平面机械臂的动作仿真

代码2.1　双连杆平面机械臂动作仿真

　d1 = 0; d2 = 0;
　l1 = 0.1; l2 = 0.2;
　alpha1 = 0; alpha2 = 0;
　theta1 = deg2rad（[20:1:50]）; theta2 = deg2rad（[0:1:30]）;
　st = 1;　　　　　　% 时间

　P0 = [0 0 0 1]'; % x0y0z0坐标系原点
　P1 = [0 0 0 1]'; % x1y1z1坐标系原点
　P2 = [0 0 0 1]'; % x2y2z2坐标系原点

　for i=1:length（theta1）
　　t（i） = st*（i-1）;
　　% x0y0z0 坐标系看x1y1z1坐标原点 p1的坐标
　　P1_0 = DHMat（theta1（i）, d1, l1, alpha1）*P1;
　　% x0y0z0 坐标系看x2y2z2坐标原点 p1的坐标
　　P2_0 = DHMat（theta1（i）, d1, l1, alpha1）*DHMat（theta2（i）, d2, l2, alpha2）*P2;

　　figure（1）
　　plot（[P0（1） P1_0（1） P2_0（1）], [P0（2） P1_0（2） P2_0（2）], '-bo'）;
　　xlabel（'x （m）'）;
　　ylabel（'y （m）'）;
　　hold on;
　end

代码2.2　DH变换矩阵

function y = DHMat（delta, d, a, alpha）

y=[cos（delta）,-sin（delta）*cos（alpha）,sin（delta）*sin（alpha）,a*cos（delta）;
　sin（delta）, cos（delta）*cos（alpha）, -cos（delta）*sin（alpha）, a*sin（delta）;
　0,　　　　sin（alpha）,　　　　　 cos（alpha）,　　　　　　 d;
　0,　　　　0,　　　　　　　　　　　　0,　　　　　　　　　　　　 1];

接下来，讲解使用DH参数法解决简单逆向运动学问题的过程。在求解逆向运动学问题中，最基本的方法是使用逆矩阵。观察关系式（2.40），可以发现DH矩阵是4个矩阵顺次相乘的形式。计算DH矩阵的逆矩阵之前，请先记住如下ABCD矩阵的逆矩阵关系式。

（2.46）

因此，应用关系式（2.37）~式（2.39），可以把DH矩阵的逆矩阵表示成如下关系式：

（2.47）

对于前面提到的双连杆平面机械臂问题做变形，当为特定角度时，采用某个方法计算角度，使坐标系的原点最大限度地靠近指定位置，让我们想一想如何计算。在此情形之下，由于是未知数，为了方便计算，在两边乘上关系式（2.43）右侧第一个矩阵的逆矩阵，如下所示：

（2.48）

上式中，左侧逆矩阵相当于把关系式（2.47）的DH逆变换矩阵中的4个参数设置为=、=0、=、=0，因此有如下关系式成立：

（2.49）

把上式代入关系式（2.48）中，得到如下形式：

（2.50）

比较上式左右两侧，有如下关系式成立：

（2.51）

由于我们的目标是让坐标系的原点（=0，=0）位于参考坐标系的指定坐标（和）上，可以把关系式（2.51）简化如下：

（2.52）

接着，需要计算满足该关系式的。由于上式中只包含与的式子，可以使用 =这一关系式求解，如下所示：

（2.53）

在代码2.3中，当在20°~50°之间每秒旋转1°时，可以使用关系式（2.53）计算，它可以使坐标系的原点最大限度地靠近目标点（0.22,0.2,0）。通过计算坐标系的原点时，会出现多大距离误差，由该段代码进行确认查看。MATLAB源代码包含于ik_two_link_planar_robot.m文件中，你可以从http://office.ucloud.com的My ucloud下的guest文件夹下载它。图2.13~图2.15是代码在计算机中的运行画面。

代码2.3　双连杆平面机械臂的逆向运动学仿真

　　d1 = 0; d2 = 0; l1 = 0.1; l2 = 0.2; alpha1 = 0; alpha2 = 0;
　　theta1 = deg2rad（[20:1:50]）;
　　st = 1;　　　　　　　　　　　　  % 时间
　　des_P2_0 = [0.22 0.2 0 1]’;　　　　　　 % 目标P2_0的位置

　　P0 = [0 0 0 1]';　　　　　　　 % x0y0z0坐标系原点
　　P1 = [0 0 0 1]';　　　　　　　 % x1y1z1坐标系原点
　　P2 = [0 0 0 1]';　　　　　　　 % x2y2z2坐标系原点

　　figure（1）
　　plot（des_P2_0（1）, des_P2_0（2）, '-rx'）;
　　hold on

　　for i=1:length（theta1）
　　　　t（i）=st*（i-1）;
　　　　P1_0 = DHMat（theta1（i）, d1, l1, alpha1）*P1;
　　　　% 计算使P2_0位于目标点的theta_2
　　　　theta2（i）=atan（（-sin（theta1（i））*des_P2_0（1）+cos（theta1（i））*des_P2_0（2））/
　　　　　　　　（cos（theta1（i））*des_P2_0（1）+sin（theta1（i））*des_P2_0（2）-l1））;
　　　P2_0 = DHMat（theta1（i）, d1, l1, alpha1）*DHMat（theta2（i）, d2, l2, alpha2）*P2;
　　　err_dist（i） = norm（des_P2_0-P2_0）; % 计算与目标点之间的误差

　　　figure（1）
　　　plot（[P0（1） P1_0（1） P2_0（1）], [P0（2） P1_0（2） P2_0（2）], '-ko'）;
　　　xlabel（'x （m）'）;
　　　ylabel（'y （m）'）;
　　　axis（'equal'）;
　　　hold on
　　end

　　figure（2）
　　subplot（2,1,1）
　　plot（t, theta1, 'k'）
　　ylabel（'theta_1'）;
　　subplot（2,1,2）
　　plot（t, theta2, 'k'）
　　ylabel（'theta_2'）;
　　xlabel（'time （sec）'）;

　　figure（3）
　　plot（t, err_dist, 'k'）
　　axis（[0, 30, 0, 0.02]）;
　　ylabel（'distance error （m）'）;
　　xlabel（'time （sec）'）;

图2.13显示的是逐渐减小的情形，此时坐标系原点不断接近目标点。与图2.12相比，可以看到它们之间的区别很明显。图2.14显示的是的轨迹。当在20°到50°之间做线性变化时，由关系式（2.53）计算得到。与图2.14类似，图2.15显示的是当与变化时，坐标系的目标原点与实际原点之间距离误差的变化情况。由于两个连杆与是刚体，因此在大约11秒（=31°，=17°）时，坐标系原点几乎与目标点重合。除此之外的其他情形，距离误差会增加。换言之，对于给定连杆长度的双连杆平面机械臂，使其末端执行机构准确位于（0.22,0.2,0）的两个角度值分别为=31°、=17°，它们也是该逆向运动学问题的解。

图2.13　双连杆平面机械臂的逆向运动学仿真

图2.14　与角度变化轨迹

图2.15　坐标系目标原点与实际原点之间的距离变化

2.2.2　在仿人机器人中应用DH参数法

本节讲解利用前面学过的DH参数法表示仿人机器人动作的方法。仿人机器人的全身结构与连杆-关节模仿了人体，因此有必要先了解一下人体的关节、骨骼、肌肉构造与功能。图2.16是人体的二维断面，分别为正面、侧面与顶部示意图。

图2.16　人体的3个平面

从正面观看人体时，看到的平面叫冠状面（coronal plane或frontal plane）；从侧面观看时，看到的平面叫矢状面（sagittal plane）；从顶部看到的平面叫横切面（transverse plane）。当人做前后运动时，要移动矢状面上的关节；当向旁侧运动时，要旋转冠状面上的关节；当从站立状态或步行中变换前方方向时，需要旋转横切面上的关节。

许多机器人相关的著作与论文中都遵守Euler的三维角定义，把在矢状面旋转的关节叫pitch关节，把在冠状面旋转的关节叫roll关节，把在横切面旋转的关节叫yaw关节。Robotis OP的代码也遵循这种规则。pitch关节对应于矢状面关节，roll关节对应于冠状面关节，yaw关节对应于横切面关节。这两组术语含义是一样的。至于选用哪一组术语，由你决定。本书第2章到第5章采用按平面划分的关节角定义，在第6章介绍OP源代码时将混用Euler的关节角定义。

在人体矢状面做旋转的关节有上体的颈关节（neck joint）、肩关节（shoulder joint）、肘关节（elbow joint）、腕关节（wrist joint），以及下体的髋关节（hip joint）、膝关节（knee joint）、踝关节（ankle joint）。在冠状面做旋转的关节有上体的颈关节、肩关节、腕关节，以及下体的髋关节与踝关节。在横切面做旋转的关节有上体的颈关节、肩关节、腕关节，以及下体的髋关节。由此可知，人体最复杂、最巧妙的关节是拥有三自由度（degree of freedom）的颈关节、肩关节、腕关节以及髋关节，然后是拥有两个自由度的踝关节。以上内容整理如表2.2所示。

本书选用ROBOTIS-OP（下称OP）系列机器人（OP1、OP2）作为仿人机器人平台。它们不带有用作腕关节的执行机构，因此在后面做运动学建模时都把腕关节排除在外。

表2.2　各平面中的旋转关节一览表

在图2.17中，OP类型的仿人机器人用左脚站立时，用连杆与关节表现下体的样子，各个连杆坐标系都遵守DH方法的参数约定方式。当机器人用左脚站立时，坐标系位于左侧踝关节上；当用右脚站立时，坐标系位于右侧踝关节。参考坐标系的这种变化会对所有下体连杆坐标系的设置产生影响，特别是在两侧的大腿部，DH参数的角度定义会不同。

图2.17　左脚支撑时仿人机器人下体DH法建模

图中，XYZ坐标系位于支撑脚的中心点上，它代表世界坐标系（world coordinate frame）或绝对坐标系（absolute coordinate frame）。本书采用学术界广泛使用的方式，把机器人前进方向设为X轴，从右向左的侧面方向设为Y轴，垂直方向设为Z轴。在机器人前进过程中改变方向后，支撑脚的坐标系与XYZ坐标系相互错开，形成角度γ。在第5章与第6章中，将会利用这个角度实现静止状态或步行中改变方向。

从图2.17中可以知道，仿人机器人的下体由两个脚掌面、9个连杆（），以及12个关节（、、）组成。由于人体呈现对称性，连杆是成对出现的，比如跟骨（calcaneus）与、胫骨（tibia）与、股骨（femur）与，以及执行机构支架与。另外，本书中人体骨骼与肌肉等名称均遵循参考文献[4]中的标记法。

对于关节变量的名称，根据图2.16中显示的人体的3种断面，矢状面关节变量名中使用，冠状面与横切面的关节变量名中分别使用φ与ψ。像这样，之所以把关节变量的名称设得不同，是因为负责仿人机器人运动生成的关节电动机的旋转角轨迹在每个断面上的模式都不同，在生成各种运动时操作起来更直观。

也就是说，在一个步行周期内，矢状面上关节角的初始角与终止角不同，并产生多种轨迹，而冠状面上关节角的轨迹形状从0°开始最终又回到0°。相反，对于横切面上的关节，只要不改变步行方向，总是保持0°；当改变方向时，只有初始支撑腿的横切面髋关节旋转到目标角度，然后再回到0°，返回原来位置。有关详细内容，将在第5章中进行讲解。

关于关节角正方向的定义，不同关节变量略有不同。对于冠状面与横切面的关节角，把围绕中心旋转轴沿逆时针旋转的方向指定为正方向；而对于矢状面上的关节角，把围绕中心旋转轴沿顺时针方向旋转的方向定义为正方向。原因是，对于由绝对坐标系X轴与Z轴组成的矢状面，从图上来说就是从机器人右侧往左侧看的方向。如图2.11所示，在该面上旋转的角的正方向就是图2.17中的顺时针方向。换言之，矢状面的关节角把绝对坐标系（非相对坐标系）的-Y轴作为右手法则的旋转轴来确定正方向，这是为了与投影法中的矢状面的角度定义保持一致。

同样，当关节电动机旋转时，对于参考轴（0°）的定义也随着断面的不同而不同。对于矢状面关节角，它把与地面平行的X轴作为参考轴（基准轴），如图2.11所示；对于冠状面关节角，选取与地面垂直的Z轴为参考轴。这样定义之后，在直立姿势下，冠状面上的所有关节角都是0°，行走时也不会与直立姿势相差太大，角度值在0°附近变化。对于横切面上的关节角，把机器人在世界坐标系中朝向正面的姿势定义为0°。

对于下体的连杆坐标系中的任意一个点，在参考坐标系中的坐标应该如何计算呢？首先，对于支撑腿（support leg）脚踝坐标系中的一点，从坐标系看时的扩展向量表示如下（根据上一节的内容）：

（2.54）

在上式的向量中，上标0表示参考坐标系是第0个坐标系，下标1表示目标坐标系是第一个坐标系（）。是DH变换矩阵。从图2.17可知，4个DH参数的关系为：=+，=0，。表2.3中把左脚支撑时下体所有连杆坐标系的DH变换矩阵整理出来，一目了然。表格中，矢状面关节角的符号都是负的。如前所述，这是为了与投影法中的矢状面关节角保持一致。把沿右手法则反方向所做的旋转定义为矢状面关节角的正方向。

表2.3　左脚支撑时仿人机器人下肢连杆坐标系的DH变换矩阵与追加变换矩阵

坐标系位于支撑腿的胫骨连杆上，对于其中一点，应用关系式（2.44）中定义的复合变换矩阵，有如下关系式：

（2.55）

坐标系位于支撑腿的股骨连杆上，是其中的一点；坐标系位于髋关节部位上，是其中一点。对于与在参考坐标系中的位置，依次应用复合变换矩阵表示如下：

　 （2.56）

　　 （2.57）

坐标系位于支撑腿的骨盆，如图2.18所示，骨盆关节结构比较特殊，使得DH坐标系间的第二个规则（轴必须与轴相交）不成立，所以无法直接应用DH参数法。为了解决这一问题，针对坐标系，设置满足DH法应用约定的坐标系，如图2.18所示，然后做一次变换，得到坐标系。该过程用数学关系式表示如下：

图2.18　仿人机器人下肢DH局部坐标模型

（2.58）

上式中，乘在DH矩阵之后的矩阵是追加变换矩阵，根据关系式（2.9），很容易理解。

摆动腿（swing leg）可以从地面上抬起，坐标系负责摆动腿骨盆部分横切面的旋转，应用表2.3的DH矩阵，表示如下：

（2.59）

在摆动腿的骨盆部位，坐标系负责冠状面旋转，由坐标系做DH变换（矩阵）后，再沿着新的轴把坐标系原点移动，因此必须再乘上移动变换矩阵，如下所示：

（2.60）

摆动腿的其他连杆坐标系（从坐标系到坐标系）也遵循一定的规则，第k个坐标系中的点的绝对坐标表示如下：

（2.61）

到目前为止，我们得到连杆坐标系中的点（）在参考坐标系中的坐标关系式，接着将其变换为绝对坐标系（见图2.17）XYZ坐标系中的坐标值（），为此要再乘上坐标系变换矩阵，如下所示：

（2.62）

观察上述关系式，可以发现在变换矩阵的元素中出现了。这是因为坐标系的原点沿着XYZ坐标系的Z轴方向移动了。

上面介绍了使用DH方法对仿人机器人左脚撑地右腿抬起的姿势进行表示的方法。接下来，使用DH方法对机器人右脚撑地左脚抬起的下肢连杆坐标系进行表示（见图2.19），并了解一下与前面的不同。机器人用右脚支撑时，充当所有连杆坐标系的参考坐标系的坐标系位于右侧踝关节上，如图2.19所示。并且以它为基准，使用DH变换表示坐标系时，所应用的原理与用左脚支撑时是一样的。对于这些坐标系的3种关节角（）的定义方法与左脚支撑时是一样的，因此DH变换矩阵参数也是相同的。然而，由于坐标系位于与左脚支撑时相反的方向上，于是坐标系的变换关系也不同，需要修改DH参数，如表2.4所示。

图2.19　右脚支撑时仿人机器人下肢的DH局部坐标系模型

表2.4　右脚支撑时仿人机器人下肢连杆坐标系的DH参数修改一览

与表2.3相比，的由变为，这是因为轴围绕轴旋转之后，还要再旋转180°，才能变为左侧髋关节的x轴。的由变为，在追加变换矩阵中变为，这是因为坐标系的方向变了，但是坐标系设置的方向还与用左脚支撑时是一样的。此外，对于的与轴之间的旋转角，在左脚支撑时，沿着正方向加，而在右脚支撑时沿着负方向加，为了进行修正，使用代替表2.3中的。对于坐标系，与用左脚支撑时设置的一样，因此DH变换矩阵未发生变化。

到此为止，仿人机器人下肢连杆坐标系计算式都有了，接下来要对组成上体的各个连杆坐标系应用DH方法，如图2.20所示。机器人的上体固定在下体骨盆坐标系上，因此头部、躯干、胳膊部位的坐标系都以它为基准进行计算。此时需要注意的是，图2.20显示的是左脚支撑时上体的坐标系（见图2.17），在右脚支撑时，只要把图中坐标系与坐标系的位置彼此交换即可，上体所有连杆坐标系的索引编号都是一样的。

图2.20　左脚支撑时仿人机器人上体的DH方法模型

首先，了解一下左胳膊参考坐标系坐标系与坐标系之间的变换关系，在求图2.20中位于左胳膊上的关节坐标系时会用到。坐标系可以这样得到：把坐标系的原点沿着轴方向移动，再沿着轴方向移动，然后沿着轴方向移动，再如图2.21所示旋转坐标系。把这一过程使用齐次变换矩阵表示如下：

图2.21　左脚支撑时坐标系与坐标系方向比较

（2.63）

对于坐标系中的一点，利用上述关系式，可以计算它在参考坐标系中的扩展坐标，如下所示：

（2.64）

同理，位于右肩关节的坐标系可以这样得到：把坐标系原点沿轴方向移动，再沿轴方向移动，再沿着轴移动，然后采用图2.21中的方式旋转坐标系。因此，对于坐标系中的一点，它在参考坐标系中的扩展坐标可以使用如下矩阵式子进行计算：

（2.65）

接下来，通过图2.22了解一下肩部两个连杆坐标系的变换关系。首先，观察左肩的坐标系与坐标系的关系。对于坐标系，以轴为中心，把坐标系旋转，建立轴，沿着轴与轴移动坐标系原点，移动距离分别为2.4 cm与1.6 cm。请注意该数值是对OP测量得出的。最后，以轴为中心，把轴旋转90°，得到轴。使用全部4个DH参数，计算参考坐标系中的坐标，如下：

（2.66）

右肩的坐标系与坐标系的关系如图2.23所示。对于坐标系，以轴为中心，把坐标系旋转，建立轴，沿着轴与轴移动坐标系原点，移动距离分别为2.4 cm与1.6 cm。最后，以轴为中心，把轴旋转90°，得到轴，在参考坐标系中的位置可使用如下式子表示：

（2.67）

图2.22　左肩关节的两个坐标系

图2.23　右肩关节的两个坐标系

位于左上臂骨（肱骨，humerus）与下臂骨（桡骨，radius）连杆上的坐标系与坐标系，位于右上臂骨与下臂骨的坐标系与坐标系，利用表2.3的DH变换矩阵，可以使用与关系式（2.61）一样的普通矩阵关系式进行表示。

在图2.20中，为了求得头部与颈部关节的坐标系，先了解一下坐标系与坐标系之间的变换关系。对于坐标系，把坐标系的原点沿着轴方向移动，再沿着轴方向移动，然后做图2.24所示的坐标旋转，即可建立出来。这一过程使用齐次变换矩阵表示如下：

（2.68）

利用上式，对于坐标系中的一点，其在参考坐标系中的扩展坐标表示如下：

（2.69）

接下来，参照图2.25，应用DH参数法，描述坐标系（位于颈部底部）与坐标系（位于头部旋转轴上）的关系。坐标系可以这样得到：以轴为中心，把坐标系旋转，建立轴，沿着轴移动（颈长），然后以轴为中心，把轴旋转90°。

图2.24　左脚支撑时坐标系与 坐标系的方向比较

图2.25　位于颈部与头部的两个坐标系

最后，应用DH方法表示坐标系与坐标系（位于头顶）之间的关系，以轴为中心，把轴旋转90°，即得到轴。然后沿着轴将轴移动（头长），得到轴。

把左脚支撑时上体的所有DH参数整理在表2.5中。

表2.5　左脚支撑时仿人机器人上体连杆坐标系的DH变换矩阵

图2.20显示的是左脚支撑时上体的连杆坐标系，图2.26显示的是右脚支撑时上体模型中髋关节部位坐标系的变化。相应地，坐标系可以这样得到：沿着轴的方向，把坐标系的原点移动，再沿着轴方向移动，然后沿着轴方向移动，再如图2.27所示旋转坐标系。这一过程可以用齐次变换矩阵表示如下：

图2.26　右脚支撑时仿人机器人上体的DH方法模型

图2.27　右脚支撑时坐标系与坐标系的方向比较

（2.70）

坐标系位于右肩关节上，可以通过如下变换得到：沿着轴方向，把坐标系的原点移动-，再沿着轴方向移动，然后沿着轴方向移动-，再采用图2.27所示的方式旋转坐标系。这一过程可以用以坐标系为参考基准的的齐次变换矩阵表示如下：

（2.71）

坐标系位于颈部，可以通过如下变换得到：沿着轴方向，把坐标系的原点移动-，再沿着轴方向移动，然后如图2.28所示旋转坐标系。这一过程可以用齐次变换矩阵表示如下：

图2.28　右脚支撑时坐标系与坐标系的方向比较

（2.72）

此外，其他上体连杆坐标系的DH变换矩阵如表2.5所示。

利用上面这些坐标变换关系式，并给定全身的关节角时，OP就能做出相应动作，下面先用MATLAB做一下模拟。代码2.4包含在sim_pose.m文件中。你可以从http://office.ucloud.com的My ucloud/guest文件夹的code/matlab code目录下找到该文件，把与动作姿势相关的no_pose设为1，即可模拟仿人机器人站立动作。代码中需要注意的是，即使同一个站立姿势，也要注意区分左脚支撑与右脚支撑两种情况。左脚支撑时，是，如图2.17所示；而右脚支撑时，是-，如图2.19所示。这看上去多少有点混乱。第5章中将介绍关节电动机相关概念，那时你自然会理解，现在不必太担心。

代码2.4　sim_pose.m站立姿势模拟代码

DH = 1; PB = 2;

md = DH;　　　　　　　　　　　　　% 运动学方法：DH：Denavit-Hartenberg，PB：投影法
sf = 'LEFT_SUPP';　　　　　　　 % 左脚支撑
%sf = 'RIGHT_SUPP';　　　　　　 % 右脚支撑
sim_type = 'pose_sim';　　　　 % 姿势模拟
sp = [0, 0, 0];　　　　　　　　　% 支撑脚的中心位置

spec = SpecROBOTIS_OP2（md）;　% 导入OP2的规格明细
link = spec（1:21）;　　　　　　 % 连杆长度
mass = spec（22:44）;　　　　　　% 连杆中心点质量

view_ang = [45, 20];　　　　　　% 机器人模拟观察者角度
gamma = 0;　　　　　　　　　　　　% 行走方向转角
no_pose = 1;　　　　　　　　　　　% 姿势编号

flags = zeros（1,7）;　　　　　　% 运动生成时显示图的标记
fig_no = 1;　　　　　　　　　　　 % 模拟窗编号
no_pose = 1;　　　　　　　　　　　% 姿势编号

% 站立姿势
th = [pi/2 0 0 pi 0 0 pi 0 pi 0 0]; % 矢状面关节角
phi = [0 0 pi 0 pi pi];　　　　% 冠状面关节角
psi = [0 0 0];　　　　　　　　　 % 横切面关节角

% 静止姿势时所有关节角的微分值设为0
th_1d = zeros（1,11）;
th_2d = zeros（1,11）;
phi_1d = zeros（1,6）;
phi_2d = zeros（1,6）;
psi_1d = zeros（1,3）;
psi_2d = zeros（1,3）;

if md==DH
　 res = GenMotionsWithDH（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　 psi_1d, psi_2d, fig_no）;
else
　 res = GenMotionsWithPB（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　 fig_no）;
end

上面代码中，md是建模方法，可以是DH方法，也可以是投影法；sf表示支撑腿，可以是左腿，也可以是右腿，它是字符串类型的，可取'LEFT_SUPP'（左腿支撑）与'RIGHT_SUPP'（右腿支撑）两种值。sp表示支撑脚中心点的位置（support position），设为参考坐标系的原点[0,0,0]。sim_type用于确定模拟目标是在原位做静止姿势（'pose_sim'），还是往前行走（'walk_sim'），还是像在跑步机上一样原地踏步走（'tread_sim'），还是执行优化（'opt_pso'），决定模拟画面的X轴范围。

spec是与仿人机器人的各个连杆长度与质量有关的详细数据，代码中从SpecROBOTIS_OP2.m文件读取数据。该文件包含OP2各个连杆长度与质量等信息。对于不同类别的仿人机器人，该详细文件中的连杆数据也要做相应修改。link与mass表示所读取的机器人连杆的长度与质量。flags是与模拟结果显示窗口和运动特性相关的标记，静止姿势时全为0。关于flags更详细的内容，将在第5章中进行讲解。

th、th_1d、th_2d分别表示矢状面关节角向量、1次微分向量与2次微分向量，phi、phi_1d、phi_2d分别表示冠状面关节角向量、1次微分向量与2次微分向量，psi、psi_1d、psi_2d分别表示横切面关节角向量、1次微分向量与2次微分向量。在静止姿势下，所有微分值都被设为0。

gamma表示绝对坐标系与参考坐标系之间的旋转角，如图2.17与图2.19所示，在前进与静止姿势下为0°。fig_no是MATLAB中用于显示模拟的窗口编号，当存在多个窗口时，只要把值设置为大于1且不重复数字即可。当不想显示窗口时，比如使用PSO优化仿人机器人姿势时，只要把fig_no 设为-1即可。

为了通过模拟把OP2的特定姿势准确实现出来，必须先知道各个连杆的长度。进入ROBOTIS e-用户手册（http://support.robotis.com/ko/）页面中，可以看到OP1/OP2的详细身体尺寸，如图2.29所示。

通过这些尺寸，把OP上体与下体的连杆长度做一下整理，如表2.6所示。表中的DH连杆名指的是前面上体与下体的运动学模型中出现的连杆名，实际连杆名是对应于各个连杆并根据人体骨骼名称而起的名字。

图2.29　OP各个平面运动学尺寸机器人平台>DARwIn-OP>References>Reference>HardwareSpecifications>Mechanics>Kinematics

表2.6　OP全身连杆长度明细

OP的脚掌呈现为矩形，且左右脚是对称的，如图2.30所示，带有4个尺寸（l_ft_for、l_ft_back、l_ft_outer、l_ft_inner）。并且根据支撑腿是左腿还是右腿，l_ft_inner与l_ft_outer的位置互换。OP的脚掌尺寸整理在表2.7中，定义在SpecROBOTIS_OP2.m文件中。

图2.30　OP的左脚与右脚尺寸

表2.7　OP脚掌尺寸

代码2.5显示的是从GenMotionsWithDH.m文件中选取的与运动学相关的代码（与动力学相关的代码将在第3章中讲解）。代码中，O0_0、O1_0……O23_0表示所有连杆坐标系的原点在参考坐标系中的坐标，O0_0w、O1_0w……O23_0w是在绝对坐标系XYZ中的连杆坐标系的原点。在代码最底部使用了plot3函数。该MATLAB函数以三维方式显示机器人的姿势动作，在其中又使用了mz函数。当坐标值小于10^-5时，它会将其替换为0，这样可以防止plot3函数在运行时发生中断。

代码2.5　GenMotionsWithDH.m文件的运动学代码

function y = GenMotionsWithDH（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d,
　　　　　　　psi, psi_1d, psi_2d, fig_no）

g = 9.8062;

% 矢状面角度
th1 = th（1）; th2 = th（2）; th3 = th（3）; th4 = th（4）; th5 = th（5）; th6 = th（6）;
th7 = th（7）; th8 = th（8）; th9 = th（9）; th10 = th（10）; th11 = th（11）;
% 冠状面角度
phi1 = phi（1）; phi2 = phi（2）; phi3 = phi（3）; phi4 = phi（4）; phi5 = phi（5）;
phi6 = phi（6）;
% 横切面角度
psi1 = psi（1）; psi2 = psi（2）; psi3 = psi（3）;

% 连杆长度
l0 = link（1）; l1 = link（2）; l2 = link（3）; l3 = link（4）; l4 = link（5）; l5 = link（6）;l6 = link（7）; l7 = link（8）; l8 = link（9）; l9 = link（10）; l10 = link（11）; l11 =link（12）; l12 = link（13）; l13=link（14）; l14=link（15）; l_sh=link（16）;l_tr=link（17）; l_ft_for=link（18）; l_ft_back=link（19）; l_ft_inner=link（20）;l_ft_outer=link（21）;
% 连杆质量
m1 = mass（1）; m2 = mass（2）; m3 = mass（3）; m4 = mass（4）; m5 = mass（5）; m6 = mass（6）;m7 = mass（7）; m8 = mass（8）; m9 = mass（9）; m10 = mass（10）; m11 = mass（11）;m12 = mass（12）; m13 = mass（13）; m14 = mass（14）; m15 = mass（15）; m16 = mass（16）;m18 = mass（18）; m19 = mass（19）; m20 = mass（20）; m22 = mass（21）; m23 = mass（22）;
O = [0 0 0 1]'; % 连杆坐标系的原点坐标
Z = [0 0 1 1]'; % 连杆坐标系的z轴坐标

% 连杆坐标系的连杆重心坐标
C1 = [0 0 0 1]'; C2 = [-l1/2 0 0 1]'; C3 = [-l2/2 0 0 1]'; C4 = [0 0 0 1]'; C5 =[0 0 -l3/2 1]'; C6 = [-l4/2 0 0 1]';
if strcmp（sf, 'LEFT_SUPP'）
　　C7 = [0 -l5/2 0 1]';
elseif strcmp（sf, 'RIGHT_SUPP'）
　　C7 = [0 l5/2 0 1]';
end
C8 = [0 0 0 1]'; C9 = [-l6/2 0 0 1]'; C10 = [-l7/2 0 0 1]'; C11 = [0 0 0 1]';

C12 = [-l8/2 0 0 1]'; C13 = [-l_tr/2 0 -l_sh/2 1]'; C14 = [-0.016/2 -0.024/2
　　　0 1]';
C15 = [-l9/2 0 0 1]'; C16 = [-l10/2 0 0 1]'; C18 = [-0.016/2 0.024/2 0 1]';
C19 = [-l11/2 0 0 1]'; C20 = [-l12/2 0 0 1]'; C22 = [0 -l13/2 0 1]'; C23 = [-l14/2 0 0 1]';

% 把局部坐标系变换为全局坐标系
W=[cos（-alpha） 0 sin（-alpha） 0; 0 1 0 0; -sin（-alpha） 0 cos（-alpha） 0; 0 0 0 1]*[-sin（gamma） 0 cos（gamma） 0; cos（gamma） 0 sin（gamma） 0; 0 1 0 l0; 0 0 0 1];

G=[0 -g 0]';
O0_0 = O; Z0_0 = Z;
O0_0w = W*O+SP;
% 下体支撑腿的关节坐标
B1 = DHMat（phi1+pi/2, 0, 0, pi/2）; T1_0 = B1; O1_0= T1_0*O; O1_0w= W*O1_0+SP
B2 = DHMat（-th1+pi/2, 0, l1, 0）; T2_0 = B1*B2; O2_0 = T2_0*O; O2_0w= W*O2_0+SP;
B3 = DHMat（-th2, 0, l2, 0）; T3_0 = T2_0*B3; O3_0 = T3_0*O; O3_0w= W*O3_0+SP;
B4 = DHMat（-th3, 0, 0, -pi/2）; T4_0 = T3_0*B4; O4_0 = T4_0*O; O4_0w= W*O4_0+SP;
B5 = DHMat（phi2, 0, l3, 0）; T5_0 = T4_0*B5*[0 0 1 0; 1 0 0 0; 0 1 0 0; 0 0 0 1];
O5_0 = T5_0*O; O5_0w= W*O5_0+SP;
if strcmp（sf, 'LEFT_SUPP'） B6 = DHMat（psi1, 0, l4, 0）;
elseif strcmp（sf, 'RIGHT_SUPP'） B6 = DHMat（psi1+pi, 0, l4, 0）;
end
T6_0 = T5_0*B6; O6_0 = T6_0*O; O6_0w= W*O6_0+SP;
% 身体中心点
pc = （O5_0w（1:3） + O6_0w（1:3））'/2;

% 下体摆动腿的关节坐标
if strcmp（sf, 'LEFT_SUPP'）
　　 B7 = DHMat（psi2, 0, 0, -pi/2）;　 T7_0 = T6_0*B7*[1 0 0 0; 0 1 0 l5; 0 0 1 0; 0 0 0 1];
　　 B8 = DHMat（phi3-pi/2, 0, 0, -pi/2）;
elseif strcmp（sf, 'RIGHT_SUPP'）
　　 B7 = DHMat（psi2, 0, 0, pi/2）;　 T7_0 = T6_0*B7*[1 0 0 0; 0 1 0 -l5; 0 0 1 0; 0 0 0 1];

　　 B8 = DHMat（phi3+pi/2, 0, 0, -pi/2）;
end
O7_0 = T7_0*O; O7_0w= W*O7_0+SP;
T8_0 = T7_0*B8; O8_0 = T8_0*O;　 O8_0w= W*O8_0+SP;
B9 = DHMat（-th4+pi, 0, l6, 0）;　 T9_0 = T8_0*B9; O9_0 = T9_0*O;
O9_0w=W*O9_0+SP;
B10 = DHMat（-th5, 0, l7, 0）;　 T10_0 = T9_0*B10; O10_0 = T10_0*O;
O10_0w= W*O10_0+SP;
B11 = DHMat（-th6, 0, 0, pi/2）;　 T11_0 = T10_0*B11; O11_0 = T11_0*O;
O11_0w= W*O11_0+SP;
B12 = DHMat（phi4, 0, l8, 0）;　 T12_0 = T11_0*B12; O12_0 = T12_0*O;
O12_0w= W*O12_0+SP;

% 摆动腿脚掌中心点坐标
tip_x = O12_0w（1）; tip_y = O12_0w（2）;　 tip_z = O12_0w（3）;

if strcmp（sf, 'LEFT_SUPP'）
　 % 支撑腿脚掌的4个坐标
　 pf_0_1w = W*[l_ft_outer -l0 l_ft_for 1]'+SP;
　　　　　　 pf_0_2w = W*[-l_ft_inner -l0 l_ft_for 1]'+SP;
　 pf_0_3w = W*[-l_ft_inner -l0 -l_ft_back 1]'+SP;
　　　　　　 pf_0_4w = W*[l_ft_outer -l0 -l_ft_back 1]'+SP;
　 % 摆动腿脚掌的4个坐标
　 pf_12_1 = T12_0*[0 l_ft_inner l_ft_for 1]';
　　　　　　 pf_12_2 = T12_0*[0 -l_ft_outer l_ft_for 1]';
　 pf_12_3 = T12_0*[0 -l_ft_outer -l_ft_back 1]';
　　　　　　 pf_12_4 = T12_0*[0 l_ft_inner -l_ft_back 1]';
elseif strcmp（sf, 'RIGHT_SUPP'）
　 pf_0_1w = W*[l_ft_inner -l0 l_ft_for 1]'+SP;
　　　　　　 pf_0_2w = W*[-l_ft_outer -l0 l_ft_for 1]'+SP;
　 pf_0_3w = W*[-l_ft_outer -l0 -l_ft_back 1]'+SP;
　　　　　　 pf_0_4w = W*[l_ft_inner -l0 -l_ft_back 1]'+SP;
　 pf_12_1 = T12_0*[0 l_ft_outer l_ft_for 1]';
　　　　　　 pf_12_2 = T12_0*[0 -l_ft_inner l_ft_for 1]';
　 pf_12_3 = T12_0*[0 -l_ft_inner -l_ft_back 1]';
　　　　　　 pf_12_4 = T12_0*[0 l_ft_outer -l_ft_back 1]';
end
% 上体左胳膊的关节坐标
if strcmp（sf, 'LEFT_SUPP'）
　 T13_6 = [0 0 -1 -l4/2-l_sh/2; 0 1 0 0; 1 0 0 l_tr; 0 0 0 1];
elseif strcmp（sf, 'RIGHT_SUPP'）
　 T13_6 = [0 0 1 -l4/2+l_sh/2; 0 -1 0 0; 1 0 0 l_tr; 0 0 0 1];
end
T13_0 = T6_0*T13_6; O13_0 = T13_0*O; O13_0w = W*O13_0+SP;
B14 = DHMat（-th7, 0.024, 0.016, pi/2）; T14_0 = T13_0*B14; O14_0 = T14_0*O;
　 O14_0w = W*O14_0+SP;
B15 = DHMat（phi5-pi, 0, l9, -pi/2）; T15_0 = T14_0*B15; O15_0 = T15_0*O;
　 O15_0w = W*O15_0+SP;
B16 = DHMat（-th8, 0, l10, 0）; T16_0 = T15_0*B16; O16_0 = T16_0*O;
　 O16_0w = W*O16_0+SP;

% 上体右胳膊的关节坐标
if strcmp（sf, 'LEFT_SUPP'）
　 T17_6 = [0 0 -1 -l4/2+l_sh/2; 0 1 0 0; 1 0 0 l_tr; 0 0 0 1];
else
　 T17_6 = [0 0 1 -l4/2-l_sh/2; 0 -1 0 0; 1 0 0 l_tr; 0 0 0 1];
end
T17_0 = T6_0*T17_6; O17_0 = T17_0*O; O17_0w = W*O17_0+SP;
B18 = DHMat（-th9, -0.024, 0.016, pi/2）; T18_0 = T17_0*B18; O18_0 = T18_0*O;
O18_0w = W*O18_0+SP;
B19 = DHMat（phi6-pi, 0, l11, -pi/2）; T19_0 = T18_0*B19; O19_0 = T19_0*O;
O19_0w = W*O19_0+SP;
B20 = DHMat（-th10, 0, l12, 0）; T20_0 = T19_0*B20; O20_0 = T20_0*O;
O20_0w = W*O20_0+SP;

% 颈部关节坐标
if strcmp（sf, 'LEFT_SUPP'）
　 T21_6 = [0 1 0 -l4/2; -1 0 0 0; 0 0 1 l_tr; 0 0 0 1];
elseif strcmp（sf, 'RIGHT_SUPP'）
　 T21_6 = [0 -1 0 -l4/2; 1 0 0 0; 0 0 1 l_tr; 0 0 0 1];
end
T21_0 = T6_0*T21_6; O21_0 = T21_0*O; O21_0w = W*O21_0+SP;
B22 = DHMat（psi3, l13, 0, pi/2）; T22_0 = T21_0*B22; O22_0 = T22_0*O;
O22_0w = W*O22_0+SP;
B23 = DHMat（-th11+pi/2, 0, l14, 0）; T23_0 = T22_0*B23; O23_0 = T23_0*O;
O23_0w = W*O23_0+SP;

if strcmp（sf, 'LEFT_SUPP'） % 左胳膊与腿为绿色，右边为红色
　 c_sup_lg1 = '-bo'; c_sup_lg2 = '-b'; c_sway_lg1 = '-ro'; c_sway_lg2 = '-r';
elseif strcmp（sf, 'RIGHT_SUPP'）
　 c_sup_lg1 = '-ro'; c_sup_lg2 = '-r'; c_sway_lg1 = '-bo'; c_sway_lg2 = '-b';
end
if fig_no >= 1
　　figure（fig_no）
% 支撑腿图
　 plot3（mz（[sp（1） O0_0w（1） O1_0w（1） O2_0w（1） O3_0w（1） O4_0w（1） O5_0w（1）
pc（1）]）, mz（[sp（2） O0_0w（2） O1_0w（2） O2_0w（2） O3_0w（2） O4_0w（2） O5_0w（2）
pc（2）]）, mz（[sp（3） O0_0w（3） O1_0w（3） O2_0w（3） O3_0w（3） O4_0w（3） O5_0w（3）pc（3）]）, c_sup_lg1）;
　axis（’equal’）; hold on;
% 移动腿图
　plot3（mz（[pc（1） O6_0w（1） O7_0w（1） O8_0w（1） O9_0w（1） O10_0w（1） O11_0w（1）O12_0w（1）]）, mz（[pc（2） O6_0w（2） O7_0w（2） O8_0w（2） O9_0w（2） O10_0w（2）O11_0w（2） O12_0w（2）]）, mz（[pc（3） O6_0w（3） O7_0w（3） O8_0w（3） O9_0w（3）O10_0w（3） O11_0w（3） O12_0w（3）]）, c_sway_lg1）;
axis（'equal'）; hold on;
% 左胳膊图
plot3（[O21_0w（1） O13_0w（1） O14_0w（1） O15_0w（1） O16_0w（1）], [O21_0w（2）
O13_0w（2） O14_0w（2） O15_0w（2） O16_0w（2）], [O21_0w（3） O13_0w（3） O14_0w（3）O15_0w（3） O16_0w（3）], '-bo'）;
axis（'equal'）; hold on;
% 右胳膊图
plot3（[O21_0w（1） O17_0w（1） O18_0w（1） O19_0w（1） O20_0w（1）], [O21_0w（2）
O17_0w（2） O18_0w（2） O19_0w（2） O20_0w（2）], [O21_0w（3） O17_0w（3） O18_0w（3）O19_0w（3） O20_0w（3）], '-ro'）;
axis（'equal'）; hold on;
% 头部与颈部、上体图
plot3（[O22_0w（1） O21_0w（1） pc（1）], [O22_0w（2） O21_0w（2） pc（2）], [O22_0w（3）
O21_0w（3） pc（3）], '-ko'）;
axis（'equal'）; hold on;
% 支撑脚图
plot3（mz（[pf_0_1w（1） pf_0_2w（1） pf_0_3w（1） pf_0_4w（1） pf_0_1w（1）]）,
mz（[pf_0_1w（2） pf_0_2w（2） pf_0_3w（2） pf_0_4w（2） pf_0_1w（2）]）,
mz（[pf_0_1w（3） pf_0_2w（3） pf_0_3w（3） pf_0_4w（3） pf_0_1w（3）]）, c_sup_lg2）;
axis（'equal'）; hold on;
% 移动腿图
plot3（mz（[pf_12_1w（1） pf_12_2w（1） pf_12_3w（1） pf_12_4w（1） pf_12_1w（1）]）,mz（[pf_12_1w（2） pf_12_2w（2） pf_12_3w（2） pf_12_4w（2） pf_12_1w（2）]）,mz（[pf_12_1w（3） pf_12_2w（3） pf_12_3w（3） pf_12_4w（3） pf_12_1w（3）]）,c_sway_lg2）;
axis（'equal'）;
　　　　　　　⋮
view（view_ang）;
xlabel（'X （m）'）; ylabel（'Y （m）'）; zlabel（'Z （m）'）;
hold on; grid on;

在sim_pose.m（代码2.4）中，把no_pose设置为1并运行。图2.31给出了站立姿势的模拟结果，分别是左脚支撑时（sf = 'LEFT_SUPP'）的站立姿势与右脚支撑时（sf = 'RIGHT_SUPP'）的站立姿势。左右两幅图看上去类似，但仔细观察可以发现绝对坐标系的原点坐标分别位于左脚中心点与右脚中心点上。

图2.31　利用DH方法模拟仿人机器人的站立姿势

接下来，在代码2.4的左脚支撑状态（sf = 'LEFT_SUPP'）下，任意设置如下关节角（no_pose=2），对机器人的动作姿势进行模拟仿真。代码2.6中，deg2rad函数用来把向量值的角度（degree）形式转换为弧度（radian）形式。

代码2.6　动作姿势的关节角

th = deg2rad（[80 10 0 210 -10 -5 170 10 200 20 0]）; 
phi = deg2rad（[-10 5 170 10 200 170]）; 
psi = deg2rad（[10 –5 0]）;

把以上关节角设置输入sim_pose.m文件，对仿人机器人的动作姿势进行模拟，如图2.32所示。

图2.32　对输入关节角的仿人机器人姿势模拟

2.2.3　利用优化算法做DH法的逆向运动学计算

仿人机器人的逆向运动学问题是指为了把机器人的末端部位（双手与抬起的脚）移动到指定的坐标而对下体与上体的所有相关关节的旋转角进行计算。如图2.13所示，一般而言，用于把末端部位移动到指定坐标的关节角与拥有多个解。就仿人机器人来说，由于拥有超过20个关节，因此采用类似于关系式（2.48）~式（2.53）的方式求关节角的解会相当困难。针对这一问题，本书将介绍使用优化算法（computational optimization method），让计算机在较短时间内得到解的方法。

根据要优化的成本函数（cost function）是否使用微分值，优化算法又可划分为间接法（indirect method）与直接法（direct method）。由于对机器人运动生成中所应用的成本函数进行微分相当复杂或者根本不可能，我们必须选用直接法。在多个工程技术领域中，PSO（Particle Swarm Optimization，粒子群优化算法）是被广泛使用的算法。本书也将选用它作为优化算法。

PSO是1995年由Kennedy与Eberhart提出的全局优化算法，它模拟了鸟群或鱼群彼此共享信息搜寻食物的过程，属于自然模仿型（nature-inspired）算法^[5]。在鸟群或鱼群搜寻食物的过程中，包含群体内的各个对象（particle）自身知道的最优解信息与群体全体共享的最优解信息，各个对象以它们为基础进行行动，PSO从这种模式中得到启发并用于解决优化问题。在PSO中，不是只有一个对象，而是由多个对象形成群体进行搜索。这一点与遗传算法（Genetic Algorithm，GA）类似，但没有遗传算法的“交叉”（crossover）与“变异”（mutation）操作，因此更简单，更容易实现，并且计算耗时少，也不需要占用大量内存。此外，PSO算法同样适用于解决连续型与离散型问题。

在PSO算法中，开始搜寻后，各个对象会把自己所找到的最优解的位置向量（）与成本函数J（）保存到内存中，并且把整个种群目前所找到的最优解的位置向量（）与成本函数J（）在对象之间进行共享。在整个群体中，第k个对象利用与的信息，根据关系式（2.73）与式（2.74），计算下一次迭代（iteration）的方向向量（）与位置向量（）^[7]。

（2.73）

（2.74）

以上关系式中，与分别是第k个对象的当前方向向量与位置向量。如你所见，下一个位置向量由当前位置向量与新的方向向量相加而得到。另外，关系式中w表示方向向量的惯性权重值，与表示学习因子（learning factor）的权重值系数，与表示用于随机搜索周围点的均匀分布（uniformly distributed）的随机值（介于0到1之间）。在关系式（2.74）中，N表示总对象数。

图2.33　在二维搜索平面上PSO的第k个对象的位置与方向变化过程

图2.33描述了二维搜索平面中PSO的第k个对象从当前位置（）移动到下一个位置（）的原理。第k个对象拥有最优解（）的信息，这是从其以往的经验中得来的，并且共享群体发现的全局解（global minimum）的信息。为了从当前位置（）更新搜索点，第k个对象必须知道新方向（）。此时，PSO一方面要维持当前方向向量（）的惯性，另一方面要考虑指向对象最优解的方向（）与指向全局最优解的方向（）。图中，表示指向对象最优解与全局最优解方向的和，使用关系式表示如下：

（2.75）

利用上式，把代入关系式（2.73），简化如下：

（2.76）

接着，计算，利用关系式（2.74），即可求得对象的下一个位置向量，这一过程不断重复进行，直到达到最大循环次数。

PSO算法计算过程如下^[7]：

1）利用随机数为所有对象设置初始位置向量s与方向向量v。此时，初始位置向量s即为。

2）把1）中成本函数最小（最大化问题时是最大）的用作整个群体的全局最优向量。

3）使用式子（2.76）更新对象的方向向量v，使用式子（2.74）更新位置向量s。

4）若对象当前位置的成本函数值J（s）优于J（）成本函数值，则用替换s。

5）对于全体对象，若J（）成本函数值优于J（），则用替换。

6）返回3），重复执行循环，直至得到有满意成本函数值的解，或者达到足够多的迭代数。

为了演示使用PSO算法探索全局解的过程，下面以Branin函数（常用作二维的测试函数）为对象进行优化。Branin函数定义如下：

（2.77）

Branin函数带有3个全局解，全局解的位置是=（-3.142,12.275）,（3.142,2.275）,（9.425,2.425），全局解的函数值为0.397 887 358。在比较某个优化函数性能时通常的做法是，不断计算那个函数，使之不断逼近测试函数已知的全局解的函数值，并且保证误差在10^-6之内，然后把函数的计算次数与使用其他算法得到的次数进行比较。

使用PSO算法执行Branin函数优化的代码包含在code/matlab code目录下的pso.m与TestBR.m文件中，你可以在代码2.7中看到。在关系式（2.75）与（2.76）中，与相同，全部设为2，ω设为0.8。由于关系式（2.75）的第2项与第3项的系数是0到1之间的随机数与权重值系数的乘积，如上设置之后可使平均值为1^[7]。并且，把对象数N设为30，这样的PSO参数参考了[8]。

代码2.7　pso.m代码与TestBR.m代码

% 设置变量
dimPar = 2;　　　　　　　　　　　 % 变量个数
upBnd = [10 15]';　　　　　　　　% 变量上限值
lowBnd = [-5 0]';　　　　　　　　% 变量下限值
globMinCost = 0.397887358;　　　% Branin函数的全局解
n_part = 30;　　　　　　　　　　　 % 对象个数
v_max = 5*ones（1, dimPar）';　　% 方向上限值
v_min = -5*ones（1, dimPar）';　 % 方向下限值

w = 0.8;　　　　　　　　　　　　　　% 方向的惯性权重值
c1 = 2; c2 = 2;　　　　　　　　　 % 学习因子权重值
　　　　　　　　　　　　　　　　　　　⋮
% 初始值
for i=1:n_part
　　x_pos（i,:） = （（upBnd-lowBnd）.*rand（dimPar,1） + lowBnd）';
　　vel（i,:） = （（v_max-v_min）.*rand（dimPar,1） + v_min）';
end
cost = 1e10*ones（n_part）; % 初始成本函数
% 对象最优解的位置与函数值
pos_pbest = x_pos;
cost_pbest = cost;
% 全局最优解的位置与函数值
pos_gbest = [];
cost_gbest = 1e10;

iter = 1　　　　　　 % 迭代次数
while iter == 1 || err > 1e-6　 % 继续优化的条件
　　for pt = 1: n_part
　　　　 % 计算成本函数
　　　　 cost（pt） = TestBR（x_pos（pt, :））;
　　% 把当前对象与pbest比较
　　if cost（pt） < cost_pbest（pt）
　　　　pos_pbest（pt, :） = x_pos（pt, : ）;
　　　　cost_pbest（pt） = cost（pt） ;
　　end
　　% 把当前对象与gbest比较
　　if cost（pt） < cost_gbest
　　　 pos_gbest = x_pos（pt, :）;
　　　 cost_gbest = cost（pt） ;
　　end
% 计算新移动方向的速度
　　vel_mod = c1*rand*（pos_pbest（pt,:） - x_pos（pt, :）） + c2*rand*（pos_gbest
　　-x_pos（pt, :））;
　　vel（pt, :） = w*vel（pt, :） + vel_mod;
% 速度的最大值与最小值限制
　　vel（pt, :） = min（v_max', vel（pt, :））;
　　vel（pt, :） = max（v_min', vel（pt, :））;
　　% 计算下一次的对象位置
　　x_pos（pt, :） = x_pos（pt, :） + vel（pt, :）;

　　% 限制位置向量
　　x_pos（pt, :） = min（upBnd', x_pos（pt, :））;
　　x_pos（pt, :） = max（lowBnd', x_pos（pt, :））;
　　% 表示对象位置
　　figure（1）
　　plot（x_pos（pt, 1）, x_pos（pt,2）, 'ko'）;
　　axis（[lowBnd（1） upBnd（1） lowBnd（2） upBnd（2）]）;
　　hold on;
end
err=cost_gbest-globMinCost;
sprintf（'iter = %d, pos_gbest = [%.2f %.2f], cost_gbest=%.4e, error=
　　%.4e',iter, pos_gbest（1）, pos_gbest（2）, cost_gbest, err）　　　　　　　　　 % 在命令行窗口显示对象值
gcost（iter） = cost_gbest;
iter=iter+1; % 迭代次数

figure（1）
contour（fx1, fx2, f'）;
plot（-3.142, 12.275, 'bx', 3.142, 2.275, 'bx', 9.425, 2.425, 'bx'）;
　　xlabel（'x1'）;
　　ylabel（'x2’）;
　　hold off;
end
iters = 1:iter-1;

figure（2）
plot（iters, gcost）;
xlabel（'iteration'）;
ylabel（'cost'）;
grid
title（'使用PSO做函数优化'）

% Branin 定义Branin函数
function y=TestBR（x）
x1=x（1）; x2=x（2）;
y=（x2-5.1/（4*pi^2）*x1^2 +5/pi*x1-6）^2+10*（1-1/（8*pi））*cos（x1）+10;

图2.34与图2.35描述了使用PSO探索Branin函数全局解的过程。在图2.34中可以看到，初次探索时（iter=1），有30个对象随机分布在探索区域中；从探索中期（iter=24）到后期（iter=100），大部分对象都向全局解收敛。如图2.35所示，在PSO算法中，随着探索反复进行，成本函数不断减小，当重复95次之后成功找到全局解。以后，代替用于计算Branin函数的TestBR函数，根据要优化的目标，替换设计好的成本函数，运行PSO即可。

接下来，让我们一起思考仿人机器人中一个简单的逆向运动学问题：机器人在抓取置于左脚前方的物体时，如何计算全身的关节角？

就仿人机器人来说，在移动或做姿势时，最先要考虑的是姿势的稳定性（posture stability）。比如，机器人在抓取某个物体而向前伸手时，如果零力矩点（Zero Moment Point，ZMP）或重心（Center of Mass，CoM）超出了撑地脚掌的前部（脚趾）、旁侧（脚掌一侧），或后部（脚后跟），方向就会错掉。因此，在做某个姿势，计算全身的关节角时，注意不要违背这一条件。关于零力矩点，将在3.2节中详细讲解。当机器人处于静止状态时，零力矩点与重心是同一个，所以可以把更易理解的重心看作稳定性的衡量指标。

图2.34　PSO探索时对象的分布趋势（iter：重复次数）

图2.35　使用PSO算法对Branin函数值的全局解的探索趋势

当空间中存在多个拥有质量与位置向量的粒子时，这些粒子质量分布的平均位置向量就是重心。为了用数学关系式表示它，假设某个空间中存在n个粒子，各个粒子,i=1,…,n质量为，在空间中的坐标为。那么，对于重心坐标R，有如下关系式成立：

（2.78）

从上述关系式中解出R，如下：

（2.79）

为了更方便理解，如图2.36所示，假设空间中存在、、 3个粒子，它们的质量与位置向量各不相同，下面求重心R。图中，假定粒子大小与质量成正比关系，则有成立。由关系式（2.79）可知，重心R更接近最重的粒子。因此，重心位于、粒子附近，如图2.36所示。

为了计算仿人机器人全身的重心，首先必须知道各个连杆的重心，而后利用关系式（2.79）进行计算。各个连杆重心的准确位置可以通过CAD软件或精密测量得到，但是为了方便起见，大部分时候都假设它存在于连杆某个特定位置上。本书假设各个连杆的重心位于连杆两端关节之间的内分点上。

图2.36　拥有质量与位置向量的3个粒子的重心位置向量R

下面以长度为的第k个连杆为例进行讲解（见图2.37），它位于坐标系与坐标系之间。假设连杆重心所在的点把连杆切分为:两部分，并且+=1，。根据DH方法的坐标系设置规则，以轴为中心，把坐标系旋转，而后沿着新的轴移动，即可得到坐标系。该坐标系位于第k个连杆的末端部分。

并且，由于第k个连杆的重心属于坐标系，因此在坐标系中的三维坐标值为（）。对于连杆重心的内分比率与，必须对各个连杆单独精确测量并计算。但是为了方便起见，假设位于连杆中心位置，即=0.5。

图2.37　连接坐标系与坐标系的连杆的重心

与图2.17中显示的仿人机器人下肢模型相比，在图2.38中，把连杆的重心也标识出来了。利用该图，对仿人机器人下体各个连杆重心的局部坐标进行计算，如下：

图2.38　左脚支撑时对标有连杆重心的机器人下肢建模

（2.80）

图2.39是右脚支撑时仿人机器人的下体模型，其下体连杆上的重心都标出来了。与图2.38相比，可以发现轴的方向正好反过来了。这样一来，带有坐标系的连杆的重心坐标从关系式（2.80）变为如下形式：

（2.81）

图2.39　右脚支撑时标有连杆重心的机器人下体模型

同理，上体连杆的重心也可以根据图2.40计算出来。左胳膊连杆的重心的坐标值在各连杆坐标系中表示如下：

（2.82）

同样，可以求出右胳膊各个连杆重心的局部坐标，如下：

（2.83）

图2.40　左脚支撑时标有连杆重心的机器人上体模型

图中颈部与头部的重心在坐标系与坐标系中分别表示如下：

（2.84）

当仿人机器人用右脚支撑时，图2.40中只有坐标系与坐标系的位置互换，因此上体连杆的重心位置与质量不变。

表2.8　OP全身的连杆质量明细表

表2.8中，把OP上体与下体部位和连杆的质量整理出来了。在ENGLISH版本网站中，进入Support > e-Manual > Product Information > DARWIN-OP > References > Reference > Hardware Specifications > Mechanics > Dynamics页面，可以下载DARWIN-OP_Dynamics.zip文件。解压缩之后，可以看到许多文本文件，里面准确地记录着各个连杆与部位的CAD文件和质量、重心位置、惯性张量（inertia tensor）等。

根据图2.38与图2.40，以及Robotis公司提供的CAD文件，经过仔细分析，可以知道各个连杆的质量有如下关系：

（2.85）

这些值由OP的全身结构决定，它们定义在SpecROBOTIS_OP2.m文件中。

如果仿人机器人要抓取绝对坐标为的物体，就需要用PSO计算左胳膊要伸出多少、腰部要弯几度、膝部要弯曲多少。如图2.17与图2.20所示，OP借助矢状面上的11个关节、冠状面上的6个关节以及横切面上的3个关节来做出各种姿势动作。所以，需要优化的参数总共有20个，如下所示。

（2.86）

PSO的成本函数大致要满足如下3个要求：第一，摆动腿的脚掌应该位于地面上；第二，为了得到稳定的姿势，机器人全身重心的x轴与y轴的绝对坐标（）必须位于支撑在地面上的脚掌所形成的支撑多边形（support polygon）以内；第三，为了执行给定的任务，左胳膊末端部位终点的绝对坐标（）必须最大限度地靠近物体的绝对坐标。

满足上面3个条件就可以做到以下3点：一是让摆动腿的脚掌最低点大于0；二是当重心超出支撑脚所形成的多边形时，给出较大的成本函数值作为惩罚；三是末端部位与目标物体的三维距离要最小，可以使用如下成本函数表示：

（2.87）

上式中，是与第一个条件相关的罚函数，其中指摆动腿的脚部4个顶点高度的最小值，若该值小于0，则返回大值100作为成本函数值。因此，不管左胳膊末端终点的绝对坐标（）多么靠近，若重心不在支撑脚掌之内（稳定性条件），或者抬起的脚不在地面之上（现实条件），则成本函数返回较大值，PSO会视为差解。在代码2.8中使用MATLAB代码实现了，des_ht是函数因子，将其设置为0，判断抬起的脚是否高于地面。

在关系式（2.87）中，是与第二个条件相关的罚函数，按x轴与y轴分别计算全身重心与支撑脚中心点之间的距离，若其中有值大于指定的值，则输出大值。

代码2.8　PenaltySwingFootHeight.m代码

function cost = PenaltySwingFootHeight（sft_z_min, des_ht）

cost = 0;
if sft_z_min < des_ht % 当抬起的脚的最低点低于des_ht时
　　cost = 100;
end

为了讲解这种罚函数，如图2.41所示，在两只脚上，把脚的中心点与重心位置标出来，脚的中心点并不位于脚掌的正中央，在通过两点之间的距离差与评估稳定程度时，支撑脚不同，所做的评估也不同。也就是说，当大于时（重心位于前方时），距离值必须小于1_ft_for才能保持稳定；反之，当小于时（重心位于后方时），必须小于1_ft_back才行。

图2.41　支撑脚上和重心相关的罚函数计算方法

对于，它随支撑脚（左脚或右脚）不同而不同。在右脚支撑状态下，当大于时（向左侧倾斜时），必须小于1_ft_inner，才能保持姿势稳定；而当小于时（向右侧倾斜时），必须小于1_ft_outer才能保持稳定。左脚支撑时，恰好相反。当大于时，必须小于1_ft_outer，才能保持稳定；而当小于时，必须小于1_ft_inner，才能做出稳定姿势。其中原因在于左右脚形状是相互对称的，如图2.41所示。

在关系式（2.87）中，罚函数检查与，当全部位于稳定区域时，返回0，而只要其中任意一个不在稳定区域，就会返回一个相对较大的值100。在做稳定性评估时，机器人全身的重心是很重要的因素，它可以使用如下关系式计算得到。

（2.88）

上式中，与分别表示第i个连杆重心的绝对坐标与质量。

使用MATLAB代码把罚函数实现出来，如代码2.9所示。代码中，br表示边界比（boundary ratio），通过它，可以把前面设置为稳定区域边界的触地脚掌面（br=1.0）进一步变窄，使稳定姿势的标准更为严格。当把br设置为小于1.0的值时，稳定的重心位置区域在图2.42中用虚线表示出来。经过这样设置后，仿人机器人做出更接近直立的姿势，因而伸出的手与物体间的距离也可能变得更远了。

在使用DH方法建模（以后讲PB方法），并且用左脚进行支撑时，使用PSO在合适的范围内对20个变量进行探索，相应代码如代码2.10所示。

在设置各个关节角变量的最大值（upBnd）与最小值（lowBnd）时，请参考图2.17与图2.20，设置合适的范围很重要。与代码2.7（使用PSO探索Branin函数的全局解）相比，这段代码中使用了CalcCost4PostureStabilty函数来计算关系式（2.87）（计算成本函数）。由于不知道成本函数减小到何值以下该停下，所以编码时指定当更新到最大值（max_iter）时就终止探索。

图2.42　右脚支撑时让稳定的重心区域进一步变窄（）

代码2.9　PenaltyStability.m代码

function cost = PenaltyStability（sf, x0, y0, x_com, y_com, l_ft_for, l_ft_back,
　　　l_ft_inner, l_ft_outer, br）

cost = 0;

if x_com >= x0　　　　　　　　% 当重心位于前面时
　　if abs（x_com-x0） > br*l_ft_for
　　　　cost = 100;
　　　　return
　　end
else　　　　　　　　　　　　　　% 当重心位于后面时
　　if abs（x_com-x0） > br*l_ft_back
　　　　cost = 100;
　　　　return
　　end
end
if y_com >= y0 % 当重心倾向左侧时
　　if strcmp（sf, 'LEFT_SUPP'）
　　　　if abs（y_com-y0） > br*l_ft_outer
　　　　　　cost = 100;
　　　　　　return
　　　　end
　　elseif strcmp（sf, 'RIGHT_SUPP'）
　　　　if abs（y_com-y0） > br*l_ft_inner
　　　　　　cost = 100;
　　　　　　return
　　　　end
　　end
else　　　　　　　　　　　　　 % 当重心倾向右侧时
　　if strcmp（sf, 'LEFT_SUPP'）
　　　　if abs（y_com-y0） > br*l_ft_inner
　　　　　　cost = 100;
　　　　　　return
　　　　end
　　elseif strcmp（sf, 'RIGHT_SUPP'）
　　　　if abs（y_com-y0） > br*l_ft_outer
　　　　　　cost = 100;
　　　　　　return
　　　　end
　　　　end
end

代码 2.10　pso_posture_opt.m代码

global DH PB link mass sim_type flags
global alpha gamma view_ang
global br pick_hand des_end_pt des_lh_pt des_rh_pt del_Xc del_Zc

DH = 1; PB = 2;
%%%%%%%%%%%%%%%%%% 机器人种类 %%%%%%%%%%%%%%%%%%
md = DH;　　　　 % 运动学方法: DH: Denavit-Hartenberg, PB: Projection-Based
sf = 'LEFT_SUPP'　　　　　　　% 左脚支撑
sim_type = 'pose_sim'　　　　% 姿势模拟
sp = [0, 0, 0];　　　　　　　　% 支撑脚中心点位置
gamma = 0;　　　　　　　　　　　% 步行方向曲折角
view_ang = [45, 20];　　　　　% 机器人模拟观察者角度
spec = SpecROBOTIS_OP2（md）; % 读入OP2的规格明细
link = spec（1:21）;　　　　　　% 连杆长度
mass = spec（22:44）;　　　　　 % 连杆中心点质量
flags = zeros（1,7）;　　　　　 % 运动生成时与显示图相关的标记
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 设置变量
dimPar = 20;　　　 % 变量个数
upBnd = deg2rad（[ 90 60 0 270 0 30 270 90 270 90 40 30 30 210 30 270 180
　　　　30 30 20]）';
lowBnd = deg2rad（[ 30 0 -90 90 -90 -30 90 0 90 0 -40 -30 -30 150 -30 180
　　　　90 -30 -30 -20]）';
n_part = 50;　　　　% 对象数
max_iter = 200;　　% 最大迭代次数

br = 1.0;　　　　　　　　　　　　　% 目标稳定区域的边界比
pick_hand = 'left_hand'　　　　　% 抓物手，右手'right_hand'
des_end_pt=[0.1 0.05 0.07];　　 % 左脚前的物体位置
v_max = 0.2*（upBnd-lowBnd）;　 % 方向上限值：设置移动范围为全体区域的20%以内
v_min = -v_max;　　　　　　　　　　% 方向下限
w = 0.8;　　　　　　　　　　　　　　% 方向的惯性权重值
c1 = 2; c2 = 2;　　　　　　　　　 % 学习因子权重值

% 设置PSO对象初始位置与速度
for i=1:n_part
　　x_pos（i,:） = （（upBnd-lowBnd）.*rand（dimPar,1） + lowBnd）';

　　vel（i,:） = （（v_max-v_min）.*rand（dimPar,1） + v_min）';
end
cost = 1e10*ones（n_part）;
% 对象最优解的位置与函数值
pos_pbest = x_pos; cost_pbest= cost;
% 全局最优解的位置与函数值
pos_gbest = []; cost_gbest = 1e10;
%%%%%%%%%%%%%%%%%% PSO %%%%%%%%%%%%%%%%%%%%%%%%
iter = 1; % 迭代次数
while iter== 1 || iter <= max_iter
　　for pt = 1: n_part
　　　　cost（pt） = CalcCost4PostureStabilty（no_pose, md, sf, sp, sl, sag_tr,
　　　　　x_pos（pt, :）,-1）;
　　　　% 把当前对象与pbest比较
　　　　if cost（pt） < cost_pbest（pt）
　　　　　pos_pbest（pt, :） = x_pos（pt, : ）;
　　　　　cost_pbest（pt） = cost（pt）;
　　　　end
　　　　% 把当前对象与gbest比较
　　　　if cost（pt） < cost_gbest
　　　　　pos_gbest = x_pos（pt, :）;
　　　　　cost_gbest = cost（pt）;
　　　　end
　　　　% 计算新移动方向
　　　　vel_mod=c1*rand*（pos_pbest（pt,:）-x_pos（pt, :）） + c2*rand*（pos_gbest
　　　　　-x_pos（pt, :））;
　　　　vel（pt, :） = w*vel（pt, :） + vel_mod;
　　　　% 方向值的限制
　　　　vel（pt, :） = min（v_max', vel（pt, :））;
　　　　vel（pt, :） = max（v_min', vel（pt, :））;
　　　　% 计算下次对象位置
　　　　x_pos（pt, :） = x_pos（pt, :） + vel（pt, :）;
　　　　% 位置向量的限制
　　　　x_pos（pt, :） = min（upBnd', x_pos（pt, :））;
　　　　x_pos（pt, :） = max（lowBnd', x_pos（pt, :））;
　　end
　　pgb=rad2deg（pos_gbest）;
　　sprintf（'iter=%d, cost_gbest=%.4e, pos_gbest=[%.2f %.2f %.2f %.2f %.2f %.2f
　　　　%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f
　　　　%.2f', iter, cost_gbest, pgb（1）, pgb（2）, pgb（3）, pgb（4）, pgb（5）,
　　　　pgb（6）, pgb（7）, pgb（8）, pgb（9）, pgb（10）, pgb（11）, pgb（12）, pgb（13）,
　　　　pgb（14）, pgb（15）, pgb（16）, pgb（17）, pgb（18）, pgb（19）, pgb（20））
　　gcost（iter） = cost_gbest;
　　iter=iter+1;
end
iters = 1:iter-1;

figure（1）
plot（iters, gcost）;
xlabel（'iteration'）;
ylabel（'cost'）;
title（'成本函数趋势'）
grid

代码2.11　CalcCost4PostureStabilty.m代码

function cost=CalcCost4PostureStabilty（pose, md, sf, sp, sl, sag_tr, x,
　　fig_no）
global DH PB link
global br pick_hand des_end_pt des_lh_pt des_rh_pt
global del_Xc del_Zc

l_ft_for=link（18）;l_ft_back=link（19）;l_ft_inner=link（20）;l_ft_outer=link（21）;
th = x（1:11）;
phi = x（12:17）;
if md==DH,　　　　psi = x（18:20）;
elseif md==PB,　　 psi = [0 0 0];
end

% 静止时，角速度与角加速度全为0
th_1d = zeros（1,11）; th_2d = zeros（1,11）;
phi_1d = zeros（1,6）; phi_2d = zeros（1,6）;
psi_1d = zeros（1,3）; psi_2d = zeros（1,3）;

if md==DH
　　res = GenMotionsWithDH（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　psi_1d, psi_2d, fig_no）;
else
　　res = GenMotionsWithPB（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　fig_no）;

end

com_x = res（1）; com_y = res（2）;
lh_x = res（6）; lh_y = res（7）; lh_z = res（8）;
rh_x = res（9）; rh_y = res（10）; rh_z = res（11）; sft_z_min = res（15）;
cost = PenaltySwingFootHeight（sft_z_min, 0） + PenaltyStability（sf, sp（1）,
　　sp（2）, com_x, com_y, l_ft_for, l_ft_back, l_ft_inner, l_ft_outer, br）;
if strcmp（pick_hand,'left_hand'）==1 % 左手抓握时
　　cost = cost + sqrt（（des_end_pt（1）-lh_x）^2 + （des_end_pt（2）-lh_y）^2 +
　　　　（des_end_pt（3）-lh_z）^2）;
elseif strcmp（pick_hand,'right_hand'）==1 %% 右手抓握时
　　cost = cost + sqrt（（des_end_pt（1）-rh_x）^2 + （des_end_pt（2）-rh_y）^2 +
　　　　（des_end_pt（3）-rh_z）^2）;
end

在代码2.11中，作为计算成本函数值的MATLAB代码，GenMotionsWithDH函数返回的res向量由如下值组成。

% GenMotionsWithDH.m函数返回值
y = [com_x, com_y, com_z, zmp_x, zmp_y, lh_x, lh_y, lh_z, rh_x, rh_y, rh_z,
　　sft_x, sft_y, sft_z, sft_z_min, pc_x, pc_y, pc_z];

在代码2.11中，函数的第7个参数x是PSO的当前对象，指关系式（2.86）中出现的20个关节角。从关节角组合产生的机器人姿势计算抬起脚的4个顶点坐标、重心（）与左胳膊终点的绝对坐标（），然后计算关系式（2.87）中的成本函数值cost。该值由CalcCost4PostureStabilty函数计算出来，返回给pso_posture_opt。

通过执行pso_posture_opt代码，对由50个对象组成的群体做200次PSO迭代时，成本函数值不断减小，80次迭代之后就完成收敛，如图2.43所示。经过这一系列的迭代后，最终得到最优关节变量，使用角度表示如下：

图2.43　用于计算抓握物体姿势最佳关节角的成本函数减小趋势

与该变量对应的成本函数值为，由于机器人重心的x轴与y轴绝对坐标是（0.05,0.04），未超出支撑脚范围，故姿势稳定。使用得到的最优关节角进行模拟，OP姿势的前视图与顶视图如图2.44所示。图中，位于身体中心部位的四边形表示全身的重心，左手部位的星形图形表示目标物体。通过模拟结果，可以知道由PSO计算得到的最优关节角所产生的姿势准确接触到了绝对坐标为（0.1,0.05,0.07）的目标物体。然而，通过把br设置为1，如图2.44（b）所示，让重心的投影点位于左脚前端的左侧角上，这种姿势很危险，稍微往前推一下就会摔倒。在sim_pose.m代码中，把no_pose设为3，把no_sub_pose设为1，运行模拟，也会产生一样的结果。

接下来，把目标物体放到机器人右脚前，用左手抓物体，将p_obj设置为（0.1,-0.05,0.07）（对应代码的是des_end_pt=[0.1 -0.05 0.07]），把br设置为0.7，运行pso_posture_opt代码。把最终得到的最优解用角度表示如下，成本函数为。

图2.44　模拟用PSO探索得到的抓握物体的最优姿势

在图2.45中，你可以看到利用最优关节角所实现的姿势，当左手碰到目标物体时，姿势仍然保持稳定。在该姿势下，全身重心投影点的坐标（X,Y）是（0.03,0.01）。与图2.44（b）相比，它更靠近左脚中心点，所以这是一种十分稳定的姿势（在sim_pose.m代码中，把no_pose设为3、no_sub_pose设为2后运行）。

下面把支撑腿换成右腿，同样用左手去抓位于左前方的物体，让我们看一下是否可以用PSO计算最优姿势。为此，在pso_posture_opt.m代码中，做如下修改：sf='RIGHT_SUPP'、pick_hand='left_hand'、des_end_pt=[0.1 0.05 0.07]，执行PSO。在这些条件下，用PSO算出最优姿势。图2.46显示了姿势的前视图。从图中可以看到，机器人姿势稳定，并且用左手准确抓到了目标物体（在sim_pose.m代码中，把no_pose设为3、no_sub_pose设为3之后运行）。

此时，成本函数值为，用角度表示最优关节角如下：

最后一种情况是，在右脚支撑状态之下，用右手去抓位于右脚前方的物体，使用PSO计算形成这种姿势的关节角。为此，先做如下设置：pick_hand='right_hand'、des_end_pt=[0.1 -0.05 0.07]，再运行pso_posture_opt。需要特别注意的是，为了观看重心更加靠近支撑脚中心点时的姿势变化，特意把br设置为0.4。在给定的姿势条件下，使用PSO求出的最优姿势的前视图如图2.47所示，从图中可以看到姿势很稳定，类似于人类的姿势，并且右手准确触碰到了右前方的物体。此时，成本函数值为，用角度表示最优关节角如下：

该姿势下，全身重心投影点坐标（X,Y）为（-0.01,-0.01），最靠近支撑脚的中心点，这是目前为止稳定性最高的姿势。这是因为把br设置为0.4之后，目标稳定区域缩小为整个支撑脚面积的40%。在sim_pose.m代码中，设置no_pose=3、no_sub_pose=4之后运行，就能模拟出这种姿势。

图2.45　左脚支撑时抓握右脚前方物体的最优姿势

图2.46　右脚支撑时用左手抓握左前方物体的最优姿势

图2.47　右脚支撑时用右手抓握右前方物体的最优姿势

如前所述，在维持姿势稳定的同时，把末端部位准确移动到指定坐标需要计算出全身20个关节角，计算过程中要用到复杂的逆向运动学公式。而使用PSO算法极大简化了这一过程，只要确定出支撑腿是哪条，再对成本函数所对应的代码做一些修改，即可在短时间内求出最优解。然而，由于使用PSO算法时需要很多实体对象与100次以上的迭代次数，因此它不适合用在对实时性要求较高（毫秒级）的姿势计算中。

2.3　利用投影法对仿人机器人建模

2.3.1　投影法

DH方法仅适用于一个关节只有一种旋转或平移运动的情形。而仿人机器人的踝关节、髋关节、肩关节都有两个自由度以上的多种旋转，依据优先对待哪种旋转运动对坐标系进行变换，结果值会有细微的差别。

为了解决这一问题，我基于投影（projection）原理开发出了用来求绝对坐标系中各个关节坐标的投影法（Projection Based Kinematic Method）^[2]。该方法不像DH方法那样求连杆或末端部位的坐标系位置与方向，而求各个关节的绝对坐标，这与DH方法有些不同。在表现机器人姿势的过程中，所需要的计算量相当少，这是它的一大优点。根据最近的比较实验，在一台配有Intel i5-2400 CPU（3.1 GHz，2GB RAM）的PC上，对全身关节坐标计算40 000次，使用DH方法需要3.35秒，而使用投影法只需0.78秒。由此可见，投影法比DH方法快了约4.3倍^[9]。

为了了解投影法的原理，设想有一个2自由度的关节电动机在旋转一个连杆。如图2.48所示，先以x轴为中心轴，沿逆时针方向，把连杆（起初在z轴上且长度为l）旋转φ角，再以y轴为中心轴旋转角。

图2.49显示的是连杆的三维运动在矢状面（x-z平面）与冠状面（y-z平面）上的投影。首先，以x轴为中心轴，把连杆旋转φ角。从矢状面来看，连杆的长度好像从原来的l缩短为，如图2.49（a）所示。接着，再以y轴为中心轴，把连杆旋转角，连杆末端在x轴上的坐标计算如下：

（2.89）

上式中，表示连杆在矢状面上的投影长度。

下面从冠状面来看同一个复合旋转。以x轴为中心轴，把长度为l的连杆旋转角。从冠状面看（见图2.49（b）），连杆的长度没有变化，连杆末端在y轴上的坐标变为。然后，以y轴为中心轴，旋转角，连杆的长度看上去缩短为，连杆末端在y轴上的坐标计算如下：

（2.90）

图2.48　做2自由度旋转的连杆的三维坐标

图2.49　在三维空间中做2自由度旋转的连杆在二维平面上的投影

上式中，表示连杆在冠状面上的投影长度。连杆末端在z轴上的坐标（从任意平面看）可以采用相同方式计算出来，如下：

（2.91）

综合关系式（2.89）~（2.91），可知图2.48中连杆末端三维位置可表示如下：

（2.92）

2.3.2　向仿人机器人应用投影法

下面应用投影法，计算仿人机器人各个关节的绝对坐标。当OP用左脚支撑时，其全身三维模型在矢状面与冠状面上的二维投影如图2.50所示，14个连杆与关节坐标被表示为它们在各个平面上的连杆投影与绝对坐标。为了与DH方法兼容，机器人模型中出现的3种关节角（、、）的名称和旋转角定义与2.2节中关节角名称和旋转角定义保持一致。为了用投影法计算关节坐标，就像之前在关系式（2.89）~式（2.91）中看到的那样，先要计算连杆在各个平面中的投影长度。

利用图2.17中定义的关节角，把连杆投影在矢状面上，在用左脚支撑时，下体的连杆长度表示如下：

（2.93）

图2.50　仿人机器人用左脚支撑时的二维投影模型

同样，下体连杆在冠状面上的投影长度可表示如下：

（2.94）

上式中，表示。不同于关系式（2.94）与式（2.93），这里之所以使用sin函数是因为是从X轴而非Z轴开始的（见图2.50（a）），因此有关系成立。

由关系式（2.93）与式（2.94）可知，对于连杆在矢状面与冠状面上的投影长度，除了之外，其他长度值都随着时间发生变化。当横切面（水平面）关节或发生旋转时，连杆投影长度才发生变化。在横切面关节发生旋转时，也可以应用投影法计算各关节坐标的位置，但是关系式会变得非常复杂。对于有换向发生的运动，本书中将使用DH方法代替投影法进行计算。

到这里，我们已经求得连杆在矢状面与冠状面上的投影长度。所以，在左脚支撑状态下，可以把各个关节的三维绝对坐标依次表示如下：

（2.95）

上式中，、、分别表示、、。

与DH方法一样，在投影法中，当机器人用右脚支撑时，也要另外计算下体关节坐标。当仿人机器人用右脚支撑时，其全身模型在矢状面与冠状面上的二维投影模型如图2.51所示。

图2.51　仿人机器人用右脚支撑时的二维投影模型

此时，对于连杆在矢状面上的投影长度，只要如下修改一下（骨盆连杆长度）就可以了，其他保持不变。这是因为骨盆的相对方向与左脚支撑时恰好相反，这和DH方法中的情况类似。

（2.96）

出于同样的原因，在三维关节坐标中，只要如下修改一下坐标的计算关系式即可，其他关节坐标的计算关系式不变。

（2.97）

比较两个关系式，在冠状面上，摆动腿关节与支撑腿关节所形成的角度当用左脚支撑时为90°，用右脚支撑时为-90°。这种相位差异会对连杆的投影长度与Y轴坐标产生影响。

在投影法中，对上体的各个关节计算三维坐标时与支撑腿无关。图2.50与图2.51中定义的上体连杆在矢状面上的投影长度表示如下：

（2.98）

上式中，表示。

同理，上体连杆在冠状面上的投影长度表示如下：

（2.99）

由上式可知，决定上体（躯干）全体关节角的角度与支撑腿关节角的和是相等的，在矢状面上是，在冠状面上是。为了方便起见，把上体角度定义如下：

（2.100）

作为计算上体关节坐标的参考点（基准点），图2.50中的全身中心坐标（）与颈部坐标（）定义如下：

（2.101）

（2.102）

上式中，与分别表示连杆（代表机器人上体）在矢状面与冠状面上的投影，根据投影法可表示如下：

（2.103）

利用关系式（2.102）的颈部坐标，计算上体关节坐标，如下所示：

（2.104）

上述关系式中，表示肩部连杆在冠状面上的投影长度。因假定横切面上没有旋转，故有关系式成立。在关系式（2.104）中，表示左胳膊上的关节的全局坐标，表示右胳膊上的关节的全局坐标。表示颈上部的全局坐标，表示头顶的全局坐标。

正如在上述关系式中看到的一样，借助投影法，我们可以把所有关节的全局坐标用三角函数关系式表示出来，因此可以通过三角函数的微分关系式把连杆中心的线速度（linear velocity）与加速度（linear acceleration）计算出来。线速度用来计算动能，加速度用来计算零力矩点（zero moment point）。为了计算线速度与加速度，关系式（2.95）与式（2.104）必须能够进行微分，为此有必要把这些关系式表示为两个以上三角函数相乘的形式。比如，利用连杆原来的长度与绝对值变换公式（其中，sgn函数为符号函数，当参数大于0时，返回1；小于0时，返回-1），可以把关系式（2.95）中膝关节的全局坐标关系式表示如下：

（2.105）

把上式与关系式（2.95）进行比较，可以看到上式中出现了两个三角函数相乘的项。由于每次都把关系式转换为三角函数相乘的形式很麻烦，为此我们新定义三角函数的积函数，如下：

（2.106）

该函数中，下标S与C分别表示sin函数与cos函数，函数的参数依据下标指示的顺序与相应的三角函数结合再相乘。请注意，与是不同的。

利用上面定义的三角函数的积函数，可以把关系式（2.95）中下体关节的全局坐标（左脚支撑时）表示如下：

（2.107）

当机器人用右脚站立时，在第3个关节坐标关系式中，把在冠状面角度上增加角度由替换为，如下所示：

（2.108）

同样，利用关系式（2.102）~式（2.104）与式（2.106）的三角函数的积函数，可以把上体关节的全局坐标表示如下：

（2.109）

当用投影法模拟机器人的直立姿势时，只要在代码2.4的sim_pose.m文件中，设置md='PB'即可。代码2.12是GenMotionsWithPB.m文件中与运动学有关的代码，同时给出了代码注释，供大家阅读参考。

代码2.12　GenMotionsWithPB.m文件中的运动学相关代码

function y = GenMotionsWithPB（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d,
　　　psi, fig_no）

global link mass sim_type flags view_ang

g = 9.8062;
flag_show_zmp = flags（3）;　　 % 0: 不显示ZMP, 1: 显示ZMP
flag_show_2d = flags（4）;　　　% 0: 不显示2D平面模型, 1: 显示2D平面模型

% 矢状面角度
th1=th（1）; th7=th（7）; th8=th（8）; th9=th（9）; th10=th（10）; th11=th（11）;
th12=sum（th（1:2））;
th123 = sum（th（1:3））; th1234 = sum（th（1:4））; th12345 = sum（th（1:5））;
th123456 = sum（th（1:6））;
th1237 = th123+th7; th12378 = th1237+th8; th1239 = th123+th9;
th123910 = th1239+th10;
th12311=th123+th11; th12311=th123+th11;

% 冠状面角度
phi1 = phi（1）; phi2 = phi（2）; phi3 = phi（3）; phi4 = phi（4）; phi5 = phi（5）;
phi6 = phi（6）;
phi12 = sum（phi（1:2））; phi123 = sum（phi（1:3））; phi1234 = sum（phi（1:4））;
phi125 = phi12+phi5; phi126 = phi12+phi6;

% 连杆长度
l0 = link（1）; l1 = link（2）; l2 = link（3）; l3 = link（4）; l4 = link（5）; l5 = link（6）;
l6 = link（7）;
l7 = link（8）; l8 = link（9）; l9 = link（10）; l10 = link（11）; l11 = link（12）;
l12 = link（13）; l13= link（14）; l14 = link（15）; l_sh=link（16）; l_tr=link（17）;
l_ft_for=link（18）; l_ft_back=link（19）; l_ft_inner=link（20）;
l_ft_outer=link（21）;

% 连杆质量
M1 = mass（1）; M2 = mass（2）; M3 = mass（3）; M4 = mass（4）; M6 = mass（6）; M7 = mass（7）;
M8 = mass（8）; M9 = mass（9）; M10 = mass（10）; M11 = mass（11）; M12 = mass（12）;
M13 = mass（13）; M14 = mass（14）;

% 下体连杆投影
l1s = l1*abs（cos（phi1））;　 l2s = l2*abs（cos（phi1））;
if strcmp（sf, 'LEFT_SUPP'）
　　　l4s = l4*abs（cos（phi12+pi/2））;
elseif strcmp（sf, 'RIGHT_SUPP'）
　　　l4s = l4*abs（cos（phi12-pi/2））;
end
l6s = l6*abs（cos（phi123））;　 l7s = l7*abs（cos（phi123））;
l8s = l8*abs（cos（phi1234））;

l1c = l1*abs（sin（th1））;　　　　　　l2c = l2*abs（sin（th12））;
l4c = l4; l6c = l6*abs（sin（th1234））;
l7c = l7*abs（sin（th12345））;　　　 l8c = l8*abs（sin（th123456））;
% 上体连杆投影
l9s = l9*abs（cos（phi125））;　　　　l10s = l10*abs（cos（phi125））;
l11s = l11*abs（cos（phi126））;
l12s = l12*abs（cos（phi126））;　　　l13s = l13*abs（cos（phi12））;
l14s = l14*abs（cos（phi12））;
l9c = l9*abs（sin（th1237））;　　　　l10c = l10*abs（sin（th12378））;
l11c = l11*abs（sin（th1239））;
l12c = l12*abs（sin（th123910））;　　l13c = l13*abs（sin（th123））;
l14c = l14*abs（sin（th12311））;
l_trs = l_tr*cos（phi12）;　　　　　 l_trc = l_tr*sin（th123）; % 上体连杆
l_shc = l_sh;　　　　　　　　　　　　　　　　　　　　　　　　　　　　 % 肩部连杆

% 下体关节坐标
p0 = [sp（1） sp（2） sp（3）+l0];
p1 = p0 + [l1s*cos（th1）　　　 -l1c*sin（phi1）　　　 l1s*sin（th1）];
p2 = p1 + [l2s*cos（th12）　　　-l2c*sin（phi1）　　　 l2s*sin（th12）];
if strcmp（sf, 'LEFT_SUPP'）
　　　p3 = p2 + [l4s*cos（th123） -l4c*sin（phi12+pi/2） l4s*sin（th123）];
elseif strcmp（sf, 'RIGHT_SUPP'）
　　　p3 = p2 + [l4s*cos（th123） -l4c*sin（phi12-pi/2） l4s*sin（th123）];
end
p4 = p3 + [l6s*cos（th1234） -l6c*sin（phi123） l6s*sin（th1234）];
p5 = p4 + [l7s*cos（th12345） -l7c*sin（phi123） l7s*sin（th12345）];
p6 = p5 + [l8s*cos（th123456） -l8c*sin（phi1234） l8s*sin（th123456）];

% 上体关节坐标
pc = （p2+p3）/2;　　　% 中心坐标
pc_x = pc（1）; pc_y = pc（2）; pc_z = pc（3）;
pn = pc + [l_trs*cos（th123）　　　 -l_trc*sin（phi12）　　　l_trs*sin（th123）];
　　　% 颈部坐标

% 左肩关节坐标
p7 = pn + [0　　　　　　　　l_shc/2*sin（phi12+pi/2）　 l_shc/2*sin（phi12）];
% 左胳膊肘关节坐标
p8 = p7 + [l9s*cos（th1237）　　 -l9c*sin（phi125）　　　l9s*sin（th1237）];
% 左手腕坐标
p9 = p8 + [l10s*cos（th12378） -l10c*sin（phi125）　　　l10s*sin（th12378）];
% 右肩关节坐标
p10 = pn + [0　　　　l_shc/2*sin（phi12-pi/2）　　　　　-l_shc/2*sin（phi12）];
% 右胳膊肘关节坐标
p11 = p10 + [l11s*cos（th1239）　-l11c*sin（phi126）　　l11s*sin（th1239）];
% 右手腕坐标
p12 = p11 + [l12s*cos（th123910） -l12c*sin（phi126）　 l12s*sin（th123910）];
% 颈上部坐标
p13 = pn + [l13s*cos（th123）　　 -l13c*sin（phi12）　　 l13s*sin（th123）];
% 头顶坐标
p14 = p13 + [l14s*cos（th12311）　　 -l14c*sin（phi12）　l14s*sin（th12311）];

% 脚掌顶点坐标
if strcmp（sf, 'LEFT_SUPP'）
　　　% 支撑腿
　　　pf_supp = [sp; sp; sp; sp] + [l_ft_for l_ft_outer 0;　　% 左脚左前
　　　　　　　　　-l_ft_back l_ft_outer 0;　　　　　% 左脚左后
　　　　　　　　　-l_ft_back -l_ft_inner 0;　　　　 % 左脚右后
　　　　　　　　　l_ft_for -l_ft_inner 0];　　　　　% 左脚右前
　　　% 摆动腿
　　　pf_sway = [p6; p6; p6; p6] + ...
　　　[l_ft_for*cos（th123456 + pi/2） l_ft_inner l_ft_for*sin（th123456 + pi/2）;
　　　% 右脚左前
　　　l_ft_back*cos（th123456 - pi/2） l_ft_inner l_ft_back*sin（th123456 -
　　　pi/2）; % 右脚左后
　　　l_ft_back*cos（th123456 - pi/2） -l_ft_outer l_ft_back*sin（th123456 -
　　　pi/2）; % 右脚右后
　　　l_ft_for*cos（th123456 + pi/2） -l_ft_outer l_ft_for*sin（th123456 +
　　　pi/2）]; % 右脚右前
elseif strcmp（sf, 'RIGHT_SUPP'）
　　　% 支撑腿
　　　pf_supp = [sp; sp sp; sp] + [l_ft_for l_ft_inner 0;　 % 右脚左前
　　　　　　　　　-l_ft_back l_ft_inner 0;　　　　 % 右脚左后
　　　　　　　　　-l_ft_back -l_ft_outer 0;　　　　% 右脚右后
　　　　　　　　　l_ft_for -l_ft_outer 0];　　　 % 右脚右前
　　% 摆动腿
　　　pf_sway = [p6; p6; p6; p6] + ...
　　　[l_ft_for*cos（th123456 + pi/2） l_ft_outer l_ft_for*sin（th123456
　　　+ pi/2）; % 左脚左前
　　　l_ft_back*cos（th123456 - pi/2） l_ft_outer l_ft_back*sin（th123456
　　　- pi/2）; % 左脚左后
　　　l_ft_back*cos（th123456 - pi/2） -l_ft_inner l_ft_back*sin（th123456
　　　- pi/2）; % 左脚右后
　　　l_ft_for*cos（th123456 + pi/2） -l_ft_inner l_ft_for*sin（th123456
　　　+ pi/2）]; % 左脚右前
end

pf_sway = rot_mat_xy（pf_sway, p6（1:2）, psi（1））;　% 横切面旋转时脚掌旋转计算
tip_z_min = min（pf_sway（:,3））;　　　　　　　　　　　% 脚掌最低点
tip_x = p6（1）; tip_y = p6（2）; tip_z = p6（3）;　 % 摆动腿的脚部中心点

图2.52（a）模拟的是机器人左脚支撑时（sf='LEFT_SUPP'）的直立姿势，图2.52（b）模拟的是机器人右脚支撑时（sf='RIGHT_SUPP'）的直立姿势。与DH方法的模拟结果类似，两幅图看上去一样，但是绝对坐标系的原点位于不同支撑脚的中心点上。

为了创建任意机器人姿势，在代码2.4的sim_pose.m文件中，设置sf='LEFT_SUPP'、md='PB'、no_pose=2，并如下修改关节角，然后进行模拟。与DH方法的代码不同的是，使用投影法时，我们要在代码2.13中把psi的值设为0，如下所示。

图2.52　利用投影法模拟仿人机器人的直立姿势

代码 2.13　任意姿势的关节角

th = deg2rad（[80 10 0 210 -10 -5 170 10 200 20 0]）;
phi = deg2rad（[-10 5 170 10 200 170]）;
psi = deg2rad（[0 0 0]）;

把上面关节角设置输入到sim_pose.m文件，进行姿势模拟。图2.53显示的是仿人机器人的模拟结果。即使对肩关节与股关节部位进行了简化，但还是与使用DH方法模拟的结果几乎一模一样。由此可见，DH方法适合仿人机器人，投影法更适合表现人类的身体骨骼。充分运用两种方法的优点，有利于为仿人机器人实时生成动作以及实现实时控制。

图2.53　对任意角度的仿人机器人的姿势模拟（投影法）

2.3.3　利用投影法的仿人机器人逆向运动学

2.2.3小节中曾经讲过，在使用优化算法求目标姿势的关节角时要反复计算成本函数，这需要耗费很长时间进行运算，所以不太适合在实时运算中应用。如果机器人在运动过程中必须使用这种方式计算新姿势，并且主控机只有一个CPU，那么在执行优化运算时，机器人的动作就有可能暂停下来，这会让人觉得不自然，在某些作业环境下甚至会带来危险。如果主控机配备多核CPU，那么多个CPU内核可以并行执行优化运算，最后把计算得到的关节角发送给主CPU。

与DH方法不同，投影法中的关节坐标表示为三角函数积的形式。因此，我们可以使用“泰勒逼近多项式”（Taylor approximation polynomial）对当前姿势稍微做点改变，并且不必使用最优算法。根据泰勒逼近法，一切函数在x=x₀处都可以使用如下n次多项式进行逼近。

（2.110）

投影法的关系式中主要用到sin函数与cos函数。在处，它们可以使用如下1次多项式进行近似：

（2.111）

下面举个用投影法做逆向运动学计算的例子。如图2.54所示，假设机器人在直立状态下弯曲左胳膊来抓握某个物体。做该姿势时，全身关节角任意确定如代码2.14所示。

代码2.14　直立姿势下用左胳膊抓物体姿势的全身关节角

th = deg2rad（[90 0 0 180 0 0 200 100 180 0 0]）;
phi = deg2rad（[0 0 180 0 200 180]）;
psi = [0 0 0];

在此情形之下，利用关系式（2.109），以颈部坐标为基准，左手的绝对坐标拥有如下关系：

图2.54　直立状态下抬起左胳膊抓物体的姿势

（2.112）

位于左胳膊上的关节是、、，通过调整这些值，把移动到。下面了解一下调整方法。由图2.50与图2.51可知，主要由与决定，主要由决定，由3个角度的组合决定。

首先，为了把与移动到目标坐标，要计算一下把与改变多少。假设把变为，把变为后，变为，则由关系式（2.112）可知有如下关系式成立。

（2.113）

上式中，。此时，应用泰勒逼近关系式（2.111），关系式（2.113）中的与表示如下：

（2.114）

把上式代入关系式（2.113），得如下关系式：

（2.115）

从上式中减去关系式（2.112）中的，得到如下关系式：

（2.116）

接下来，假设改变与之后，移动至，则由关系式（2.112）可知如下关系式成立。

（2.117）

同样，应用泰勒逼近关系式（2.111），上式中的与表示如下：

（2.118）

把上式代入关系式（2.117）中，如下：

（2.119）

从上式中减去关系式（2.112）中的，得到如下关系式：

（2.120）

现在我们想知道的是，把左手移动与时相应的与是多少。为此，只要利用求解二元一次联立方程式的方法，解出上述关系式即可。使用矩阵表示前面求得的关系式（2.116）与式（2.120），如下：

（2.121）

利用逆矩阵，计算与，如下：

（2.122）

为了把其余坐标移动到目标位置，需要计算把改变多少，计算过程与上面类似。先假设把变为之后，移动到，则由关系式（2.112）可知有如下关系式成立。

（2.123）

上式中，。

同样，应用泰勒逼近关系式（2.111），上式中的与表示如下：

（2.124）

把上式代入关系式（2.123）中，如下：

（2.125）

从上式中，减去关系式（2.112）中的，得到如下关系式：

（2.126）

由该关系式，可以计算出与对应的，如下：

（2.127）

为了验证关系式（2.122）与式（2.127），在图2.54所示的姿势中，把左手位置分别沿x轴、y轴、z轴方向移动cm。在change_posture_pb.m文件中使用MATLAB代码把前面过程实现出来，代码2.15显示的是其中主要部分。

代码2.15　change_posture_pb.m文件中与左手位置变化相关的运动学代码

md = PB;
　　　　　　　　　　　　　　　　　　　⋮
del_eep = [0.02 0.0 0.0]'; % change of end-effector point

th = deg2rad（[90 0 0 180 0 0 200 100 180 0 0]）; % 矢状面基准关节角
phi = deg2rad（[0 0 180 0 200 180]）;　　　　　　　　% 冠状面基准关节角
psi = [0 0 0];　　　　　　　　　　　　　　　　　　　　　 % 横切面基准关节角

if md==DH
　　　res= GenMotionsWithDH（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　psi_1d, psi_2d, fig_no）;
else
　　　res= GenMotionsWithPB（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　fig_no）;
end
% 双手的绝对坐标
lh_init=res（6:8）; rh_init=res（9:11）;

th1 = th（1）; th12 = sum（th（1:2））; th123 = sum（th（1:3））; th1237 = th123+th（7）;
th12378 = th1237+th（8）; th1239 = th123+th（9）; th123910 = th1239+th（10）;
phi1 = phi（1）; phi12 = sum（phi（1:2））; phi125 = phi12 + phi（5）;
phi126 = phi12+phi（6）;

% 逆向运动学计算
inv_km_mat = inv（[-sign（cos（phi125））*（l9*P_cs（phi125,th1237）+l10*P_cs（phi125, th12378））,
　　　　-sign（cos（phi125））*l10*P_cs（phi125, th12378）;
　　　　sign（cos（phi125））*（l9*P_cc（phi125, th1237）+l10*P_cc（phi125, th12378））,
　　　　sign（cos（phi125））*l10*P_cc（phi125, th12378）]）;
del_th = inv_km_mat*[del_eep（1） del_eep（3）]';
del_ph = -1/（ sign（sin（th1237））*l9*P_cs（phi125,th1237）+sign（th12378）*
　　　　l10*P_cs（phi125,th12378））*del_eep（2）;

th（7） = th（7）+del_th（1）;
th（8） = th（8）+del_th（2）;
phi（5） = phi（5）+del_ph;

if md==DH
　　res = GenMotionsWithDH（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　psi_1d, psi_2d, fig_no）;
else
　　res = GenMotionsWithPB（sf, sp, th, th_1d, th_2d, phi, phi_1d, phi_2d, psi,
　　　　fig_no）;
end
lh_mod=res（6:8）; rh_mod=res（9:11）;
lh_dif = lh_mod-lh_init;

首先，为了把左手坐标沿着绝对坐标系的x轴向前移动2 cm，在代码中设置del_eep=[0.02 0.0 0.0]，运行change_posture_pb.m，在MATLAB运行画面中出现如下结果。从结果可以知道，由200°变为217.87°，由100°变为78.84°，机器人左手位置的变化量ΔX₉为0.02（eep_dif的第一个值）。

del_th7 = 17.87, del_th8 = -21.16, del_phi5 = 0.00 
th7 = 217.87, th8 = 78.84, phi5 = 200.00 
eep_dif = [0.02 -0.01 0.00]

接着，要沿着绝对坐标系的x轴，把左手向右移动2 cm（-2 cm）。为此，在代码中设置del_eep=[-0.02 0.0 0.0]，运行代码出现如下结果，通过相反角度的变化，把左手的绝对坐标移动指定的大小。

del_th7=-17.87, del_th8=21.16, del_phi5=0.00 
th7=182.13, th8=121.16, phi5=200.00 
eep_dif=[-0.02 0.00 0.00]

把这两种姿势变化与初始姿势做比较，模拟结果如图2.55所示。

图2.55　用change_posture_pb.m代码改变左手x轴坐标的姿势

接下来，沿着绝对坐标系y轴，把左手向左移动2 cm。为此，在代码中设置del_eep=[0.0 0.02 0.0]，运行change_posture_pb.m，得到如下运算结果。由结果可知，要把值增加2 cm，只需把从200°增加到210.09°即可。

del_th7=0.00, del_th8=0.00, del_phi5=10.09 
th7=200.00, th8=100.00, phi5=210.09 
eep_dif=[-0.01 0.02 -0.00]

接着，沿绝对坐标系的y轴，把左手向右移动-2 cm（2 cm）。为此，在代码中设置del_eep=[0.0 -0.02 0.0]，运行逆向运动学代码，得到如下结果。显而易见，把沿着相反方向旋转，即可把左手移动到指定位置。

del_th7=0.00, del_th8=0.00, del_phi5=-10.09 
th7=200.00, th8=100.00, phi5=189.91 
eep_dif=[0.01 -0.02 0.00]

使用MATLAB模拟y轴上的姿势变化，如图2.56所示。

图2.56　用change_posture_pb.m代码改变左手y轴坐标的姿势

接下来，沿着绝对坐标的z轴，把左手向上移动2 cm。为此，在代码中设置del_eep=[0.0 0.0 0.02]，运行change_posture_pb.m，得到如下运算结果。从运行结果可以知道，若想把增加2 cm，只要把从200°变为210.32°，把从100°变为98.70°即可。

del_th7=10.32, del_th8=-1.30, del_phi5=0.00 
th7=210.32, th8=98.70, phi5=200.00 
eep_dif=[-0.00 0.00 0.02]

反之，沿着z轴，把左手向下移动2 cm（-2 cm）。为此，在代码中设置del_eep=[0.0 0.0 -0.02]，运行代码，得到如下运算结果。

del_th7=-10.32, del_th8=1.30, del_phi5=0.00 
th7=189.68, th8=101.30, phi5=200.00 
eep_dif=[-0.00 –0.01 -0.02]

使用MATLAB模拟z轴上的姿势变化，如图2.57所示。

图2.57　改变左手z轴坐标的姿势

下面了解一下在改变右手绝对坐标时右胳膊关节角的变化量应该如何计算。为此，需要先把全身的关节角做如代码2.16的定义，使用MATLAB模拟姿势如图2.58所示。

图2.58　直立状态下抬起右胳膊抓物姿势

代码2.16　直立状态下用右胳膊抓物姿势时的全身关节角

th = deg2rad（[90 0 0 180 0 0 180 0 200 100　0]）;　 
phi = deg2rad（[0 0 180 0 180 160]）;　　　　　　　 
psi = [0 0 0];

由图2.50可以知道，位于右胳膊上的关节是、、，通过把这些值调整为、、，可以把右手的绝对坐标（）更改为（）。

由关系式（2.109）可知，左胳膊关节关系式与右胳膊关节关系式呈现对称关系。经过与之前一样的过程，对于与，可以得到如下关系式：

（2.128）

与前面一样，为了把右手末端的Y坐标（）移动到指定的坐标，需要推导出用于计算相应变化量的关系式。假设把变为后，变为，则采用同样方式展开，可以得到如下关系式：

（2.129）

代码2.17　hange_posture_pb.m文件中与右手位置变化相关的运动学代码

 inv_km_mat=inv（[-sign（cos（phi126））*（l11*P_cs（phi126,th1239）+l12*P_cs（phi126, th123910））,
　　　　　　-sign（cos（phi126））*l12*P_cs（phi126, th123910）;
　　　　　　sign（cos（phi126））*（l11*P_cc（phi126, th1239）+l12*P_cc（phi126, th123910））,　　　　　 
sign（cos（phi126））*l12*P_cc（phi126, th123910）]）; 
del_th = inv_km_mat*[del_eep（1） del_eep（3）]'; 
del_ph=-1/（ sign（sin（th1239））*l11*P_cs（phi126,th1239）+sign（th123910）*l12...
　　　*P_cs（phi126,th123910） ）*del_eep（2）; 
th（9） = th（9）+del_th（1）; 
th（10） = th（10）+del_th（2）; 
phi（6） = phi（6）+del_ph;

为了验证上面的逆向运动学方法，在图2.58所示的姿势下，把右手位置同时沿着x轴、y轴、z轴移动3 cm。为此，在change_posture_pb.m代码中，把no_pose设为2，设置del_eep = [0.03 0.03 0.03]，而后运行代码，在画面中出现如下结果。从运行结果可以知道，只要把从200°变为242.29°，把θ₁₀从100°变为66.32°，把从160°变为175.13°，OP的右手就移动到指定的位置处。

del_th9=42.29, del_th10=-33.68, del_phi6=15.13 
th9=242.29, th10=66.32, phi6=175.13 
eep_dif=[0.03 0.03 0.04]

把初始姿势与使用change_posture_pb.m代码调整后的姿势作比较，模拟如图2.59所示。

图2.59　改变右手坐标

2.3.4　投影法中的重心计算

在使用投影法为仿人机器人生成动作时，必须使用适用于投影法的重心计算方法。如图2.37所示，各个连杆的重心位于两端关节坐标之间，所以利用两个关节的“插值法”（interpolation），可以很方便地进行公式化。在图2.37中，第k个连杆的重心（）把点（）与（）之间长度分割为，且，应用插值法，得到如下关系式：

（2.130）

仿人机器人的连杆是刚体，在连杆运动期间，重心位置在连杆上保持不变。并且在投影法中，各个关节坐标使用XYZ坐标系（全局坐标系）中的坐标表示，把关系式（2.130）中的局部坐标系变量换成全局坐标系变量，如下：

（2.131）

图2.60显示的是仿人机器人用左脚支撑时在矢状面与冠状面上的投影模型。与图2.50不同的是，图中标出了各个连杆的重心。需要注意的是，由于投影法模型是对DH方法的简化，因此投影法中连杆的质量使用表2.8中的OP连杆的质量值表示如下：

图2.60　左脚支撑时标有连杆重心的仿人机器人的投影法模型

（2.132）

此外，在用投影法计算重心时，要把关系式（2.79）做如下修改：

（2.133）