CN113103241A - 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及*** - Google Patents

用于实现g2连续的机器人双nurbs轨迹插补方法、装置及*** Download PDF

Info

Publication number
CN113103241A
CN113103241A CN202110473012.6A CN202110473012A CN113103241A CN 113103241 A CN113103241 A CN 113103241A CN 202110473012 A CN202110473012 A CN 202110473012A CN 113103241 A CN113103241 A CN 113103241A
Authority
CN
China
Prior art keywords
nurbs
interpolation
trajectory
discrete
track
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.)
Granted
Application number
CN202110473012.6A
Other languages
English (en)
Other versions
CN113103241B (zh
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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202110473012.6A priority Critical patent/CN113103241B/zh
Publication of CN113103241A publication Critical patent/CN113103241A/zh
Application granted granted Critical
Publication of CN113103241B publication Critical patent/CN113103241B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明提供了用于实现G2连续的机器人双NURBS轨迹插补方法、装置及***，涉及机器人轨迹规划控制技术领域。本发明所述的方法，包括：获取机器人离散轨迹；生成位置NURBS轨迹；对位置NURBS轨迹进行离散，根据离散点确定弧长参数，建立u‑s模型；生成姿态NURBS轨迹；建立w‑u模型；根据S型速度模型、u‑s模型和w‑u模型，按照固定采样周期，得到双NURBS轨迹的离散位姿插补点，以通过逆运动学求解离散插补点，得到机器人关节空间的插补轨迹。本发明所述的技术方案，通过采用双NURBS轨迹，保证机器人末端位姿轨迹G2连续，保证了运动的平顺，且能连续通过所有位姿点；通过建立位置NURBS轨迹参数与弧长参数的映射关系，减少插补过程中的速度波动，实现机器人轨迹的有效规划。

Description

用于实现G2连续的机器人双NURBS轨迹插补方法、装置及***
技术领域
本发明涉及机器人轨迹规划控制技术领域，具体而言，涉及用于实现G2连续的机器人双NURBS轨迹插补方法、装置及***。
背景技术
机器人加工领域需要保证末端位姿轨迹满足误差约束和加速度/冲击的连续性。由计算机辅助(CAM，Computer aided manufacture)软件产生的机器人轨迹通常是小线段轨迹，而小线段表示的轨迹G0连续性，G1、G2不连续，***频繁地加减速，容易引起机器人关节磨损，降低零件的加工精度和表面质量。
笛卡尔空间的机器人末端轨迹包含了位置和姿态信息，因此机器人轨迹规划包含位置和姿态的规划。NURBS(Non-Uniform Rational B-Splines，非均匀有理NURBS)曲线以其通用性和易实现的特性被广泛应用在机器人位置NURBS轨迹拟合中。NURBS由控制点和节点矢量即可定义，以其表示轨迹可以减少数据存储量。此外NURBS本身具备较高的连续性，三次NURBS曲线就可以保证G2连续，同时采用S型曲线速度控制，可减小速度突变对机器人的冲击、超程、失步或震荡。
机器人末端姿态轨迹被要求充分光滑，这样可以提高机器人的轨迹跟踪性能。根据机器人姿态描述方式的不同，主要有：旋转矩阵、欧拉角、RPY和四元数等。四元数因为描述简单，直观，计算量等优点而被广泛应用在机器人姿态轨迹拟合中。四元数姿态插补主要包括：四元数线性插补、(2)球面线性插补，以及球面样条插补。但是，球面线性插值只有C0连续性，且球面样条插补也仅有C1连续性，无法满足实际规划的需要。为了得到连续性更高的C2连续的四元数样条曲线被提出。
但目前对机器人工具中心点(TCP，Tool Center Point)轨迹点和姿态轨迹进行整体规划和插补方法很难同时满足机器人位置和姿态轨迹G2连续，且双NURBS(Non-UniformRational B-Splines，非均匀有理NURBS)轨迹会存在参数不同步的问题。同时由于样条曲线的弧长和参数之间是非线性关系，这种非线性关系使得插补中产生速度波动，影响机器人正常轨迹。上述问题均会影响到机器人的轨迹规划。
发明内容
本发明解决的问题是如何有效实现G2连续的机器人轨迹规划。
为解决上述问题，本发明提供一种用于实现G2连续的机器人双NURBS轨迹插补方法，包括：获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)；对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)；采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，通过采用双NURBS轨迹，保证机器人末端位姿轨迹G2连续，保证了运动的平顺，且能连续通过所有位姿点；通过建立位置NURBS轨迹参数u与弧长参数s的映射关系，减少插补过程中的速度波动，实现机器人轨迹的有效规划。
可选地，所述对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)包括：采用三次NURBS曲线对所述工具中心点{pi}n i＝0进行拟合以生成所述位置NURBS轨迹c(u)。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，通过采用三次NURBS曲线对工具中心点进行拟合以生成位置NURBS轨迹c(u)，有效提高位置NURBS轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
可选地，所述对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对所述位置NURBS轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，通过对位置NURBS轨迹c(u)进行等参数采样，从而确定离散点{c(ui)n i＝0}，进而实现机器人轨迹的有效规划。
可选地，所述根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0包括：采用布尔公式估计相邻两个所述离散点{c(ui)n i＝0}之间的弧长，根据所述弧长确定所述弧长参数{si}n i＝0。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，采用布尔公式精确估计相邻两个离散点{c(ui)n i＝0}之间的弧长，再根据弧长确定弧长参数{si}n i＝0，有效提高了弧长参数的准确性，进而实现机器人轨迹的有效规划。
可选地，所述对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)包括：对于每个姿态点对应的四元数：
Figure BDA0003046238600000031
建立所述四元数qi与所述四元数映射向量qi'的映射关系qi→qi'：
Figure BDA0003046238600000032
其中，qi0≠0；
采用三次NURBS插值拟合，根据所述四元数映射向量qi'生成所述姿态NURBS轨迹c(w)。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，通过三次NURBS插值拟合生成姿态NURBS轨迹c(w)，进而实现机器人轨迹的有效规划。
可选地，所述根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；根据所述u-s模型和所述弧长参数，确定所述位置NURBS轨迹c(u)对应的采样参数和离散点；根据所述w-u模型，确定所述四元数姿态NURBS轨迹c(w)对应的采样参数和离散点；根据所述位置NURBS轨迹c(u)对应的离散点和所述姿态NURBS轨迹c(w)对应的离散点，确定所述双NURBS轨迹对应的离散插补点；插补时，计算所述姿态NURBS轨迹c(w)对应的离散点时，假设对应的采样参数为w0，将所述采样参数w0代入所述姿态NURBS轨迹c(w)，求出
Figure BDA0003046238600000033
计算参数
Figure BDA0003046238600000034
Figure BDA0003046238600000041
根据所述参数η反求四元数
Figure BDA0003046238600000042
Figure BDA0003046238600000043
则qw为所述采样参数w0对应的离散点；根据具体机器人，对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，根据S型速度模型、u-s模型和w-u模型,按照固定采样周期，对位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，然后结合具体机器人，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹，实现机器人轨迹的有效规划。
可选地，所述工具中心点{pi}n i＝0和所述四元数姿态{qi}n i＝0的数量大于或等于5。
本发明所述的用于实现G2连续的机器人双NURBS轨迹插补方法，通过设置工具中心点和四元数姿态的数量大于或等于5，有利于生成位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)，实现机器人轨迹的有效规划。
本发明还提供一种用于实现G2连续的机器人双NURBS轨迹插补装置，包括：获取模块，用于获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；位置拟合模块，用于对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)；采样模块，用于对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；第一拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；姿态拟合模块，用于对所述四元数姿态
Figure BDA0003046238600000044
进行拟合生成NURBS轨迹c(w)；第二拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。所述用于实现G2连续的机器人双NURBS轨迹插补装置与上述用于实现G2连续的机器人双NURBS轨迹插补方法相对于现有技术所具有的优势相同，在此不再赘述。
本发明还提供一种用于实现G2连续的机器人双NURBS轨迹插补***，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的用于实现G2连续的机器人双NURBS轨迹插补方法。所述用于实现G2连续的机器人双NURBS轨迹插补***与上述用于实现G2连续的机器人双NURBS轨迹插补方法相对于现有技术所具有的优势相同，在此不再赘述。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的用于实现G2连续的机器人双NURBS轨迹插补方法。所述计算机可读存储介质与上述用于实现G2连续的机器人双NURBS轨迹插补方法相对于现有技术所具有的优势相同，在此不再赘述。
附图说明
图1为本发明实施例的用于实现G2连续的机器人双NURBS轨迹插补方法的示意图；
图2为本发明实施例的7段的S型速度模型；
图3为本发明实施例的双NURBS曲线参数模型；
图4为本发明实施例中机器人D-H坐标系；
图5为本发明实施例中规划轨迹逆解求出的关节角位移、速度和加速度曲线。
具体实施方式
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
如图1所示，本发明实施例提供一种用于实现G2连续的机器人双NURBS轨迹插补方法，包括：获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)；对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)；采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
具体地，在本实施例中，用于实现G2连续的机器人双NURBS轨迹插补方法包括：获取机器人离散轨迹(由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成)，其中，机器人的姿态可以由旋转矩阵、旋转向量、四元数和欧拉角等形式表示，本实施例中采用四元数表示；由于本实施例中采用三次NURBS曲线拟合算法，控制点与数据点个数相等，NURBS曲线的控制点数一般大于等于5，因此工具中心点和四元数姿态大于等于5。
对工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)，即对上述所有工具中心点进行拟合，生成一条NURBS轨迹，定义该轨迹为c(u)。
对位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，一般采用等参数采样，即按照等参数对位置NURBS轨迹c(u)进行离散，c(u)中参数u的取值范围为0至1，采用等参数采样方法进行离散以确定离散点{c(ui)n i＝0}，再根据离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0。
采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与弧长参数{si}n i＝0之间的u-s模型，通过建立位置NURBS轨迹参数u与弧长参数s的映射关系，减少插补过程中的速度波动。
对四元数姿态进行拟合生成姿态NURBS轨迹c(w)，对于每个姿态点对应的四元数为：
Figure BDA0003046238600000061
因为qi0≠0，建立映射关系qi→qi'为：
Figure BDA0003046238600000062
然后采用三次NURBS插值拟合，根据所述四元数映射向量qi'生成一条NURBS轨迹，定义该轨迹为c(w)。
采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型，即参数同步模型，通过建立参数同步模型，实现双NURBS轨迹在插补过程中的同步运动。
根据S型速度模型、u-s模型和w-u模型,按照固定采样周期，对所述双NURBS轨迹，即位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)进行离散，确定所述双NURBS轨迹对应的离散插补点。
在前述步骤完成规划的双NURBS轨迹，得到双NURBS轨迹的离散位姿插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹，从而实现机器人轨迹的有效规划。
在本实施例中，通过采用双NURBS轨迹，保证机器人末端位姿轨迹整体G2连续，保证了运动的平顺，且能连续通过所有位姿点；通过建立位置NURBS轨迹参数u与弧长参数s的映射关系，减少插补过程中的速度波动，实现机器人轨迹的有效规划。
可选地，所述对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)包括：采用三次NURBS曲线对所述工具中心点{pi}n i＝0进行拟合以生成所述位置NURBS轨迹c(u)。
具体地，在本实施例中，对工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)包括：采用三次NURBS曲线对工具中心点{pi}n i＝0进行拟合以生成位置NURBS轨迹c(u)，由于NURBS曲线具有直观性、局部性、凸包行、保凸性、几何不变性和变差减少等性质，便于局部修改，因此采用三次NURBS曲线对工具中心点{pi}n i＝0进行拟合以生成位置NURBS轨迹c(u)，能够有效提高位置NURBS轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
在本实施例中，通过采用三次NURBS曲线对工具中心点进行拟合以生成位置NURBS轨迹c(u)，有效提高位置NURBS轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
可选地，所述对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对所述位置NURBS轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
具体地，在本实施例中，参见前述描述，对位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对位置NURBS轨迹c(u)进行等参数采样以确定离散点{c(ui)n i＝0}，即按照等参数对位置NURBS轨迹c(u)进行离散，c(u)中参数u的取值范围为0至1，采用等参数采样方法进行离散以确定离散点{c(ui)n i＝0}。通过对位置NURBS轨迹c(u)进行等参数采样以确定离散点{c(ui)n i＝0}，进而实现机器人轨迹的有效规划。
在本实施例中，通过对位置NURBS轨迹c(u)进行等参数采样，从而确定离散点{c(ui)n i＝0}，进而实现机器人轨迹的有效规划。
可选地，所述根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0包括：采用布尔公式估计相邻两个所述离散点{c(ui)n i＝0}之间的弧长，根据所述弧长确定所述弧长参数{si}n i＝0。
具体地，在本实施例中，根据离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0包括：采用布尔公式估计相邻两个离散点{c(ui)n i＝0}之间的弧长，根据弧长确定弧长参数{si}n i＝0。
曲线c(u)在两个参数区间[a，b]上的弧长公式为：
Figure BDA0003046238600000081
由于三次NURBS曲线计算弧长时无解析解，因此利用数值积分方法计算弧长，该处采用布尔公式：
Figure BDA0003046238600000082
Figure BDA0003046238600000083
通过布尔公式，可以获得所有相邻两离散点{c(ui)n i＝0}之间对应的弧长
Figure BDA0003046238600000084
则可得到弧长参数{si}n i＝0：
Figure BDA0003046238600000085
具体指依次对弧长进行累加和单位化，得到弧长参数{si}n i＝0。
在本实施例中，采用布尔公式估计相邻两个离散点{c(ui)n i＝0}之间的弧长，再根据弧长确定弧长参数{si}n i＝0，有效提高了弧长参数的准确性，进而实现机器人轨迹的有效规划。
可选地，所述对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)包括：对于每个姿态点对应的四元数：
Figure BDA0003046238600000086
建立所述四元数qi与所述四元数映射向量qi'的映射关系qi→qi'：
Figure BDA0003046238600000091
其中，qi0≠0；
采用三次NURBS插值拟合，根据所述四元数映射向量qi'生成所述姿态NURBS轨迹c(w)。
具体地，在本实施例中，对四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)包括：对于每个姿态点对应的四元数为：
Figure BDA0003046238600000092
因为qi0≠0，建立映射关系qi→qi'为：
Figure BDA0003046238600000093
然后采用三次NURBS插值拟合，根据所述四元数映射向量qi'生成所述姿态轨迹c(w)，进而实现机器人轨迹的有效规划。
在本实施例中，通过三次NURBS插值拟合生成姿态NURBS轨迹c(w)，进而实现机器人轨迹的有效规划。
可选地，所述根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；根据所述u-s模型和所述弧长参数，确定所述位置NURBS轨迹c(u)对应的采样参数和离散点；根据所述w-u模型，确定所述四元数姿态NURBS轨迹c(w)对应的采样参数和离散点；根据所述位置NURBS轨迹c(u)对应的离散点和所述姿态NURBS轨迹c(w)对应的离散点，确定所述双NURBS轨迹对应的离散插补点；插补时，计算所述姿态NURBS轨迹c(w)对应的离散点时，假设对应的采样参数为w0，将所述采样参数w0代入所述姿态NURBS轨迹c(w)，求出
Figure BDA0003046238600000094
计算参数
Figure BDA0003046238600000095
根据所述参数η反求四元数
Figure BDA0003046238600000096
则qw为所述采样参数w0对应的离散点；根据具体机器人，对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
具体地，在本实施例中，根据S型速度模型、u-s模型和w-u模型,按照固定采样周期，对所述双NURBS轨迹，即位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)进行离散，得到插补点，包括：根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数，其中，结合图2所示，从上至下依次为位移s(t)、速度v(t)、加速度a(t)和加加速度j(t)随时间t的变化示意图，7段的S型速度模型的加速度曲线是连续的，因而速度能够平滑转接，避免产生电机冲击，可用于精度要求较高的控制中。
根据所述u-s模型和所述弧长参数，确定所述位置NURBS轨迹c(u)对应的采样参数和离散点；根据所述w-u模型，确定所述四元数姿态NURBS轨迹c(w)对应的采样参数和离散点；根据所述位置NURBS轨迹c(u)对应的离散点和所述姿态NURBS轨迹c(w)对应的离散点，确定所述双NURBS轨迹对应的离散插补点；
插补时，计算所述姿态NURBS轨迹c(w)对应的离散点时，假设对应的采样参数为w0，将所述采样参数w0代入所述姿态NURBS轨迹c(w)，求出
Figure BDA0003046238600000101
Figure BDA0003046238600000102
由于泛化四元数的范数等于1，计算参数
Figure BDA0003046238600000103
根据所述参数η反求四元数
Figure BDA0003046238600000104
则qw为采样参数w0对应的离散点；
其中，分析机器人轨迹可知，位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)是同步对应的关系，即对任意时刻曲线c(u)的参数值u∈[uk，uk+1]时，都有曲线c(w)的参数值w∈[wk，wk+1]，其中uk和wk分别为原始数据点在NURBS曲线上对应的数据点参数。
根据具体机器人，对离散插补点进行逆运动学求解，得到机器人关节空间的插补轨迹。
为了保证双NURBS曲线同步平滑运动，建立w-u模型，结合图3所示，w-u模型是根据工具中心点和姿态点在双NURBS曲线中分别对应的数据点参数值，并利用三次样条插值建立得到的。
在本实施例中，根据S型速度模型、u-s模型和w-u模型对位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)进行离散以确定离散位姿插补点，然后结合具体机器人，其中具体机器人D-H坐标系如图4所示，连杆参数如表1所示。进行逆运动学求解，得到关节空间的插补轨迹如图5所示，从中可以看出机器人关节轨迹加速度连续，即G2连续，从而实现机器人轨迹的有效规划。
Figure BDA0003046238600000111
表1
可选地，所述工具中心点{pi}n i＝0和所述四元数姿态{qi}n i＝0的数量大于或等于5。
具体地，在本实施例中，工具中心点和四元数姿态的数量大于或等于5，有利于生成位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)，实现机器人轨迹的有效规划。。
在本实施例中，通过设置工具中心点和四元数姿态的数量大于或等于5，有利于生成位置NURBS轨迹c(u)和姿态NURBS轨迹c(w)，实现机器人轨迹的有效规划。
本发明另一实施例提供一种用于实现G2连续的机器人双NURBS轨迹插补装置，获取模块，用于获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；包括：位置拟合模块，用于对所述工具中心点进行拟合生成位置NURBS轨迹c(u)；采样模块，用于对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；第一拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；姿态拟合模块，用于对所述四元数姿态
Figure BDA0003046238600000121
进行拟合生成NURBS轨迹c(w)；第二拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
本发明另一实施例提供一种用于实现G2连续的机器人双NURBS轨迹插补***，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的用于实现G2连续的机器人双NURBS轨迹插补方法。
本发明另一实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的用于实现G2连续的机器人双NURBS轨迹插补方法。
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (10)

