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

Abstract

本发明提出了一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，包括：示教得到工业机器人的圆弧轨迹的目标点的位置；根据目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；根据目标点的位置，采用矢量算法计算圆弧轨迹的圆心空间坐标O；根据圆心空间坐标和目标点的位置，计算圆弧轨迹的半径，并计算圆弧坐标系与基坐标系的齐次变换矩阵，以根据基坐标系和齐次变换矩阵计算圆弧坐标系；分别计算向量和，计算点积值，进而求解得到圆心角，根据圆弧长和圆心角的关系计算圆心角对应的圆弧长。本发明采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。

Description

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

技术领域

本发明涉及工业机器人技术领域，特别涉及一种基于空间解析几何求解工业机器人中圆弧轨迹的方法。

背景技术

在工业机器人领域，一般都是采用示教方法进行轨迹规划。工业机器人示教过程主要包括将工业机器人移动到几个要求的目标点，并把这些目标点的位置记录下来，存储到控制系统的存储器中，然后根据目标点位置进行最优轨迹规划，定义相应的曲线轨迹类型及轨迹过程中对应的关节旋转速度。当定义的曲线轨迹是圆弧时，对于空间几何再结合实际通常做法来说，需要知道圆弧曲线轨迹的三个目标点：起点，中间点，终点。这样问题就体现到如何根据空间任意三点判断圆弧轨迹是否可以生成，现在技术一般做法，通过空间三点先求取圆弧圆心，再求圆半径等，而求圆心是圆弧轨迹生成的关键点。一般来说空间解析几何求解比线性代数方程组求解更简单，计算量更小。

目前空间三点求圆弧圆心的技术有以下几种方法：

1、基础线性代数方程组解法。示教得到三个目标点(起点，中间点，终点)坐标(Xi,Yi,Zi)，其中i＝1,2,3。根据空间三点确定的平面方程，结合三点到空间圆心坐标的距离相等约束条件，可以得到圆心空间坐标的线性代数方程组，然后求解线性代数方程的解，求得圆心空间坐标。

2、矢量叉积和矩阵运算解法。该解法在已发表文章《叶伯生.机器人空间三点圆弧功能的实现[J].华中科技大学学报:自然科学版,2007,35(8):5-8.》中有详细阐述，先根据三个目标点，构成相应的矢量，然后通过相应矢量叉积方法，结合矢量平行特性，再后面计算又类似于基础线性代数方程组解法，通过矩阵求逆等运算方法，求得圆心空间坐标。

3、矢量叉积和两条中垂线求交点解法。该解法在已发布文章《曾辉,柳贺.机器人空间三点圆弧算法的研究与实现[J].中国新技术新产品,2014(12):5-6.》中进行了详细论述。该解法同样根据三个目标点，构成相应矢量，然后通过矢量叉积运算，得到三点构成的空间平面的法向量，然后通过起点和中间点构成的矢量中垂线和中间点和终点构成的矢量中垂线相交，而这两条中垂线的交点就是所求的圆弧的圆心。

上述技术方式的主要缺陷与不足在于：求解过程复杂，线性代数方程组求解存在矩阵求逆等等繁琐复杂的过程，运算量大，计算速度慢，耗时长。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。

为了实现上述目的，本发明的实施例提供一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，包括如下步骤：

步骤S1，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，所述目标点包括：起点A、中间点B和终点C；

步骤S2，根据所述目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；

步骤S3，根据所述目标点的位置，采用矢量算法计算所述圆弧轨迹的圆心空间坐标O；

步骤S4，根据所述圆心空间坐标和所述目标点的位置，计算所述圆弧轨迹的半径R，并计算所述圆弧坐标系与基坐标系的齐次变换矩阵，以根据所述基坐标系和所述齐次变换矩阵计算所述圆弧坐标系；

步骤S5，分别计算向量和计算点积值根据点积值得正负号判断和是否同向，进而求解得到圆心角θ，根据圆弧长和圆心角的关系计算所述圆心角对应的圆弧长，其中，

