CN116175286A - 一种基于dmp的复杂曲面打磨轨迹规划方法 - Google Patents

一种基于dmp的复杂曲面打磨轨迹规划方法 Download PDF

Info

Publication number
CN116175286A
CN116175286A CN202211611015.2A CN202211611015A CN116175286A CN 116175286 A CN116175286 A CN 116175286A CN 202211611015 A CN202211611015 A CN 202211611015A CN 116175286 A CN116175286 A CN 116175286A
Authority
CN
China
Prior art keywords
polishing
track
point
curvature
dmp
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
Application number
CN202211611015.2A
Other languages
English (en)
Inventor
黄浩晖
杨辰光
董佳乐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Foshan Newhinken Intelligent Technology Co ltd
Original Assignee
Foshan Newhinken Intelligent Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Foshan Newhinken Intelligent Technology Co ltd filed Critical Foshan Newhinken Intelligent Technology Co ltd
Priority to CN202211611015.2A priority Critical patent/CN116175286A/zh
Publication of CN116175286A publication Critical patent/CN116175286A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B24—GRINDING; POLISHING
    • B24B—MACHINES, DEVICES, OR PROCESSES FOR GRINDING OR POLISHING; DRESSING OR CONDITIONING OF ABRADING SURFACES; FEEDING OF GRINDING, POLISHING, OR LAPPING AGENTS
    • B24B1/00—Processes of grinding or polishing; Use of auxiliary equipment in connection with such processes
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B24—GRINDING; POLISHING
    • B24B—MACHINES, DEVICES, OR PROCESSES FOR GRINDING OR POLISHING; DRESSING OR CONDITIONING OF ABRADING SURFACES; FEEDING OF GRINDING, POLISHING, OR LAPPING AGENTS
    • B24B51/00—Arrangements for automatic control of a series of individual steps in grinding a workpiece
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T1/00—General purpose image data processing
    • G06T1/0014—Image feed-back for automatic industrial control, e.g. robot with camera
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/64—Analysis of geometric attributes of convexity or concavity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/30—Computing systems specially adapted for manufacturing

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Economics (AREA)
  • Strategic Management (AREA)
  • Operations Research (AREA)
  • Mechanical Engineering (AREA)
  • Data Mining & Analysis (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Development Economics (AREA)
  • Geometry (AREA)
  • Game Theory and Decision Science (AREA)
  • Robotics (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Marketing (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Grinding And Polishing Of Tertiary Curved Surfaces And Surfaces With Complex Shapes (AREA)

Abstract

本发明提供一种基于DMP的复杂曲面打磨轨迹规划方法，包括利用深度相机获取打磨区域内的点云信息；在分割的打磨工件的点云数据中规划出打磨路径；计算打磨路径上各点的法向量和曲率，根据法向量计算打磨头在打磨点的姿态；基于打磨路径各点的曲率将打磨路径进行分割，将整条打磨路径轨迹分割成多个片段轨迹；对于每个片段轨迹分别用位置DMP1、方向DMP2、曲率DMP3对轨迹的位置和姿态、曲率进行建模；本发明能够实现轨迹分割，轨迹建模，打磨速度自适应调整；利用点云和DMP规划打磨路径，大大减少了在复杂曲面打磨时人的工作量，提高了打磨效率与打磨质量；使位置和姿态同步控制，根据曲率给进方向摩擦力对打磨速度进行控制。

Description

一种基于DMP的复杂曲面打磨轨迹规划方法
技术领域
本发明涉及机器人打磨技术领域，尤其是一种基于DMP的复杂曲面打磨轨迹规划方法。
背景技术
随着机器人技术的不断发展，以及机械制造成本、零部件成本的逐渐降低，机器人逐渐从工业领域渗透进了我们的日常生活中。
服务机器人的不断普及，渐渐提升了人们对机器人的接受程度，市场上出现的机器人也逐渐由单一的送餐、快递机器人，拓展到具有医疗护养、解说导览等丰富功能的机器人，这同时也对机器人的稳定性、功能性与实用性有了更高的要求。在机器人打磨领域中，打磨路径的规划仍然是一个难点，并且现有技术较少考虑打磨速度对打磨质量的影响。例如：
专利CN202011493670.3公开了一种基于机器视觉的打磨路径规划方法。其提出了一种基于打磨过程中的打磨参数对打磨路径进行优化的方法，然而其方法繁琐，且没有考虑打磨速度对打磨质量的影响。
专利CN202111384297.2公开了一种基于实时点云的大型复杂构件表面打磨路径规划方法。其提出了一种基于表面点云的迭代规划路径方法，考虑了表面曲率对打磨的影响。但是没有考虑打磨速度与表面粗糙度的影响。
发明内容
针对现有技术的不足，本发明提供一种基于DMP的复杂曲面打磨轨迹规划方法，本发明提出了一种自适应打磨速度的方法，大大减少了在复杂曲面打磨时人的工作量，提高了打磨效率与打磨质量。
本发明的技术方案为：一种基于DMP的复杂曲面打磨轨迹规划方法，包括以下步骤：
S1)、利用深度相机获取打磨区域内的点云信息，然后切割出带打磨工件部分的点云数据；
S2)、在分割的打磨工件的点云数据中规划出打磨路径；
S3)、计算打磨路径上各点的法向量和曲率，根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示；
S4)、基于打磨路径各点的曲率将打磨路径进行分割，将整条打磨路径轨迹分割成多个片段轨迹；
S5)、对于每个片段轨迹，用位置DMP1、和方向DMP2对每个片段轨迹的位置和姿态进行建模，并对打磨路径的曲率用曲率DMP3进行建模；
S6)、打磨时，控制器根据DMP1和DMP2分别控制打磨头的位置和姿态，打磨的速度由DMP3和打磨给进方向摩擦力共同决定。
作为优选的，步骤S1)中，利用深度相机获取打磨区域内的点云信息后通过pcl库可视化。
作为优选的，步骤S1)中，所述的切割出带打磨工件部分的点云数据为：在点云中找出工件的边界，根据边界将工件部分点云切割出。
作为优选的，步骤S2)中，根据打磨头的工作半径，在工件的点云中规划出打磨路径，使得打磨头沿着这些轨迹打磨可以覆盖整个工件的表面。
作为优选的，步骤S3)中，所述的打磨路径上各点法向量采用pcl::NormalEstimation获得。
作为优选的，步骤S3)中，所述的打磨路径上各点曲率的计算方法如下：
S31)、设打磨路径由点集P＝{p1,p2......pn}，每个点pi＝{Xi,Qi,ki}，Xi是该点的位置，Qi表示该点的姿态，ki表示该点的曲率；点pi的曲率ki由点pi-1、pi、pi+1计算得到；
S32)、曲率ki的计算方法为：
设Xi＝(xi,yi,zi)、Xi-1＝(xi-1,yi-1,zi-1)、Xi+1＝(xi+1,yi+1,zi+1)、并且以Xo＝(x0,y0,z0)为pi-1、pi、pi+1三点所在圆的圆心；建立如下方程：
Figure BDA0003999484310000031
联立(1)(2)消元得到：
Figure BDA0003999484310000034
Figure BDA0003999484310000035
记为：
A2＝2×(xi-1-xi)；
B2＝2×(yi-1-yi)；
C2＝2×(zi-1-zi)；
Figure BDA0003999484310000032
联立(1)(3)消元得到：
Figure BDA0003999484310000033
记为：
A3＝2×(xi+1-xi)
B3＝2×(yi+1-yi)
C3＝2×(Zi+1-zi)
Figure BDA0003999484310000041
根据三点共面约束可确定平面方程：
Figure BDA0003999484310000042
A1＝yi×zi-1-yi-1×zi-yi×zi+1+yi+1×zi+yi-1×zi+1-yi+1×zi-1
B1＝-(xi×zi-1-xi-1×zi-xi×zi+1+xi+1×zi+xi-1×zi+1-xi+1×zi-1)
C1＝xi×yi-1-xi-1×yi-xi×yi+1+xi+1×yi+xi-1×yi+1-xi+1×yi-1
D1＝-(xi×yi-1×zi+1-xi×yi+1×zi-1-xi-1×yi×zi+1+xi-1×yi+1×zi+xi+1×yi×zi-1-xi+1×yi-1×zi)
通过以上(4)(5)(6)获得A～D系数建立线性方程组，三个未知数三个方程即可求解圆心和半径：
Figure BDA0003999484310000043
Figure BDA0003999484310000044
Figure BDA0003999484310000045
Figure BDA0003999484310000046
式中，R为pi-1、pi、pi+1三点所在圆的半径。
作为优选的，步骤S3)中，所述的根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示，具体为：
设深度相机和机械臂基座之间的旋转矩阵为A，机械臂在打磨p点时末端
相对于基座的旋转矩阵H的计算方法为：
假设打磨点p的坐标为p＝(x,y,z)T，法向量为-a＝(-xf,-yf,-zf)T，以法向量a为z轴的单位向量建立坐标系，假设x轴和y轴的单位向量分别为b＝(x1,y1,y1)T，c＝(x2,y2,y2)T，其中x和y轴单位向量的选取有无数种可能，任意选择一种即可；当前所建立坐标系与相机坐标系之间的旋转矩阵H为：
Figure BDA0003999484310000051
当前坐标系与机器人基座之间的旋转矩阵Rt＝AH，旋转矩阵Rt转四元数通过scipy.spatial.transform中的函数求得。
作为优选的，步骤S4)中，整条打磨路径轨迹点集为P＝{p1,p2......pn}，根据曲率将打磨路径分成多段，以提高DMP对轨迹的拟合精度，分割方法为：
通过设置分界曲率k*，然后找出满足(ki-k*)*(ki-1-k*)＜0的点集，以它们为分割点对打磨路径轨迹点集P进行切割；分割完后，片段轨迹集合表示为pl＝{p1,p2......pm}，其中，pj(1≤j≤m)表示第j个片段轨迹的点的集合。
作为优选的，步骤S5)中，对于分割后的每个片段轨迹pj，用DMP1对轨迹内的位置和刚度进行建模，用方向DMP2对姿态进行建模，用DMP3对曲率进行建模。
作为优选的，步骤S5)中，用DMP1对轨迹内的位置和刚度进行建模表示为：
Figure BDA0003999484310000052
Figure BDA0003999484310000053
式中，f(x)为拟合时的跟随项，该项用于控制实际轨迹的形状，由高斯核Φ(x)的线性组合和来自于正则***的时间衰减系数x组成；Y为示教点轨迹，τ为控制使用时中机器人末端运动速度的时间常数，g定义为使用示教轨迹时定义的机器人末端位置，Z、
Figure BDA0003999484310000061
分别表示经过时间常数τ放缩后的示教轨迹速度与示教轨迹加速度，αz、βz为控制器的控制参数，/>
Figure BDA0003999484310000062
表示示教轨迹速度；
学习示教轨迹Y的过程为通过已知示教点拟合跟随项f(x)的调整参数ωi；轨迹开始生成时，跟随项f(x)通过修改机器人末端加速度从而改变轨迹方向，当时间趋于无穷时，强迫跟随项f(x)趋于0；轨迹回到指定点；所述的跟随项f(x)表示为：
Figure BDA0003999484310000063
式中，ωi表示第i个调整参数，Φi(x)表示第i个高斯核，调整参数与高斯核共有N个，N由用户指定，x为时间衰减系数；
第i个高斯核Φi(x)的生成方式如下：
Φi(x)＝exp(-hi(x-ci)2)；
其中，hi表示第i个高斯核的宽度、ci分表示第i个高斯核的中心，均由用户指定；x为时间衰减系数；
时间衰减系数x生成方式如下：
Figure BDA0003999484310000064
式中，
Figure BDA0003999484310000065
表示时间衰减项，αx为用户指定的常数；
调整参数ωi拟合方式如下：
Figure BDA0003999484310000066
式中，ftarget为拟合时由已知的轨迹生成的目标跟随项，T表示矩阵的转置；
学习示教轨迹的过程为通过已知示教点拟合目标方程ftarget；通过更新调整参数ωi，使f(x)的输出靠近ftarget：
Figure BDA0003999484310000071
示教轨迹生成的过程是由已知的示教轨迹y学习f(x)使其成为目标跟随项ftarget，而重现的过程为由已知跟随项ftarget生成重现轨迹yr；
式中，
Figure BDA0003999484310000072
表示示教轨迹加速度。
作为优选的，步骤S5)中，用方向DMP对四元数进行建模表示为：
Figure BDA0003999484310000073
Figure BDA0003999484310000074
式中，q∈S3为单位四元数，ω∈R3、
Figure BDA0003999484310000075
分别为角速度和加速度，τ为时间尺度因子，*表示两个四元数的乘积，e(,)表示两个四元数之间的误差，/>
Figure BDA0003999484310000076
为角速度四元数，
Figure BDA0003999484310000077
即/>
Figure BDA0003999484310000078
为一个以标量部分为零，角速度为矢量部分的四元数；/>
Figure BDA0003999484310000079
为q的导数，S3为三维空间中的单位球体、R3表示三维空间。
作为优选的，步骤S6)中，为了提高打磨的质量，在曲率大和粗糙度高的地方减慢打磨速率，通过控制DMP1和DMP2中的时间尺度因子τ对打磨速度进行控制，具体如下：
Figure BDA00039994843100000710
Figure BDA00039994843100000711
Figure BDA00039994843100000712
v＝vt-vt-1；
其中，τ(t)适用于DMP1和DMP2、DMP3，
Figure BDA0003999484310000081
为设置的曲率阈值，τmax为时间系数的最大值，f是打磨前进方向上的打磨力，F是设置参考摩擦力，k(t)是DMP3的输出，u＝(fx,fy,fz)是力传感器在三个方向的力组成的向量，vt、vt-1分别表示DMP1在当前时刻t和上一个时刻t-1的输出；v为DMP1在当前时刻t和上一个时刻t-1的输出之差，通过上述方式，可以在曲率大和粗糙度高的地方延长打磨的时间，提升打磨的质量。
本发明的有益效果为：
1、本发明能够实现集成了轨迹分割，轨迹建模，打磨速度自适应调整；利用点云和DMP规划打磨路径，大大减少了在复杂曲面打磨时人的工作量，提高了打磨效率与打磨质量；
2、本发明根据曲率将打磨轨迹进行切割，通过分段建模，从而提高了精确度，用位置和方向DMP对打磨位置和姿态进行建模，使位置和姿态同步控制，根据曲率、给进方向摩擦力对打磨速度进行控制，提升了打磨质量。
附图说明
图1为本发明方法的流程框架图；
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
如图1所示，本实施例提供一种基于DMP的复杂曲面打磨轨迹规划方法，包括以下步骤：
S1)、利用深度相机获取打磨区域内的点云信息，并通过pcl库可视化；
然后在点云信息中找出工件的边界，根据边界将工件部分的点云切割出来；
S2)、在分割的打磨工件的点云数据中规划出打磨路径；本实施例中，通过打磨头的工作半径，在工件的点云中规划出打磨路径，使得打磨头沿着这些轨迹打磨可以覆盖整个工件的表面；
S3)、计算打磨路径上各点的法向量和曲率，根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示；
S4)、基于打磨路径各点的曲率将打磨路径进行分割，将整条打磨路径轨迹分割成多个片段轨迹；
S5)、对于每个片段轨迹，用位置DMP1、和方向DMP2对轨迹的位置和姿态进行建模，并对打磨路径的曲率用曲率DMP3进行建模；
S6)、打磨时，DMP1和DMP2分别控制打磨头的位置和姿态，打磨的速度由DMP3和打磨给进方向摩擦力共同决定。
作为本实施例优选的，步骤S3)中，所述的打磨路径上各点法向量采用pcl::NormalEstimation获得。
作为本实施例优选的，步骤S3)中，所述的打磨路径上各点曲率的计算方法如下：
S31)、设打磨路径由点集P＝{p1,p2......pn}构成，每个点pi＝{Xi,Qi,ki}，Xi为该点的位置，Qi表示该点的姿态，ki表示该点的曲率；点pi的曲率ki由点pi-1、pi、pi+1计算得到；
S32)、曲率ki的计算方法为：
设Xi＝(xi,yi,zi)、Xi-1＝(xi-1,yi-1,zi-1)、Xi+1＝(xi+1,yi+1,zi+1)、并且以Xo＝(x0,y0,z0)为pi-1、pi、pi+1三点所在圆的圆心；建立如下方程：
Figure BDA0003999484310000101
联立(1)(2)消元得到：
Figure BDA0003999484310000102
记为：
A2＝2×(xi-1-xi)；
B2＝2×(yi-1-yi)；
C2＝2×(zi-1-zi)；
Figure BDA0003999484310000103
联立(1)(3)消元得到：
Figure BDA0003999484310000104
记为：
A3＝2×(xi+1-xi)
B3＝2×(yi+1-yi)
C3＝2×(zi+1-zi)
Figure BDA0003999484310000105
根据三点共面约束可确定平面方程：
Figure BDA0003999484310000106
A1＝yi×zi-1-yi-1×zi-yi×zi+1+yi+1×zi+yi-1×zi+1-yi+1×zi-1
B1＝-(xi×zi-1-xi-1×zi-xi×zi+1+xi+1×zi+xi-1×zi+1-xi+1×zi-1)
C1＝xi×yi-1-xi-1×yi-xi×yi+1+xi+1×yi+xi-1×yi+1-xi+1×yi-1
D1＝-(xi×yi-1×zi+1-xi×yi+1×zi-1-xi-1×yi×zi+1+xi-1×yi+1×zi
+xi+1×yi×zi-1-xi+1×yi-1×zi)
通过以上(4)(5)(6)获得A～D系数建立线性方程组，三个未知数三个方程即可求解打磨头的工作圆心和半径：
Figure BDA0003999484310000111
Figure BDA0003999484310000112
Figure BDA0003999484310000113
/>
Figure BDA0003999484310000114
式中，R为pi-1、pi、pi+1三点所在圆的半径，xi,yi,zi为位置Xi的坐标。
作为本实施例优选的，步骤S3)中，所述的根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示，具体为：
设深度相机和机械臂基座之间的旋转矩阵为A，机械臂在打磨点p时末端相对于基座的旋转矩阵H的计算方法为：
假设打磨点p的坐标为p＝(x,y,z)T，法向量为-a＝(-xf,-yf,-zf)T，以法向量a为z轴的单位向量建立坐标系，假设x轴和y轴的单位向量分别为b＝(x1,y1,y1)T，c＝(x2,y2,y2)T，其中x和y轴单位向量的选取有无数种可能，任意选择一种即可；当前所建立坐标系与相机坐标系之间的旋转矩阵H为：
Figure BDA0003999484310000121
当前坐标系与机器人基座之间的旋转矩阵Rt＝AH，旋转矩阵Rt转四元数通过scipy.spatial.transform中的函数求得。
作为优选的，步骤S4)中，整条打磨路径轨迹点集为P＝{p1,p2......pn}，根据曲率将打磨路径分成多段，以提高DMP对轨迹的拟合精度，分割方法为：
通过设置分界曲率k*，然后找出满足(ki-k*)*(ki-1-k*)＜0的点集，以它们为分割点对打磨路径轨迹点集P进行切割；分割完后，片段轨迹集合表示为pl＝{p1,p2......pm}，其中，pj(1≤j≤m)表示第j个片段轨迹的点的集合。
作为优选的，步骤S5)中，对于分割后的每个片段轨迹pj，用DMP1对轨迹内的位置Xj和刚度进行建模，用方向DMP2对四元数Qj进行建模，用DMP3对曲率进行建模。
作为优选的，步骤S5)中，用DMP1对轨迹内的位置和刚度进行建模表示为：
Figure BDA0003999484310000122
Figure BDA0003999484310000123
式中，f(x)为拟合时的跟随项，该项用于控制实际轨迹的形状，由高斯核Φ(x)的线性组合和来自于正则***的时间衰减系数x组成；Y为示教点轨迹，τ为控制使用时中机器人末端运动速度的时间常数，g定义为使用示教轨迹时定义的机器人末端位置，Z、
Figure BDA0003999484310000124
分别表示经过时间常数τ放缩后的示教轨迹速度与示教轨迹加速度，αz、βz为控制器的控制参数，
Figure BDA0003999484310000125
表示示教轨迹速度；
学习示教轨迹Y的过程为通过已知示教点拟合跟随项f(x)的调整参数ωi；轨迹开始生成时，跟随项f(x)通过修改机器人末端加速度从而改变轨迹方向，当时间趋于无穷时，强迫跟随项f(x)趋于0；轨迹回到指定点；所述的跟随项f(x)表示为：
Figure BDA0003999484310000131
式中，ωi表示第i个调整参数，Φi(x)表示第i个高斯核，调整参数与高斯核共有N个，N由用户指定，x为时间衰减系数；
第i个高斯核Φi(x)的生成方式如下：
Φi(x)＝exp(-hi(x-ci)2)；
其中，hi表示第i个高斯核的宽度、ci分表示第i个高斯核的中心，均由用户指定；x为时间衰减系数；
时间衰减系数x生成方式如下：
Figure BDA0003999484310000132
式中，
Figure BDA0003999484310000133
表示时间衰减项，αx为用户指定的常数；
调整参数ωi拟合方式如下：
Figure BDA0003999484310000134
式中，ftarget为拟合时由已知的轨迹生成的目标跟随项，T表示矩阵的转置；
学习示教轨迹的过程为通过已知示教点拟合目标方程ftarget；通过更新调整参数ωi，使f(x)的输出靠近ftarget：
Figure BDA0003999484310000135
示教轨迹生成的过程是由已知的示教轨迹y学习f(x)使其成为目标跟随项ftarget，而重现的过程为由已知跟随项ftarget生成重现轨迹yr；
式中，
Figure BDA0003999484310000141
表示示教轨迹加速度。
作为优选的，步骤S5)中，用方向DMP2对姿态进行建模表示为：
Figure BDA0003999484310000142
Figure BDA0003999484310000143
式中，q∈S3为单位四元数，ω∈R3、
Figure BDA0003999484310000144
分别为角速度和加速度，τ为时间尺度因子，*表示两个四元数的乘积，e(,)表示两个四元数之间的误差，/>
Figure BDA00039994843100001416
为角速度四元数，
Figure BDA00039994843100001417
即/>
Figure BDA00039994843100001418
为一个以标量部分为零，角速度为矢量部分的四元数；/>
Figure BDA00039994843100001411
为q的导数，S3为三维空间中的单位球体、R3表示三维空间。
作为优选的，步骤S6)中，为了提高打磨的质量，在曲率大和粗糙度高的地方减慢打磨速率，通过控制DMP1和DMP2中的时间尺度因子τ对打磨速度进行控制，具体如下：
Figure BDA00039994843100001412
Figure BDA00039994843100001415
Figure BDA00039994843100001413
v＝vt-vt-1；
其中，τ(t)适用于DMP1和DMP2、DMP3，
Figure BDA00039994843100001414
为设置的曲率阈值，τmax为时间系数的最大值，f是打磨前进方向上的打磨力，F是设置参考摩擦力，k(t)是DMP3输出的曲率，u＝(fx,fy,fz)是力传感器在三个方向的力组成的向量，vt、vt-1分别表示DMP1在当前时刻t和上一个时刻t-1的输出；v为DMP1在当前时刻t和上一个时刻t-1的输出之差，通过上述方式，可以在曲率大和粗糙度高的地方延长打磨的时间，提升打磨的质量。
上述实施例和说明书中描述的只是说明本发明的原理和最佳实施例，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。

Claims (9)

1.一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于，包括以下步骤：
S1)、利用深度相机获取打磨区域内的点云信息；
S2)、在分割的打磨工件的点云数据中规划出打磨路径；通过打磨头的工作半径，在工件的点云中规划出打磨路径，使得打磨头沿着这些轨迹打磨可以覆盖整个工件的表面；
S3)、计算打磨路径上各点的法向量和曲率，根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示；
S4)、基于打磨路径各点的曲率将打磨路径进行分割，将整条打磨路径轨迹分割成多个片段轨迹；
S5)、对于每个片段轨迹，用位置DMP1、和方向DMP2对轨迹的位置和姿态进行建模，并对打磨路径的曲率用曲率DMP3进行建模；
S6)、打磨时，控制器根据DMP1和DMP2分别控制打磨头的位置和姿态，打磨的速度由DMP3和打磨给进方向摩擦力共同决定。
2.根据权利要求1所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：
步骤S3)中，所述的打磨路径上各点法向量采用pcl::NormalEstimation获得。
3.根据权利要求1所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S3)中，所述的打磨路径上各点曲率的计算方法如下：
S31)、设打磨路径由点集P＝{p1,p2......pn}构成，每个点pi＝{Xi,Qi,ki}，Xi为该点的位置，Qi表示该点的姿态，ki表示该点的曲率；点pi的曲率ki由点pi-1、pi、pi+1计算得到；
S32)、曲率ki的计算方法为：
设定Xi＝(xi,yi,zi)、Xi-1＝(xi-1,yi-1,zi-1)、Xi+1＝(xi+1,yi+1,zi+1)，并且以Xo＝(x0,y0,z0)作为三点所在圆的圆心；建立如下方程：
Figure FDA0003999484300000021
联立(1)(2)消元得到：
Figure FDA0003999484300000022
记为：
A2＝2×(xi-1-xi)；
B2＝2×(yi-1-yi)；
C2＝2×(zi-1-zi)；
Figure FDA0003999484300000023
联立(1)(3)消元得到：
Figure FDA0003999484300000024
记为：
A3＝2×(xi+1-xi)
B3＝2×(yi+1-yi)
C3＝2×(zi+1-zi)
Figure FDA0003999484300000025
根据三点共面约束可确定平面方程：
Figure FDA0003999484300000031
A1＝yi×zi-1-yi-1×zi-yi×zi+1+yi+1×zi+yi-1×zi+1-yi+1×zi-1
B1＝-(xi×zi-1-xi-1×zi-xi×zi+1+xi+1×zi+xi-1×zi+1-xi+1×zi-1)
C1＝xi×yi-1-xi-1×yi-xi×yi+1+xi+1×yi+xi-1×yi+1-xi+1×yi-1
D1＝-(xi×yi-1×zi+1-xi×yi+1×zi-1-xi-1×yi×zi+1+xi-1×yi+1×zi
+xi+1×yi×zi-1-xi+1×yi-1×zi)
通过以上(4)(5)(6)获得A～D系数建立线性方程组，三个未知数三个方程即可求解打磨头的工作圆心和半径：
Figure FDA0003999484300000032
Figure FDA0003999484300000033
Figure FDA0003999484300000034
Figure FDA0003999484300000035
式中，R为pi-1、pi、pi+1所在圆的半径，xi,yi,zi为位置Xi的坐标。
4.根据权利要求3所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S3)中，所述的根据法向量计算打磨头在打磨点的姿态，并用四元数形式表示，具体为：设深度相机和机械臂基座之间的旋转矩阵为A，机械臂在打磨点p时末端相对于基座的旋转矩阵H的计算方法为：
假设打磨点p的坐标为p＝(x,y,z)T，法向量为-a＝(-xf,-yf,-zf)T，以法向量a为z轴的单位向量建立坐标系，假设x轴和y轴的单位向量分别为b＝(x1,y1,y1)T，c＝(x2,y2,y2)T，其中x和y轴单位向量的选取有无数种可能，任意选择一种即可；当前所建立坐标系与相机坐标系之间的旋转矩阵H为：
Figure FDA0003999484300000041
当前坐标系与机器人基座之间的旋转矩阵Rt＝AH，旋转矩阵Rt转四元数通过scipy.spatial.transform中的函数求得。
5.根据权利要求1所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S4)中，整条打磨路径轨迹点集为P＝{p1,p2......pn}，根据曲率将打磨路径分成多段，以提高DMP对轨迹的拟合精度，分割方法为：
通过设置分界曲率k*，然后找出满足(ki-k*)*(ki-1-k*)＜0的点集，以它们为分割点对打磨路径轨迹点集P进行切割；分割完后，片段轨迹集合表示为pl＝{p1,p2......pm}，其中，pj(1≤j≤m)表示第j个片段轨迹的点的集合。
6.根据权利要求5所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S5)中，对于分割后的每个片段轨迹pj，用DMP1对轨迹内的位置Xj和刚度进行建模，表示为：
Figure FDA0003999484300000042
Figure FDA0003999484300000043
式中，f(x)为拟合时的跟随项，该项用于控制实际轨迹的形状，由高斯核Φ(x)的线性组合和来自于正则***的时间衰减系数x组成；Y为示教点轨迹，τ为控制使用时中机器人末端运动速度的时间常数，g定义为使用示教轨迹时定义的机器人末端位置，Z、
Figure FDA0003999484300000044
分别表示经过时间常数τ放缩后的示教轨迹速度与示教轨迹加速度，αz、βz为控制器的控制参数，/>
Figure FDA0003999484300000045
表示示教轨迹速度；
学习示教轨迹Y的过程为通过已知示教点拟合跟随项f(x)的调整参数ωi；轨迹开始生成时，跟随项f(x)通过修改机器人末端加速度从而改变轨迹方向，当时间趋于无穷时，强迫跟随项f(x)趋于0；轨迹回到指定点；所述的跟随项f(x)表示为：
Figure FDA0003999484300000051
式中，ωi表示第i个调整参数，Φi(x)表示第i个高斯核，调整参数与高斯核共有N个，N由用户指定，x为时间衰减系数；
第i个高斯核Φi(x)的生成方式如下：
Φi(x)＝exp(-hi(x-ci)2)；
其中，hi表示第i个高斯核的宽度、ci分表示第i个高斯核的中心，均由用户指定；x为时间衰减系数；
时间衰减系数x生成方式如下：
Figure FDA0003999484300000052
/>
式中，
Figure FDA0003999484300000053
表示时间衰减项，αx为用户指定的常数；
调整参数ωi拟合方式如下：
Figure FDA0003999484300000054
式中，ftarget为拟合时由已知的轨迹生成的目标跟随项，T表示矩阵的转置；
学习示教轨迹的过程为通过已知示教点拟合目标方程ftarget；通过更新调整参数ωi，使f(x)的输出靠近ftarget：
Figure FDA0003999484300000055
示教轨迹生成的过程是由已知的示教轨迹y学习f(x)使其成为目标跟随项ftarget，而重现的过程为由已知跟随项ftarget生成重现轨迹yr；
式中，
Figure FDA0003999484300000061
表示示教轨迹加速度。
7.根据权利要求6所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S5)中，对于分割后的每个片段轨迹pj，用方向DMP2对姿态进行建模表示为：
Figure FDA0003999484300000062
Figure FDA0003999484300000063
式中，q∈S3为单位四元数，ω∈R3、
Figure FDA0003999484300000064
分别为角速度和加速度，τ为时间尺度因子，*表示两个四元数的乘积，e(,)表示两个四元数之间的误差，/>
Figure FDA0003999484300000065
为角速度四元数，
Figure FDA0003999484300000066
即/>
Figure FDA0003999484300000067
为一个以标量部分为零，角速度为矢量部分的四元数；/>
Figure FDA0003999484300000068
为q的导数，S3为三维空间中的单位球体、R3表示三维空间。
8.根据权利要求6所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S6)中，为了提高打磨的质量，在曲率大和粗糙度高的地方减慢打磨速率，通过控制DMP1和DMP2中的时间尺度因子τ对打磨速度进行控制，具体如下：
Figure FDA0003999484300000069
Figure FDA00039994843000000610
Figure FDA00039994843000000611
v＝vt-vt-1；
其中，τ(t)适用于DMP1和DMP2、DMP3，
Figure FDA0003999484300000071
为设置的曲率阈值，τmax为时间系数的最大值，f是打磨前进方向上的打磨力，F是设置参考摩擦力，k(t)是DMP3输出的曲率，u＝(fx,fy,fz)是力传感器在三个方向的力组成的向量，vt、vt-1分别表示DMP1在当前时刻t和上一个时刻t-1的输出；v为DMP1在当前时刻t和上一个时刻t-1的输出之差，通过上述方式，可以在曲率大和粗糙度高的地方延长打磨的时间，提升打磨的质量。
9.根据权利要求1所述的一种基于DMP的复杂曲面打磨轨迹规划方法，其特征在于：步骤S1)中，在获取点云信息后并通过pcl库可视化；然后在点云信息中找出工件的边界，根据边界将工件部分的点云切割出来。
CN202211611015.2A 2022-12-14 2022-12-14 一种基于dmp的复杂曲面打磨轨迹规划方法 Pending CN116175286A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211611015.2A CN116175286A (zh) 2022-12-14 2022-12-14 一种基于dmp的复杂曲面打磨轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211611015.2A CN116175286A (zh) 2022-12-14 2022-12-14 一种基于dmp的复杂曲面打磨轨迹规划方法

Publications (1)

Publication Number Publication Date
CN116175286A true CN116175286A (zh) 2023-05-30

Family

ID=86441215

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211611015.2A Pending CN116175286A (zh) 2022-12-14 2022-12-14 一种基于dmp的复杂曲面打磨轨迹规划方法

Country Status (1)

Country Link
CN (1) CN116175286A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116985150A (zh) * 2023-09-28 2023-11-03 北京市农林科学院智能装备技术研究中心 串果柔顺收集轨迹规划方法及装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116985150A (zh) * 2023-09-28 2023-11-03 北京市农林科学院智能装备技术研究中心 串果柔顺收集轨迹规划方法及装置
CN116985150B (zh) * 2023-09-28 2023-12-26 北京市农林科学院智能装备技术研究中心 串果柔顺收集轨迹规划方法及装置

Similar Documents

Publication Publication Date Title
US11446820B2 (en) Robot path generating device and robot system
Liao et al. Region-based toolpath generation for robotic milling of freeform surfaces with stiffness optimization
CN110298854B (zh) 基于在线自适应与单目视觉的飞行蛇形臂协同定位方法
CN112666831A (zh) 一种机器人磨抛加工接触力的主动控制方法
CN116175286A (zh) 一种基于dmp的复杂曲面打磨轨迹规划方法
CN109202864B (zh) 评价信息输出装置、评价信息输出方法以及存储介质
Hirano et al. Image-based object recognition and dexterous hand/arm motion planning using rrts for grasping in cluttered scene
Fitzpatrick et al. Humanoids
Lagneau et al. Active deformation through visual servoing of soft objects
Strub et al. Using haptics to extract object shape from rotational manipulations
CN113910232B (zh) 一种自适应姿态跟踪方法、装置、存储介质及电子设备
Ma et al. Stylized robotic clay sculpting
Cheng et al. Trajectory planning method with grinding compensation strategy for robotic propeller blade sharpening application
Song et al. Distortion-free robotic surface-drawing using conformal mapping
Liu et al. Multi-fingered tactile servoing for grasping adjustment under partial observation
CN111709095B (zh) 一种面向复杂曲面6d虚拟夹具构造方法
Zhou et al. Combination of dynamical movement primitives with trajectory segmentation and node mapping for robot machining motion learning
Radmehr et al. Experimental Study on the Imitation of the Human Head-and-Eye Pose Using the 3-DOF Agile Eye Parallel Robot with ROS and Mediapipe Framework
Long et al. Robotic cutting of soft materials using force control & image moments
CN113618728A (zh) 机器人运动轨迹的补偿方法、装置及计算机存储介质
CN114734435B (zh) 一种基于超球面的编码器校准方法、装置及***
CN109542094B (zh) 无期望图像的移动机器人视觉镇定控制
Okamura Haptic exploration of unknown objects
Lepora et al. Pose-based servo control with soft tactile sensing
KR102623672B1 (ko) 모션 트래킹 로봇의 원격 제어 방법

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