CN115142877A - 基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 - Google Patents
基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 Download PDFInfo
- Publication number
- CN115142877A CN115142877A CN202210775344.4A CN202210775344A CN115142877A CN 115142877 A CN115142877 A CN 115142877A CN 202210775344 A CN202210775344 A CN 202210775344A CN 115142877 A CN115142877 A CN 115142877A
- Authority
- CN
- China
- Prior art keywords
- connecting rod
- spraying
- joint connecting
- track
- tunnel
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000005507 spraying Methods 0.000 title claims abstract description 149
- 238000000034 method Methods 0.000 title claims abstract description 49
- 239000007921 spray Substances 0.000 claims abstract description 74
- 230000033001 locomotion Effects 0.000 claims abstract description 48
- 230000009466 transformation Effects 0.000 claims description 78
- 239000011159 matrix material Substances 0.000 claims description 72
- 238000012795 verification Methods 0.000 claims description 29
- 230000014509 gene expression Effects 0.000 claims description 15
- 229910000831 Steel Inorganic materials 0.000 claims description 12
- 239000010959 steel Substances 0.000 claims description 12
- 238000012546 transfer Methods 0.000 claims description 11
- 230000005540 biological transmission Effects 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 3
- 210000001503 joint Anatomy 0.000 claims 2
- 238000004891 communication Methods 0.000 claims 1
- 239000011378 shotcrete Substances 0.000 description 7
- 238000010422 painting Methods 0.000 description 5
- 238000011156 evaluation Methods 0.000 description 3
- 238000005422 blasting Methods 0.000 description 2
- 239000011248 coating agent Substances 0.000 description 2
- 238000000576 coating method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007591 painting process Methods 0.000 description 1
- 239000002002 slurry Substances 0.000 description 1
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E21—EARTH OR ROCK DRILLING; MINING
- E21D—SHAFTS; TUNNELS; GALLERIES; LARGE UNDERGROUND CHAMBERS
- E21D11/00—Lining tunnels, galleries or other underground cavities, e.g. large underground chambers; Linings therefor; Making such linings in situ, e.g. by assembling
- E21D11/04—Lining with building materials
- E21D11/10—Lining with building materials with concrete cast in situ; Shuttering also lost shutterings, e.g. made of blocks, of metal plates or other equipment adapted therefor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
- B25J11/0075—Manipulators for painting or coating
Landscapes
- Engineering & Computer Science (AREA)
- Architecture (AREA)
- Structural Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Civil Engineering (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geochemistry & Mineralogy (AREA)
- Geology (AREA)
- Application Of Or Painting With Fluid Materials (AREA)
Abstract
本发明公开了一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，涉及隧道领域，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，包括：S1：根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；S2：根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；S3:根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷，其提供了一种能快速、准确计算的逆向运动学模型以得出机械臂各个关节连杆的旋转角度，并通过该旋转角度控制机械臂以预设轨迹进行喷涂，以在脱离人工控制的情况下实现对隧道全面的快速的自动喷涂。
Description
技术领域
本发明涉及隧道领域，尤其涉及一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统。
背景技术
机器人的运动学建模是控制喷浆机机械臂以预设轨迹进行喷涂的基础，其主要包括正向运动学与逆向运动学两个方面。其中，正向运动学建模的目的是根据已知机械臂的关节变量，计算出机械臂末端喷嘴相对于机械臂首个关节连杆坐标系的变换矩阵，该矩阵能够提供末端喷头相对于首个关节连杆坐标系的位置和朝向信息。而逆向运动学建模的目的是根据末端喷头相对于首个关节连杆坐标系的位置和朝向，反向求出此时机械臂的各个关节连杆的运动参数。
针对七自由度的机械臂(包含多个旋转、伸缩关节连杆)，如果使用传统的D-H参数法来建立正向运动学模型不仅较为复杂，还会增大逆向运动学求解的难度。另外，通过逆向运动学直接对七自由度的机械臂进行求解其难度与计算量均较大，会进一步的提升控制喷浆机机械臂以预设轨迹进行喷涂的难度，因此，提供一种能快速、准确计算的逆向运动学模型以得出机械臂各个关节连杆的旋转角度，并通过该旋转角度控制喷浆机机械臂以预设轨迹进行喷涂，以在脱离人工控制的情况下实现对隧道全面的快速自动喷涂，亟待实现。
发明内容
为了在脱离人工控制的情况下实现对隧道全面的快速自动喷涂，本发明提出了一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述方法包括步骤：
S1：根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
S2：根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
S3:根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
进一步地，所述步骤S2至S3之间还包括旋转角度的验证，具体为：
S21：根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确，进入步骤S3。
进一步地，所述机械臂的关节连杆包括依次连接的：
第一旋转关节连杆、第二旋转关节连杆、第三旋转关节连杆、第四伸缩关节连杆、第五伸缩关节连杆、第六旋转关节连杆以及第七旋转关节连杆，所述第四伸缩关节连杆与第五伸缩关节连杆合为一个移动关节连杆，其中第七旋转关节连杆与喷头连接。
进一步地，所述预设正向运动学模型的建模过程为：
第一步：设定隧道湿喷轨迹中任意一点的坐标为(a，b，c)；
第二步：根据坐标(a，b，c)得到移动关节连杆对应平移操作的齐次变换矩阵为：
第三步：通过对旋转关节连杆做旋转角度为θ的旋转变换，得到旋转关节连杆对应旋转操作在x、y、z轴三个方向上的变换矩阵分别为：
式中，Rot(x,θ)为旋转关节连杆对应旋转操作在x轴方向上的变换矩阵；
式中，Rot(y,θ)为旋转关节连杆对应旋转操作在y轴方向上的变换矩阵；
式中，Rot(z,θ)为旋转关节连杆对应旋转操作在z轴方向上的变换矩阵；
第四步，通过对x轴、y轴、z轴方向上的变换矩阵以及齐次变换矩阵四个公式进行相乘，得到相邻关节连杆的传递变换矩阵通式为：
式中，i＝1，2，3，4，5，6；θi为第i个关节连杆的旋转角度；di为第i个关节连杆的偏距；αi为第i个关节连杆的预设扭角；ai为第i个关节连杆的预设杆长；Ti-1,i为第i-1个关节连杆至第i个关节连杆的变换矩阵，其中，当i＝1时，T0,1表示从机械臂基座建立的预设基坐标系至第一个关节连杆的变换矩阵；
第五步，获取预设正向运动学模型，所述预设正向运动学模型即各个相邻关节连杆间其传递变换矩阵通式的乘积，其表达式为：
式中，T12表示第一旋转关节连杆至第二旋转关节连杆的变换矩阵；T23表示第二旋转关节连杆至第三旋转关节连杆的变换矩阵；T34表示第三旋转关节连杆至移动关节连杆的变换矩阵；T45表示移动关节连杆至第六旋转关节连杆的变换矩阵；T56表示第六旋转关节连杆至第七旋转关节连杆的变换矩阵；[Px,Py,Pz]T表示喷头的位置；[nx,ny,nz]T，[ox,oy,oz]T，[ax,ay,az]T表示喷头的朝向；T06表示预设基坐标系至喷头的变换矩阵即预设正向运动学模型。
进一步地，所述步骤S2具体为：
第一步，根据步骤S1中获取的喷头在运动位姿轨迹上各点的位置与朝向，设定预设基坐标系至喷头的变换矩阵为R，其表达式为：
并通过预设正向运动学模型得到：
R＝T06＝T01×T12×T23×T34×T45×T56；
第二步，固定第三旋转关节连杆与第四伸缩关节连杆，即对其参数进行预设定，所述参数包含旋转角度、偏距、预设扭角、预设杆长；获取各相邻关节连杆间传递变换矩阵通式的逆，并通过预设基坐标系至喷头的变换矩阵与相邻关节连杆间传递变换矩阵通式的逆，获取各关节连杆的旋转角度。
进一步地，所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取方法包括步骤：
S01：在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
S02：获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
S03：通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
S04：沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
本发明还提出了一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的系统，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，其特征在于，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述系统包括：
获取模块，用于根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
旋转角度获取模块，用于根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
控制模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
进一步地，所述系统还包括：
验证模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确。
进一步地，所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取包括：
切割模块，用于在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
交点获取模块，用于获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
法向量获取模块，用于通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
轨迹获取模块，用于沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
与现有技术相比，本发明至少含有以下有益效果：
(1)本发明通过运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向，并根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度，并根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷，其实现了在脱离人工控制的情况下对隧道全面的快速的喷涂；
(2)本发明在预设正向运动学模型的建模过程中，通过在机械臂基座建立预设基坐标系，从预设基坐标系至第一关节连杆，第一关节连杆至第二关节连杆，以此类推，直到第六关节连杆至第七关节连杆，其不同于传统的D-H参数法只在每个关节连杆上设置坐标系，本发明通过在机械臂基座建立的基坐标系开始进行平移与旋转(在机械臂基座建立一个基础坐标系后，直接对基坐标系进行平移操作，移动到下一个关节的中心位置，若下一个关节为平移关节，则继续对坐标系进行平移操作；若下一个关节为旋转关节，则对坐标系进行旋转操作，以此类推得到最终各关节的坐标系)，该方法比传统的D-H参数法更加清晰，避免了关节歧异，且降低了逆向运动学求解的难度；
(3)为了解决通过逆向运动学直接对七自由度的机械臂进行求解其难度与计算量均较大的问题，本发明在预设逆向运动学模型的建模过程中，固定第三旋转关节连杆与第四伸缩关节连杆，即采用固定关节角法进行逆向运动学模型的求解，以求解出剩余五个关节的解析解，该方法在降低了计算难度的同时极大的减轻了计算量，因而也极大的提升了机械臂喷涂的速度；
(4)本发明通过预设正向运动学模型对预设逆向运动学模型求出来的解析解进行校验，其提高了解析解的准确度，因而进一步提高了喷浆机机械臂在喷涂过程中的喷涂精度。
附图说明
图1为基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法流程图；
图2为基于隧道湿喷轨迹控制喷浆机机械臂喷涂的系统模块图；
图3为喷浆机机械臂关节连杆图；
图4为机械臂投影图。
具体实施方式
以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。
实施例一
为了提供一种能快速、准确计算的逆向运动学模型以得出机械臂各个关节连杆的旋转角度，并通过该旋转角度控制喷浆机机械臂以预设轨迹进行喷涂，以在脱离人工控制的情况下实现对隧道全面的快速自动喷涂，如图1所示，本发明提出了一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述方法包括步骤：
S1：根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
如图3所示，所述机械臂的关节连杆包括依次连接的：
第一旋转关节连杆(关节1)、第二旋转关节连杆(关节2)、第三旋转关节连杆(关节3)、第四伸缩关节连杆(关节4)、第五伸缩关节连杆(关节5)、第六旋转关节连杆(关节6)以及第七旋转关节连杆(关节7)，所述第四伸缩关节连杆与第五伸缩关节连杆合为一个移动关节连杆，其中第七旋转关节连杆与喷头连接。
本发明采用的喷浆机为七自由度机械臂，且同时存在旋转与伸缩两种关节，使用传统的D-H参数法来建立正向运动学模型不仅较为复杂，还会增大逆向运动学求解的难度，与传统的参数法不同，本发明通过在机械臂基座建立基坐标系使得参数比传统D-H法更为清晰，同时也可以避免关节歧异。
本发明在预设正向运动学模型的建模过程中，通过在机械臂基座建立预设基坐标系，从预设基坐标系至第一关节连杆，第一关节连杆至第二关节连杆，以此类推，直到第六关节连杆至第七关节连杆，其不同于传统的D-H参数法只在每个关节连杆上设置坐标系，本发明通过在机械臂基座建立的基坐标系开始进行平移与旋转(在机械臂基座建立一个基础坐标系后，直接对基坐标系进行平移操作，移动到下一个关节的中心位置，若下一个关节为平移关节，则继续对坐标系进行平移操作；若下一个关节为旋转关节，则对坐标系进行旋转操作，以此类推得到最终各关节的坐标系)，该方法比传统的D-H参数法更加清晰，避免了关节歧异，且降低了逆向运动学求解的难度。
S2：根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
所述步骤S2至S3之间还包括旋转角度的验证，具体为：
S21：根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确，进入步骤S3。
所述预设正向运动学模型的建模过程为：
第一步：设定隧道湿喷轨迹中任意一点的坐标为(a，b，c)；
第二步：根据坐标(a，b，c)得到移动关节连杆对应平移操作的齐次变换矩阵为：
第三步：通过对旋转关节连杆做旋转角度为θ的旋转变换，得到旋转关节连杆对应旋转操作在x、y、z轴三个方向上的变换矩阵分别为：
式中，Rot(x,θ)为旋转关节连杆对应旋转操作在x轴方向上的变换矩阵；
式中，Rot(y,θ)为旋转关节连杆对应旋转操作在y轴方向上的变换矩阵；
式中，Rot(z,θ)为旋转关节连杆对应旋转操作在z轴方向上的变换矩阵；
第四步，通过对x轴、y轴、z轴方向上的变换矩阵以及齐次变换矩阵四个公式进行相乘，得到相邻关节连杆的传递变换矩阵通式为：
式中，i＝1，2，3，4，5，6；θi为第i个关节连杆的旋转角度；di为第i个关节连杆的偏距；αi为第i个关节连杆的预设扭角；ai为第i个关节连杆的预设杆长；Ti-1,i为第i-1个关节连杆至第i个关节连杆的变换矩阵，其中，当i＝1时，T0,1表示从机械臂基座建立的预设基坐标系至第一个关节连杆的变换矩阵；
本实施例中的关节连杆参数如下表所示(由于第四伸缩关节连杆与第五伸缩关节连杆合为一个移动关节连杆，因此一起是6个关节连杆，表中d4的取值范围为1982～6582mm)：
通过关节连杆参数表中的数据与预设正向运动学模型中的相邻关节连杆的传递变换矩阵通式，得到：
第五步，获取预设正向运动学模型，所述预设正向运动学模型即各个相邻关节连杆间其传递变换矩阵通式的乘积，其表达式为：
式中，T12表示第一旋转关节连杆至第二旋转关节连杆的变换矩阵；T23表示第二旋转关节连杆至第三旋转关节连杆的变换矩阵；T34表示第三旋转关节连杆至移动关节连杆的变换矩阵；T45表示移动关节连杆至第六旋转关节连杆的变换矩阵；T56表示第六旋转关节连杆至第七旋转关节连杆的变换矩阵；[Px,Py,Pz]T表示喷头的位置；[nx,ny,nz]T，[ox,oy,oz]T，[ax,ay,az]T表示喷头的朝向；T06表示预设基坐标系至喷头的变换矩阵即预设正向运动学模型。
本实施例中，代入数据后得到：
nx＝sinθ6(cosθ1cosθ2sinθ3-cosθ1sinθ2cosθ3)-sinθ1sinθ5cosθ6-cosθ1cosθ5cosθ6(sinθ2sinθ3+cosθ2cosθ3)；
ny＝cosθ1sinθ5cosθ6+sinθ1cosθ5cosθ6(sinθ2sinθ3+cosθ2cosθ3)+sinθ1sinθ6(cosθ2sinθ3-sinθ2cosθ3)；
nz＝cosθ5cosθ6(cosθ2sinθ3-sinθ2cosθ3)-sinθ6(sinθ2sinθ3+cosθ2cosθ3)；
ox＝sinθ1sinθ5sinθ6+cosθ1cosθ5sinθ6(sinθ2sinθ3+cosθ2cosθ3)+cosθ1sinθ6(cosθ2sinθ3-sinθ2cosθ3)；
oy＝sinθ1cosθ6(cosθ2sinθ3-sinθ2cosθ3)-cosθ1sinθ5sinθ6+sinθ1cosθ5sinθ6(sinθ2sinθ3+cosθ2cosθ3)；
oz＝cosθ5sinθ6(sinθ2cosθ3-cosθ2sinθ3)-cosθ6(sinθ2sinθ3+cosθ2cosθ3)；
ax＝-sinθ1cosθ5-cosθ1sinθ5(sinθ2sinθ3+cosθ2cosθ3)；ay＝cosθ1cosθ5-sinθ1sinθ5(sinθ2sinθ3+cosθ2cosθ3)；
az＝sinθ5(sinθ2cosθ3-cosθ2sinθ3)；
Px＝3219cosθ1cosθ2-426sinθ1-325sinθ1cosθ5+cosθ1(d4-990)(cosθ2sinθ3-sinθ2cosθ3)-325cosθ1sinθ5(sinθ2sinθ3+cosθ2cosθ3)；
Py＝3219sinθ1cosθ2+426cosθ1+325cosθ1cosθ5+sinθ1(d4-990)(cosθ2sinθ3-sinθ2cosθ3)-325sinθ1sinθ5(sinθ2sinθ3+cosθ2cosθ3)；
Pz＝(990-d4)(sinθ2sinθ3+cosθ2cosθ3)-3219sinθ2-426sinθ1-325sinθ5(cosθ2sinθ3-sinθ2cosθ3)；
所述步骤S2具体为：
第一步，根据步骤S1中获取的喷头在运动位姿轨迹上各点的位置与朝向，设定预设基坐标系至喷头的变换矩阵为R，其表达式为：
并通过预设正向运动学模型得到：
R＝T06＝T01×T12×T23×T34×T45×T56 (式8)；
第二步，固定第三旋转关节连杆与第四伸缩关节连杆，即对其参数进行预设定，所述参数包含旋转角度、偏距、预设扭角、预设杆长；获取各相邻关节连杆间传递变换矩阵通式的逆，并通过预设基坐标系至喷头的变换矩阵与相邻关节连杆间传递变换矩阵通式的逆，获取各关节连杆的旋转角度。
为了解决通过逆向运动学直接对七自由度的机械臂进行求解其难度与计算量均较大的问题，本发明在预设逆向运动学模型的建模过程中，固定第三旋转关节连杆与第四伸缩关节连杆，即采用固定关节角法进行逆向运动学模型的求解，以求解出剩余五个关节连杆的解析解，该方法在降低了计算难度的同时极大的减轻了计算量，因而也极大的提升了机械臂喷涂的速度。
通过关节连杆参数表中的数据，对关节连杆旋转角度的计算过程具体展示如下：
通过预设正向运动学模型中的相邻关节连杆的传递变换矩阵通式，得到：
对式7左边左乘T01的逆，得到(式10)：
对式7右边左乘T01的逆，得到(式11)：
式中，C23表示cos(θ2+θ3)，S23表示sin(θ2+θ3)，C2表示cos(θ2)，C5表示cos(θ5)，C6表示cos(θ6)，S5表示sin(θ5)，S6表示sin(θ6)；
1)求解θ1
由(式10)与(式11)中第2行4列的元素相等有：
Pycosθ1-Px sinθ1＝325cosθ5+426 (式12)；
由(式10)与(式11)中第2行3列的元素相等有：
aycosθ1-ax sinθ1＝cosθ5 (式13)；
将(式13)代入(式12)消去cos(θ5)得到关于θ1的三角方程，可解出θ1的两个值分别为：
式中，
2)求解θ5
根据(式13)与解出的θ1可推算出：
θ5的取值范围为[0,2π]，而反余弦算出的范围为[0,π]，故而θ5还有另外两组解：
3)求解θ6
由(式10)与(式11)中第2行1列的元素相等，第2行2列的元素相等，并消去sinθ5有
考虑θ6的取值范围，求解θ6有：
4)求解θ2+θ3，下面的求解中，C1表示cos(θ1)，C6表示cos(θ6)，S1表示sin(θ1)，Sx表示sin(θ6)；
对(式10)两边右乘T06的逆，得到：
对(式11)两边右乘T06的逆，得到：
式中，C23表示cos(θ2+θ3)，S23表示sin(θ2+θ3)，C2表示cos(θ2)，C5表示cos(θ5)，S2表示sin(θ2)；
由(式21)与(式22)中第1行2列的元素相等，第3行2列的元素相等，并消去cos(θ5)得到：
5)求解θ3
所述第二关节连杆、第三关节连杆、第四关节连杆相互耦合，在上述矩阵元素中难以建立合适的等量关系进行剥离求解，故而将机械臂投影至一关节连杆的x1o1z1平面(如图4所示)进行计算。
五个关节连杆在一关节坐标系下的位姿关系通过(式10)表示，即图4中的点o5对应x5在x1o1z1坐标系下的横纵坐标分别为：
图4中o1o3的单位方向向量可由z5方向表示，即(式22)矩阵中的第三列元素，进而可推算出o1到o1o3的距离doD的两种解为：
另外，可在三角形o1o3D中建立doD与θ3的关系如下：
由(式25)与(式26)两式相对可求出θ3为：
6)求解θ2
根据求解的θ2+θ3与θ3的值得到θ2为：
7)求解d4
由(式21)与(式22)中第3行4列的元素相等，可求出d4有
至此完成各关节的解析解求取，通过不同关节的组合可得到多组解(对于每一个变换矩阵，经过预设逆向运动学模型可以求得16组解)，将求解出的这16组解分别输入预设正向运动学模型，其计算得到的16个变换矩阵，与求解该解析解时输入的变换矩阵完全一致，则表示解析解的求解正确，以此验证预设逆向运动学模型的正确性。
机器人逆运动学解是一个多解问题，要利用逆运动学解来驱动机械臂的关节连杆运动，需要在多组解中选出最适合机械臂实际运动轨迹的解。逆解选解问题的常见解决方法是满足最短行程原则法，即通过利用“多移动小关节，少移动大关节”的原则来进行优化。
基于此，我们对预设逆向运动学模型求解出的16组解增加筛选条件，包括当前解与上一组解之间各个关节变量的变化尽可能最小；优先改变距离末端喷头更近的关节的关节变量，即遵循“多移动小关节，少移动大关节”的原则；俯仰、横摆等旋转关节以及伸缩等移动关节的增量方向尽可能保持一致等。通过以上筛选条件可以选出最优的一组解作为机械臂运动时需要输入的关节变量。本实施例中，拟采用权重系数法将多解优化问题简化为单目标优化问题，第i组逆解的评价函数为：
S3:根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
本发明通过运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向，并根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度，并根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷，其实现了在脱离人工控制的情况下对隧道面的快速喷涂。
所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取方法包括步骤：
S01：在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
所述各个弧形钢拱依次等间距的设置在隧道中。
所述步骤S01中，并对待喷区域垂直切割为宽度相等的两个区域，具体为：
通过点云切片技术利用与X＝0平面平行的平面将待喷区域垂直切割为宽度相等的两个区域。
具体地说，是对待喷区域以与隧道中轴线垂直的方向，垂直切割为宽度相等的两个区域。
S02：获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
S03：通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
S04：沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
本发明通过获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，以此得到形成隧道湿喷轨迹的待喷点，并依次连接各待喷点以获取完整的隧道湿喷轨迹，其提高了隧道湿喷轨迹的精准度。
在本实施例中，采用S形轨迹进行喷浆。考虑到实际湿喷车及臂架朝向，臂架前方朝向隧道外侧，而1关节连杆的运动范围为±180°，故将隧道分左右两半分别进行轨迹规划，从隧道底部起喷，喷枪末端沿着隧道湿喷轨迹在隧道两侧来回运动，层层递进，以实现对隧道连续、全面、自动的快速喷涂。
在实际的喷浆支护作业中，喷浆轨迹规划是极为重要的一个问题，只有准确并连续的规划出了隧道湿喷轨迹，才能通过本发明的预设逆向运动学模型控制机械臂精准的以隧道湿喷轨迹为运动喷涂轨迹对隧道进行全面、连续、快速的自动喷涂。
实施例二
如图2所示，本发明还提出了一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的系统，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述系统包括：
获取模块，用于根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
旋转角度获取模块，用于根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
控制模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
所述系统还包括：
验证模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确。
本发明通过预设正向运动学模型对预设逆向运动学模型求出来的解析解进行校验，其提高了解析解的准确度，因而进一步提高了喷浆机机械臂在喷涂过程中的喷涂精度。
所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取包括：
切割模块，用于在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
交点获取模块，用于获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
法向量获取模块，用于通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
轨迹获取模块，用于沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中如涉及“第一”、“第二”、“一”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
Claims (9)
1.一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，其特征在于，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述方法包括步骤：
S1：根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
S2：根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
S3:根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
2.根据权利要求1所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，其特征在于，所述步骤S2至S3之间还包括旋转角度的验证，具体为：
S21：根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确，进入步骤S3。
3.根据权利要求2所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述机械臂的关节连杆包括依次连接的：
第一旋转关节连杆、第二旋转关节连杆、第三旋转关节连杆、第四伸缩关节连杆、第五伸缩关节连杆、第六旋转关节连杆以及第七旋转关节连杆，所述第四伸缩关节连杆与第五伸缩关节连杆合为一个移动关节连杆，其中第七旋转关节连杆与喷头连接。
4.根据权利要求3所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述预设正向运动学模型的建模过程为：
第一步：设定隧道湿喷轨迹中任意一点的坐标为(a，b，c)；
第二步：根据坐标(a，b，c)得到移动关节连杆对应平移操作的齐次变换矩阵为：
第三步：通过对旋转关节连杆做旋转角度为θ的旋转变换，得到旋转关节连杆对应旋转操作在x、y、z轴三个方向上的变换矩阵分别为：
式中，Rot(x,θ)为旋转关节连杆对应旋转操作在x轴方向上的变换矩阵；
式中，Rot(y,θ)为旋转关节连杆对应旋转操作在y轴方向上的变换矩阵；
式中，Rot(z,θ)为旋转关节连杆对应旋转操作在z轴方向上的变换矩阵；
第四步，通过对x轴、y轴、z轴方向上的变换矩阵以及齐次变换矩阵四个公式进行相乘，得到相邻关节连杆的传递变换矩阵通式为：
式中，i＝1，2，3，4，5，6；θi为第i个关节连杆的旋转角度；di为第i个关节连杆的偏距；αi为第i个关节连杆的预设扭角；ai为第i个关节连杆的预设杆长；Ti-1,i为第i-1个关节连杆至第i个关节连杆的变换矩阵，其中，当i＝1时，T0,1表示从机械臂基座建立的预设基坐标系至第一个关节连杆的变换矩阵；
第五步，获取预设正向运动学模型，所述预设正向运动学模型即各个相邻关节连杆间其传递变换矩阵通式的乘积，其表达式为：
式中，T12表示第一旋转关节连杆至第二旋转关节连杆的变换矩阵；T23表示第二旋转关节连杆至第三旋转关节连杆的变换矩阵；T34表示第三旋转关节连杆至移动关节连杆的变换矩阵；T45表示移动关节连杆至第六旋转关节连杆的变换矩阵；T56表示第六旋转关节连杆至第七旋转关节连杆的变换矩阵；[Px,Py,Pz]T表示喷头的位置；[nx,ny,nz]T，[ox,oy,oz]T，[ax,ay,az]T表示喷头的朝向；T06表示预设基坐标系至喷头的变换矩阵即预设正向运动学模型。
5.根据权利要求4所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述步骤S2具体为：
第一步，根据步骤S1中获取的喷头在运动位姿轨迹上各点的位置与朝向，设定预设基坐标系至喷头的变换矩阵为R，其表达式为：
并通过预设正向运动学模型得到：
R＝T06＝T01×T12×T23×T34×T45×T56；
第二步，固定第三旋转关节连杆与第四伸缩关节连杆，即对其参数进行预设定，所述参数包含旋转角度、偏距、预设扭角、预设杆长；获取各相邻关节连杆间传递变换矩阵通式的逆，并通过预设基坐标系至喷头的变换矩阵与相邻关节连杆间传递变换矩阵通式的逆，获取各关节连杆的旋转角度。
6.根据权利要求1所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取方法包括步骤：
S01：在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
S02：获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
S03：通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
S04：沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
7.一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的系统，所述机械臂包括若干个依次连接的关节连杆，所述机械臂的末端关节连杆连接有喷头，其特征在于，所述隧道湿喷轨迹即喷浆机在喷涂过程中其喷头的运动位姿轨迹，所述系统包括：
获取模块，用于根据运动位姿轨迹获取喷头在该轨迹上各点的位置与朝向；
旋转角度获取模块，用于根据喷头的位置与朝向利用预设逆向运动学模型，获取喷浆机喷头沿运动位姿轨迹进行湿喷时在轨迹各点处各关节连杆的旋转角度；
控制模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度控制机械臂的运动，以对待喷区域进行涂喷。
8.根据权利要求7所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法，其特征在于，所述系统还包括：
验证模块，用于根据喷头在轨迹各点处各关节连杆的旋转角度利用预设正向运动学模型获取喷头在轨迹各点处的位置与朝向为验证位置与验证朝向；并依次将各点的验证位置与验证朝向分别与步骤S1中对应点的位置与朝向进行比对，比对一致，则表示各点对应的各关节连杆的旋转角度求解正确。
9.根据权利要求7所述的一种基于隧道湿喷轨迹控制喷浆机机械臂喷涂的系统，所述隧道中设置有与隧道中轴线垂直的若干个弧形钢拱，所述隧道主体模型通过点云数据进行体现，其特征在于，所述隧道湿喷轨迹的获取包括：
切割模块，用于在隧道主体模型中获取相邻钢拱之间的区域为待喷区域，并对待喷区域垂直切割为宽度相等的两个区域，分别为左喷区域与右喷区域；
交点获取模块，用于获取左喷区域与右喷区域中距离切割面最近的点云，并依次获取左喷区域中最近点至右喷区域中各最近点之间的距离，以获取距离最小的两个最近点为匹配点，连接匹配点，以获取连接线与切割面之间的交点；
法向量获取模块，用于通过交点获取隧道面其点云数据的表面法向量，该表面法向量为内侧方向法向量；
轨迹获取模块，用于沿表面法向量的方向将各交点偏移预设距离，得到偏移后的交点为待喷点，依次连接各待喷点以获取隧道湿喷轨迹。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210775344.4A CN115142877A (zh) | 2022-07-01 | 2022-07-01 | 基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210775344.4A CN115142877A (zh) | 2022-07-01 | 2022-07-01 | 基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115142877A true CN115142877A (zh) | 2022-10-04 |
Family
ID=83409684
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210775344.4A Pending CN115142877A (zh) | 2022-07-01 | 2022-07-01 | 基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115142877A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117001662A (zh) * | 2023-07-20 | 2023-11-07 | 北京纳通医用机器人科技有限公司 | 一种机械臂控制方法、装置、设备及存储介质 |
| CN117001662B (zh) * | 2023-07-20 | 2024-05-31 | 北京纳通医用机器人科技有限公司 | 一种机械臂控制方法、装置、设备及存储介质 |
-
2022
- 2022-07-01 CN CN202210775344.4A patent/CN115142877A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117001662A (zh) * | 2023-07-20 | 2023-11-07 | 北京纳通医用机器人科技有限公司 | 一种机械臂控制方法、装置、设备及存储介质 |
| CN117001662B (zh) * | 2023-07-20 | 2024-05-31 | 北京纳通医用机器人科技有限公司 | 一种机械臂控制方法、装置、设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107901041B (zh) | 一种基于图像混合矩的机器人视觉伺服控制方法 | |
| CN111347431B (zh) | 一种手持工具示教的机器人示教喷涂方法和装置 | |
| CN108789404B (zh) | 一种基于视觉的串联机器人运动学参数标定方法 | |
| US5781705A (en) | Method and apparatus for controlling the motion of a redundancy manipulator | |
| CN108621162A (zh) | 一种机械臂运动规划方法 | |
| CN101362511B (zh) | 基于四个定位器的飞机部件位姿调整协同控制方法 | |
| CN113127989B (zh) | 一种六自由度机械臂逆运动学解析求解控制方法 | |
| CN112091979A (zh) | 一种基于位置级逆运动学的七自由度机械臂限位优化方法 | |
| US10926372B2 (en) | Automatic polishing system | |
| CN115211970A (zh) | 机械臂的位姿变换方法、系统及机械臂组件 | |
| CN112405519A (zh) | 上下料机器人运动轨迹规划方法 | |
| CN110039548B (zh) | 一种拼装机控制方法、装置及设备 | |
| CN111993414A (zh) | 一种机械臂多关节联动控制方法 | |
| CN115142877A (zh) | 基于隧道湿喷轨迹控制喷浆机机械臂喷涂的方法与系统 | |
| CN116330267A (zh) | 一种基于工业机器人腕部奇异点计算的控制方法 | |
| CN111430022A (zh) | 一种穿刺手术机器人穿刺算法 | |
| KR20170135150A (ko) | 곡블록 협소구간 작업이 가능한 용접로봇 시스템의 제어방법 | |
| CN110026986B (zh) | 多臂协同作业柔顺控制方法 | |
| CN112156915B (zh) | 基于关节驱动负载的喷涂机器人安装位置确定方法及装置 | |
| CN114310915A (zh) | 基于视觉反馈的空间机械臂对接末端工具轨迹规划方法 | |
| JPH11191005A (ja) | ロボット制御装置 | |
| JP3884928B2 (ja) | 多関節ロボットの姿勢算出方法および姿勢算出装置 | |
| CN116175035B (zh) | 一种基于深度学习的钢结构高空焊接机器人智能焊接方法 | |
| CN116077190B (zh) | 一种提高手术机器人系统精度的系统 | |
| CN115305980B (zh) | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |