基于空间解析几何求解工业机器人中圆弧轨迹的方法

基于空间解析几何求解工业机器人中圆弧轨迹的方法
【专利摘要】本发明提出了一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，包括：示教得到工业机器人的圆弧轨迹的目标点的位置；根据目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；根据目标点的位置，采用矢量算法计算圆弧轨迹的圆心空间坐标O；根据圆心空间坐标和目标点的位置，计算圆弧轨迹的半径，并计算圆弧坐标系与基坐标系的齐次变换矩阵，以根据基坐标系和齐次变换矩阵计算圆弧坐标系；分别计算向量和，计算点积值，进而求解得到圆心角，根据圆弧长和圆心角的关系计算圆心角对应的圆弧长。本发明采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。
【专利说明】
基于空间解析几何求解工业机器人中圆弧轨迹的方法
技术领域
[0001] 本发明涉及工业机器人技术领域，特别涉及一种基于空间解析几何求解工业机器 人中圆弧轨迹的方法。
【背景技术】
[0002] 在工业机器人领域，一般都是采用示教方法进行轨迹规划。工业机器人示教过程 主要包括将工业机器人移动到几个要求的目标点，并把这些目标点的位置记录下来，存储 到控制系统的存储器中，然后根据目标点位置进行最优轨迹规划，定义相应的曲线轨迹类 型及轨迹过程中对应的关节旋转速度。当定义的曲线轨迹是圆弧时，对于空间几何再结合 实际通常做法来说，需要知道圆弧曲线轨迹的三个目标点:起点，中间点，终点。这样问题就 体现到如何根据空间任意三点判断圆弧轨迹是否可以生成，现在技术一般做法，通过空间 三点先求取圆弧圆心，再求圆半径等，而求圆心是圆弧轨迹生成的关键点。一般来说空间解 析几何求解比线性代数方程组求解更简单，计算量更小。
[0003] 目前空间三点求圆弧圆心的技术有以下几种方法：
[0004] 1、基础线性代数方程组解法。示教得到三个目标点(起点，中间点，终点）坐标(Xi， Yi，Zi)，其中i = l，2,3。根据空间三点确定的平面方程，结合三点到空间圆心坐标的距离相 等约束条件，可以得到圆心空间坐标的线性代数方程组，然后求解线性代数方程的解，求得 圆心空间坐标。
[0005] 2、矢量叉积和矩阵运算解法。该解法在已发表文章《叶伯生.机器人空间三点圆弧 功能的实现[J].华中科技大学学报：自然科学版，2007,35(8) :5-8.》中有详细阐述，先根据 三个目标点，构成相应的矢量，然后通过相应矢量叉积方法，结合矢量平行特性，再后面计 算又类似于基础线性代数方程组解法，通过矩阵求逆等运算方法，求得圆心空间坐标。
[0006] 3、矢量叉积和两条中垂线求交点解法。该解法在已发布文章《曾辉，柳贺.机器人 空间三点圆弧算法的研究与实现[J].中国新技术新产品，2014(12) :5-6.》中进行了详细论 述。该解法同样根据三个目标点，构成相应矢量，然后通过矢量叉积运算，得到三点构成的 空间平面的法向量，然后通过起点和中间点构成的矢量中垂线和中间点和终点构成的矢量 中垂线相交，而这两条中垂线的交点就是所求的圆弧的圆心。
[0007] 上述技术方式的主要缺陷与不足在于:求解过程复杂，线性代数方程组求解存在 矩阵求逆等等繁琐复杂的过程，运算量大，计算速度慢，耗时长。

【发明内容】

[0008] 本发明的目的旨在至少解决所述技术缺陷之一。
[0009]为此，本发明的目的在于提出一种基于空间解析几何求解工业机器人中圆弧轨迹 的方法，采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解 更快速简便。
[0010]为了实现上述目的，本发明的实施例提供一种基于空间解析几何求解工业机器人 中圆弧轨迹的方法，包括如下步骤：
[0011] 步骤Sl，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，所述目标点包 括:起点A、中间点B和终点C;
[0012] 步骤S2,根据所述目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执 行步骤S3,否则结束求解；
[0013] 步骤S3,根据所述目标点的位置，采用矢量算法计算所述圆弧轨迹的圆心空间坐 标〇;
[0014] 步骤S4,根据所述圆心空间坐标和所述目标点的位置，计算所述圆弧轨迹的半径 R，并计算所述圆弧坐标系与基坐标系的齐次变换矩阵，以根据所述基坐标系和所述齐次变 换矩阵计算所述圆弧坐标系；
[0015 ]步骤S 5，分别计算向量J和r,计算点积值0 s U:11. = S?r .，根据点积值得正负号判断 和？是否同向，进而求解得到圆心角Θ，根据圆弧长和圆心角的关系计算所述圆心角对应 的圆弧长，其中，? = 53 X _，？ = X
[0016] 进一步，在所述步骤S2中，
[0017] 计算矢量B与瓦，当瓦'为0时，则判断起点A和终点C重合，无法确定唯一的圆弧 轨迹，结束求解；
[0018] 当?与;^共线时，无法确定唯一的圆弧轨迹，结束求解；
[0019] 当通与双:'不共线时，确定唯一的圆弧轨迹，执行步骤S3。
[0020] 进一步，在所述步骤S3中，
[0021] 首先，计算中间参数t，
[0022]
[0023] 然后，根据中间参数t，计算所述圆弧轨迹的圆心相对于基坐标系的空间坐标：
[0024]
[0025] 其中，0为圆心点坐标、P为所述工业机器人的基坐标系的原点坐标、D为线段AB的 中点坐标、E为线段AC的中点坐标、=ix瓦
[0026] 进一步，在所述步骤S4中，
[0027] 首先，定义圆弧坐标系为：以圆心为坐标原点，圆心指向起点的向量为X轴，垂直于 圆弧平面的方向为z轴；
[0028] 然后，计算所述圆弧轨迹的半径R，r = ab.；3 (副）,其中，痴为由圆心指向起点 的矢量；
[0029]最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵CirCle_frame，其中，
[0030] Circle_frame =MFrame(Orient_matrix,Circle_center)
[0031 ] Orient_matrix为所述圆弧坐标系的旋转矩阵，Circle_center为圆心的空间坐 标。
[0032] 进一步，在所述步骤S5中，
[0033] 当result> = 0,弧长ABC> = JiR，则判断弧长对应的圆心角θ>180°，则求解得到的角 度即为圆心角Θ;
[0034] 当result〈0,弧长ABC〈JiR，则判断弧长对应的圆心角Θ小于180°，则求解得到的角 度即为(2π-θ)，其中，Θ为圆心角；
[0035] 根据圆弧长与圆心角的关系，计算圆心角Θ对应的圆弧长L = Q · R。
[0036] 根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法，基于 空间解析几何矢量，相对于现有技术中单一线性代数方程组解法或线性代数方程组合解析 几何矢量求解联合的方法，本发明采用解析几何矢量法求取空间三点圆心过程简单易懂， 并且计算复杂度更低，求解更快速简便。
[0037] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0038] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解，其中：
[0039] 图1为根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法 的流程图；
[0040] 图2为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为正值时圆弧轨 迹示意图；
[0041] 图3为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为负值时圆弧轨 迹示意图；
[0042]图4为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为零时圆弧轨迹 示意图；
[0043] 图5为根据本发明实施例的线段AC的中点E与所求圆心0点重合时所求圆弧轨迹示 意图；
[0044] 图6为根据本发明实施例的四种空间三点求圆心方法的运算量对比数据绘制曲线 图。
【具体实施方式】
[0045] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0046] 本发明提出一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，该方法主 要应用于工业机器人轨迹规划中。由于机器人实时工作，可能频繁地调用轨迹生成算法，采 用本发明可以快速简便生成圆弧轨迹。
[0047] 如图1所示，本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方 法，包括如下步骤：
[0048] 步骤Sl，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，目标点包括:起 点A、中间点B和终点C。
[0049] 步骤S2,根据目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步 骤S3,否则结束求解。
[0050] 具体地，计算矢量与/?Γ ,当,彳(1'为0时，则判断起点A和终点C重合，无法确定唯 一的圆弧轨迹，结束求解；
[0051 ]当崩与/!C共线时，无法确定唯一的圆弧轨迹，结束求解；
[0052]当与不共线时，确定唯一的圆弧轨迹，执行步骤S3。
[0053]步骤S3,根据目标点的位置，采用矢量算法计算圆弧轨迹的圆心空间坐标0。
[0054]首先，计算中间参数t。
[0055]具体地，设过A，B，C的圆弧轨迹的圆心为0,工业机器人的基坐标系的原点为P，D为 线段AB的中点，E为线段AC的中点，直线Ll为线段AB的中垂线，直线L2为线段AC的中垂线。 [0056]当AB与AC不共线时，根据几何学知识可以知道任意不共线相交的两条线段可以在 唯一确定的圆上，两条中垂线的交点为该圆的圆心〇,设同时垂直矢量通与^的法向量 ?.，即《 = d5x /iC ,
[0057] 如图2所示，根据三角形正弦定理，
[0058]
[0059] 其中，Θ为面与涵之间的夹角，β为涵与两之间的夹角，需求解出矢量M，其中
t为数值常量。
[0060]
[0061]
[0062] ?多内角和为：π，得到α = π-(β+θ)，其中 aSlS与〃/X)之间的夹角，三角变换有sinPsin9 = cosa+cos0cos0
最后等式的分子分母均为余弦项。
[0063] 下面根据圆心在基坐标系的不同位置，分别对t的求解进行说明。
[0064] (1)圆心坐标在z轴投影值为正值。圆心在Y轴正向与Z轴正向构成象限内，如图2所 示。ZW与矢量方向相反，所以t = -|t I，可以得到以下关系式：
[0065]

