CN111844005B - 一种应用于隧道湿喷的2r-p-2r-p-2r机械臂运动规划方法 - Google Patents

Abstract

本发明公开了一种应用于隧道湿喷的2R‑P‑2R‑P‑2R机械臂运动规划方法。步骤1：利用标准D‑H法对2R‑P‑2R‑P‑2R机械臂进行建模；步骤2：通过模型对2R‑P‑2R‑P‑2R机械臂进行正运动学分析；步骤3：在实际隧道湿喷作业时，机械臂第六关节所连臂杆与隧道轴线方向平行需要增加两个约束条件；步骤4：利用两个约束条件，将不可计算的八自由度逆解问题转换为可计算的六自由度逆解问题，然后计算机械臂的逆运动学解；步骤5：机械臂控制器根据步骤3的计算结果，驱动机械臂的各个关节使得机械臂有效精准地完成给定的末端规划任务。本发明计算量小、实时性强。

Description

一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法

技术领域

本发明属于机械臂控制领域；具体涉及一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法。

背景技术

2R-P-2R-P-2R机械臂是一种应用于隧道湿喷作业的八自由度冗余度机械臂，其关节组成为：第一关节为旋转关节、第二关节为旋转关节、第三关节为直线关节、第四关节为旋转关节、第五关节为旋转关节、第六关节为直线关节、第七关节为旋转关节、第八关节为旋转关节。

运动规划是冗余度机械臂应用研究中的一个重要问题，即给定机械臂末端的期望轨迹，我们需要实时计算得到机械臂相应的关节变量。传统的冗余度机械臂控制方法主要是基于伪逆的方法：即把问题的解转化成求一个最小范数解加上一个同类解。其缺点是在处理不等式约束上有困难，计算量大，实时性差，无法满足隧道湿喷作业对机械臂控制高实时性的要求。

发明内容

本发明提供一种应用于隧道湿喷的大型2R-P-2R-P-2R机械臂运动规划方法，目的在于克服现有方法的不足，本方法计算量小、实时性强。

本发明通过以下技术方案实现：

一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，所述2R-P-2R-P-2R机械臂的第一关节为旋转关节、第二关节为旋转关节、第三关节为直线关节、第四关节为旋转关节、第五关节为旋转关节、第六关节为直线关节、第七关节为旋转关节、第八关节为旋转关节；

利用2R-P-2R-P-2R机械臂所述运动规划方法包括如下步骤：

步骤1：利用标准D-H法对2R-P-2R-P-2R机械臂进行建模；

步骤2：通过模型对2R-P-2R-P-2R机械臂进行正运动学分析；

步骤3：在实际隧道湿喷作业时，机械臂第六关节所连臂杆与隧道轴线方向平行需要增加两个约束条件；

步骤4：利用两个约束条件，将不可计算的八自由度逆解问题转换为可计算的六自由度逆解问题，然后计算机械臂的逆运动学解；

步骤5：机械臂控制器根据步骤3的计算结果，驱动机械臂的各个关节完成给定的末端规划任务。

进一步的，所述步骤1具体为，机械臂的大臂、二臂、小臂分别对应l₂、l₄、l₇，O₀X₀Z₀为基座标系，O₈X₈Z₈为机械臂末端坐标系；

使用D-H参数完成坐标系变换过程，其过程如下：

步骤1.1：将Z_i-1轴绕X_i坐标轴旋转α_i角度，使Z_i-1轴与Z_i轴平行或者重合；

步骤1.2：沿X_i坐标系轴移动a_i距离，使i-1坐标系原点与i坐标系原点重合；

步骤1.3：将X_i-1轴绕Z_i坐标轴旋转θ_i角度，使X_i-1轴与X_i轴平行或者重合；

步骤1.4：沿Z_i轴移动d_i距离，使i-1坐标系与i坐标系完全重合；

根据步骤1.1-步骤1.4变换可将坐标系i-1完全变换到坐标系i，如公式(1)所示：

进一步解算公式(1)得到公式(2):

分别计算机械臂各个连杆的变换矩阵，将所有的变换矩阵相乘就得到机械臂正运动学模型，及坐标系{n}相对于坐标系{0}的变换矩阵，如公式(3)所示：

