CN113103240A - 用于实现c2连续的机器人轨迹规划方法、装置及系统 - Google Patents

用于实现c2连续的机器人轨迹规划方法、装置及系统 Download PDF

Info

Publication number
CN113103240A
CN113103240A CN202110472862.4A CN202110472862A CN113103240A CN 113103240 A CN113103240 A CN 113103240A CN 202110472862 A CN202110472862 A CN 202110472862A CN 113103240 A CN113103240 A CN 113103240A
Authority
CN
China
Prior art keywords
discrete
track
trajectory
interpolation
model
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
CN202110472862.4A
Other languages
English (en)
Other versions
CN113103240B (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 CN202110472862.4A priority Critical patent/CN113103240B/zh
Publication of CN113103240A publication Critical patent/CN113103240A/zh
Application granted granted Critical
Publication of CN113103240B publication Critical patent/CN113103240B/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

Abstract

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

Description

用于实现C2连续的机器人轨迹规划方法、装置及系统
技术领域
本发明涉及机器人轨迹规划控制技术领域，具体而言，涉及一种用于实现C2连续的机器人轨迹规划方法、装置及系统。
背景技术
机器人加工领域需要保证末端位姿轨迹满足误差约束和加速度/冲击的连续性。随着工业机器人刚度和精度的提高，在需要控制轨迹轮廓精度的领域如铣削、抛光和去毛刺等加工领域得到了广泛应用。由于关节空间轨迹规划无法保证轨迹点之间的路径，而对于轮廓运动类型，需要保证末端位姿轨迹满足误差约束和加速度/冲击的连续性。
由计算机辅助(CAM，Computer aided manufacture)软件产生的轨迹通常是小线段轨迹，而小线段表示的轨迹G0连续性，G1、G2不连续，系统频繁地加减速，容易引起机器人关节磨损，降低零件的加工精度和表面质量。
笛卡尔空间的机器人末端轨迹包含了位置和姿态信息，因此机器人轨迹规划包含位置和姿态的规划。NURBS(Non-Uniform Rational B-Splines，非均匀有理NURBS)曲线以其通用性和易实现的特性被广泛应用在机器人位置轨迹拟合中。NURBS由控制点和节点矢量即可定义，以其表示轨迹可以减少数据存储量。此外NURBS本身具备较高的连续性，三次NURBS曲线就可以保证C1/C2连续，同时采用S型曲线速度控制，可减小速度突变对机器人的冲击、超程、失步或震荡。
机器人末端姿态轨迹被要求充分光滑，这样可以提高机器人的轨迹跟踪性能。根据机器人姿态描述方式的不同，主要有：旋转矩阵、欧拉角、RPY和四元数等。四元数因为描述简单，直观，计算量等优点而被广泛应用在机器人姿态轨迹拟合中。四元数对姿态的描述简单，且能够有效对欧拉角旋转时产生的万向节锁死问题进行避免，此外，基于欧拉角和旋转的姿态插补效率要低于四元数。四元数姿态插补主要包括：(1)四元数线性插补(LERP,Linear Interpolation)、(2)球面线性插补(SLERP,Spherical linear interpolation)，以及球面样条插补(SQUAD,Spherical and Quadrangle)。
球面线性插值只有C0连续性，且球面样条插补也仅有C1连续性，无法满足实际规划的需要。为了得到连续性更高的四元数样条曲线，非专利文献《一类C2连续的单位四元数插值样条曲线》、《Interpolation Solid Orientations with a C2-continus B-splineQuaternion Curve》和《A C2-continuous B-spline quaternion curve interpolating agiven sequence of solid orientations》分别提出了C2连续的四元数样条曲线。
目前对机器人位置和姿态轨迹单独进行C2连续规划的方法很多，但是同时对位姿轨迹进行规划，使得机器人位姿轨迹满足C2连续的方法很少。且这些方法主要集中在对两个位姿之间进行规划，无法满足连续轨迹的规划要求。
发明内容
本发明解决的问题是如何实现C2连续的机器人轨迹规划。
为解决上述问题，本发明提供一种用于实现C2连续的机器人轨迹规划方法，包括：获取机器人的离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
本发明所述的用于实现C2连续的机器人轨迹规划方法，通过采用NURBS位置轨迹和C2连续的四元数姿态轨迹，保证机器人末端位姿轨迹整体C2连续，保证了运动的平顺，同时可减少插补过程中的速度波动，实现机器人轨迹的有效规划。
可选地，所述对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)包括：采用三次NURBS曲线对所述数据点{pi}n i＝0进行拟合以生成所述位置轨迹c(u)。
本发明所述的用于实现C2连续的机器人轨迹规划方法，通过三次NURBS曲线对工具中心点{pi}n i＝0进行拟合以生成位置轨迹c(u)，能够有效提高位置轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
可选地，所述对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对所述位置轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
本发明所述的用于实现C2连续的机器人轨迹规划方法，通过对位置轨迹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。
本发明所述的用于实现C2连续的机器人轨迹规划方法，采用布尔公式估计相邻两个离散点{c(ui)n i＝0}之间的弧长，再根据弧长确定弧长参数{si}n i＝0，有效提高了弧长参数的准确性，进而实现机器人轨迹的有效规划。
可选地，所述对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)包括：根据所述四元数姿态{qi}n i＝0，首先定义两个姿态之间的插值样条曲线ci(w)；其中，由于ci(w)各段曲线均定义在局部参数w∈[0,1]上，所以需要对数据点参数重新定义，每个数据点对应的参数为：
Figure BDA0003046170180000031
i表示数据点下标；从而将所述插值样条曲线定义在统一的连续参变量区间[0,1]上，以生成C2连续的所述四元数姿态轨迹
Figure BDA0003046170180000032
本发明所述的用于实现C2连续的机器人轨迹规划方法，通过定义样条曲线的方式生成C2连续的四元数姿态轨迹c(w)，保证机器人笛卡尔空间和关节空间轨迹C2连续，满足连续轨迹的规划要求。
可选地，所述根据S型速度模型、所述u-s模型和所述w-u模型,按照固定的采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：1)根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；2)根据所述u-s模型，和对应的弧长参数，确定所述位置轨迹c(u)对应的采样参数和离散点；3)根据所述w-u模型，确定所述四元数姿态轨迹c(w)对应的采样参数和离散点；4)根据所述位置轨迹c(u)对应的离散点和所述姿态轨迹c(w)对应的离散点，确定所述离散插补点；5)对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
本发明所述的用于实现C2连续的机器人轨迹规划方法，根据S型速度模型、u-s模型和w-u模型,按照固定采样周期，对位置轨迹c(u)和姿态轨迹c(w)进行采样以确定离散插补点，然后进行逆运动学求解，得到关节空间的插补轨迹，从而实现机器人的插补运动。
本发明还提供一种用于实现C2连续的机器人轨迹规划装置，包括：获取模块，用于获取机器人的离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；位置拟合模块，用于对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；采样模块，用于对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；第一拟合模块，用于采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；姿态拟合模块，用于对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；第二拟合模块，用于采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。所述用于实现C2连续的机器人轨迹规划装置与上述用于实现C2连续的机器人轨迹规划方法相对于现有技术所具有的优势相同，在此不再赘述。
本发明还提供一种用于实现C2连续的机器人轨迹规划系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的用于实现C2连续的机器人轨迹规划方法。所述用于实现C2连续的机器人轨迹规划系统与上述用于实现C2连续的机器人轨迹规划方法相对于现有技术所具有的优势相同，在此不再赘述。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的用于实现C2连续的机器人轨迹规划方法。所述计算机可读存储介质与上述用于实现C2连续的机器人轨迹规划方法相对于现有技术所具有的优势相同，在此不再赘述。
附图说明
图1为本发明实施例的用于实现C2连续的机器人轨迹规划方法的示意图；
图2为本发明实施例的7段的S型速度模型；
图3为本发明实施例的参数同步模型；
图4为本发明实施例中机器人D-H坐标系；
图5为本发明实施例中规划轨迹逆解求出的关节角位移、速度和加速度曲线。
具体实施方式
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
如图1所示，本发明实施例提供一种用于实现C2连续的机器人轨迹规划方法，包括：获取机器人的离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
具体地，在本实施例中，用于实现C2连续的机器人轨迹规划方法包括：获取机器人的离散轨迹，其中，离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成，数据点也称工具中心点，其中，机器人的姿态可以由旋转矩阵、旋转向量、四元数和欧拉角等形式表示，本实施例中采用四元数表示，四元数对姿态的描述简单，可以避免万向节锁死问题。
对数据点{pi}n i＝0进行拟合生成位置轨迹c(u)。
对位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0。
采用三次样条插值，建立位置样条参数{ui}n i＝0与弧长参数{si}n i＝0之间的u-s模型。通过建立位置样条参数u与弧长参数s的映射关系，减少插补过程中的速度波动。
对四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)。
采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型，即参数同步模型，通过建立参数同步模型，实现位置和姿态在插补过程中的同步光滑运动。
根据S型速度模型、u-s模型和w-u模型,按照固定采样周期，对所述位置轨迹c(u)和四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解离散插补点，得到机器人关节空间的插补轨迹，保证机器人笛卡尔空间和关节空间C2连续，即速度、加速度连续。通过采用NURBS位置轨迹和C2连续的四元数姿态轨迹，保证机器人末端位姿轨迹整体C2连续，保证了运动的平顺，同时通过建立位置样条参数u与弧长参数s的映射关系，可减少插补过程中的速度波动，实现机器人轨迹的有效规划。
在本实施例中，根据S型速度模型、u-s模型和w-u模型，按照固定采样周期，对位置轨迹c(u)和四元数姿态轨迹c(w)采样，得到离散插补点，以通过逆运动学求解离散插补点，得到机器人关节空间的插补轨迹，保证机器人笛卡尔空间和关节空间轨迹C2连续，满足连续轨迹的规划要求。
可选地，所述对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)包括：采用三次NURBS曲线对所述数据点{pi}n i＝0进行拟合以生成所述位置轨迹c(u)。
具体地，在本实施例中，对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)包括：采用三次NURBS曲线对数据点{pi}n i＝0进行拟合以生成位置轨迹c(u)，由于三次NURBS曲线具有直观性、局部性、凸包行、保凸性、几何不变性和变差减少等性质，便于局部修改，因此采用三次NURBS曲线对工具中心点{pi}n i＝0进行拟合以生成位置轨迹c(u)，能够有效提高位置轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
在本实施例中，通过三次NURBS曲线对工具中心点{pi}n i＝0进行拟合以生成位置轨迹c(u)，能够有效提高位置轨迹c(u)的拟合效果，进而实现机器人轨迹的有效规划。
可选地，所述对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对所述位置轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
具体地，在本实施例中，对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：对位置轨迹c(u)进行等参数采样以确定离散点{c(ui)n i＝0}，即按照等参数对位置轨迹c(u)进行离散，c(u)中参数u的取值范围为0至1，采用等参数采样方法进行离散以确定离散点{c(ui)n i＝0}。通过对位置轨迹c(u)进行等参数采样以确定离散点{c(ui)n i＝0}，进而实现机器人轨迹的有效规划。
在本实施例中，通过对位置轨迹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 BDA0003046170180000071
由于三次NURBS曲线计算弧长时无解析解，因此利用数值积分方法计算弧长，该处采用布尔公式：
Figure BDA0003046170180000072
Figure BDA0003046170180000073
假设数据点参数为V＝[v0,v1,…,vn]，则通过布尔公式，可以计算相邻两个区间位置点的弧长参数和总弧长。
在本实施例中，采用布尔公式估计相邻两个离散点{c(ui)n i＝0}之间的弧长，再根据弧长确定弧长参数{si}n i＝0，有效提高了弧长参数的准确性，进而实现机器人轨迹的有效规划。
可选地，所述对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)包括：根据所述四元数姿态{qi}n i＝0，首先定义两个姿态之间的插值样条曲线ci(w)；其中，由于各段曲线ci(w)均定义在局部参数w∈[0,1]上，所以需要对数据点参数重新定义，每个数据点对应的参数为：
Figure BDA0003046170180000081
i表示数据点下标；从而将所述样条曲线定义在统一的连续参变量区间[0,1]上，以生成C2连续的所述四元数姿态轨迹
Figure BDA0003046170180000082
具体地，在本实施例中，对四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)包括：根据所述四元数姿态{qi}n i＝0，首先定义两个姿态之间的插值样条曲线ci(w)；由于ci(w)各段曲线均定义在局部参数w∈[0,1]上，所以需要对数据点参数重新定义，每个数据点对应的参数为：
Figure BDA0003046170180000083
i表示数据点下标；从而将所述样条曲线定义在统一的连续参变量区间[0,1]上，以生成C2连续的所述四元数姿态轨迹
Figure BDA0003046170180000084
定义姿态序列姿态
Figure BDA0003046170180000085
令q-1＝q0，qn+1＝qn，定义：
Figure BDA0003046170180000086
为第i段单位四元数插值样条曲线，其中，w∈[0,1],i＝0,1,2,…,n-1；即样条曲线为：
Figure BDA0003046170180000091
Figure BDA0003046170180000092
曲线
Figure BDA0003046170180000093
中的各段曲线ci(w)均定义在局部参数w∈[0,1]上，若整条曲线定义在统一的连续参变量区间上，且节点分割均匀，那么在等距节点下任意2条相邻曲线在连接点处C2连续。
故将
Figure BDA0003046170180000094
中的各段曲线ci(w)定义在局部参数区间[0,1]上时，所以，若需要将整体曲线定义在同一的连续区间[0,1]时，则需要对数据点参数从新定义，每个数据点对应的参数为：
Figure BDA0003046170180000095
i表示数据点下标；从而将所述样条曲线定义在统一的连续参变量区间[0,1]上，以生成C2连续的所述四元数姿态轨迹
Figure BDA0003046170180000096
在本实施例中，通过定义样条曲线的方式生成C2连续的四元数姿态轨迹c(w)，保证机器人笛卡尔空间和关节空间轨迹C2连续，满足连续轨迹的规划要求。
可选地，所述根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：1)根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；2)根据所述u-s模型，和对应的弧长参数，确定所述位置轨迹c(u)对应的采样参数和离散点；3)根据所述w-u模型，确定所述四元数姿态轨迹c(w)对应的采样参数和离散点；4)根据所述位置轨迹c(u)对应的离散点和所述姿态轨迹c(w)对应的离散点，确定所述离散插补点；5)对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
具体地，在本实施例中，根据所述S型速度模型对位置轨迹c(u)规划确，结合图2所示，从上至下依次为位移s(t)、速度v(t)、加速度a(t)和加加速度j(t)随时间t的变化示意图，加加速度j(t)分为7段类似方波的曲线，对应地，7段的S型速度模型的加速度曲线是连续的，因而速度能够平滑转接，避免产生电机冲击，可用于精度要求较高的控制中；七段过程分别是：匀加加速段、匀加速段、匀减加速段、匀速段、匀加减速段、匀减速段和匀减减速段。
根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；根据所述u-s模型，和对应的弧长参数，确定所述位置轨迹c(u)对应的采样参数和离散点；根据所述w-u模型，确定所述四元数姿态轨迹c(w)对应的采样参数和离散点；根据所述位置轨迹c(u)对应的离散点和所述姿态轨迹c(w)对应的离散点，确定所述离散插补点；对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
其中，结合图3所示，对于任意时刻c(u)的参数值u∈[vk，vk+1]，都有曲线c(w)的参数值w∈[wk，wk+1]，为了保证两条轨迹之间平滑运动，利用三次样条曲线进行插值，建立两者之间的关系模型，即利用三次样条插值，建立位置样条参数u与姿态样条参数w之间的参数同步模型，即w-u模型；参数同步模型表示机器人在运动过程中，需要同时到达位姿轨迹中同一索引的TCP点(Tool Center Point，工具中心点){pi}n i＝0和姿态点{qi}n i＝0。
对于任意时刻c(u)的参数值u∈[vk，vk+1]，都有曲线c(w)的参数值w∈[wk，wk+1]。为了保证两条轨迹之间平滑运动，利用三次样条曲线进行插值，建立两者之间的关系模型，即参数同步模型(w-u模型)。
本发明另一实施例提供一种用于实现C2连续的机器人轨迹规划装置，包括：获取模块，用于获取离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；位置拟合模块，用于对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；采样模块，用于对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；第一拟合模块，用于采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；姿态拟合模块，用于对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；第二拟合模块，用于采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。其中，机器人具体D-H坐标系如图4所示，连杆参数如表1所示。进行逆运动学求解，得到关节空间的插补轨迹如图5所示，从中可以看出机器人关节轨迹加速度连续，即C2连续，从而实现机器人轨迹的有效规划。
Figure BDA0003046170180000111
表1
本发明另一实施例提供一种用于实现C2连续的机器人轨迹规划系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的用于实现C2连续的机器人轨迹规划方法。
本发明另一实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的用于实现C2连续的机器人轨迹规划方法。
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (9)