1.一种用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，包括：
获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；
对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)；
对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；
采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；
对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)；
采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；
根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
2.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)包括：
采用三次NURBS曲线对所述工具中心点{pi}n i＝0进行拟合以生成所述位置NURBS轨迹c(u)。
3.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：
对所述位置NURBS轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
4.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0包括：
采用布尔公式估计相邻两个所述离散点{{c(ui)n i＝0}}之间的弧长，根据所述弧长确定所述弧长参数{si}n i＝0。
5.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述对所述四元数姿态{qi}n i＝0进行拟合生成姿态NURBS轨迹c(w)包括：
对于每个姿态点对应的四元数：
Figure FDA0003046238590000021
建立所述四元数qi与所述四元数映射向量qi'的映射关系qi→qi'：
Figure FDA0003046238590000022
其中，qi0≠0；
采用三次NURBS插值拟合，根据所述四元数映射向量qi'生成所述姿态NURBS轨迹c(w)。
6.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：
根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；
根据所述u-s模型和所述弧长参数，确定所述位置NURBS轨迹c(u)对应的采样参数和离散点；
根据所述w-u模型，确定所述四元数姿态NURBS轨迹c(w)对应的采样参数和离散点；
根据所述位置NURBS轨迹c(u)对应的离散点和所述姿态NURBS轨迹c(w)对应的离散点，确定所述双NURBS轨迹对应的离散插补点；
插补时，计算所述姿态NURBS轨迹c(w)对应的离散点时，假设对应的采样参数为w0，将所述采样参数w0代入所述姿态NURBS轨迹c(w)，求出
Figure FDA0003046238590000023
Figure FDA0003046238590000024
计算参数
Figure FDA0003046238590000031
根据所述参数η反求四元数
Figure FDA0003046238590000032
Figure FDA0003046238590000033
则qw为所述采样参数w0对应的离散点；
根据具体机器人，对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
7.根据权利要求1所述的用于实现G2连续的机器人双NURBS轨迹插补方法，其特征在于，所述工具中心点{pi}n i＝0和所述四元数姿态{qi}n i＝0的数量大于或等于5。
8.一种用于实现G2连续的机器人双NURBS轨迹插补装置，其特征在于，包括：
获取模块，用于获取机器人离散轨迹，其中，所述离散轨迹由工具中心点{pi}n i＝0和四元数姿态{qi}n i＝0组成；
位置拟合模块，用于对所述工具中心点{pi}n i＝0进行拟合生成位置NURBS轨迹c(u)；
采样模块，用于对所述位置NURBS轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；
第一拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；
姿态拟合模块，用于对所述四元数姿态{qi}n i＝0进行拟合生成NURBS轨迹c(w)；
第二拟合模块，用于采用三次样条插值，建立位置NURBS轨迹参数u与姿态NURBS轨迹参数w之间的w-u模型；
插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置NURBS轨迹c(u)和所述姿态NURBS轨迹c(w)离散，得到双NURBS轨迹的离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
9.一种用于实现G2连续的机器人双NURBS轨迹插补***，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如权利要求1至7任一项所述的用于实现G2连续的机器人双NURBS轨迹插补方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如权利要求1至7任一项所述的用于实现G2连续的机器人双NURBS轨迹插补方法。
CN202110473012.6A 2021-04-29 2021-04-29 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及*** Active CN113103241B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110473012.6A CN113103241B (zh) 2021-04-29 2021-04-29 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110473012.6A CN113103241B (zh) 2021-04-29 2021-04-29 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及***

