CN106041932A

CN106041932A - 一种ur机器人的运动控制方法

Info

Publication number: CN106041932A
Application number: CN201610518479.7A
Authority: CN
Inventors: 吴磊; 关日钊; 陈新度; 罗坚铭; 杨宗泉; 柯科勇
Original assignee: Guangdong University of Technology
Current assignee: Guangdong University of Technology
Priority date: 2016-07-01
Filing date: 2016-07-01
Publication date: 2016-10-26
Anticipated expiration: 2036-07-01
Also published as: CN106041932B

Abstract

本发明公开了一种UR机器人的运动控制方法，包括：建立UR机器人连杆坐标系，获得UR机器人七根杆件和六个轴之间的空间位置关系；获取UR机器人对应的G指令文件后，读取UR机器人目标运动位置；根据该目标运动位置以及UR机器人几何关系，获取UR机器人的DH参数表后，依次计算UR机器人六个轴的关节角度；分别将计算出的六个关节角度转化成每个轴对应的运动脉冲数后，发送到UR机器人的驱动器；驱动器根据接收的每个轴对应的运动脉冲数，依次驱动对应的电机带动相应的轴进行运动。本发明基于几何关系，对UR机器人进行运动逆解，从而实现对UR机器人的运动控制，运算速度快，计算效率高且控制效率高，可广泛应用于UR机器人的控制行业中。

Description

一种UR机器人的运动控制方法

技术领域

本发明涉及机器人的控制领域，特别是涉及一种UR机器人的运动控制方法。

背景技术

名词解释：

UR机器人：Universal Robot，由丹麦UR公司研发出来的机器人。

随着科技的发展，工业机器人在各种生产场所得到了较为广泛的应用，在制造、安装、检测或物流等领域中应用广泛。采用工业机器人替代人工作业，不仅可以提高劳动生产率，还可以达到更高的操作精度，从而提高产品的生产质量，总体上也降低了生产成本。

工业机器人的运动学是工业机器人控制与轨迹规划的基础，其内容包括正运动学和逆运动学。当给定机器人所有关节转过的角度时，可以通过机器人的正动学方程来确定其末端操作器的位解；当已知机器人末端所需要到达的空间位置时，则可根据运动学逆解获得机器人末端能够到达该位置下的各关节角的大小。

UR机器人(UR机器人)是一款轻量级的柔性工业机器人，一向以编程的简易性、以及与人类一起工作的协作性和安全可靠性享誉业内，随着UR3的推出，UR机器人将其独特的技术扩展到了更多的应用领域，同时可立即受益于机器人装配和生产自动化的企业的数量和类型也大大增加。目前对UR机器人的运动控制过程中，进行运动轨迹规划主要是靠示教编程来实现，由人工引导机器人末端执行器，对UR机器人进行编程，使机器人完成预期的动作。这种方式，UR机器人运动轨迹的准确性和合理性往往取决于人的技术经验，从而导致机器人运动轨迹的可靠性得不到保证。为了保证UR机器人运动轨迹的准确性，操作人员往往需要花费大量的体力和时间来不停检验运动轨迹的可靠性和准测性，因此这种方式过于依赖操作人员的技术经验，难以准确、有效率地对UR机器人进行运动轨迹规划。

发明内容

为了解决上述的技术问题，本发明的目的是提供一种UR机器人的运动控制方法。

本发明解决其技术问题所采用的技术方案是：

一种UR机器人的运动控制方法，包括步骤：

S1、建立UR机器人连杆坐标系，获得UR机器人七根杆件和六个轴之间的空间位置关系；

S2、获取UR机器人对应的G指令文件后，读取UR机器人目标运动位置；

S3、根据该目标运动位置以及UR机器人几何关系，获取UR机器人的DH参数表后，依次计算UR机器人六个轴的关节角度；

S4、分别将计算出的六个关节角度转化成每个轴对应的运动脉冲数后，发送到UR机器人的驱动器；

S5、驱动器根据接收的每个轴对应的运动脉冲数，依次驱动对应的电机带动相应的轴进行运动。

进一步，所述步骤S1，其具体为：

建立UR机器人的连杆坐标系，获得UR机器人的七根杆件和六个轴之间的空间位置关系，七根杆件的长度分别为L₁、L₂、L₃、L₄、L₅、L₆和L₇，六个轴的关节角度分别为θ₁、θ₂、θ₃、θ₄、θ₅和θ₆，并将经过其关节五的Z轴且垂直于其第二关节的Z轴的平面作为UR机器人的计算参考平面。

