第2期 王立权，等：6R关节型机器人运动学建模 ·159· 符，则应继续寻找约束条件，别除增根， 4实验系统及算法检验 本文机器人构型最多有8组逆解，得到8组逆 解，说明代数求解方法没有引入增根. 4.1实验系统 3.3多解问题 根据课题需要，开发了控制系统的软、硬件，图 由第2节推导的结果可知，可能的逆解有8组， 3是机器人实验系统照片.硬件系统以P℃机作为上 如图2所示.实际控制系统只能按其中一组解来执 位机，通过CAN9820总线控制多个下位单片机，每 行，这里采用结构体变量Tree,并引入最佳柔顺准则 个单片机控制一个关节.基于VC+和OpenGL技 式(7)选取惟一解. 术实现机器人的虚拟主从控制，详情见文献[6]. ex=min(公[8(k+)-0,()]) (7) 式中：0(k+1)为目标角度，0(k)为当前角度，k为 权值. 图3机器人系统照片 0. Fig.3 Photo of the robot system 本文所讨论的逆解算法作为算法模块的一个子 0, 程序.由于D-H坐标系设置时将基座、工具形状等 、:4 0e5 因素从核心运动学算法中分离，算法模块可供系统 图2逆解树 中多个机器人共用，并且能够方便地修改其基座位 Fig.2 Tree of inverse solutions 置和工具形状。 Struct Tree/保存逆解的结构体 4.2逆解结果检验 intP://结束标识，初始值为0 利用实验系统可直观地验证逆解结果.为精确 /1,2…表示结束计算的原因 地分析逆解过程，从软件系统中获取运行数据，举例 double thetal; 分析其中一个位姿8T=[1000;010120;001 220;0001],取权值k1=2=k3=1,k4=k5=k6= double theta6;/角度值 0.2.得到的结果如表2所示，与几何方法分析的结 ouble ex; /柔顺值 果一致.图4中给出了4组解，肘关节翻转后可再生 成4组解.表2中加“*”的角度值是参考值（实际 }; 程序中是随机数)，最终程序选择了第4组解.将角 具体计算步骤是：计算01→计算02（若2+u2- 度值代人正解表达式，正解结果与给定值一致 m2<0,则P=1,ex=10,结束该组解计算；否则执行 下一步)→判断02是否为增根（若为增根，则P=2, 38.89 82.49° ex=10',结束该组解计算；否则执行下一步)→计算 6.46 67.83 03→判断03是否为增根→计算0→判断0是否为增 3关节Q -151.419 5关节 根→计算0，→计算0。→调用子程序，检验障碍等约束 51.11 -187.51 条件，若不满足则置P值和ex值，否则继续下一步→ 3关节 3关节 计算ex=名k[0,(k+1)-0()].最后，从中选出 -178.56 -148.62 96.429 ex值最小的一组解，并判断ex.若ex<l0',则将该结 2关节 30o-31.38 构体中的角度值作为最终逆解结果，这样就得到了惟 1关 2关节 一逆解.若不满足ex<10',则表明没有可行解，检查 图4机器人的4组解 P值，可以知道原因。 Fig.4 Four solutions of the robot