Publications (2)

Publication Number Publication Date
CN113103241A true CN113103241A (zh) 2021-07-13
CN113103241B CN113103241B (zh) 2022-08-23

Family

ID=76720439

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110473012.6A Active CN113103241B (zh) 2021-04-29 2021-04-29 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及***

Country Status (1)

Country Link
CN (1) CN113103241B (zh)

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140005823A1 (en) * 2012-06-28 2014-01-02 Fanuc Corporation Numerical controller having a tool posture control function for multi-axis machining machines
CN105773620A (zh) * 2016-04-26 2016-07-20 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN106393106A (zh) * 2016-09-13 2017-02-15 东南大学 参数自适应密化的机器人nurbs曲线运动插补方法
CN110083160A (zh) * 2019-05-16 2019-08-02 哈尔滨工业大学（深圳） 一种基于深度学习的机器人轨迹规划方法
US20190278292A1 (en) * 2018-03-06 2019-09-12 Zoox, Inc. Mesh Decimation Based on Semantic Information
CN110686595A (zh) * 2019-09-27 2020-01-14 天津大学 非正交轴系激光全站仪的激光束空间位姿标定方法
CN110948493A (zh) * 2019-12-26 2020-04-03 广州机械科学研究院有限公司 机器人曲线路径生成方法、***、装置和存储介质
CN111444467A (zh) * 2020-04-21 2020-07-24 南京大学 基于实时定位轨迹数据进行局部线性插值和预测的方法
CN111452033A (zh) * 2019-01-18 2020-07-28 兰州交通大学 一种工业机器人双nurbs曲线铣削轨迹规划方法
CN111736537A (zh) * 2020-07-21 2020-10-02 天津大学 一种自由曲面加工中双nurbs路径极限速度的计算方法
US20200320781A1 (en) * 2015-07-17 2020-10-08 Bao Tran Systems and Methods for Fitting Product
CN111791236A (zh) * 2020-07-17 2020-10-20 广东拓斯达科技股份有限公司 一种工业机器人笛卡尔空间轨迹过渡方法
CN112099493A (zh) * 2020-08-31 2020-12-18 西安交通大学 一种自主移动机器人轨迹规划方法、***及设备
CN112269356A (zh) * 2020-10-27 2021-01-26 南京溧航仿生产业研究院有限公司 一种机器人nurbs轨迹插补方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140005823A1 (en) * 2012-06-28 2014-01-02 Fanuc Corporation Numerical controller having a tool posture control function for multi-axis machining machines
US20200320781A1 (en) * 2015-07-17 2020-10-08 Bao Tran Systems and Methods for Fitting Product
CN105773620A (zh) * 2016-04-26 2016-07-20 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN106393106A (zh) * 2016-09-13 2017-02-15 东南大学 参数自适应密化的机器人nurbs曲线运动插补方法
US20190278292A1 (en) * 2018-03-06 2019-09-12 Zoox, Inc. Mesh Decimation Based on Semantic Information
CN111452033A (zh) * 2019-01-18 2020-07-28 兰州交通大学 一种工业机器人双nurbs曲线铣削轨迹规划方法
CN110083160A (zh) * 2019-05-16 2019-08-02 哈尔滨工业大学（深圳） 一种基于深度学习的机器人轨迹规划方法
CN110686595A (zh) * 2019-09-27 2020-01-14 天津大学 非正交轴系激光全站仪的激光束空间位姿标定方法
CN110948493A (zh) * 2019-12-26 2020-04-03 广州机械科学研究院有限公司 机器人曲线路径生成方法、***、装置和存储介质
CN111444467A (zh) * 2020-04-21 2020-07-24 南京大学 基于实时定位轨迹数据进行局部线性插值和预测的方法
CN111791236A (zh) * 2020-07-17 2020-10-20 广东拓斯达科技股份有限公司 一种工业机器人笛卡尔空间轨迹过渡方法
CN111736537A (zh) * 2020-07-21 2020-10-02 天津大学 一种自由曲面加工中双nurbs路径极限速度的计算方法
CN112099493A (zh) * 2020-08-31 2020-12-18 西安交通大学 一种自主移动机器人轨迹规划方法、***及设备
CN112269356A (zh) * 2020-10-27 2021-01-26 南京溧航仿生产业研究院有限公司 一种机器人nurbs轨迹插补方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
PHAM, HL: "Set-point control of robot end-effector pose using dual quaternion feedback", 《ROBOTICS AND COMPUTER-INTEGRATED MANUFACTURING》 *
SALMERON-QUIROZ BERNARDINO BENITO: "ESTIMATION OF ATTITUDE VIA QUATERNION IN AN INDUSTRIAL ROBOT", 《2012 IEEE INTERNATIONAL CONFERENCE ON TECHNOLOGIES FOR PRACTICAL ROBOT APPLICATIONS (TEPRA)》 *
XIE, ZW: "Robot learning from demonstration for path planning: A review", 《SCIENCE CHINA-TECHNOLOGICAL SCIENCES》 *
YANG LIU: "Trajectory planning of robot manipulators based on unit quaternion", 《2017 IEEE INTERNATIONAL CONFERENCE ON ADVANCED INTELLIGENT MECHATRONICS (AIM)》 *
乔正: "基于四元数的手术机器人圆弧轨迹规划", 《机械与电子》 *
倪晓伟: "一种高效的机器人自由曲线运动位姿同步规划方法", 《机床与液压》 *
林海明: "基于LinuxCNC的六轴机器人位姿规划算法研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
赵崇光: "加工轨迹C2连续优化算法研究", 《科学技术与工程》 *
郭闯强: "多目标位姿约束下空间机器人载体姿态扰动优化", 《空间科学学报》 *