进一步，所述步骤S2中所述UR机器人的目标运动位置包括UR机器人的姿态和UR机器人的位置矢量。

进一步，所述步骤S3，包括：

S31、获取目标运动位置的坐标(P_x,P_y,P_z)，同时获取UR机器人的DH参数表后计算获得UR机器人的腕关节点的坐标(P_hx,P_hy,P_hz)；

S32、计算关节点二与关节点六之间的连线在世界坐标系XOY平面上的投影的长度；

S33、判断该投影的长度是否等于基准投影的长度，若是，则执行步骤S34，反之，执行步骤S35；

S34、获得θ₂＝θ₃＝θ₄＝90°，根据UR机器人的空间位置关系依次计算获得关节角度θ₁和θ₅，进而获取目标运动位置的姿态[n o a]后计算关节角度θ₆并结束；

S35、计算该投影与基准投影之间的夹角，并获取目标运动位置的姿态[n o a]后，结合该夹角以及UR机器人的空间位置关系依次计算获得关节角度θ₁、θ₅、θ₂、θ₃、θ₄和θ₆；

其中，所述基准投影指关节点二与关节点六之间的连线在关节点二的X轴与关节点三的X轴之间的公法线上的投影，n、o和a分别表示目标运动位置的法向矢量、方位矢量和接近矢量，且n＝o×a，P_hx、P_hy和P_hz分别表示腕关节点的X轴、Y轴和Z轴坐标值。

进一步，所述步骤S31中所述UR机器人的腕关节点的坐标是通过下式计算获得的：

P_h＝transMatrix×P₀

上式中，P_h表示UR机器人的腕关节点的坐标(P_hx,P_hy,P_hz)，P₀表示UR机器人末端的目标运动位置的坐标(P_x,P_y,P_z)，transMatrix为UR机器人的转换矩阵，满足下式：

t r a n s M a t r i x = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D \\ 0 & 0 & 0 & 1 \end{matrix}];

D表示DH参数表中的参数。

进一步，所述步骤S32，其具体为：

根据下式，计算关节点二与关节点六之间的连线在世界坐标系XOY平面上的投影的长度：

L = \sqrt{P_{h x}^{2} + P_{h y}^{2}}

上式中，L表示关节点二与关节点六之间的连线在世界坐标系XOY平面上的投影的长度。

进一步，所述步骤S34，具体包括：

S341、获得θ₂＝θ₃＝θ₄＝90°，并根据下式计算UR机器人的关节角度θ₁：

θ_{1} = a t a n 2 (P_{h y}, P_{h x}) + \frac{π}{2}

S342、根据下式，计算UR机器人的关节角度θ₅：

θ₅＝Angle<Vector,Z₆>

上式中，Vector表示第一过程向量且Vector＝(P_hx,P_hy,0)，Z₆表示UR机器人的第六轴的杆件坐标系的Z轴方向向量，且Z₆＝-a，且θ₅的方向是根据以下条件确定的：根据下式计算获得第一临时角度tempAngle，当tempAngle＝0时，θ₅为负，当tempAngle＝π时，θ₅为正：

