工业机器人操作空间路径复合限制求解方法
【技术领域】
[0001] 本发明涉及工业机器人技术领域，特别涉及一种工业机器人操作空间路径复合限 制求解方法。
【背景技术】
[0002] 工业机器人末端执行器的轨迹规划在工业机器人控制系统中占重要地位。轨迹规 划生成伺服驱动器的指令值，从而控制机器人各轴电机产生相应的运动，各轴的运动组成 末端执行器的操作空间的运动。若使得末端执行器产生用户期望的运动，如跟踪用户指定 的曲线，轨迹规划就必须满足伺服驱动器和电机的输入要求，主要体现为:轨迹规划的输出 不能使得各轴电机超速;不能使得各轴电机超出容许的最大力矩;不能使得各轴电机超过 伺服驱动器容许的加速度和加加速度。同时生成的操作空间运动也不能超过用户指定的最 大速度和最大加速度。因此，机器人的轨迹规划，作为规划问题而言，其约束条件是复合的。
[0003] 现有的轨迹规划方法通常将操作空间的运动映射到机器人的关节空间，映射方法 通常是先采样然后在关节空间应用样条曲线进行插补，用这样的方法将电机和伺服驱动器 的约束条件直接体现在关节空间，在根据这些条件进行规划。
[0004] 然而，现有的这种方法有如下缺点:将操作空间路径在关节空间用样条曲线表示， 不可避免会与原来的路径有所差别，特别是当采样点较大时，差别尤为明显。并且，这种方 法不能体现出操作空间的约束条件。
[0005] 专利CN 104742127A公开了一种机器人的控制方法及机器人，有效控制了机械臂 末端的运行轨迹，提高了机械臂末端的控制精度。但是，该技术方案仍是基于关节空间进行 路径规划，采用插补的形式约束机械臂末端的运行轨迹，这种方式不能体现出操作空间的 约束条件。
【发明内容】
[0006] 本发明的目的旨在至少解决所述技术缺陷之一。
[0007]为此，本发明的目的在于提出一种工业机器人操作空间路径复合限制求解方法， 能够避免样条曲线插补造成的路径跟踪误差，有效地求解关节空间和操作空间的多种限 制条件，将这些限制条件统一地用变换为路径参数s的限制，即将复合限制统一的表示出 来。
[0008] 为了实现上述目的，本发明一方面的实施例提供一种工业机器人操作空间路径复 合限制求解方法，包括如下步骤：
[0009] 步骤S1，设置工业机器人的操作空间路径，所述操作空间路径包括:平动部分路径 /H.0和旋转部分路径q(s);
[0010] 步骤S2,根据平动部分路径h.v)和旋转部分路径q(s)，计算所述工业机器人关节 空间的各轴的速度限制条件和加速度限制条件，根据所述各轴的速度限制条件和加速度限 制条件计算参数s的二阶导数的限制条件；
[0011]步骤S3,根据所述工业机器人的刚体动力学模型和旋转部分路径q(s)，计算所述 工业机器人的各轴力矩限制条件，并根据所述力矩限制条件计算参数S的一阶导数的限制 条件；
[0012]步骤S4,根据所述工业机器人的操作空间的六维速度旋量计算操作工件的速度限 制条件和加速度限制条件，根据所述速度限制条件和加速度限制条件计算对参数S的限制 条件；
[0013] 步骤S5,根据步骤S2、S3和S4中计算得到对参数S、参数S的一阶导数、参数S的二阶 导数的限制条件，将对所述工业机器人的复合限制条件统一制对路径参数s的限制条件的 计算上。
[0014] 进一步，在所述步骤S1中，
[0015]所述平动部分路径为三维欧式空间的一条曲线，用参数化几何曲线的形式来 表示如下：
[0017] 所述旋转部分路径q(s)采用参数化曲线的形式，以四元数表示工业机器人的末端 执行器的姿态
[0018] q(s)=slerp(qo，qi，s)，
[0019] 其中，s为路径参数，g平动部分的起点坐标，瓦为平动部分的终点坐标，qo为旋转 部分的起点坐标，qi为旋转部分的终点坐标。
[0020] 进一步，在所述步骤S2中，
[0021 ]设置所述工业机器人的关节空间和操作空间速度有雅克比矩阵映射关系如下：
[0022] y = Jq
[0023] 其中，v为操作空间的六维速度旋量，可以表示为:V = Vd ? sd，sd表示参数s的一阶 导数，
[0024] 计算得到各轴的速度限制可表示为： ? ittin ^ max
[0025] < a, (s)scl < ,k = \ ,2...dof
[0026] 设置关节空间和操作空间加速度的映射关系：《=^+^，$ =
[0027] 其中，笛卡尔加速度：a = + L、《/_\Sdd表示路径参数s的二阶导数，计算关 节空间各轴的加速度限制为： 丨_ ?? ??_
[0028] < < ,k = \,2...aof
[0029]当机器人沿某路径运动时，运动状态为（s，sd)，此时机器人可用的沿路径的加速 度sdd，需要满足下述不等式，求解该不等式，得到对参数S的二阶导数的限制条件。 .'min .....max ? - ?? -
[0030] -p, (s)sct < a, (s)sdd < -fi,(s)sct H k H k '
[0031] 其中，/：?々(S)=J 丨⑴ 士⑴-^/ VJCOJ '(.v)
[0032] 进一步，在所述步骤S3中，
[0033] 设置所述工业机器人的刚体动力学模型为：M(y)t/_fC(y,(/) + (，'((；/) = r，
[0034] 将参数s带入上式，可改写为参数s的形式，
[0035] (M(s)a(s))sdd+(M(s)(s)+C(s))sd2+G(s) = x
[0036]由于力矩约束条件为 ，则 a(s)sdd+b(S)Sd2 + c(SH〇;
[0037]上述不等式在（sd2，sdd)平面构成了一个可行域，通过一般的线性规划方法求得 各轴力矩限制对参数s的一阶导数的约束条件。
[0038] 进一步，在所述步骤S4中，
[0039] 设置操作空间的六维速度旋量为:v = Vd ? sd，每个速度分量的限制可以表示为v <Vmax，计算上述不等式得到操作空间速度分量的约束条件对参数s的一阶导数的限制条 件；
[0040] 计算平动速度大小的约束对参数s的限制条件为：|Vdp| ? scK |v|P max，旋转速度 大小的约束对参数S的限制条件为：| Vdr I ? SCK I V I r max，求解得到操作空间速度约束对路 径参数S的限制条件；
[0041 ]设置操作空间的六维加速度旋量的参数化形式如下：a = & . W ,每个加 速度分量的约束条件可以表示为c^amax，求解该不等式以得到操作空间每个方向的加速度 约束对参数s的限制条件。
[0042] 根据本发明实施例的工业机器人操作空间路径复合限制求解方法，结合关节空间 和操作空间约束条件对路径参数限制，将机器人轨迹规划所涉及的各种约束条件统一地体 现到操作空间的路径上，作为沿路径速度规划的输入，能够避免样条曲线插补造成的路径 跟踪误差，有效地求解关节空间和操作空间的多种限制条件，将这些限制条件统一地用变 换为路径参数s的限制，即将复合限制统一的表示出来，从而为精确的轨迹跟踪等应用提供 了先决条件。
[0043] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0044] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解，其中：
[0045] 图1为根据本发明实施例的工业机器人操作空间路径复合限制求解方法的流程 图；
[0046] 图2为根据本发明实施例的工业机器人操作空间路径复合限制求解方法的示意 图；
[0047] 图3为根据本发明实施例的生成的操作空间的一条圆弧轨迹的对于参数s的一阶 导数的复合限制的示意图；
[0048] 图4为根据本发明实施例的操作空间的路径示意图。
【具体实施方式】
[0049] 下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同 或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描 述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0050] 本发明提出一种工业机器人操作空间路径复合限制求解方法，该方法使用统一参 数对操作空间路径进行参数化，并将机器人复合限制条件统一到此参数上。
[0051] 如图1所示，本发明实施例的工业机器人操作空间路径复合限制求解方法，包括如 下步骤：
[0052] 步骤S1，设置工业机器人的操作空间路径，操作空间路径包括:平动部分路径》(.〇 和旋转部分路径q(s)。
[0053] 具体地，机器人的末端执行器可视为刚体，操作空间路径可视为刚体的运动路径。 其中，操作空间路径包括:平动部分路径/心)和旋转部分路径q(s)，两部分和在一起组成SE ⑶。
[0054] 在本步骤中，平动部分路径A.s")为三维欧式空间的一条曲线，用参数化几何曲线 的形式来表示。操作空间直线路径的平动部分可以表示为，起点坐标加上沿起点到终点的 方向增量的形式，如下所示：
[0056] 其中，s为从0到1的参数，当s从0变到1时直线从起点变到终点。
[0057] 旋转部分路径q(s)采用参数化曲线的形式，以四元数表示工业机器人的末端执行 器的姿态
[0058] q(s) = s 1 erp(qo, qi, s),
[0059] 其中，i平动部分的起点坐标，$为平动部分的终点坐标，qo为旋转部分的起点坐 标，qi为旋转部分的终点坐标，s为路径参数，从0到1的参数，slerp为标准的四元数线性插 补函数，当s从0变到1时末端执行器的姿态从初始姿态变化到终点姿态。所以，可以将工业 机器人操作空间路径表示为统一的参数化的形式P(s)。这样，工业机器人的多种限制条件 就可以转化为对s的约束条件。
[0060] 步骤S2,根据平动部分路径/;(.v)和旋转部分路径q(s)，计算工业机器人关节空间 的各轴的速度限制条件和加速度限制条件，根据各轴的速度限制条件和加速度限制条件计 算参数s的二阶导数的限制条件。（算法1)
[0061] 具体地，参考图2,设置工业机器人的关节空间和操作空间速度有雅克比矩阵映射 关系如下：
[0062] - Jq
[0063] 其中，v为操作空间的六维速度旋量，可以表示为:V = Vd ? sd，即速度方向（包括平 动和旋转)乘以速度大小s d表示参数s的一阶导数，
[0064] § - {J ) vt/ = a(.v) sit
[0065] 计算得到各轴的速度限制可表示为： min .max ? ?
[0066] < ak{s)sd < ，kd...(Jof '^'k h ，.
[0067] 得到2Xdof(自由度)个不等式，通过解这些方程就可以得到sd的取值范围。
[0068] 设置关节空间和操作空间加速度的映射关系：J +J c/，$二^ i(a _
[0069] 其中，笛卡尔加速度：《 = v，(, _ W + sdd表示路径参数s的二阶导数，即切向 加速度(切向方向乘sdd，sdd表示路径参数s的二阶导数)加上法向加速度(速度方向的导数 乘速度平方）。带入得到，
[0070] .§ = J .(、\! L + v" _、'd1 - J
[0071] 其中，a(s) =J，J =,/(.、'),?/，
[00