1.一种用于实现C2连续的机器人轨迹规划方法，其特征在于，包括：
获取机器人的离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；
对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；
对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；
采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；
对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；
采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；
根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
2.根据权利要求1所述的用于实现C2连续的机器人轨迹规划方法，其特征在于，所述对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)包括：
采用三次NURBS曲线对所述数据点{pi}n i＝0进行拟合以生成所述位置轨迹c(u)。
3.根据权利要求1所述的用于实现C2连续的机器人轨迹规划方法，其特征在于，所述对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}包括：
对所述位置轨迹c(u)进行等参数采样以确定所述离散点{c(ui)n i＝0}。
4.根据权利要求1所述的用于实现C2连续的机器人轨迹规划方法，其特征在于，所述根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0包括：
采用布尔公式估计相邻两个所述离散点{{c(ui)n i＝0}}之间的弧长，根据所述弧长确定所述弧长参数{si}n i＝0。
5.根据权利要求1所述的用于实现C2连续的机器人轨迹规划方法，其特征在于，所述对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)包括：
根据所述四元数姿态{qi}n i＝0，首先定义两个姿态之间的插值样条曲线ci(w)；其中，由于ci(w)各段曲线均定义在局部参数w∈[0,1]上，所以需要对数据点参数重新定义，每个数据点对应的参数为：
Figure FDA0003046170170000021
i表示数据点下标；从而将所述插值样条曲线定义在统一的连续参变量区间[0,1]上，以生成C2连续的所述四元数姿态轨迹
Figure FDA0003046170170000022
6.根据权利要求1所述的用于实现C2连续的机器人轨迹规划方法，其特征在于，所述根据S型速度模型、所述u-s模型和所述w-u模型，按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹包括：
1)根据所述S型速度模型，通过每个采样周期点对应的时间，计算出对应的弧长参数；
2)根据所述u-s模型，和对应的弧长参数，确定所述位置轨迹c(u)对应的采样参数和离散点；
3)根据所述w-u模型，确定所述四元数姿态轨迹c(w)对应的采样参数和离散点；
4)根据所述位置轨迹c(u)对应的离散点和所述姿态轨迹c(w)对应的离散点，确定所述离散插补点；
5)对所述离散插补点进行逆运动学求解，得到所述机器人关节空间的插补轨迹。
7.一种用于实现C2连续的机器人轨迹规划装置，其特征在于，包括：
获取模块，用于获取机器人的离散轨迹，其中，所述离散轨迹由数据点{pi}n i＝0和四元数姿态{qi}n i＝0组成；
位置拟合模块，用于对所述数据点{pi}n i＝0进行拟合生成位置轨迹c(u)；
采样模块，用于对所述位置轨迹c(u)进行采样确定离散点{c(ui)n i＝0}，根据所述离散点{c(ui)n i＝0}确定弧长参数{si}n i＝0；
第一拟合模块，用于采用三次样条插值，建立位置样条参数{ui}n i＝0与所述弧长参数{si}n i＝0之间的u-s模型；
姿态拟合模块，用于对所述四元数姿态{qi}n i＝0进行拟合生成C2连续的四元数姿态轨迹c(w)；
第二拟合模块，用于采用三次样条插值，建立位置样条参数u与姿态样条参数w之间的w-u模型；
插补模块，用于根据S型速度模型、所述u-s模型和所述w-u模型,按照固定采样周期，对所述位置轨迹c(u)和所述四元数姿态轨迹c(w)进行采样，得到离散插补点，以通过逆运动学求解所述离散插补点，得到机器人关节空间的插补轨迹。
8.一种用于实现C2连续的机器人轨迹规划系统，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如权利要求1至6任一项所述的用于实现C2连续的机器人轨迹规划方法。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如权利要求1至6任一项所述的用于实现C2连续的机器人轨迹规划方法。
CN202110472862.4A 2021-04-29 2021-04-29 用于实现c2连续的机器人轨迹规划方法、装置及系统 Active CN113103240B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110472862.4A CN113103240B (zh) 2021-04-29 2021-04-29 用于实现c2连续的机器人轨迹规划方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110472862.4A CN113103240B (zh) 2021-04-29 2021-04-29 用于实现c2连续的机器人轨迹规划方法、装置及系统