进一步，在所述步骤S2中，

计算矢量与当为0时，则判断起点A和终点C重合，无法确定唯一的圆弧轨迹，结束求解；

当与共线时，无法确定唯一的圆弧轨迹，结束求解；

当与不共线时，确定唯一的圆弧轨迹，执行步骤S3。

进一步，在所述步骤S3中，

首先，计算中间参数t，

$t=\frac{ED\_EO\×EO\_DO-ED\_DO\×EO\_EO}{EO\_EO\×DO\_DO-EO\_DO\×EO\_DO},$

然后，根据中间参数t，计算所述圆弧轨迹的圆心相对于基坐标系的空间坐标：

其中，O为圆心点坐标、P为所述工业机器人的基坐标系的原点坐标、D为线段AB的中点坐标、E为线段AC的中点坐标、

进一步，在所述步骤S4中，

首先，定义圆弧坐标系为：以圆心为坐标原点，圆心指向起点的向量为x轴，垂直于圆弧平面的方向为z轴；

然后，计算所述圆弧轨迹的半径R，其中，为由圆心指向起点的矢量；

最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵Circle_frame，其中，

Circle_frame＝MFrame(Orient_matrix,Circle_center)

Orient_matrix为所述圆弧坐标系的旋转矩阵，Circle_center为圆心的空间坐标。

进一步，在所述步骤S5中，

当result>＝0，弧长ABC>＝πR，则判断弧长对应的圆心角θ>180°，则求解得到的角度即为圆心角θ；

当result<0，弧长ABC<πR，则判断弧长对应的圆心角θ小于180°，则求解得到的角度即为(2π-θ)，其中，θ为圆心角；

根据圆弧长与圆心角的关系，计算圆心角θ对应的圆弧长L＝θ·R。

根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法，基于空间解析几何矢量，相对于现有技术中单一线性代数方程组解法或线性代数方程组合解析几何矢量求解联合的方法，本发明采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法的流程图；

图2为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为正值时圆弧轨迹示意图；

图3为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为负值时圆弧轨迹示意图；

图4为根据本发明实施例的空间三点所求圆心坐标在z轴投影值为零时圆弧轨迹示意图；

图5为根据本发明实施例的线段AC的中点E与所求圆心O点重合时所求圆弧轨迹示意图；

图6为根据本发明实施例的四种空间三点求圆心方法的运算量对比数据绘制曲线图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明提出一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，该方法主要应用于工业机器人轨迹规划中。由于机器人实时工作，可能频繁地调用轨迹生成算法，采用本发明可以快速简便生成圆弧轨迹。

如图1所示，本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法，包括如下步骤：

步骤S1，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，目标点包括：起点A、中间点B和终点C。

步骤S2，根据目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解。

具体地，计算矢量与当为0时，则判断起点A和终点C重合，无法确定唯一的圆弧轨迹，结束求解；

当与共线时，无法确定唯一的圆弧轨迹，结束求解；

当与不共线时，确定唯一的圆弧轨迹，执行步骤S3。

步骤S3，根据目标点的位置，采用矢量算法计算圆弧轨迹的圆心空间坐标O。

首先，计算中间参数t。

具体地，设过A,B,C的圆弧轨迹的圆心为O，工业机器人的基坐标系的原点为P，D为线段AB的中点，E为线段AC的中点，直线L1为线段AB的中垂线，直线L2为线段AC的中垂线。

当AB与AC不共线时，根据几何学知识可以知道任意不共线相交的两条线段可以在唯一确定的圆上，两条中垂线的交点为该圆的圆心O，设同时垂直矢量与的法向量即

如图2所示，根据三角形正弦定理，

其中，θ为与之间的夹角，β为与之间的夹角，需求解出矢量其中由得到并且有如下关系t为数值常量。

综上可得，

由于已知，需求出比值以得到t。