进一步的，所述步骤2具体为，

定义S_i＝sinθ_i、C_i＝cosθ_i、S_ik＝sin(θ_i+θ_k)、C_ik＝cos(θ_i+θ_k)，其中，i和k为关节序号，S_i为变换矩阵的中间变量，C_i为变换矩阵的中间变量，S_ik为变换矩阵的中间变量，C_ik为变换矩阵的中间变量，使用式(2)推导各连杆转换矩阵得到：

其中，l₁、l₃、l₅、l₆、l₁₀为各个连杆的长度，

由式(3)得到小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

由于在湿喷机械臂工作时，小臂始终与隧道轴线方向平行，因此有：

θ₁+θ₅＝180° (5)

θ₂+θ₄＝180° (6)

将式(4)和式(6)得小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

因此坐标系5到基础坐标系的变换矩阵为：

由式(5)和式(8)得：

机械臂末端到基础坐标系的变换矩阵为：

变换矩阵T₈的矢量形式为：

其中，P_z，a_z，o_z，n_z均表示姿态和位置的变量；

根据式(10)和(11)可以求得正运动学解为：

n_x＝C₈ (12)

n_y＝C₇S₈ (13)

n_z＝S₇S₈ (14)

o_x＝-S₈ (15)

o_y＝C₇C₈ (16)

o_y＝S₇C₈ (17)

a_x＝0 (18)

a_y＝-S₇ (19)

a_z＝C₇ (20)

P_x＝d₆+l₇+l₈+l₄C₁+l₁₀C₈-l₃C₁S₂+(d₃+l₂)C₁C₂ (21)

P_y＝-l₅+l₄S₁-l₉S₇-l₃S₁S₂+l₁₀C₇S₈+(d₃+l₂)S₁C₂ (22)

P_z＝l₁+l₆+l₃C₂+l₉C₇+l₁₀S₇S₈+(d₃+l₂)S₂ (23)。

进一步的，小臂支撑臂末端的坐标系4到基础坐标的变换矩阵为：

由式(7)和式(24)得：

P_4x＝l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂ (25)

湿喷机械臂工作过程中，小臂支撑臂的末端始终在同一竖直截面内运动，因此湿喷机械臂在基础坐标系中的X轴方向的坐标为定值。由此可知，在式(24)的变换矩阵T₄中的P_4x为常数。假设P_4x为常数m，即：

l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂＝m (26)

把式(16)和式(19)代入式(22)中得：

l₄S₁-l₃S₁S₂+(d₃+l₂)S₁C₂＝P_y+l₅-a_yl₉-n_yl₁₀ (27)

式(26)与式(27)相除得：

因此关节变量θ₁为：

进一步的，把式(14)和式(20)代入式(1-23)中得：

l₃C₂+(d₃+l₂)S₂＝P_z-l₁-l₆-a_zl₉-n_zl₁₀ (30)

设P_z-l₁-l₆-a_zl₉-n_zl₁₀＝J

由式(26)得：

设

将式(30)的平方与式(31)的平方相加得：

化简得：

将式(30)与式(31)相除得：

解得关节变量θ₂为：

进一步的，由式(6)得关节变量θ₄为：

θ₄＝180°-θ₂ (36)

由式(5)得关节变量θ₅为：

θ₅＝180°-θ₁ (37)。

进一步的，把式(11)和式(26)代入式(21)中得：

P_x＝d₆+l₇+l₈+n_xl₁₀+m (38)

则d₆为：

d₆＝P_x-l₇-l₈-n_xl₁₀-m (39)。

进一步的，由式(19)得关节变量θ₇为：

θ₇＝-arcsina_y (40)

由式(12)得关节变量θ₈为：

θ₈＝arccosn_x (41)。

本发明的有益效果是：

利用解析法对冗余机械臂求逆解，具有准确度高、实时性强的特点；结合隧道湿喷作业特点对2R-P-2R-P-2R机械臂的逆解进行降维处理，在简化2R-P-2R-P-2R机械臂逆解运算的同时增加了2R-P-2R-P-2R机械臂对隧道湿喷作业的适用性。

附图说明

附图1本发明机械臂构型示意图。

附图2本发明机械臂连杆坐标系示意图。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，本实例中，2R-P-2R-P-2R机械臂构型示意图如图1所示，机械臂由8个关节组成，所述2R-P-2R-P-2R机械臂的第一关节为旋转关节、第二关节为旋转关节、第三关节为直线关节、第四关节为旋转关节、第五关节为旋转关节、第六关节为直线关节、第七关节为旋转关节、第八关节为旋转关节；

利用2R-P-2R-P-2R机械臂所述运动规划方法包括如下步骤：

步骤1：利用标准D-H法对2R-P-2R-P-2R机械臂进行建模；

步骤2：通过模型对2R-P-2R-P-2R机械臂进行正运动学分析；

步骤3：在实际隧道湿喷作业时，机械臂第六关节所连臂杆与隧道轴线方向平行需要增加两个约束条件；把臂杆看成线，两个约束条件是两个线与面平行，为让臂杆与轴线平行，必须保证臂杆和地面平行+臂杆和隧道纵切面平行，也就是俩个约束条件；

步骤4：利用两个约束条件，将不可计算的八自由度逆解问题转换为可计算的六自由度逆解问题，然后计算机械臂的逆运动学解；

步骤5：机械臂控制器根据步骤3的计算结果，驱动机械臂的各个关节使得机械臂有效精准地完成给定的末端规划任务。

进一步的，所述步骤1具体为，机械臂的大臂、二臂、小臂分别对应l₂、l₄、l₇，O₀X₀Z₀为基座标系，O₈X₈Z₈为机械臂末端坐标系；

使用D-H参数完成坐标系变换过程，其过程如下：

步骤1.1：将Z_i-1轴绕X_i坐标轴旋转α_i角度，使Z_i-1轴与Z_i轴平行或者重合；

步骤1.2：沿X_i坐标系轴移动a_i距离，使i-1坐标系原点与i坐标系原点重合；

步骤1.3：将X_i-1轴绕Z_i坐标轴旋转θ_i角度，使X_i-1轴与X_i轴平行或者重合；

步骤1.4：沿Z_i轴移动d_i距离，使i-1坐标系与i坐标系完全重合；

根据步骤1.1-步骤1.4变换可将坐标系i-1完全变换到坐标系i，如公式(1)所示：

进一步解算公式(1)得到公式(2):

分别计算机械臂各个连杆的变换矩阵，将所有的变换矩阵相乘就得到机械臂正运动学模型，及坐标系{n}相对于坐标系{0}的变换矩阵，如公式(3)所示：

表1喷浆机械臂D-H参数

进一步的，所述步骤2具体为，

定义S_i＝sinθ_i、C_i＝cosθ_i、S_ik＝sin(θ_i+θ_k)、C_ik＝cos(θ_i+θ_k)，其中，i和k为关节序号，S_i为变换矩阵的中间变量，C_i为变换矩阵的中间变量，S_ik为变换矩阵的中间变量，C_ik为变换矩阵的中间变量，使用式(2)推导各连杆转换矩阵得到：

其中l₁，l₃，l₅，l₆，l₁₀为各个连杆的长度，

由式(3)得到小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

由于在湿喷机械臂工作时，小臂始终与隧道轴线方向平行，因此有：

θ₁+θ₅＝180° (5)

θ₂+θ₄＝180° (6)

将式(4)和式(6)得小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

因此坐标系5到基础坐标系的变换矩阵为：

由式(5)和式(8)得：

机械臂末端到基础坐标系的变换矩阵为：

变换矩阵T₈的矢量形式为：

其中，P_z，a_z，o_z，n_z均表示姿态和位置的变量；

根据式(10)和(11)可以求得正运动学解为：

n_x＝C₈ (12)

n_y＝C₇S₈ (13)

n_z＝S₇S₈ (14)

o_x＝-S₈ (15)

o_y＝C₇C₈ (16)

o_y＝S₇C₈ (17)

a_x＝0 (18)

a_y＝-S₇ (19)

a_z＝C₇ (20)

P_x＝d₆+l₇+l₈+l₄C₁+l₁₀C₈-l₃C₁S₂+(d₃+l₂)C₁C₂ (21)

P_y＝-l₅+l₄S₁-l₉S₇-l₃S₁S₂+l₁₀C₇S₈+(d₃+l₂)S₁C₂ (22)

P_z＝l₁+l₆+l₃C₂+l₉C₇+l₁₀S₇S₈+(d₃+l₂)S₂ (23)。

进一步的，小臂支撑臂末端的坐标系4到基础坐标的变换矩阵为：

由式(7)和式(24)得：

P_4x＝l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂ (25)

湿喷机械臂工作过程中，小臂支撑臂的末端始终在同一竖直截面内运动，因此湿喷机械臂在基础坐标系中的X轴方向的坐标为定值。由此可知，在式(24)的变换矩阵T₄中的P_4x为常数。假设P_4x为常数m，即：

l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂＝m (26)

把式(16)和式(19)代入式(22)中得：

l₄S₁-l₃S₁S₂+(d₃+l₂)S₁C₂＝P_y+l₅-a_yl₉-n_yl₁₀ (27)

式(26)与式(27)相除得：

因此关节变量θ₁为：

进一步的，把式(14)和式(20)代入式(1-23)中得：

l₃C₂+(d₃+l₂)S₂＝P_z-l₁-l₆-a_zl₉-n_zl₁₀ (30)

设P_z-l₁-l₆-a_zl₉-n_zl₁₀＝J

由式(26)得：

设

将式(30)的平方与式(31)的平方相加得：

化简得：

将式(30)与式(31)相除得：

解得关节变量θ₂为：

进一步的，由式(6)得关节变量θ₄为：

θ₄＝180°-θ₂ (36)

由式(5)得关节变量θ₅为：

θ₅＝180°-θ₁ (37)。

进一步的，把式(11)和式(26)代入式(21)中得：

P_x＝d₆+l₇+l₈+n_xl₁₀+m (38)

则d₆为：

d₆＝P_x-l₇-l₈-n_xl₁₀-m (39)。

进一步的，由式(19)得关节变量θ₇为：

θ₇＝-arcsina_y (40)

由式(12)得关节变量θ₈为：

θ₈＝arccosn_x (41)

至此得到了每个关节的关节值。

Claims (5)

1.一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，其特征在于，所述2R-P-2R-P-2R机械臂的第一关节为旋转关节、第二关节为旋转关节、第三关节为直线关节、第四关节为旋转关节、第五关节为旋转关节、第六关节为直线关节、第七关节为旋转关节、第八关节为旋转关节；

利用2R-P-2R-P-2R机械臂所述运动规划方法包括如下步骤：

步骤1：利用标准D-H法对2R-P-2R-P-2R机械臂进行建模；

步骤2：通过模型对2R-P-2R-P-2R机械臂进行正运动学分析；

步骤3：在实际隧道湿喷作业时，机械臂第六关节所连臂杆与隧道轴线方向平行需要增加两个约束条件；把臂杆看成线，两个约束条件是两个线与面平行，为让臂杆与轴线平行，必须保证臂杆和地面平行以及臂杆和隧道纵切面平行；

步骤4：利用两个约束条件，将不可计算的八自由度逆解问题转换为可计算的六自由度逆解问题，然后计算机械臂的逆运动学解；

步骤5：机械臂控制器根据步骤3的计算结果，驱动机械臂的各个关节完成给定的末端规划任务；

所述步骤1具体为，机械臂的大臂、二臂、小臂分别对应l₂、l₄、l₇，O₀X₀Z₀为基座标系，O₈X₈Z₈为机械臂末端坐标系；

使用D-H参数完成坐标系变换过程，其过程如下：

步骤1.1：将Z_i-1轴绕X_i坐标轴旋转α_i角度，使Z_i-1轴与Z_i轴平行或者重合；

步骤1.2：沿X_i坐标系轴移动a_i距离，使i-1坐标系原点与i坐标系原点重合；

步骤1.3：将X_i-1轴绕Z_i坐标轴旋转θ_i角度，使X_i-1轴与X_i轴平行或者重合；

步骤1.4：沿Z_i轴移动d_i距离，使i-1坐标系与i坐标系完全重合；

根据步骤1.1-步骤1.4变换可将坐标系i-1完全变换到坐标系i，如公式(1)所示：

进一步解算公式(1)得到公式(2):

