有理贝塞尔曲线在机器人轨迹规划中的应用_黄玉钏.pdf
5页
2 0 1 5年7 3第5期＾有理贝塞尔曲线在机器人轨迹规划中的应用国家安全生产监督管理总局通信信息中心 黄玉钏摘 要： 基于有理贝塞尔曲线的机器人轨迹规划融合了有理贝塞尔曲线计算机算法、透射投影、 机器人轨迹规划等多个领域的技术 本文堤出了基于有理贝塞尔曲线的机器人轨迹规划方案， 设计并实现了基于有理贝塞尔曲线的机器人轨迹规划算法，以线性插值运算为核心建立了机器人轨迹规划策略关健词： 有理贝塞尔曲线机器人 轨迹规划1§｜ｇ系数一般具有重要的几何意义 ， 使得设计具有直观性， 易统计显示 ，近年来中国工业机器人销售处于快速增长于产生数值稳定的算法阶段， 2 0 1 3年中国已成为全球第一大工业机器人市场 工业机器人广泛应用于焊接、 打磨、 喷涂、 搬运等领域 ， 其2机器人运动学建模轨迹控制方式主要采用示教 ，机器人由示教作业过渡为 自2 ． 1正向运动学建模主作业的关键技术之一就是离线编程轨迹规划［ “3 1 国内离、｜Ｘｓ“ 6线编程技术发展还比较缓慢 ，上海交通大学栾楠研究了基Ｚｓ于ＭＯＴＯＭＡＮ的工业机器人离线编程系统专利 ， 浙江大学 （3ＴＱＱ9＜徐国桦提出了装配机器人离线编程系统的设计， 但大都处 0 6于研究阶段。
工业机器人若要完成离线编程轨迹规划 ， 首“ｈ“5ｙ＇先必须实现对各种平面和空间轨迹的规划 这就要求利用一种合适的方法表示空间轨迹 ， 这种方法不仅要易于计算、机实现， 且要易于与机器人现有控制策略融合几何造型中， 两种最常用的曲线曲面表示方法是隐式表示和参数表示方法本文主要考虑参数方法表示机器人Ｑ．平面和空间轨迹 参数形式表示机器人轨迹主要有以下优／Ｙ点［ 4 ’ 5 ］＾ｙ0ｙｘ〈2（ 1 ）方便表示曲线和曲面的方向 ；（ 2 ）对于参数形式曲线 ， 容易生成曲线上的有序点列；图1 建立坐标的通用工业机器人运动学模型对于参数形式曲面， 容易生成曲面上的网格点 ：机器人正向运动学旳问题是机器人运用它对自身的关（ 3 ）参数形式曲线和曲面易于计算机进行设计和表示，节角度和手臂长度等信息来判断机器人末端在三维空间中ＷＷＷ．ＣＡ1 6 8 ． ＣＯＭＪｕｌＳｅｒｖｏＣｏｎｔｒｏｌ3 7技术＾2 1 5年7月ｍｍ所处的位置和姿态ｐ＝ｄ4ｓ＾ｓ2 ｌ＋＾35，ｃ2 3 ＋ｄ2ｓｘｃ2 ＋ｄｘｓｘ这里我们给出通用6 自由度工业机器人的正向运动学模？广 广 广＿ ｅ 。
ｅ“：一2 34562 3462 356型，其中前3 个自 由度是铰链构型，主要完成定位功能，后ｏ＝－ｓ ｅ ｅ ｓ－ｓｓｃ－Ｃ Ｓ Ｓ3 个自由度是旋转轴相交于一点的球型腕， 主要完成姿态调整功能ａ＇Ｗ5ＣａＣｓ按照ＤＨ方法＿建立的通用 6 自由度工业机器人的坐标Ｐ＇￣￣ｄ＇Ｃ＂＋仏＋仏模型， 如图 1所示 附表是通用 6 自由度工业机器人的ＤＨ参2 ． 2 逆向运动学建模数表逆向运动学与正向运动学正好相反，它解决机器人如附表 通用工业机器人运动学参数表何移动才能达到合适的姿势（改变关节位置）这一问题 我们给出通用 6 自 由度工业机器人逆向运动学求解的各关节1°°°9＇角度，如公式（ 3 ）－ （ 8 ）所示，其中ｄ5 为机器人工具坐标系原点在机器人6轴ｚ方向延伸的距离 ， 公式中Ａｔａｎ 2表示反正切函30ｄ200 34 9 0ｄ？＋ｌｇｏ＋ 0 4数 另外 ， 我们暂不考虑机器人处于奇异点位置的情况 ，59 00 01 8 0＋ 0 5例如Ｓ5＝0的情况：69 0000 6Ｇ、二喊、二？－冗（3 ）图 1 中 ，＾＝1 ， 2 ， ． ． 6表示机器人6个关节， Ｓ｜1 6个自 由度；「＾。
表示坐标系 ｛ 丨 ｝ 的三个坐标轴 现设想连杆坐标系⑴其中￣ａ－ｄＪ＋＾．￣ａ，ｄＪ与 ｛ ｉ－ｌ｝完全重合， 那么经过如下四个运动 ， 即可获得坐标系 ｛ｉ ｝与 ｛ｉ－ 1｝的相对关系 ： Ｉ， 、：ＭｆＭ）（ 4 ）⑴绕ＸＭ轴旋转ｏｃ 卜丨角 ’ 单位为度；0’＝ Ｘ ｔａｎ 2（—’ ±ｍＨ（ 2 ）沿及丨轴移动年．丨， 单位为厘米；， ｖ．（ 3 ）绕Ｚｉ轴旋转 0 ；角 ， 单ｋ为度；⑷沿Ｚｊ轴移动ｄｊ， 单位为厘米ｐ＝＾（ 2 ｄｌｄｔｙ ＋ （ 2 ｄ！ｄＪｊ＝Ａｔａｎ 2 ｛ｌｄ ｄ，， 2 ｄ，ｄＡ ）因为这些变换是相对于动坐标系描述的，按照 ＂ 从左02 ＝Ａｔａｎ 2 （／ｍ－ｑｍ， ｐｍ＋ｑｎ）－ 0，（ 5 ）到右“ 的原则 ， 我们可以得到公式（ 1）：ｍ＝ｃ，ｐ， ＋ ｓ， Ｐｙ－ｃ，ａＡ－ ｓ，ａＡ－ ｄ，＂＼Ｔ＝Ｒｏｔ（Ｘ，ａＭ）Ｔｒａｎｓ（Ｘ， ａ）Ｒｏｔ（Ｚ，ｄｔ）Ｔｒａｎｓ（Ｚ， ｄ：）⑴其中’＜＂ ＿叉＿孓5机器人正向运动学用°Ｊ？表示 ， 如公式（2 ）所示。
其中”七仏、ｓｉｓＣｉ分别表示ｓｉ ｎ 9 ，和ｃｏｓ 0 ｉ ，ｉ＝ｌ ， 2 ． ． 6；ｃ 23、 ｓ2 3分别表示ｒ《《Ｔ0＝＾ ｔａｎ 2 （ｊ ａ－ｃ ａ．ｃ＾ａ，＋ｓ ｃｉａｒ＋ ｓｕａ：）ｓｔ＞ 0ｃｏｓ（0 ，＋ 0 3 ）和ｓｉｎ（ 0 ？＞＋ 0 3 ） 1 （ 6 ）Ｉ 0，＝Ａｔａｎ 2 （－（ 5，ａ－ ｃ、ａｙ），￣（ｃ］ｃ2ｉａｉ＋ｓ ｃ2 3ａ＞ ＋ ））ｓｓ＜ 0「Ｄ＂ｌ＇：利用以上的结果，容易得到公式（7 ）和（ 8 ）“ ｎｏａＰ ｎｏａＰ°ｒｐ＿＿ｙ ｙ ｙ ｙ （Ｚ ）？ 1 00ｉ ｊ＝？0．ａ：Ｐ：0 5＝Ａｔａｎ 2 （ｓ5 ，ｃ3 ） （ 7 ）＿0ｏｏｌ ｊｅ 6＝Ａｔａｎ 2 （ｓ 6 ，ｃ 6 ）（8 ）ｎｘ＝ｃ． ｃ．Ａｃ ．ｃ ｃｈ－ｓ ｓ）－ ｃ． ｓ ｓ． ｃ＾＋ｓｘｓ ｃ ｃ＾＋ 5ｔｃ＾／46ｉ2 3 5 6 、… …3 机器人轨迹规划°＇￣Ｃ＾（￣Ｃ＾＇Ｓ＾＋Ｃ＇Ｓ＂Ｓ＾＿ＷＡ＋Ｗ63 ． 1 基于有理贝塞尔曲线的轨迹规划ｆｌ，＝ ｃｉｗ5＋ｃ，，2 3ｃ5 ＋，ｉｖ53 ． 1 ． 1轨迹规划方法综述Ｐ＇＝ ｄ＇°＇Ｓ＇－＇＋ｄ＇Ｃ－ｃ＾＋ｄ＾＇Ｃ2＋ ｄｌｃ， 常用的轨迹规划方法有： 3－ 4 － 5正则多项式和4 － 5 － 6 － 7＂．Ｖ＝ｓ，ｃ2 3 （Ｃ4Ｃ5Ｃ6 －＾4＾6 ）－ － ｃ，（ｓＡｃｓｃ6 ＋ｃ456）正则多项式，摆线运动与抛物线拟合的线性函数等＿。
°ｒ＝ ＾ｃ2 3 （－Ｃ4Ｃ556－ 54 Ｃ6 ）＋ 5，52 3Ｖ6 ＋Ｃ，（Ｓ4Ｃ556 －ＣＡＣ6）4 － 5 － 6 － 7正则多项式可以保证加速度的变化没有急动；摆线ａｖ＝、ｃ2 3ｃ4？＋、、ｃ5 －ｃ，ｖ5运动与抛物线拟合的线性函数方法对要求通过中间位姿的3 8ＳｅｒｖｏＣｏｎｔｒｏ ｌＪｕｌＷＷＷ． ＣＡ1 6 8 ．ＣＯＭ情况处理起来较为复杂 当机器人要求通过中间位姿时 ，有理贝塞尔曲线有非负性、 规范性、 凸包性、 仿射不大多采用高阶正则多项式表示轨迹 ，Ｋａｈａｎｅｒ ，Ｍｏ ｌ ｅｒ和变性、变差减少性 ，可以形成机器人任意空间轨迹 当所Ｎａｓｈ等指出中间位姿数量增加时这种采用多项式插值的方有加权系数 ｃｏｒｌ时 ， 有理贝塞尔曲线变为贝塞尔曲线， 即法变得不切实际 ， 这是因为中间位姿的增加会使得多项式贝塞尔曲线是有理贝塞尔曲线的特例系数的方程组的条件数变大 ， 相对舍入误差等于舍入误差3 ． 2有理贝塞尔曲线轨迹规划策略乘以一个放大系数， 从而方程组的解失真 用有理贝塞尔3 ． 2 ． 1 机器人运动学控制策略曲线或曲面表示机器人轨迹可以有效解决以上问题。
图 2是机器人运动学控制策略示意图，机器人上位机和3 ． 1 ． 2 有理贝塞尔曲线／曲面轨迹规划控制器根据实际任务需要 ， 完成有理贝塞尔曲线根据机器人的任务要求 ， 确定机器人需要经过的平面轨迹规划 ， 生成规划的关节位置 、 速度、 加速度信息 机或空间点 ， 然后确定有理贝塞尔曲线的次数ｎ ，ｎ次有理贝器人下位机 ， 根据规划的位置、 速度、 加速度信息生成各塞尔曲线由公式（ 9 ）表示 ： 关节电机控制指令，一般是生成脉冲数， 使得电机运动到ｆ＾Ｂｉ ｎ （ｕ） ｃｏｉＰｉ规划的位置 关节传感器使得系统构成闭环Ｃ（ｗ ）＝＂ 4；0＜ ｗ ＜ ｌ（ 9）＾ ￣｜工Ｂｉ？ 上 控ｂｊｊｅＬ＿Ｊ￣ｉｖｉｋ￣ｂｉｄ机器＇－1 制器＆轨 ？机器—＾ 人电＿＾ 入关其中 ， Ｐｉ＝（ Ｘ ｉ， ｙｉ， Ｚｉ ） 为控制点集 ， …为权重因子 ，〒器6－＞ｎ2｜＜ ＝ （科ｕ，乂，叫，， 4 ）为齐次坐标控制点集， 透视投影映射｜‘要求 ｜ＬＬｌ〒Ｈ‘ 麄Ｔ ， 如公式（ 1 0 ）所示： ＴＵｘＶＺ ． ）ｃｏ． ＾ 0图2 通用工业机器人运动学控制策略示意图ｐ＝Ｔ（ｐ＇）＝ｒ？ 7 ｌ 1 1（ 1 0 ）‘［ｄｉｒｅｃｔｉｏｎｉｘ， ， ，ｚ，． ）ｃｏ，＝ 03 ． 2 ． 2机器人轨迹规划策略利用齐次坐标特点： ｎ＋ 1维空间的幂基曲线表示维空间机器人上位机和轨迹规划器中的策略如图3所示。
上位中的一条有理曲线 如公式（ 1 1 ）：机和轨迹规划器任务要求一般来自 ： 自由曲面ＣＡＤ模型、Ｃ（ｉｌ） ＝Ｔ（Ｃ＂（ｕ））＝ＴＹＢｌ？（1 1）工艺和参数要求 、 工具模型和机器人模型等方面的要求／＝0 ／Ｖ 自由曲面同理 ，有理贝塞尔曲面Ｓ（Ｕ，Ｖ）可以表示齐次坐标公式的）ＣＡＤ？Ｍ透视投影，如公式（ 1 2 ）和（ 1 3 ）所示 ：（ＮＫ｜规划的— ＮＩＳ（ｕ ， ｖ）＝Ｔ（Ｓ＇＼Ｕ， Ｖ））＝ｒ（ＸＺ5，（？ ）＾ｍ （ｖ）＾）（1 2） ）【＾翻器及ｇ控1 ＝ 0ｊ＝ 0 ｔ￣Ｖ 工具輕和＝＝￡￡＾， ，（ｕ，ｖ）ｐ． ．图3 通用工业机器人轨迹规划策略示意图一对于固定点（Ｕ？，Ｖ（1）有理贝塞尔曲面点的计算可以采用如4线轨迹规划算法下策略 ： 先固定ｊ ， 利用』 行控制点＾＆＾＋＝ 0 ， ． ． ． 《， 然后应用4 ． 1 有理贝塞尔曲线算法， 、 ？下面给出有理Ｂｄｚｉ ｅｒ曲线算法［ 1 1 ］ｄ＿纖， 可以得到等参算法Ｖ＊线性擁算法。
计算ｍ＋ｌ次ｄｅＣａｓｔｅｌｊａｕｓ算法可以得到Ｃ？Ｍ， 然后再对到Ｃ？⑷／＊＿ｔ ：作业点集合Ｐ，ｎ为控制点个数减1 利用 1 次ｄｅＣａｓｔｅｌｊ ａｕｓ算法 ， 得至化⑷八） 同理 ，可以先计算／＊ｏｕｔｐｕｔ：—个点ｄ到Ｃ“ （ｖ）， 然后计算Ｓ（Ｕｏ，ｖｄｅＣａｓ ｔｅｌｊ ａｕｓ （Ｐ，ｎ，ｕ）两种丨丨瞎的计算需要执行的线性插值运算次数如公式（ 1 4） ：｛ｆｏｒ（ｉ＝。