考虑根据三角形内角和为π，得到α＝π-(β+θ)，其中α为与之间的夹角，三角变换有sinβsinθ＝cosα+cosβcosθ，最后等式的分子分母均为余弦项。

下面根据圆心在基坐标系的不同位置，分别对t的求解进行说明。

(1)圆心坐标在z轴投影值为正值。圆心在Y轴正向与Z轴正向构成象限内，如图2所示。与矢量方向相反，所以t＝-|t|，可以得到以下关系式：

根据式(1)～(5)得到以下关系式：

将(6)、(7)、(8)公式代入可以得到：

经过化简得到：

由得到：

$t=-\left|t\right|=\frac{ED\_EO\&times;EO\_DO-ED\_DO\&times;EO\_EO}{EO\_EO\&times;DO\_DO-EO\_DO\&times;EO\_DO},$

当E点和圆心0重合，不构成三角形。由于与不共线，对应的两条中垂线不共线，所以求t的公式，分母不可能为零。

(2)圆心坐标在z轴投影值为负值。如图3所示，圆心在Y轴正向与Z轴负向构成象限内，与矢量方向一致，所以t＝|t|。参考上述z轴投影值为正值推导过程，唯一变化的就是最后可以推导得到

本类情况所求的计算结果与第一类情况(1)求取得到t的表达式相同。

(3)圆心坐标在z轴投影值为零。当点C在Z轴上，即线段AC与Z轴共线。如图4所示，cosθ＝0，此时计算t公式同样适用。同理当C点在Z轴负半轴时，圆心点O在Y轴正向，Z轴负向构成象限中，求t同样适用。

(4)E点与圆心O点重合。当线段AC的中点E与所求圆心O点重合时，如附图5所示。公式同样适用。

$t=\frac{ED\_EO\&times;EO\_DO-ED\_DO\&times;EO\_EO}{EO\_EO\&times;DO\_DO-EO\_DO\&times;EO\_DO}.$

过三点的圆的圆心坐标根据中间参数t，计算过空间三点的圆的圆心相对于基坐标系的空间坐标：

其中，O为圆心点坐标、P为工业机器人的基坐标系的原点坐标、D为线段AB的中点坐标、E为线段AC的中点坐标、

步骤S4，根据圆心空间坐标和目标点的位置，计算圆弧轨迹的半径R，并计算圆弧坐标系与基坐标系的齐次变换矩阵，以根据基坐标系和齐次变换矩阵计算圆弧坐标系。

首先，定义圆弧坐标系为：以圆心为坐标原点，圆心指向起点的向量为x轴，垂直于圆弧平面的方向为z轴。

然后，计算圆弧轨迹的半径R，其中，为由圆心指向起点的矢量。

具体地，计算圆弧所在平面的法向量Vector对Z轴进行归一化。再计算其中，为圆心指向起点的矢量，则得到圆半径

首先，计算X轴方向的单位向量X＝OA/R，其长度为圆弧半径。然后计算Y＝Z*X。

计算圆弧坐标系旋转矩阵Orient_matrix和圆心的空间坐标Circle_center(即，圆弧坐标系的原点)，其中，

Orient_matrix＝MOrient(X,Y,Z)，

最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵Circle_frame，其中，

Circle_frame＝MFrame(Orient_matrix,Circle_center)

Orient_matrix为圆弧坐标系的旋转矩阵，Circle_center为圆心的空间坐标。

步骤S5，分别计算向量和计算点积值根据点积值得正负号判断和是否同向，进而求解得到圆心角θ，根据圆弧长和圆心角的关系计算圆心角对应的圆弧长，其中，

当result>＝0，弧长ABC>＝πR，则判断弧长对应的圆心角θ>180°，则求解得到的角度即为圆心角θ；

当result<0，弧长ABC<πR，则判断弧长对应的圆心角θ小于180°，则求解得到的角度即为(2π-θ)，其中，θ为圆心角；

根据圆弧长与圆心角的关系，计算圆心角θ对应的圆弧长L＝θ·R。

下面参考表1和图6对现有技术中的三种方法和本发明的方法的运算量进行比对。

空间给定三点求圆心方法	乘法运算次数	加法运算次数
			方法1.基本线性方程组解法	93	50
方法2.矢量叉积和矩阵运算解法	105	82
			方法3.矢量叉积和两条中垂线求交点解法	53	48
方法4.矢量叉积和点积(本发明专利)解法	42	40

表1

图6为四种空间三点求圆心方法的运算量对比数据绘制曲线图。其中，A表示乘法运算次数，B表示加法运算次数；1表示基本线性方程组解法，2表示矢量叉积和矩阵运算解法，3表示矢量叉积和两条中垂线求交点解法，4表示本发明的矢量叉积和点积解法。

通过表1和图6，可以获知前三种现有技术计算空间三点求圆心的运算量相对较大，本发明的基于解析几何矢量的方法运算量最小。

根据本发明实施例的基于空间解析几何求解工业机器人中圆弧轨迹的方法，基于空间解析几何矢量，相对于现有技术中单一线性代数方程组解法或线性代数方程组合解析几何矢量求解联合的方法，本发明采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (5)

1.一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，包括如下步骤：

步骤S1，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，所述目标点包括：起点A、中间点B和终点C；

步骤S2，根据所述目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；

步骤S3，根据所述目标点的位置，采用矢量算法计算所述圆弧轨迹的圆心空间坐标O；

步骤S4，根据所述圆心空间坐标和所述目标点的位置，计算所述圆弧轨迹的半径R，并计算所述圆弧坐标系与基坐标系的齐次变换矩阵，以根据所述基坐标系和所述齐次变换矩阵计算所述圆弧坐标系；

步骤S5，分别计算向量和计算点积值根据点积值得正负号判断和是否同向，进而求解得到圆心角θ，根据圆弧长和圆心角的关系计算所述圆心角对应的圆弧长，其中，

2.如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，在所述步骤S2中，

计算矢量与当为0时，则判断起点A和终点C重合，无法确定唯一的圆弧轨迹，结束求解；

当与共线时，无法确定唯一的圆弧轨迹，结束求解；

当与不共线时，确定唯一的圆弧轨迹，执行步骤S3。

3.如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，在所述步骤S3中，

首先，计算中间参数t，

$t=\frac{ED\_EO\&times;EO\_DO-ED\_DO\&times;EO\_EO}{EO\_EO\&times;DO\_DO-EO\_DO\&times;EO\_DO},$

然后，根据中间参数t，计算所述圆弧轨迹的圆心相对于基坐标系的空间坐标：

其中，O为圆心点坐标、P为所述工业机器人的基坐标系的原点坐标、D为线段AB的中点坐标、E为线段AC的中点坐标、

4.如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，在所述步骤S4中，

首先，定义圆弧坐标系为：以圆心为坐标原点，圆心指向起点的向量为x轴，垂直于圆弧平面的方向为z轴；

然后，计算所述圆弧轨迹的半径R，其中，为由圆心指向起点的矢量；

最后，计算圆弧坐标系与基坐标系之间的齐次变换矩阵Circle_frame，其中，

Circle_frame＝MFrame(Orient_matrix,Circle_center)

Orient_matrix为所述圆弧坐标系的旋转矩阵，Circle_center为圆心的空间坐标。

5.如权利要求1所述的基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，在所述步骤S5中，

当result>＝0，弧长ABC>＝πR，则判断弧长对应的圆心角θ>180°，则求解得到的角度即为圆心角θ；

当result<0，弧长ABC<πR，则判断弧长对应的圆心角θ小于180°，则求解得到的角度即为(2π-θ)，其中，θ为圆心角；

根据圆弧长与圆心角的关系，计算圆心角θ对应的圆弧长L＝θ·R。