分别计算机械臂各个连杆的变换矩阵，将所有的变换矩阵相乘就得到机械臂正运动学模型，及坐标系{n}相对于坐标系{0}的变换矩阵，如公式(3)所示：

所述步骤2具体为，

定义S_i＝sinθ_i、C_i＝cosθ_i、S_ik＝sin(θ_i+θ_k)、C_ik＝cos(θ_i+θ_k)，其中，i和k为关节序号，S_i为变换矩阵的中间变量，C_i为变换矩阵的中间变量，S_ik为变换矩阵的中间变量，C_ik为变换矩阵的中间变量，使用式(2)推导各连杆转换矩阵得到：

其中l₁，l₃，l₅，l₆，l₁₀为各个连杆的长度，

由式(3)得到小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

由于在湿喷机械臂工作时，小臂始终与隧道轴线方向平行，因此有：

θ₁+θ₅＝180° (5)

θ₂+θ₄＝180° (6)

将式(4)和式(6)得小臂支撑臂末端的坐标系4到基础坐标系的变换矩阵为：

因此坐标系5到基础坐标系的变换矩阵为：

由式(5)和式(8)得：

机械臂末端到基础坐标系的变换矩阵为：

变换矩阵T₈的矢量形式为：

其中，P_z，a_z，o_z，n_z均表示姿态和位置的变量；

根据式(10)和(11)可以求得正运动学解为：

n_x＝C₈ (12)

n_y＝C₇S₈ (13)

n_z＝S₇S₈ (14)

o_x＝-S₈ (15)

o_y＝C₇C₈ (16)

o_y＝S₇C₈ (17)

a_x＝0 (18)

a_y＝-S₇ (19)

a_z＝C₇ (20)

P_x＝d₆+l₇+l₈+l₄C₁+l₁₀C₈-l₃C₁S₂+(d₃+l₂)C₁C₂ (21)

P_y＝-l₅+l₄S₁-l₉S₇-l₃S₁S₂+l₁₀C₇S₈+(d₃+l₂)S₁C₂ (22)

P_z＝l₁+l₆+l₃C₂+l₉C₇+l₁₀S₇S₈+(d₃+l₂)S₂ (23)

小臂支撑臂末端的坐标系4到基础坐标的变换矩阵为：

由式(7)和式(24)得：

P_4x＝l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂ (25)

湿喷机械臂工作过程中，小臂支撑臂的末端始终在同一竖直截面内运动，因此湿喷机械臂在基础坐标系中的X轴方向的坐标为定值；由此可知，在式(24)的变换矩阵T₄中的P_4x为常数；假设P_4x为常数m，即：

l₄C₁-l₃C₁S₂+(d₃+l₂)C₁C₂＝m (26)

把式(16)和式(19)代入式(22)中得：

l₄S₁-l₃S₁S₂+(d₃+l₂)S₁C₂＝P_y+l₅-a_yl₉-n_yl₁₀ (27)

式(26)与式(27)相除得：

因此关节变量θ₁为：

2.根据权利要求1所述一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，其特征在于，把式(14)和式(20)代入式(1-23)中得：

l₃C₂+(d₃+l₂)S₂＝P_z-l₁-l₆-a_zl₉-n_zl₁₀ (30)

设P_z-l₁-l₆-a_zl₉-n_zl₁₀＝J

由式(26)得：

设

将式(30)的平方与式(31)的平方相加得：

化简得：

将式(30)与式(31)相除得：

解得关节变量θ₂为：

3.根据权利要求1所述一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，其特征在于，由式(6)得关节变量θ₄为：

θ₄＝180°-θ₂ (36)

由式(5)得关节变量θ₅为：

θ₅＝180°-θ₁ (37)。

4.根据权利要求1所述一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，其特征在于，把式(11)和式(26)代入式(21)中得：

P_x＝d₆+l₇+l₈+n_xl₁₀+m (38)

则d₆为：

d₆＝P_x-l₇-l₈-n_xl₁₀-m (39)。

5.根据权利要求1所述一种应用于隧道湿喷的2R-P-2R-P-2R机械臂运动规划方法，其特征在于，由式(19)得关节变量θ₇为：

θ₇＝-arcsina_y (40)

由式(12)得关节变量θ₈为：

θ₈＝arccosn_x (41)。

Images