\{\begin{matrix} t e m p A n g l e = A n g l e < V e c t o r 1, V e c t o r 2 > \\ V e c t o r 1 = V e c t o r \times Z_{6} \\ V e c t o r 2 = (0, 0, 1) \end{matrix}

其中，Vector1表示第二过程向量，Vector2表示世界坐标系的Z轴方向向量；

S343、获取UR机器人的目标运动位置的姿态[n o a]后，根据下式计算UR机器人的关节角度θ₆：

\{\begin{matrix} θ_{6} = A n g l e < X_{6}, n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \\ X_{6} = m a t r i x \times V e c t o r (1, 0, 0) \end{matrix}

上式中，matrix表示一变换矩阵，T₁、T₂、T₃、T₄、T₅、T₆依次表示UR机器人的第一、第二、第三、第四、第五、第六关节的DH矩阵，X₆表示UR机器人的第六轴的杆件坐标系的X轴方向向量，且θ₆的方向是根据以下条件确定的：根据下式计算获得VectorX，当VectorX＝n时，θ₆为正，否则，θ₆为负：

VectorX＝matrix×rot(θ₆)×Vector(1,0,0)

上式中，VectorX表示第三过程向量。

进一步，所述步骤S35，包括：

S350、根据下式计算该投影与基准投影之间的夹角：

θ＝arccos(L_R/L)

上式中，θ表示该投影与基准投影之间的夹角，L_R表示基准投影的长度，L表示关节点二与关节点六之间的连线在计算参考平面上的投影的长度；

S351、获取目标运动位置的姿态[n o a]后，根据下式计算UR机器人的关节角度θ₁：

θ_{1} = a t a n 2 (V_{2 y}, V_{2 x}) + \frac{π}{2}

其中，V_2x、V_2y是Vec2的X坐标和Y坐标，Vec2表示第四过程向量，是通过下式计算获得的：

Vec2＝Vec1'*L_R

其中，Vec1'表示Vec1的单位化向量，Vec1表示第五过程向量，且：

Vec1＝rot(θ)×Vector(P_hx,P_hy,0)

S352、获取目标运动位置的姿态[n o a]后，根据下式计算UR机器人的关节角度θ₅：

θ₅＝Angle<Vector(P_hx,P_hy,0),Z₆>

上式中，Z₆表示UR机器人的第六轴的杆件坐标系的Z轴方向向量，且Z₆＝-a；

S353、计算UR机器人的关节五的Z轴方向向量：

上式中，Z₅表示UR机器人的关节五的Z轴方向向量，vector_2表示vector_1的单位化向量，vector_4表示vector_3的单位化向量，vector_1表示第六过程向量，vector_3表示第七过程向量，且vector_1和vector_3满足下式：

\{\begin{matrix} v e c t o r_1 = [r o t (θ_{1}) \times t r a n s (0, - L_{R}, 0)] \times [V e c t o r < c_{1}, g_{1} >] \\ v e c t o r_3 = V e c t o r (P_{h x}, P_{h y}, 0) \times Z_{6} \end{matrix}

上式中，c₁表示杆件二末端在计算参考平面上的投影点，g₁表示关节点五在计算参考平面上的投影点；trans(0,-L_R,0)表示一平移矩阵；

S354、根据下式计算UR机器人的关节五在计算参考平面上的坐标：

\{\begin{matrix} P_{g_{1}} = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times P_{g} \\ P_{g} = P_{h} - Z_{5} \end{matrix}

P_g1表示UR机器人的关节五在计算参考平面上的坐标，P_g表示UR机器人的关节五在世界坐标系的坐标，trans(0,L_R,0)表示一平移矩阵，P_h表示UR机器人的腕关节点的坐标(P_hx,P_hy,P_hz)；

S355、根据下式计算UR机器人的关节角度θ₂：

θ_{2} = \{\begin{matrix} A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \frac{π}{2}, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \arccos (\frac{{L_{g_{1} c_{1}}}^{2} + {L_{2}}^{2} - {L_{3}}^{2}}{2 \times L_{g_{1} c_{1}} \times L_{2}}), & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} \end{matrix}

上式中，vector_5表示第八过程向量且vector_5＝(1,0,0)，Vc₁g₁表示投影点c₁到投影点g₁的向量，表示投影点c₁到投影点g₁的距离；

S356、根据下式计算UR机器人的关节角度θ₃：

θ_{3} = \{\begin{matrix} 0, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ - A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = 0 \\ A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = π \end{matrix}

其中，d₁表示第三轴关节点在计算参考平面上的投影点，angle_s表示第二临时角度，且满足下式：

angle_s＝Angle<[Vector(c₁,d₁)×Vector(d₁,g₁)],Vector(0,1,0)>

S357、判断是否等于L₂+L₃，根据下式计算UR机器人的关节角度θ₄，反之，执行步骤S358计算UR机器人的关节角度θ₄：

上式中，θ_a表示第三临时角度，θ_b表示第四临时角度，且θ_a和θ_b是通过下式计算的：

\{\begin{matrix} θ_{a} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (c_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

上式中，vector_a表示第九过程向量；

S358、根据下式计算UR机器人的关节角度θ₄：

上式中，θ_c表示第五临时角度，θ_d表示第六临时角度，且θ_c和θ_d是通过下式计算的：

\{\begin{matrix} θ_{c} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (d_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

S359、根据下式，计算UR机器人的关节角度θ₆：

\{\begin{matrix} θ_{6} = A n g l e < [m a t r i x \times V e c t o r (1, 0, 0), n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \end{matrix}

上式中，matrix表示一变换矩阵，T₁、T₂、T₃、T₄、T₅、T₆依次表示UR机器人的第一、第二、第三、第四、第五、第六关节的DH矩阵；且θ₆的方向是根据以下条件确定的：

根据下式计算获得UR机器人的第六轴的杆件坐标系的X轴方向向量X₆，当X₆＝n时，θ₆为正，否则，θ₆为负：

X₆＝matrix×rot(θ₆)×Vector(1,0,0)。

本发明的有益效果是：本发明的一种UR机器人的运动控制方法，包括步骤：S1、建立UR机器人连杆坐标系，获得UR机器人七根杆件和六个轴之间的空间位置关系；S2、获取UR机器人对应的G指令文件后，读取UR机器人目标运动位置；S3、根据该目标运动位置以及UR机器人几何关系，获取UR机器人的DH参数表后，依次计算UR机器人六个轴的关节角度；S4、分别将计算出的六个关节角度转化成每个轴对应的运动脉冲数后，发送到UR机器人的驱动器；S5、驱动器根据接收的每个轴对应的运动脉冲数，依次驱动对应的电机带动相应的轴进行运动。本方法基于几何关系，对UR机器人进行运动逆解，从而实现对UR机器人的运动控制，运算速度快，计算效率高且控制效率高。

附图说明

下面结合附图和实施例对本发明作进一步说明。

图1是本发明的一种UR机器人的运动控制方法的流程图；

图2是本发明的一种UR机器人的运动控制方法建立的UR机器人的杠件之间的关系示意图；

图3是图2中的UR机器人在计算参考平面的投影示意图。

具体实施方式

参照图1，本发明提供了一种UR机器人的运动控制方法，包括步骤：

进一步作为优选的实施方式，所述步骤S1，其具体为：

建立UR机器人的连杆坐标系，获得UR机器人的七根杆件和六个轴之间的空间位置关系，七根杆件的长度分别为L₁、L₂、L₃、L₄、L₅、L₆和L₇，六个轴的关节角度分别为θ₁、θ₂、θ₃、θ₄、θ₅和θ₆，并将经过其关节五的Z轴且垂直于其第二关节的Z轴的平面作为UR机器人的计算参考平面。UR机器人的杠件之间的关系示意图如图2所示，图2中，平面XOY表示世界坐标系下的计算参考平面XOY，计算参考平面表示经过其关节五的Z轴且垂直于其第二关节的Z轴的平面，UR机器人在计算参考平面的投影如图3所示，其中，c₁表示杆件二末端即图2中的c在计算参考平面上的投影点，d₁表示关节点三即图2中的d在计算参考平面上的投影点，g₁表示关节点五即图2中的g在计算参考平面上的投影点，h₁表示关节点六即图2中的h在计算参考平面上的投影点，a₁表示关节点一即图2中的a在计算参考平面上的投影点。

进一步作为优选的实施方式，所述步骤S2中所述UR机器人的目标运动位置包括UR机器人的姿态和UR机器人的位置矢量。

进一步作为优选的实施方式，所述步骤S3，包括：

进一步作为优选的实施方式，所述步骤S31中所述UR机器人的腕关节点的坐标是通过下式计算获得的：

P_h＝transMatrix×P₀

t r a n s M a t r i x = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D \\ 0 & 0 & 0 & 1 \end{matrix}];

D表示DH参数表中的参数。

进一步作为优选的实施方式，所述步骤S32，其具体为：

L = \sqrt{P_{h x}^{2} + P_{h y}^{2}}

进一步作为优选的实施方式，所述步骤S34，具体包括：

θ_{1} = a t a n 2 (P_{h y}, P_{h x}) + \frac{π}{2}

S342、根据下式，计算UR机器人的关节角度θ₅：

θ₅＝Angle<Vector,Z₆>

\{\begin{matrix} t e m p A n g l e = A n g l e < V e c t o r 1, V e c t o r 2 > \\ V e c t o r 1 = V e c t o r \times Z_{6} \\ V e c t o r 2 = (0, 0, 1) \end{matrix}

\{\begin{matrix} θ_{6} = A n g l e < X_{6}, n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \\ X_{6} = m a t r i x \times V e c t o r (1, 0, 0) \end{matrix}

VectorX＝matrix×rot(θ₆)×Vector(1,0,0)

上式中，VectorX表示第三过程向量。

进一步作为优选的实施方式，所述步骤S35，包括：

S350、根据下式计算该投影与基准投影之间的夹角：

θ＝arccos(L_R/L)

θ_{1} = a t a n 2 (V_{2 y}, V_{2 x}) + \frac{π}{2}

Vec2＝Vec1'*L_R

Vec1＝rot(θ)×Vector(P_hx,P_hy,0)

θ₅＝Angle<Vector(P_hx,P_hy,0),Z₆>

S353、计算UR机器人的关节五的Z轴方向向量：

\{\begin{matrix} v e c t o r_1 = [r o t (θ_{1}) \times t r a n s (0, - L_{R}, 0)] \times [V e c t o r < c_{1}, g_{1} >] \\ v e c t o r_3 = V e c t o r (P_{h x}, P_{h y}, 0) \times Z_{6} \end{matrix}

\{\begin{matrix} P_{g_{1}} = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times P_{g} \\ P_{g} = P_{h} - Z_{5} \end{matrix}

S355、根据下式计算UR机器人的关节角度θ₂：

θ_{2} = \{\begin{matrix} A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \frac{π}{2}, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \arccos (\frac{{L_{g_{1} c_{1}}}^{2} + {L_{2}}^{2} - {L_{3}}^{2}}{2 \times L_{g_{1} c_{1}} \times L_{2}}), & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} \end{matrix}

S356、根据下式计算UR机器人的关节角度θ₃：

θ_{3} = \{\begin{matrix} 0, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ - A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = 0 \\ A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = π \end{matrix}

angle_s＝Angle<[Vector(c₁,d₁)×Vector(d₁,g₁)],Vector(0,1,0)>

\{\begin{matrix} θ_{a} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (c_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

上式中，vector_a表示第九过程向量；

S358、根据下式计算UR机器人的关节角度θ₄：

\{\begin{matrix} θ_{c} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (d_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

S359、根据下式，计算UR机器人的关节角度θ₆：

\{\begin{matrix} θ_{6} = A n g l e < [m a t r i x \times V e c t o r (1, 0, 0), n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \end{matrix}

X₆＝matrix×rot(θ₆)×Vector(1,0,0)。

本发明一详细实施例

一种UR机器人的运动控制方法，包括步骤：

S1、建立UR机器人连杆坐标系，获得UR机器人七根杆件和六个轴之间的空间位置关系，具体为：

建立UR机器人的连杆坐标系，获得UR机器人的七根杆件和六个轴之间的空间位置关系，七根杆件的长度分别为L₁、L₂、L₃、L₄、L₅、L₆和L₇，六个轴的关节角度分别为θ₁、θ₂、θ₃、θ₄、θ₅和θ₆，并将经过其关节五的Z轴且垂直于其第二关节的Z轴的平面作为UR机器人的计算参考平面。UR机器人的杠件之间的关系示意图如图2所示，图2中，平面XOY表示世界坐标系下的计算参考平面XOY，计算参考平面表示经过其关节五的Z轴且垂直于其第二关节的Z轴的平面，UR机器人在计算参考平面的投影如图3所示，其中，c₁表示杆件二末端即图2中的c在计算参考平面上的投影点，d₁表示关节点三即图2中的d在计算参考平面上的投影点，g₁表示关节点五即图2中的g在计算参考平面上的投影点，h₁表示关节点六即图2中的h在计算参考平面上的投影点，a₁表示关节点一即图2中的a在计算参考平面上的投影点。根据计算参考平面的定义可知，g₁与g重合，h₁与h重合。

S2、获取UR机器人对应的G指令文件后，读取UR机器人目标运动位置，UR机器人的目标运动位置包括UR机器人的姿态和UR机器人的位置矢量。

具体的，步骤S3包括S31～S35：

S31、获取目标运动位置的坐标(P_x,P_y,P_z)，同时获取UR机器人的DH参数表后计算获得UR机器人的腕关节点的坐标(P_hx,P_hy,P_hz)；UR机器人的腕关节点指关节点六，即图2中的h点；

UR机器人的腕关节点的坐标是通过下式计算获得的：

P_h＝transMatrix×P₀

t r a n s M a t r i x = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D \\ 0 & 0 & 0 & 1 \end{matrix}];

D表示DH参数表中的参数。

S32、根据下式，计算关节点二与关节点六之间的连线在世界坐标系XOY平面上的投影的长度：

L = \sqrt{P_{h x}^{2} + P_{h y}^{2}}

详细的，本实施例中，步骤S34具体包括S341～S343：

θ_{1} = a t a n 2 (P_{h y}, P_{h x}) + \frac{π}{2}

S342、根据下式，计算UR机器人的关节角度θ₅：

θ₅＝Angle<Vector,Z₆>

\{\begin{matrix} t e m p A n g l e = A n g l e < V e c t o r 1, V e c t o r 2 > \\ V e c t o r 1 = V e c t o r \times Z_{6} \\ V e c t o r 2 = (0, 0, 1) \end{matrix}

\{\begin{matrix} θ_{6} = A n g l e < X_{6}, n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \\ X_{6} = m a t r i x \times V e c t o r (1, 0, 0) \end{matrix}

VectorX＝matrix×rot(θ₆)×Vector(1,0,0)

上式中，VectorX表示第三过程向量。

详细的，本实施例中，步骤S35具体包括S350～S359：

S350、根据下式计算该投影与基准投影之间的夹角：

θ＝arccos(L_R/L)

θ_{1} = a t a n 2 (V_{2 y}, V_{2 x}) + \frac{π}{2}

Vec2＝Vec1'*L_R

Vec1＝rot(θ)×Vector(P_hx,P_hy,0)

θ₅＝Angle<Vector(P_hx,P_hy,0),Z₆>

S353、计算UR机器人的关节五的Z轴方向向量：

\{\begin{matrix} v e c t o r_1 = [r o t (θ_{1}) \times t r a n s (0, - L_{R}, 0)] \times [V e c t o r < c_{1}, g_{1} >] \\ v e c t o r_3 = V e c t o r (P_{h x}, P_{h y}, 0) \times Z_{6} \end{matrix}

\{\begin{matrix} P_{g_{1}} = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times P_{g} \\ P_{g} = P_{h} - Z_{5} \end{matrix}

表示UR机器人的关节五在计算参考平面上的坐标，P_g表示UR机器人的关节五在世界坐标系的坐标，trans(0,L_R,0)表示一平移矩阵，P_h表示UR机器人的腕关节点的坐标(P_hx,P_hy,P_hz)；

S355、根据下式计算UR机器人的关节角度θ₂：

θ_{2} = \{\begin{matrix} A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \frac{π}{2}, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \arccos (\frac{{L_{g_{1} c_{1}}}^{2} + {L_{2}}^{2} - {L_{3}}^{2}}{2 \times L_{g_{1} c_{1}} \times L_{2}}), & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} \end{matrix}

S356、根据下式计算UR机器人的关节角度θ₃：

θ_{3} = \{\begin{matrix} 0, & i f & L_{g_{1} c_{1}} = L_{2} + L_{3} \\ - A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = 0 \\ A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, & i f & L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = π \end{matrix}

angle_s＝Angle<[Vector(c₁,d₁)×Vector(d₁,g₁)],Vector(0,1,0)>

\{\begin{matrix} θ_{a} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (c_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

上式中，vector_a表示第九过程向量；

S358、根据下式计算UR机器人的关节角度θ₄：

\{\begin{matrix} θ_{c} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (d_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

S359、根据下式，计算UR机器人的关节角度θ₆：

\{\begin{matrix} θ_{6} = A n g l e < [m a t r i x \times V e c t o r (1, 0, 0), n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \end{matrix}

X₆＝matrix×rot(θ₆)×Vector(1,0,0)。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。

Claims

1.一种UR机器人的运动控制方法，其特征在于，包括步骤：

2.根据权利要求1所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S1，其具体为：

3.根据权利要求1所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S2中所述UR机器人的目标运动位置包括UR机器人的姿态和UR机器人的位置矢量。

4.根据权利要求2所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S3，包括：

5.根据权利要求4所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S31中所述UR机器人的腕关节点的坐标是通过下式计算获得的：

P_h＝transMatrix×P₀

t r a n s M a t r i x = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D \\ 0 & 0 & 0 & 1 \end{matrix}];

D表示DH参数表中的参数。

6.根据权利要求4所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S32，其具体为：

L = \sqrt{P_{h x}^{2} + P_{h y}^{2}}

7.根据权利要求5所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S34，具体包括：

θ_{1} = a t a n 2 (P_{h y}, P_{h x}) + \frac{π}{2}

S342、根据下式，计算UR机器人的关节角度θ₅：

θ₅＝Angle<Vector,Z₆>

\{\begin{matrix} t e m p A n g l e = A n g l e < V e c t o r 1, V e c t o r 2 > \\ V e c t o r 1 = V e c t o r \times Z_{6} \\ V e c t o r 2 = (0, 0, 1) \end{matrix}

\{\begin{matrix} θ_{6} = A n g l e < X_{6}, n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \\ X_{6} = m a t r i x \times V e c t o r (1, 0, 0) \end{matrix}

VectorX＝matrix×rot(θ₆)×Vector(1,0,0)

上式中，VectorX表示第三过程向量。

8.根据权利要求4所述的一种UR机器人的运动控制方法，其特征在于，所述步骤S35，包括：

S350、根据下式计算该投影与基准投影之间的夹角：

θ＝arccos(L_R/L)

θ_{1} = a t a n 2 (V_{2 y}, V_{2 x}) + \frac{π}{2}

Vec2＝Vec1'*L_R

Vec1＝rot(θ)×Vector(P_hx,P_hy,0)

θ₅＝Angle<Vector(P_hx,P_hy,0),Z₆>

上式中，Z₆表示UR机器人的第六轴的杆件坐标系的Z轴方向向量，且

Z₆＝-a；

S353、计算UR机器人的关节五的Z轴方向向量：

\{\begin{matrix} v e c t o r_1 = [r o t (θ_{1}) \times t r a n s (0, - L_{R}, 0)] \times [V e c t o r < c_{1}, g_{1} >] \\ v e c t o r_3 = V e c t o r (P_{h x}, P_{h y}, 0) \times Z_{6} \end{matrix}

\{\begin{matrix} P_{g_{1}} = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times P_{g} \\ P_{g} = P_{h} - Z_{5} \end{matrix}

S355、根据下式计算UR机器人的关节角度θ₂：

θ_{2} = \{\begin{matrix} A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \frac{π}{2}, i f L_{g_{1} c_{1}} = L_{2} + L_{3} \\ A n g l e < v e c t o r_5, {Vc}_{1} g_{1} > - \arccos (\frac{{L_{g_{1} c_{1}}}^{2} + {L_{2}}^{2} - {L_{3}}^{2}}{2 \times L_{g_{1} c_{1}} \times L_{2}}), i f L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} \end{matrix}

上式中，vector_5表示第八过程向量且vector_5＝(1,0,0)，表示投影点c₁到投影点g₁的向量，表示投影点c₁到投影点g₁的距离；

S356、根据下式计算UR机器人的关节角度θ₃：

θ_{3} = \{\begin{matrix} 0, i f L_{g_{1} c_{1}} = L_{2} + L_{3} \\ - A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, i f L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = 0 \\ A n g l e < V e c t o r (c_{1}, d_{1}), V e c t o r (d_{1}, g_{1}) >, i f L_{g_{1} c_{1}} &NotEqual; L_{2} + L_{3} a n d a n g l e_s = π \end{matrix}

angle_s＝Angle<[Vector(c₁,d₁)×Vector(d₁,g₁)],Vector(0,1,0)>

\{\begin{matrix} θ_{a} = A n g l e < v e c t o r_a, V e c t o r (c_{1}, g_{1}) > \\ θ_{b} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (c_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

上式中，vector_a表示第九过程向量；

S358、根据下式计算UR机器人的关节角度θ₄：

\{\begin{matrix} θ_{c} = A n g l e < v e c t o r_a, V e c t o r (d_{1}, g_{1}) > \\ θ_{d} = A n g l e < V e c t o r (0, 1, 0), [v e c t o r_a \times V e c t o r (d_{1}, g_{1})] > \\ v e c t o r_a = [t r a n s (0, L_{R}, 0) \times r o t (- θ_{1})] \times Z_{5} \end{matrix}

S359、根据下式，计算UR机器人的关节角度θ₆：

\{\begin{matrix} θ_{6} = A n g l e < [m a t r i x \times V e c t o r (1, 0, 0)], n > \\ m a t r i x = T_{1} \times r o t (θ_{1}) \times T_{2} \times r o t (θ_{2}) \times T_{3} \times r o t (θ_{3}) \times T_{4} \times r o t (θ_{4}) \times T_{5} \times r o t (θ_{5}) \times T_{6} \end{matrix}

根据下式计算获得UR机器人的第六轴的杆件坐标系的X轴方向向量X₆，

当X₆＝n时，θ₆为正，否则，θ₆为负：

X₆＝matrix×rot(θ₆)×Vector(1,0,0)。