[0077]当E点和圆心0重合，不构成三角形。由于辰5与玩)不共线，对应的两条中垂线不共 线，所以求t的公式，分母不可能为零。
[0078] ( 2 )圆心坐标在z轴投影值为负值。如图3所示，圆心在Y轴正向与Z轴负向构 成象限内，而与_矢量方向一致，所以t = Itl。参考上述Z轴投影值为正值推导过 程，唯一变化的就是= "iZX) = - ￡b||"ZXi|cosa最后可以推导得到 -本类情况所求的计算结
THImJ 〇[0079] (3)圆心坐标在z轴投影值为零。当点C在Z轴上，即线段AC与Z轴共线。如图4所示， ： 〖况（1)求取得到t的表达式 JJq ~gQ
卜算t公式同样适用。同理当(3点 在Z轴负半轴时，圆心点0在Y轴正向，Z轴负向构成象限中，求t同样适用。
[0080] (4 )E点与圆心0点重合。当线段AC的中点E与所求圆心0点重合时，如附图5所示。公
I据中 间参数t，计算过空间三点的圆的圆心相对于基坐标系的空间坐标：
[0083] = - + t(nDO) ，
[0084] 其中，0为圆心点坐标、P为工业机器人的基坐标系的原点坐标、D为线段AB的中点 坐标、E为线段AC的中点坐标、= 7? X瓦.
[0085]步骤S4,根据圆心空间坐标和目标点的位置，计算圆弧轨迹的半径R，并计算圆弧 坐标系与基坐标系的齐次变换矩阵，以根据基坐标系和齐次变换矩阵计算圆弧坐标系。
[0086] 首先，定义圆弧坐标系为：以圆心为坐标原点，圆心指向起点的向量为X轴，垂直于 圆弧平面的方向为z轴。
[0087] 然后，计算圆弧轨迹的半径Rj = 其中，窗为由圆心指向起点的矢 量。
[0088] 具体地，计算圆弧所在平面的法向量Vector^= /彳βχ/iC，对Z轴进行归一化。再计 算S3 = 两，.其中，丽为圆心指向起点的矢量，则得到圆半径R=abs (S3)。
[0089] 首先，计算X轴方向的单位向量X = 0A/R，其长度为圆弧半径。然后计算Y = Z*X。
[0090] 计算圆弧坐标系旋转矩阵〇1^6111：_1]^1：1^1和圆心的空间坐标(^1'(316_061^61'(即， 圆弧坐标系的原点），其中，
[0091 ] Orient_matrix=MOrient(X,Y,Z),
[0092] Circle-.cent .er =.代^ 〇
[0093] 最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵CirCle_frame，其中，
[0094] Circle_frame =MFrame(Orient_matrix,Circle_center)
[0095] Orient_matrix为圆弧坐标系的旋转矩阵，Circle_center为圆心的空间坐标。
[0096] 步骤S5,分别计算向量:；；和？,计算点积值而Wt, resuk =「w:,根据点积值得正负号 判断孓和P是否同向，进而求解得到圆心角Θ，根据圆弧长和圆心角的关系计算圆心角对应 的圆弧长，其中，? =品X 7· = X 7Γ'
[0097] 当result> = 0,弧长ABC> = JiR，则判断弧长对应的圆心角Θ> 180°，则求解得到的角 度即为圆心角Θ;
[0098] 当result〈0,弧长ABC〈JiR，则判断弧长对应的圆心角Θ小于180°，则求解得到的角 度即为(2π-θ)，其中，Θ为圆心角；
[0099] 根据圆弧长与圆心角的关系，计算圆心角Θ对应的圆弧长L = Q · R。
[0100] 下面参考表1和图6对现有技术中的三种方法和本发明的方法的运算量进行比对。
[0102] 表1
[0103] 图6为四种空间三点求圆心方法的运算量对比数据绘制曲线图。其中，A表示乘法 运算次数，B表示加法运算次数；1表示基本线性方程组解法，2表示矢量叉积和矩阵运算解 法，3表示矢量叉积和两条中垂线求交点解法，4表示本发明的矢量叉积和点积解法。
[0104] 通过表1和图6,可以获知前三种现有技术计算空间三点求圆心的运算量相对较 大，本发明的基于解析几何矢量的方法运算量最小。
[0105] 根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法，基于 空间解析几何矢量，相对于现有技术中单一线性代数方程组解法或线性代数方程组合解析 几何矢量求解联合的方法，本发明采用解析几何矢量法求取空间三点圆心过程简单易懂， 并且计算复杂度更低，求解更快速简便。
[0106] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何 的一个或多个实施例或示例中以合适的方式结合。
[0107] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围 由所附权利要求极其等同限定。
【主权项】
1. 一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，包括如下 步骤： 步骤S1，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，所述目标点包括:起 点A、中间点B和终点C; 步骤S2,根据所述目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步 骤S3,否则结束求解； 步骤S3,根据所述目标点的位置，采用矢量算法计算所述圆弧轨迹的圆屯、空间坐标0; 步骤S4,根据所述圆屯、空间坐标和所述目标点的位置，计算所述圆弧轨迹的半径R，并 计算所述圆弧坐标系与基坐标系的齐次变换矩阵，W根据所述基坐标系和所述齐次变换矩 阵计算所述圆弧坐标系； 步骤S5,分别计算向量;和？，计算点积值= 根据点积值得正负号判断i和？ 是否同向，进而求解得到圆屯、角9，根据圆弧长和圆屯、角的关系计算所述圆屯、角对应的圆弧 长，其中，；二雨 X 沉=2. 如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征 在于，在所述步骤S2中， 计算矢量才&与雨，当;??'为0时，则判断起点A和终点C重合，无法确定唯一的圆弧轨 迹，结束求解； 当克§;与；?共线时，无法确定唯一的圆弧轨迹，结束求解； 当与JC"不共线时，确定唯一的圆弧轨迹，执行步骤S3。3. 如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征 在于，在所述步骤S3中， 首先，计算中间参数t，然后，根据中间参数t，计算所述圆弧轨迹的圆屯、相对于基坐标系的空间坐标：其中，0为圆屯、点坐标、P为所述工业机器人的基坐标系的原点坐标、D为线段AB的中点 坐标、E为线段AC的中点坐标、;二而X杰4. 如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征 在于，在所述步骤S4中， 首先，定义圆弧坐标系为圆屯、为坐标原点，圆屯、指向起点的向量为X轴，垂直于圆弧 平面的方向为Z轴； 然后，计算所述圆弧轨迹的半径R，E二absi頑)，其中，窥为由圆屯、指向起点的矢 量； 最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵Circlejrame，其中， Circle_frame=MFrame(Orient_matrix,Circle_center) 化ientjnatrix为所述圆弧坐标系的旋转矩阵，Ci;rcle_cente;r为圆屯、的空间坐标。5.如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征 在于，在所述步骤S5中， 当re suit〉= 0,弧长ABC〉=地，则判断弧长对应的圆屯、角目〉180°，则求解得到的角度即 为圆屯、角白； 当result<0，弧长ABC<地，则判断弧长对应的圆屯、角目小于180°，则求解得到的角度即 为(23Ι-Θ)，其中，Θ为圆屯、角； 根据圆弧长与圆屯、角的关系，计算圆屯、角9对应的圆弧长L=0 . R。
【文档编号】G05B19/19GK105843162SQ201610151494
【公开日】2016年8月10日
【申请日】2016年3月16日
【发明人】庹华, 陶茂生, 宋斌
【申请人】珞石（北京）科技有限公司