Publications (2)

Publication Number Publication Date
CN113103240A true CN113103240A (zh) 2021-07-13
CN113103240B CN113103240B (zh) 2022-08-23

Family

ID=76720432

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110472862.4A Active CN113103240B (zh) 2021-04-29 2021-04-29 用于实现c2连续的机器人轨迹规划方法、装置及系统

Country Status (1)

Country Link
CN (1) CN113103240B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113479353A (zh) * 2021-07-14 2021-10-08 贵州航天林泉电机有限公司 一种基于速度规划的卫星转台路径规划方法
CN113532427A (zh) * 2021-07-14 2021-10-22 贵州航天林泉电机有限公司 一种基于位置规划的卫星转台路径规划方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103802113A (zh) * 2012-11-08 2014-05-21 沈阳新松机器人自动化股份有限公司 基于任务和样条曲线的工业机器人路径规划方法
US20150173654A1 (en) * 2013-12-20 2015-06-25 Solutions Novika Activity, posture and heart monitoring system and method
CN105773620A (zh) * 2016-04-26 2016-07-20 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN106826829A (zh) * 2017-02-22 2017-06-13 武汉工程大学 一种可控误差的工业机器人光顺运动轨迹生成方法
US20170357262A1 (en) * 2016-06-10 2017-12-14 Cnh Industrial America Llc Swath tracking system for an off-road vehicle
CN108549322A (zh) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 一种针对机器人圆弧轨迹运动的位姿同步方法和装置
CN108804391A (zh) * 2018-04-27 2018-11-13 湖南农业大学 一种基于b样条的插值曲线或曲面的构造方法及系统
CN109313428A (zh) * 2017-05-12 2019-02-05 深圳配天智能技术研究院有限公司 运动轨迹平滑转接的方法和装置及其相关设备
CN110083160A (zh) * 2019-05-16 2019-08-02 哈尔滨工业大学（深圳） 一种基于深度学习的机器人轨迹规划方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法
CN112099493A (zh) * 2020-08-31 2020-12-18 西安交通大学 一种自主移动机器人轨迹规划方法、系统及设备
CN112269356A (zh) * 2020-10-27 2021-01-26 南京溧航仿生产业研究院有限公司 一种机器人nurbs轨迹插补方法
CN112356032A (zh) * 2020-11-05 2021-02-12 哈工大机器人（合肥）国际创新研究院 一种姿态平滑过渡方法及系统

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103802113A (zh) * 2012-11-08 2014-05-21 沈阳新松机器人自动化股份有限公司 基于任务和样条曲线的工业机器人路径规划方法
US20150173654A1 (en) * 2013-12-20 2015-06-25 Solutions Novika Activity, posture and heart monitoring system and method
CN105773620A (zh) * 2016-04-26 2016-07-20 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
US20170357262A1 (en) * 2016-06-10 2017-12-14 Cnh Industrial America Llc Swath tracking system for an off-road vehicle
CN106826829A (zh) * 2017-02-22 2017-06-13 武汉工程大学 一种可控误差的工业机器人光顺运动轨迹生成方法
CN109313428A (zh) * 2017-05-12 2019-02-05 深圳配天智能技术研究院有限公司 运动轨迹平滑转接的方法和装置及其相关设备
CN108549322A (zh) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 一种针对机器人圆弧轨迹运动的位姿同步方法和装置
CN108804391A (zh) * 2018-04-27 2018-11-13 湖南农业大学 一种基于b样条的插值曲线或曲面的构造方法及系统
CN110083160A (zh) * 2019-05-16 2019-08-02 哈尔滨工业大学（深圳） 一种基于深度学习的机器人轨迹规划方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法
CN112099493A (zh) * 2020-08-31 2020-12-18 西安交通大学 一种自主移动机器人轨迹规划方法、系统及设备
CN112269356A (zh) * 2020-10-27 2021-01-26 南京溧航仿生产业研究院有限公司 一种机器人nurbs轨迹插补方法
CN112356032A (zh) * 2020-11-05 2021-02-12 哈工大机器人（合肥）国际创新研究院 一种姿态平滑过渡方法及系统

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的六轴机器人位姿规划算法研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 *
王添: "基于单位四元数机器人多姿态轨迹平滑规划", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
郭闯强: "多目标位姿约束下空间机器人载体姿态扰动优化", 《空间科学学报》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113479353A (zh) * 2021-07-14 2021-10-08 贵州航天林泉电机有限公司 一种基于速度规划的卫星转台路径规划方法
CN113532427A (zh) * 2021-07-14 2021-10-22 贵州航天林泉电机有限公司 一种基于位置规划的卫星转台路径规划方法
CN113479353B (zh) * 2021-07-14 2022-09-06 贵州航天林泉电机有限公司 一种基于速度规划的卫星转台路径规划方法

