一种六自由度智能机器人本体的控制方法
【专利摘要】本申请公开了一种六自由度智能机器人本体的控制方法，包括：测量并计算六个轴的连杆长度、关节的当前角度和关节偏移量，通过矩阵方程求解出六个关节所要达到的最优位置和姿态；根据操作臂末端执行器的位置和姿态，计算八组可达所述位置和姿态的解，选择其中一组解作为最优关节角矢量；将所述连杆长度、所述最优关节角矢量、工具坐标系相对于基坐标系的位置和姿态以及所述关节偏移量代入算法，得到六自由度智能机器人的最终动作方程；根据所述操作臂末端执行器在所述基坐标系中的初始位置姿态和目标位置姿态，控制所述六自由度智能机器人本体输出各最优关节角矢量进行动作。上述控制方法能够简化算法，降低生产成本。
【专利说明】
-种六自由度智能机器人本体的控制方法
技术领域
[0001] 本发明属于智能机器人技术领域，特别是设及一种六自由度智能机器人本体的控 制方法。
【背景技术】
[0002] 当前，智能机器人是很令人关注的一个行业，目前市面上只有工业级的六轴机器 人，六轴机器人每个轴全部密封包裹，无法看见每个轴内部运动结构，也要求使用人员具有 足够专业的知识和技能。
[0003] 工业机器人的控制算法中，包括复杂的插补算法，还得考虑最末端的关节的加速 度、线速度和角速度，再加上工业机器人保护都很完善，比如说2台机器人在一个空间内运 行，怎样保证不撞机，怎样保证不撞人，运会设及到互相通讯、触碰传感急停等等子算法。
[0004] 综上所述，现有技术中的工业机器人由于算法极其复杂，导致其生产成本较高。
【发明内容】
[0005] 为解决上述问题，本发明提供了一种六自由度智能机器人本体的控制方法，能够 简化算法，降低生产成本。
[0006] 本发明提供的一种六自由度智能机器人本体的控制方法，包括：
[0007] 测量并计算六个轴的连杆长度、关节的当前角度和关节偏移量；
[000引通过矩阵方程求解出六个关节所要达到的最优位置和姿态；
[0009] 根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解，选择 其中一组解作为最优关节角矢量；
[0010] 将所述连杆长度、所述最优关节角矢量、工具坐标系相对于基坐标系的位置和姿 态W及所述关节偏移量代入算法，得到六自由度智能机器人的最终动作方程；
[0011] 根据所述操作臂末端执行器在所述基坐标系中的初始位置姿态和目标位置姿态， 利用所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关节角矢量进行动 作。
[0012] 优选的，在上述六自由度智能机器人本体的控制方法中，所述根据操作臂末端执 行器的位置和姿态，计算八组可达目标位置和姿态的解包括：
[0013] 从所述关节的当前位置进行线性插值，形成圆弧形状的路径，直到给定位置。
[0014] 优选的，在上述六自由度智能机器人本体的控制方法中，所述从所述关节的当前 位置进行线性插值，形成圆弧形状的路径还包括：
[0015] 先用线性运动插补，然后在每个路径点增加抛物线拟合区域，利用直线函数和两 个抛物线函数组合成位置与速度均连续的完整路径。
[0016] 优选的，在上述六自由度智能机器人本体的控制方法中，在所述测量并计算六个 轴的连杆长度、关节的当前角度和关节偏移量之前包括：
[0017] 第一轴、第四轴、第五轴和第六轴均包括电机、减速器和轴承，第二轴和第立轴包 括电机和减速器，且所述第二轴和所述第=轴之间利用连杆禪合。
[0018] 优选的，在上述六自由度智能机器人本体的控制方法中，所述利用所述最终动作 方程，控制所述六自由度智能机器人本体输出各最优关节角矢量进行动作包括：
[0019] 对电机参数自动识别，并对控制参数进行自动整定，针对不同的电机生成最优的 控制参数。
[0020] 优选的，在上述六自由度智能机器人本体的控制方法中，所述通过矩阵方程求解 出六个关节所要达到的最优的位置和姿态包括：
[0021] 各关节在寻找目标点的过程中速度和路径记忆并保持在系统软件中，作为保留 值。
[0022] 优选的，在上述六自由度智能机器人本体的控制方法中，所述根据操作臂末端执 行器的位置和姿态，计算八组可达目标位置和姿态的解，选择其中一组解作为最优关节角 矢量包括：
[0023] 利用正解的方法得到每个操作臂末端执行器的4*4齐次矩阵；
[0024] 利用反解的方法得到8组解；
[0025] 选择所述8组解中的一组最优解，作为所述关节的最优关节角矢量。
[0026] 优选的，在上述六自由度智能机器人本体的控制方法中，所述电机为步进电机、伺 服电机或直线电机。
[0027] 优选的，在上述六自由度智能机器人本体的控制方法中，所述利用反解的方法得 至化组解包括：
[002引利用双变量反正切函数得到8组解。
[0029] 本发明提供的上述六自由度智能机器人本体的控制方法，由于包括:测量并计算 六个轴的连杆长度、关节的当前角度和关节偏移量;通过矩阵方程求解出六个关节所要达 到的最优位置和姿态;根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿 态的解，选择其中一组解作为最优关节角矢量;将所述连杆长度、所述最优关节角矢量、工 具坐标系相对于基坐标系的位置和姿态W及所述关节偏移量代入算法，得到六自由度智能 机器人的最终动作方程;根据所述操作臂末端执行器在所述基坐标系中的初始位置姿态和 目标位置姿态，利用所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关 节角矢量进行动作，因此能够简化算法，降低生产成本。
【附图说明】
[0030] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本 发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可W根据 提供的附图获得其他的附图。
[0031] 图1为本申请实施例提供的第一种六自由度智能机器人本体的控制方法的示意 图。
【具体实施方式】
[0032] 本发明的核屯、思想在于提供一种六自由度智能机器人本体的控制方法，能够简化 算法，降低生产成本。
[0033] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0034] 本申请实施例提供的第一种六自由度智能机器人本体的控制方法如图1所示，图1 为本申请实施例提供的第一种六自由度智能机器人本体的控制方法的示意图。该方法包括 如下步骤：
[0035] SI:测量并计算六个轴的连杆长度、关节的当前角度和关节偏移量；
[0036] 机器人系统软件得到矩阵方程并解出一组最优解，就可W得到连杆长度和关节的 当前角度，相对于现有技术中的工业机器人将研究关节角的函数来描述轨迹(在空间、时 间、速度和加速度等）的生成方法，其关节角函数与其他关节函数无关，本申请实施例提供 的机器人的每个关节与其余关节都有关系，表现在第一是机械部分有关联，第二是内在的 函数和一些算法有关联。
[0037] S2:通过矩阵方程求解出六个关节所要达到的最优的位置和姿态；
[0038] 如果从基座到第一个关节，再从第一个关节到第二个关节直至到最后一个关节
日^吊'I的航右巧拖结会賠,本_就但至Il下扣毀A的6巧拖46晚_且化的晚市巧如下.
[0039]
[0040] 其中，对于六轴机器人，n = 5。
[0041] S3:根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解，选 择其中一组解作为最优关节角矢量；
[0042] 在该步骤中，首先为每个关节指定一个本地的参考坐标系，然后假设现在位于本 地的坐标系Xn-Zn,那么通过四步标准运动即可到达下一个本地坐标系Xn+1-Zn+l:旋转-平 移-平移-旋转。
[0043] 另外，用每个关节的运动函数，应用逆运动学可W解出对应于目标位置的各个关 节角。
[0044] S4:将所述连杆长度、所述最优关节角矢量、工具坐标系相对于基坐标系的位置和 姿态W及所述关节偏移量代入算法，得到六自由度智能机器人的最终动作方程；
[0045] S5:根据所述操作臂末端执行器在所述基坐标系中的初始位置姿态和目标位置姿 态，利用所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关节角矢量进 行动作。
[0046] 本申请实施例提供的上述六自由度智能机器人本体的控制方法，由于包括:测量 并计算六个轴的连杆长度、关节的当前角度和关节偏移量;通过矩阵方程求解出六个关节 所要达到的最优位置和姿态;根据操作臂末端执行器的位置和姿态，计算八组可达目标位 置和姿态的解，选择其中一组解作为最优关节角矢量;将所述连杆长度、所述最优关节角矢 量、工具坐标系相对于基坐标系的位置和姿态W及所述关节偏移量代入算法，得到六自由 度智能机器人的最终动作方程;根据所述操作臂末端执行器在所述基坐标系中的初始位置 姿态和目标位置姿态，利用所述最终动作方程，控制所述六自由度智能机器人本体输出各 最优关节角矢量进行动作，因此能够简化算法，降低生产成本。
[0047] 本申请实施例提供的第二种六自由度智能机器人本体的控制方法，是在上述第一 种控制方法的基础上，还包括如下技术特征：
[0048] 所述根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解包 括：
[0049] 从所述关节的当前位置进行线性插值，形成圆弧形状的路径，直到给定位置。
[0050] 具体的，传统的工业机器人如果要确定路径段起始点和终止点的位置、速度、加速 度，必须要求起始关节角速度和终止关节角速度均不为零，用一个五次多项式函数进行插 补才能达到运个效果，其属于函数插补。而本申请实施例提供的机器人属于线性运动插补， 比如6个关节同时从Pl点运动到P2点，进行线性运动插补后，运动路径是一条直线，插补算 法简单，也就是说，简单地从当前关节位置进行线性插值，直到终止位置，形成路径形状是 直线。
[0051] 本申请实施例提供的第=种六自由度智能机器人本体的控制方法，是在上述第二 种控制方法的基础上，还包括如下技术特征：
[0052] 所述从所述关节的当前位置进行线性插值，形成圆弧形状的路径还包括：
[0053] 先用线性运动插补，然后在每个路径点增加抛物线拟合区域，利用直线函数和两 个抛物线函数组合成位置与速度均连续的完整路径。
[0054] 具体的，直接进行线性插值将导致在起始点和终止点的关节运动速度不连续，为 了生成一条位置和速度都连续的平滑运动轨迹，先用线性运动插补，但需要在每个路径点 增加一段抛物线拟合区域，直线函数(一阶函数)和两个抛物线函数(二阶函数)组合成一条 完整的位置与速度均连续的路径。
[0055] 设to时刻的值为该关节的初始位置，在tf时刻的值为该关节的期望目标位置，有多 种光滑函数0(t)可用于对关节角进行线性插值。在本实施例提供的控制方法中，都预设初 始时刻和终止时刻关节速度为零。
[0化6]
[0化7]
[0化引化哩巧來朱巧唯一确定了一个立次多项式，目（t)=ao+ait+a2t2+a3t3;
[0化9] 对干该路轻的羊书巧底巧化速度计算公式
[0060]
[0061]
[0062] 解出方程，可W得到
[0063] ao=0(〇)
[0064] ai = 0
[00化]32 = 3/tf2*(目广白0)
[0066] a3 = -2/tf3*(目广白 0)
[0067] 运是起始和终止速度都为零的任何起始关节角位置到期望终止角位置的=次多 项式。
[0068] 本申请实施例提供的第四种六自由度智能机器人本体的控制方法，是在上述第= 种控制方法的基础上，还包括如下技术特征：
[0069] 在所述测量并计算六个轴的连杆长度、关节的当前角度和关节偏移量之前包括：
[0070] 第一轴、第四轴、第五轴和第六轴均包括电机、减速器和轴承，第二轴和第=轴包 括电机和减速器，且所述第二轴和所述第=轴之间利用连杆禪合。
[0071] 具体的，所述六自由度分别为^^2、扣^4、巧^6^1^4、巧、巧由一个步进电机、 一个减速器和一个轴承组成;J2和J3都由一个步进电机、一个减速器组成;其中J2和J3之间 通过连杆联接，实现二轴之间的禪合。
[0072] 本申请实施例提供的第五种六自由度智能机器人本体的控制方法，是在上述第四 种控制方法的基础上，还包括如下技术特征：
[0073] 所述利用所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关节 角矢量进行动作包括：
[0074] 对电机参数自动识别，并对控制参数进行自动整定，针对不同的电机生成最优的 控制参数。
[0075] 本申请实施例提供的第六种六自由度智能机器人本体的控制方法，是在上述第五 种控制方法的基础上，还包括如下技术特征：
[0076] 所述通过矩阵方程求解出六个关节所要达到的最优的位置和姿态包括：
[0077] 各关节在寻找目标点的过程中速度和路径记忆并保持在系统软件中，作为保留 值。
[0078] 传统的工业机器人对于一个路径生成算法而言，其用处之一就是满足用户指定的 期望速度。然而总是要求用户指定速度也是一个负担。本申请提供的机器人速度既可W指 定;也可W不指定，即通过手动寻找目标点（工件的具体位置），各关节在寻找目标点的过程 中速度和路径能自记忆并保持在系统软件中，自动运行时，速度和路径就是保留值。
[0079] 进一步的，在上述任一种六自由度智能机器人本体的控制方法中，所述根据操作 臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解，选择其中一组解作为最 优关节角矢量包括：
[0080] 利用正解的方法得到每个操作臂末端执行器的4*4齐次矩阵；
[0081 ]利用反解的方法得到8组解；
[0082] 选择所述8组解中的一组最优解，作为所述关节的最优关节角矢量。
[0083] 更进一步的，所述利用反解的方法得到8组解可W优选的包括：
[0084] 利用双变量反正切函数得到8组解。
[0085] 下面对本申请实施例所用的硬件进行说明如下：
[0086] 工控机硬件实现：工控机由CPU、内存、硬盘、USB接口和电源等组成，能独立运行 windows操作系统、各种编程软件和各种编译软件。
[0087] 各种运动控制程序的编写：编写各种动作(抓取、搬运、码煤、喷涂）的开源码算法 动态库和驱动动态库，验证正确之后，保存于工控机上，每种动作的开源码动态库都单独开 放接口给客户使用;动态库被调用后，就可W在工控机上运行，并生成最底层的C++语言，系 统软件输出C++语言编译后的指令给运动控制卡，使得运动控制卡能得到各种动作触发，然 后下达指令给机器人本体，运就体现了硬件的开源。
[0088] 电控箱硬件实现：内部含运动控制卡主板，I/O模块，手动编程器接口板(含控制电 缆），电源和六个步进电机驱动器。电控箱W运动控制卡为核屯、，运动控制卡得到动作触发 之后，输出信号给六个步进电机驱动器，驱动器驱动六轴机器人的本体所对应的步进电机 运动;运动控制卡没有存储单元，数据全部存储在工控机上。
[0089] 更进一步的，所述电机可W优选为步进电机、伺服电机或直线电机。
[0090] 本申请所用的六自由度智能机器人系统的离线编程与仿真软件，支持国内外工业 机器人主流厂商各种型号的机器人离线编程，动作仿真，运动轨迹及姿态规划与优化，防碰 撞测试，原来只兼容=菱设备，现在各种设备都可W兼容。另外，还可W提供仿真插件，仿真 系统兼容Sol idworks软件。
[0091] 上述系统全开源码，可W自由的进行底层开发，由于硬件和软件资源的对外开放， 极大地方便了机器人技术开发人员的技术交流及二次开发;不单提供了非常直接和丰富的 硬件接口，更重要的是一套非常浅显易懂而且内容丰富的软件库，许多现成的硬件设备都 有直接的代码样例，只需要复制和黏贴就可W使用；
[0092] 另外，硬件设计分为机械设计和电路设计两部分，软件设计分为驱动程序设计、控 制程序设计和智能算法开发，编译器设计部分主要工作分向上、向下两个方向：向下将程序 编译转换为兼容多种mcu的机器码，向上则兼容常见的多种开发语言，如：C/C++、VB、C#、 JAVA 等。
[0093] 对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。 对运些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的 一般原理可W在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明 将不会被限制于本文所示的运些实施例，而是要符合与本文所公开的原理和新颖特点相一 致的最宽的范围。
【主权项】
1. 一种六自由度智能机器人本体的控制方法，其特征在于，包括： 测量并计算六个轴的连杆长度、关节的当前角度和关节偏移量； 通过矩阵方程求解出六个关节所要达到的最优位置和姿态； 根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解，选择其中 一组解作为最优关节角矢量； 将所述连杆长度、所述最优关节角矢量、工具坐标系相对于基坐标系的位置和姿态以 及所述关节偏移量代入算法，得到六自由度智能机器人的最终动作方程； 根据所述操作臂末端执行器在所述基坐标系中的初始位置姿态和目标位置姿态，利用 所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关节角矢量进行动作。2. 根据权利要求1所述的六自由度智能机器人本体的控制方法，其特征在于，所述根据 操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解包括： 从所述关节的当前位置进行线性插值，形成圆弧形状的路径，直到给定位置。3. 根据权利要求2所述的六自由度智能机器人本体的控制方法，其特征在于，所述从所 述关节的当前位置进行线性插值，形成圆弧形状的路径还包括： 先用线性运动插补，然后在每个路径点增加抛物线拟合区域，利用直线函数和两个抛 物线函数组合成位置与速度均连续的完整路径。4. 根据权利要求3所述的六自由度智能机器人本体的控制方法，其特征在于，在所述测 量并计算六个轴的连杆长度、关节的当前角度和关节偏移量之前包括： 第一轴、第四轴、第五轴和第六轴均包括电机、减速器和轴承，第二轴和第三轴包括电 机和减速器，且所述第二轴和所述第三轴之间利用连杆耦合。5. 根据权利要求4所述的六自由度智能机器人本体的控制方法，其特征在于，所述利用 所述最终动作方程，控制所述六自由度智能机器人本体输出各最优关节角矢量进行动作包 括： 对电机参数自动识别，并对控制参数进行自动整定，针对不同的电机生成最优的控制 参数。6. 根据权利要求5所述的六自由度智能机器人本体的控制方法，其特征在于，所述通过 矩阵方程求解出六个关节所要达到的最优的位置和姿态包括： 各关节在寻找目标点的过程中速度和路径记忆并保持在系统软件中，作为保留值。7. 根据权利要求1-6任一项所述的六自由度智能机器人本体的控制方法，其特征在于， 所述根据操作臂末端执行器的位置和姿态，计算八组可达目标位置和姿态的解，选择其中 一组解作为最优关节角矢量包括： 利用正解的方法得到每个操作臂末端执行器的4*4齐次矩阵； 利用反解的方法得到8组解； 选择所述8组解中的一组最优解，作为所述关节的最优关节角矢量。8. 根据权利要求7所述的六自由度智能机器人本体的控制方法，其特征在于，所述电机 为步进电机、伺服电机或直线电机。9. 根据权利要求7所述的六自由度智能机器人本体的控制方法，其特征在于，所述利用 反解的方法得到8组解包括： 利用双变量反正切函数得到8组解。
【文档编号】G05D3/20GK105904462SQ201610362853
【公开日】2016年8月31日
【申请日】2016年5月27日
【发明人】阙正湘, 朱林
【申请人】湖南科瑞特科技股份有限公司