Also Published As

Publication number Publication date
CN113103241B (zh) 2022-08-23

Similar Documents

Publication Publication Date Title
CN107363813B (zh) 一种基于可穿戴设备的桌面工业机器人示教***和方法
US8774965B2 (en) Method and device for controlling a manipulator
JP2019517929A (ja) ロボット関節空間におけるポイントツーポイント移動の軌跡計画方法
CN112975992B (zh) 一种误差可控的机器人轨迹同步优化方法
CN113103240B (zh) 用于实现c2连续的机器人轨迹规划方法、装置及***
JP3349652B2 (ja) オフラインティーチング方法
CN114571452A (zh) 工业机器人轨迹规划方法、电子设备及可读存储介质
Pan et al. Collision-free and curvature-continuous path smoothing in cluttered environments
Guilamo et al. Manipulability optimization for trajectory generation
Peng et al. An analytical method for decoupled local smoothing of linear paths in industrial robots
JP2007000954A (ja) ロボット教示装置及び方法
CN115179306A (zh) 一种复杂木模工业机器人铣削及控制方法
CN112356032B (zh) 一种姿态平滑过渡方法及***
CN113001542B (zh) 一种机器人轨迹规划方法、装置、可读存储介质及机器人
CN113103241B (zh) 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及***
CN111515954B (zh) 一种机械臂高质量运动路径生成方法
JPS5815801B2 (ja) 工業用ロボツトの軌跡制御方式
CN113618728A (zh) 机器人运动轨迹的补偿方法、装置及计算机存储介质
Kim et al. Development of a trajectory generation method for a five-axis NC machine
CN113276116B (zh) 一种误差可控的机器人轨迹同步过渡方法
CN113608496B (zh) 空间路径g2转接光顺方法、设备及计算机可读存储介质
CN115808904A (zh) 一种工业机器人圆弧过辅助点轨迹规划方法
CN116125906A (zh) 一种数控加工的运动规划方法、装置、设备和存储介质
CN114734435A (zh) 一种基于超球面的编码器校准方法、装置及***
Liu et al. Robot continuous trajectory planning based on Frenet-Serret formulas

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
GR01 Patent grant
GR01 Patent grant