Also Published As

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

Similar Documents

Publication Publication Date Title
US11188056B2 (en) Feedrate scheduling method for five-axis dual-spline curve interpolation
Pan et al. Collision-free and smooth trajectory computation in cluttered environments
Lu et al. Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization
CN113103240B (zh) 用于实现c2连续的机器人轨迹规划方法、装置及系统
Zhang et al. Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines
JP2019517929A (ja) ロボット関節空間におけるポイントツーポイント移動の軌跡計画方法
Kabir et al. Generation of synchronized configuration space trajectories of multi-robot systems
Xie et al. Global G3 continuity toolpath smoothing for a 5-DoF machining robot with parallel kinematics
Cui et al. A Darboux-frame-based formulation of spin-rolling motion of rigid objects with point contact
Shen et al. A smooth and undistorted toolpath interpolation method for 5-DoF parallel kinematic machines
CN112269356B (zh) 一种机器人nurbs轨迹插补方法
Chang et al. Image feature command generation of contour following tasks for SCARA robots employing Image-Based Visual Servoing—A PH-spline approach
CN112975992A (zh) 一种误差可控的机器人轨迹同步优化方法
Guilamo et al. Manipulability optimization for trajectory generation
Pan et al. Collision-free and curvature-continuous path smoothing in cluttered environments
CN114571452A (zh) 工业机器人轨迹规划方法、电子设备及可读存储介质
Li et al. A novel cartesian trajectory planning method by using triple nurbs curves for industrial robots
CN112356032B (zh) 一种姿态平滑过渡方法及系统
CN111496798B (zh) 机器人传送带跟踪方法、设备及存储装置
CN113618728A (zh) 机器人运动轨迹的补偿方法、装置及计算机存储介质
CN111515954A (zh) 一种机械臂高质量运动路径生成方法
Fujii et al. Realtime trajectory smoothing with neural nets
CN113276116B (zh) 一种误差可控的机器人轨迹同步过渡方法
CN113103241B (zh) 用于实现g2连续的机器人双nurbs轨迹插补方法、装置及系统
Kim et al. Development of a trajectory generation method for a five-axis NC machine

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