人机器人虚拟示教系统的设计与实现

第36卷第1期 2008年1月 华南理工大学学报(自然科学版) JournalofSouthChinaUniversityofTechnology (NaturalScienceEdition) V01．36No．1 January2008 文章编号：1000-565X(2008)01-0018—07 仿人机器人虚拟示教系统的设计与实现木 甘志刚 肖南峰 (华南理工大学计算机科学与工程学院，广东广州510640) 摘要：引入虚拟现实技术对仿人机器人进行三维虚拟示教，对于改善仿人机器人的运 动控制算法具有非常重要的意义．文中首先提出了具有七自由度机器手臂和六自由度机 器腿的逆运动学计算方法，并采用三次多项式插值的方法来进行空间轨迹规划，在此基础 上设计和实现了仿人机器人三维虚拟示教系统．示教仿人机器人手足协调舞蹈的仿真实 验结果证明了虚拟示教系统的正确性和实用性． 关键词：仿人机器人；虚拟示教系统；冗余自由度；逆运动学；轨迹规划 中图分类号：TP242．6 文献标识码：A 为了使仿人机器人能够更好地为人类服务，研 究其运动控制方法显得特别重要．当前对仿人机器 人控制方法的研究主要集中在双足步行与双手协作 两个方面．双足步行的常用控制方法是先进行姿态 规划，再按稳定性最大的要求进行优化，以求出最稳 定的步态01-3]；而双手协作也需要先对双手的运动路 径与姿态进行规划，然后考虑其他约束条件对其运 动轨迹进行优化．由此可知，运动姿态与轨迹规划对 于仿人机器人双足步行与双手协作控制至关重要． 为了获得仿人机器人运动姿态预规划所需要的数 据，一般是在某个 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 演者(真人)的主要关节上安装 特制的标记点，然后由该表演者做出各种所需的动 作，再采用动作捕捉器快速和连续地记录下这些标 记点的空间坐标，从而获取人体的运动数据H。5J．采 用动作捕捉器虽然能够得到控制仿人机器人所需的 运动数据，但需要特定的场地和昂贵的动作捕捉设 备，阻碍了仿人机器人运动控制的研究；此外，仿人 机器人的关节数量和安装位置与真人也可能存在很 大的差异，这就使得通过采集真人运动得到的关节 运动数据与实际仿人机器人的关节运动数据可能存 在较大的误差． 随着虚拟现实技术的不断进步与完善，其在机 器人方面的应用也得到了很大的发展．Harada【61通 过三维仿真技术来对仿人机器人的动态平衡算法进 行了验证，GuanL71采用仿真技术辅助设计了仿人机 器人的步态控制算法，赵毅红等哺1采用仿真技术对 车式移动机器人轨迹跟踪控制算法进行了验证； Doulgeri等归。通过采用仿真与摄像机相结合的方法 实现对工业机器人手的示教，唐新华等¨们采用三维 仿真方法实现对焊接机器人的可视化虚拟示教．虽 然对机器人的仿真和虚拟示教的研究已经取得了很 多的成果，但仿人机器人的结构复杂，自由度众多， 使得当前的虚拟示教研究主要集中在工业机器人 上；而仿真技术主要应用在对其控制算法的验证上， 无法实现对仿人机器人的虚拟示教． 为了快速和低廉地得到仿人机器人的运动规划 数据，为后续的运动控制研究作准备，文中引入了虚 拟现实技术，通过对仿人机器人的运动姿态进行三 维虚拟示教，可以方便、快速地控制仿人机器人完成 各种复杂动作，降低了仿人机器人的控制难度． 收稿日期：2007．04．05 ·基金项目：国家自然科学基金资助项目(60375031)；国家自然科学基金与中国民用航空总局联合资助项目(60776816)；广 东省自然科学基金重点资助项目(36552) 作者简介：甘志刚(1979-)，男，博士生，主要从事仿人机器人方面的研究．E-mail：gdgzgzg@163．com 万方数据 第1期 甘志刚等：仿人机器人虚拟示教系统的设计与实现 19 1仿人机器人手臂的示教 仿人机器人的每只手臂有7个自由度，其中肩 关节3个自由度，肘关节1个自由度，腕关节3个自 由度．文中以仿人机器人的左手臂示教为例，右手的 示教方法与此相似．对该左手臂进行示教时，首先选 中需要移动的部位(如腕部)，然后按住鼠标左键拖 动腕部移动到指定的位置(设为一关键点)，并设置 好通过该关键点的时刻和角速度等，如此重复直到 设完所有关键点为止；然后采用三次多项式插值方 法将所有关键点平滑连接，完成整个示教过程．显 然，该示教过程需要对该机器手臂进行逆运动学求 解，得到该手腕到达鼠标所对应的位姿时手臂各关 节的角度．由于腕部在手臂的末端，而该手臂的自由 度超过了6个，因此具有冗余自由度．这种机器手臂 在避障和回避内部奇异等方面优于6自由度手臂， 有着广泛的应用，但是逆运动学求解比较困难．文中 采用梯度投影法¨¨来求解该手臂的逆解，通过自定 义优化指标函数，得到满意的求解结果． 1．1 正运动学方程的D．H表示法 文中采用D．H表示法来为仿人机器人模型建立 参考坐标系和D—H参数表，分别如图l和表1所示． 图1左手臂各关节附体坐标系的定义 Fig．1Definitionsofreferenceframesforjointsofleftann 表1左手臂的D．H参数 Table1 D-Hparametersofleftarm 1．2 7自由度手臂的逆运动学求解 以仿人机器人的左手臂为例，设左手腕在笛卡 儿空间中的速度为l，左手臂上各关节角位置和角 速度分别为0和口，J(0)为左手的雅克比矩阵，其中 j∈R6”，0∈R7“，匆eR7“，J(0)eR6”，由正向运 动学知： j=．，(0)西 (1) 用梯度投影法表示左手的运动学逆解为 0=．，+(秽)t+后[J一‘，+(o)J(0)][V日(0)] (2) 式中：_，+(们是．，(秽)的Moore．Penrose广义逆；I是 单位矩阵；VH(0)=a日(o)／oo是性能优化函数 日(口)的梯度矢量；七是自运动放大系数．式(2)又可 以写成如下形式‘13]： 扫=西血。+kVH(一) (3) 式中：西耐。：扫，一l譬当l扫。是最小范数解；扫，： L口iphJ 【，。一。。膏0一uv日，】；扫。=【．，。!，y】；扫，是匆的一 个特解；秒。是0的齐次解；．，‘是．，去掉某一列(该 列组成列向量y)后的6x6阶矩阵．为了充分利用7 自由度机器人手的冗余特性，并考虑各关节的运动 极限及避免左手进入奇异位形，文中设定左手的性 能优化函数为[14】 砌)=卢3COS2以+了1善7(等丧)‘(4) 式中：’ai=旦学；卢，={；：iiinn以03I><。0．．。088； 秽一小0rain,i是关节角0i的上、下限；自运动放大系数 k受各关节的角速度约束，由最速优化法可求出 k‘143，由式(3)可求得西，由于 0=Oo+0At (5) 式中：00是各个关节的初始角度；At是采样步长．因 万方数据 华南理工大学学报(自然科学版) 第36卷 此，由式(5)可得到7自由度左手臂的逆运动学解． 2仿人机器人腿的示教 2．1 正运动学方程的D—H表示法 仿人机器人的每条腿有6个自由度，其中髋关 节3个自由度，膝关节1个自由度，踝关节2个自由 度．对腿部的示教和手臂示教相似，文中首先对腿的 各个关节进行逆运动学求解．由于左脚掌受腿部所 有关节的约束，且左脚掌和右脚掌的逆运动学求解 过程相似，因此这里以左脚掌的逆运动学求解为例， 其他关节的求解方法类似．图2和表2分别是仿人 机器人左腿的附体坐标系定义和D．H参数表． 图2左腿各个关节的附体坐标系定义 Fig．2Definitionsofreferenceframesforjointsofleftleg 表2左腿的D—H参数 Table2 D—Hparametersofleftleg 2．2 6自由度腿的逆运动学求解 由于左腿有6个自由度，其中3个关节轴相互 平行，因此其存在逆运动学封闭解．设Ai是左腿第 i个关节相对于前一关节附体坐标系的旋转矩阵， o瓦= nz OJ 口J PJ ny Oy n： O： 口，PyI是鼠标拖动脚掌需要到达的 口：P：l 0 O O l 指定位姿，玎=(n，，n，，他)7、O=(吼，D，，D：)1。、a= (a。，口，，叱)1‘分别表示法线、指向和接近向量，P= (P：，P。，Pz)1’为位置向量，由正运动学可知 。瓦=AlA2A3A4A5A6 (6) 式(6)乘以相应逆矩阵变换为 。瓦AflAflAfl=AIA2A3(7) 则由式(7)等号两边矩阵的第三列相等可得 一nJsin06—0xC0$06=cosOlsin02 一nysin06一ovcos06 2 sinolsin02 (8) -，lzsin06—0zcos06=一CO$02 将式(8)中3个等式取平方后相加，可消去0。和口：， 从而得到仅含魄的等式，用Matlab求解该等式可以 求得魄(由于结果较长，省略)．然后将以代入式 (8)即可求得0，和0：(当sin02=0时，左腿处于奇 异状态，此时0。可取任意值，文中取0。=0)．式(6) 还可变换成 AflAI．1·”瓦Afl=A3A4A5(9) 取式(9)等号两边矩阵的第1行第4列和第2行第 4列元素分别相等，即 f。一c。8(口s+04)+a3eos03=hx(10) 【04sin(以+以)+D3sin03=h。 、 。 式中：^x2Pxcos02cosol+Pvcos02sinOl+P：sin02，hv= P，sinOl-p，cos01．由于^，和h，已知，用Matlab求解 式(10)即可得到以和以的值(结果较长，省略)． 由式(9)等号两边矩阵的第1行第1列和第2 行第1列元素分别相等，得 {c。8}ps+以+ps?。^j’(11) 【sin(03+04+05)=h： 。 式中：^：=(C2Cln，+C2Sl几，+Sift：)c6一(c2clo，+ c2S10y+$20：)S6；h；=(．sln，一Cl凡y)c6一(S10，一 C10，)S6；Si=sinOf；Ci=cos0。；江3，4，5．则 p3+以+05=arctan(h：，^：)(12) 由于0，和以已经求得，故可以求得以．至此，左脚 掌的逆运动学求解过程结束． 3 示教动作的轨迹规划与仿真 在示教过程中，通过设置关键点来引导仿人机 器人沿着指定的轨迹做相应动作．文中采用三次多 万方数据 第1期 甘志刚等：仿人机器人虚拟示教系统的设计与实现 21 项式轨迹规划方法将各个关键点平滑连接[121，从而 使仿人机器人能够平稳地运动到各个关键点，仿真 实验证实该方法可以取得满意的效果。 设某一关节在开始时刻to的角度为％，在t，时 刻运动到新的角度巩，且关节角度函数 口(f)=co+clt+c2t2+c3t3(13) fo(to)=00，9(tf)=0f {． ． ． ． (14) 【8(to)=00，0(“)=0f 对式(13)求导得 p(￡)=c1+2C2t+3c3t2 (15) 将式(14)代入式(13)和式(15)中，联立可求得方程 中的4个未知数为 CO=Oo，cl=00 c2=3(口f—Oo)／t；一2舀o／tf (16) c3=2(Oo—Of)It；+(西o+台f)／t； 代入式(13)便可求得p(t)． 在对某个示教动作进行轨迹规划时，首先读人 第一个关键点的关节角度岛和角速度0。，然后再读 入第二个关键点的关节角度0，和角速度0，，将这4 个数值分别代入式(13)和(14)，得到该关节在吼 到研之间角度与时间的对应关系0(t)．然后通过将 0到t，之间的不同时刻代入口(￡)，即可得到这段时 间内该关节的运动轨迹；重复处理下一个关键点，直 到所有关键点都处理完成为止，此时即完成该关节 的轨迹规划．通过对示教动作中涉及的其他关节重 复上述插值处理，即可完成整个示教过程中的轨迹 规划．其流程图如图3所示． 读入第一个关键点fo时刻的角度鼠和角速度反 《毒擎裁蛐 读入下一个关键点‘时刻的角度日和角速度百． I 将“反、0，、白代入式(14)和(16)求出p(，) ● f=fo l ：求ttlo(t)卜] <≤茎>习，：b。| l是 to=t,，00=O,，龟=碡 度、角速度和角加速度随时间的变化曲线．从图4可 以看出，由于示教过程中鼠标难免会发生震动，或者 操作速度不均匀，因而导致仿人机器人的动作不平 滑．如果直接将这种数据应用于真实仿人机器人上， 则会导致仿人机器人的控制电机速度和加速度剧烈 震颤而影响仿人机器人的控制精度，甚至损坏电机； 轨迹规划插值处理后得到的运动曲线平滑连贯，不 会出现速度和加速度的剧烈变化，对于实现仿人机 器人本体的安全运行具有明显的效果． 0 4 8 12 16 20 24 t／s (b)角速度 O 4 8 12 16 20 24 tfs (c)角加速度 图4左手臂示教过程中肘关节轨迹规划前后的角度、角速 度和角加速度曲线 Fig．4jAngle，anglevelocityandangleaccelerationCUl'Vesof elbowjointbeforeandaftertrajectoryplanningduring teachingactionofleft83131n 图3示教动作轨迹规划的流程图 Fig．3Flowchartoftraj∞toryplanniIlgforteachingacti。n 4 仿人机器人三维虚拟示教系统 图4是仿人机器人示教过程中左手臂肘关节角 仿人机器人三维虚拟示教系统是基于仿人机器 O O O O O O 0 O O 8 6 4 2 之4南罐 万方数据 华南理工大学学报(自然科学版) 第36卷 人三维实时仿真系统¨引，在Windows2000Profes． sional操作系统下，采用OpenGL(OpenGraphicsLi． brary)三维图形库与Vc++6．0开发的．OpenGL是 图形应用程序设计接口(API)，具有可移植性，在三 维仿真中可以实现逼真的三维视觉效果¨61．文中系 统的仿人机器人模型完全按照仿人机器人实体尺寸 和比例来构造，其实体图及虚拟示教系统操作界面 如图5所示． 图5仿人机器人实体及其三维虚拟示教系统 Fig．5Structureofbipedhumanoidrobotandits3Dvirtual teachingsystem 三维虚拟示教系统的软件模块主要包括机构捕 捉、鼠标牵引、关键点录制、路径规划、动作修改与重 放以及其他辅助工具等，如图6所示．其中机构捕捉 利用OpenGL中的选定模式u引，采用堆栈方式实现 对仿人机器人三维机构的捕捉与识别；鼠标牵引通 过鼠标拖拽选定的机构移动到指定位置，完成示教 动作；关键点录制将仿人机器人在关键点处的相关 参数保存到文件中；路径规划通过插值操作将示教 过程中录制的各个关键点通过平滑曲线连接起来， 使得仿人机器人能够安全平稳地完成相应动作；动 作修改允许用户对录制好的示教动作进行相应修 改，示教动作还可以在仿真平台上多次重放，用以观 察整个示教过程；其他辅助工具是指查看示教文件 中某关节的角度一时问曲线图、角速度一时问曲线 图和角加速度一时间曲线图等工具． 1 机构捕捉 ≮． 仿人 例路径规划I 机器 l 鼠标牵引 l- ≥ 人三 维虚 ≤ I动作修改与重放拟示 门 教系 I 关键点录制 ∥ 统 心 其他辅助工具 图6仿人机器人三维虚拟不教系统模块 Fig．63Dvirtualteachingsystemmodulesofbipedhumanoid robot 为说明文中示教系统的效果，对示教仿人机器 人手足舞蹈过程进行了仿真．首先进入示教模式，此 时允许用户选择需要示教的部位．用鼠标左键点击 仿人机器人左脚掌，则脚掌部位被一个绿色立方体 框住，表示该部位已被选中．选择“示教模式”菜单 中的“录制”项，则系统弹出一个“录制”工具栏，如 图7所示．接着拖动左脚掌到第一个指定位置，设置 好姿态，用相同的方法设置好仿人机器人其他关节 此时的角度，然后单击“设置关键点”按钮保存该关 键点仿人机器人所有关节的角度和角速度参数．当 第一次单击“设置关键点”按钮时，系统会要求选择 示教文件需保存的路径，选择好路径后，系统就会将 该关键点相对于上一关键点的时刻、所有关节此时 图7录制工具栏 Fig．7Recordtoolbar 万方数据 第1期 甘志刚等：仿人机器人虚拟示教系统的设计与实现 的角度和角速度都记录到文件中．为了方便查看示 教过程，系统不仅记录关键点的关节角度，而且也记 录鼠标拖动过程中各关节角度的变化．录制过程中， 所有关键点的各关节角速度值默认为零，如果需要 调整，可选择该关键点的相关关节并修改其角速度， 然后单击“保存”按钮即可保存对该关节角速度的 修改，如图7所示．重复上述操作步骤，直到整个示 教过程结束，此时单击“停止”按钮即可停止示教动 作的录制． 录制的示教文件可以通过“轨迹规划”功能来 在各个关键点之间进行插值处理，自动生成整个示 教动作的平滑轨迹．同时，用户可以查看示教文件中 各关节的角度、角速度和角加速度随时间的变化曲 线(见图4)，还可以对示教动作进行重放，其操作界 面如图8所示．图9为仿人机器人手足协调舞蹈的 示教过程截图． 图8轨迹规划操作界面 Fig．8Operationinterfaceoftrajectoryplanning 图9仿人机器人舞蹈示教 Fig．9Danceteachingofhumanoidrobot 5 结论 在仿人机器人运动学和动力学基础之上提出的 三维虚拟示教系统，通过实时地求解出各个关节的 逆运动学解，实现了仿人机器人上被选定的机构与 鼠标的跟随运动，简化了示教过程，实现了所见即所 得的示教效果．另外，采用三次多项式轨迹规划方法 在各个关键点之间进行插值处理，避免了示教过程 中由于鼠标的拖动而导致的角速度与角加速度的剧 烈变化对仿人机器人本体的影响，对于真实的仿人 机器人平稳安全运行和延长其控制电机使用寿命都 有积极的效果．另外，示教系统可以方便地回放示教 动作，这对于修改示教动作和获取运动规划数据都 非常方便和有效． 参考文献： [1]KimJ Y，ParkIW．Walkingcontrolalgorithmofbiped humanoidrobotOnunevenandinclinedfloor[J]．Journal ofIntelligent&RoboticSystems，2007，48(4)：457·484． [2]HaradaK，KajitaS．Real—timepla,mingofhumanoidrobot's gaitforforce-controlledmanipulation[J]．IEEE—ASME TransactionsonMechatronics，2007，12(1)：53-62． [3]YangHS，Yong—HoSeo，Yeong—NamChae，ct81．Design anddevelopmentofbipedhumanoidrobot，AMl2，for80- cialinteractionwithhumans[C]∥Procofthe6thIEEE． RASInternationalConferenceonHumanoidRobots．Prin． ceton：IEEE．2006：352．357． 万方数据 华南理工大学学报(自然科学版) 第36卷 [4]HiroseM，OgawaK．Hondahumanoidrobotsdevelopment [J]．PhilosophicalTransactionsoftheRoyalSocietyA： MathematicalPhysicalandEngineeringSciences，2007， 365(1850)：ll—19． [5]Nakaokas，NakazawaA．Taskmodeloflowerbodymotion forabipedhumanoidrobottoimitatehumandances[C]∥ Procof2005IEEE／RSJIntemationalConferenceonIntel· ligentRobotsandSystems．Edmonton：IEEE，2005： 3157．3162． [6]HaradaK．Dynamicsandbalanceofahumanoidrobot duringmanipulationtasks[J]．IEEETransactionsonRo- botics，2006，22(3)：568—575． [7]GuanYisheng．Steppingoverobstacleswithhumanoidro- bots[J]．IEEETransactionsonRobotics，2006，22(5)： 958—973． [8]赵毅红，朱剑英．基于综合导向的车式移动机器人轨 迹跟踪控制[J]．华南理工大学学报：自然科学版， 2006，34(11)：60-63． ZhaoYi—hong，ZhuJian—ying．Trajectorytrackingcontrol ofcal"-likemobilerobotbasedonintegratedsteering[J]． JournalofSouthChinaUniversityofTechnology：Natural ScienceEdition，2006，34(11)：60—63． [9]Doulgeriz，MatiakisT．AWebtelerobotiesystemtoteach industrialrobotpathplanningandcontrol[J]．IEEE TransactionsonEducation，2006，49(2)：263．-270． [10]唐新华，DrewsPaul．机器人三维可视化离线编程和仿 真系统[J]．焊接学报，2005，26(2)：64-68． TangXin—hua，DrewsPaul．3DVisualoff-lineprogram- [11] [12] [13] [14] [15] [16] mingandsimulationsystemforrobot[J]．Transactions ofChinaWeldingInstitution，2005，26(2)：64—68． Ramdane·Cherif，DaaehiB．BenallegueA．Kinematicin- version[C]∥ProcofIEEE／RSJInternationalConfe． FenceonIntelligentRobotsandSystem．Lausanne：IEEE， 2002：1904-1909． NikuSaeedB．机器人学导论：分析、系统及应用 [M]．孙富春，朱纪洪，刘国栋，译．北京：电子工业出 版社。2004． DubeyRV，EulerJA，BabcockSM．Real-timeimple- mentationofalloptimizationschemeforseven．．degree．．of- freedomredundantmanipulators[J]．IEEETransactions onRoboticsandAutomation，1991，7(5)：579—588． 祖迪，吴镇炜，谈大龙．一种冗余机器人逆运动学求解 的有效方法[J]．机械工程学报，2005，41(6)：71．75． Zu—Di，WuZhen—wei，TanDa·long．Aneffectivemethod forinversekinematicssolutionofredundantrobot[J]． ChineseJournalofMechanicalEngineering，2005，41 (6)：71—75． 甘志刚，肖南峰．仿人机器人三维实时仿真系统的研 究与实现[J]．系统仿真学报，2007，19(11)：2444． 2448，2518． GanZhi-gang，XiaoNan—feng．Researchandimplements- tionof3Dsimulationsystemforbipedhumanoidrobot [J3．JournalofSystemSimulation，2007。19(11)；2444． 2448，2518． [美]施仁奈，OpenGL编程权威指南[M]．第5版．徐 波，译．北京：机械工业出版社，2006． DesignandImplementationofVirtualTeaching SystemforHumanoidRobot CanZhi—g帆gXiaoNan-feng (SchoolofComputerScienceandEngineering，SouthChinaUniversityofTechnology，Guangzhou510640，Guangdong，China) Abstract：The3DvirtualteachingofhumanoidrobotViavirtualrealitytechnologyisverysignificantforimproving thecontrolalgorithmsofahumanoidrobot．Inthispaper，theinversekinematiccalculationsarefirstpresentedfor thehumanoidrobotwith7-DOF(DegreeofFreedom)armsand6-DOFlegs，andthespacetrajectory’planningof thearlTlSandlegsisthenimplementedbymeansofcubicpolynomialinterpolation．Furthermore，a3Dvirtual teachingsystemforthehumanoidrobotisdesignedandimplemented．Byteachingthehumanoidrobottodance，it isfinallyprovedthattheproposedvirtualteachingsystemiscorrectandfeasible． Keywords：humanoidrobot；virtualteachingsystem；redundantdegreeoffreedom；inversekinematics；trajectory planning 万方数据