CN112975992B - 一种误差可控的机器人轨迹同步优化方法 - Google Patents

一种误差可控的机器人轨迹同步优化方法 Download PDF

Info

Publication number
CN112975992B
CN112975992B CN202110557131.XA CN202110557131A CN112975992B CN 112975992 B CN112975992 B CN 112975992B CN 202110557131 A CN202110557131 A CN 202110557131A CN 112975992 B CN112975992 B CN 112975992B
Authority
CN
China
Prior art keywords
dimensional
track
points
linear
transition
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.)
Active
Application number
CN202110557131.XA
Other languages
English (en)
Other versions
CN112975992A (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.)
Wuhan Hanmai Technology Co ltd
Original Assignee
Wuhan Hanmai 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 Wuhan Hanmai Technology Co ltd filed Critical Wuhan Hanmai Technology Co ltd
Priority to CN202110557131.XA priority Critical patent/CN112975992B/zh
Publication of CN112975992A publication Critical patent/CN112975992A/zh
Application granted granted Critical
Publication of CN112975992B publication Critical patent/CN112975992B/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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种误差可控的机器人轨迹同步优化方法，采用多维轨迹点表示多种机器人轨迹，基于多维轨迹点的定义，建立多维轨迹点的统一运算规则和多维曲线，实现了机器人轨迹的高连续性同步优化，并基于几何迭代法实现了多种类型机器人轨迹的高精度插值，该方法能够提高工业机器人轨迹的精度和工作效率，减少机器人作业时的振动。

Description

一种误差可控的机器人轨迹同步优化方法
技术领域
本发明属于工业机器人轨迹优化领域，具体涉及一种误差可控的机器人轨迹同步优化方法。
背景技术
轨迹执行精度低和执行时的振动问题是当前工业机器人面临的主要问题。与多轴机床相比，工业机器人结构紧凑，灵活性高，适用于三维复杂应用。但工业机器人是开环***，稳定性差，在线控制***没有多轴机床成熟。随着工业机器人作业方式的多样化和复杂化，保障精度、降低振动是当前工业机器人的主要目标之一。
工业机器人运动指令主要包括线性运动指令和圆弧运动指令。直线段和圆弧表示的运动轨迹在连接处仅具有G0连续性。在机器人作业过程中，为了精确到达给定轨迹点必须降速，因此大大降低了作业效率；另外，速度和加速度不连续可能导致机器人运动时的振动，从而加快机器磨损并影响轨迹精度。轨迹平顺技术能够提高机器人轨迹的连续性，对解决振动问题和提高精度具有重要意义。
然而，目前机器人控制***中缺乏既满足连续性又保证精度的轨迹平顺方法。现有机器人控制器中大多集成了局部拐角过渡算法，但此类平顺算法在实际应用中存在以下问题：（1）无法控制过渡误差，或只能控制位置轨迹的过渡误差；（2）暂无位置和姿态的同步平顺；（3）平顺轨迹的几何形状由控制器决定，无法由用户直接控制。申请号为CN201911300865.9的发明专利提出了一种位姿同步的六轴工业机器人轨迹平顺方法，采用圆弧曲线对位置轨迹过渡，并采用四元数B样条对姿态过渡，但该过渡方法只能满足G1连续的机器人轨迹过渡，无法插值目标轨迹点。
另外，KUKA机器人和Motoman机器人提供了局部样条插值指令，生成的样条轨迹能够插值目标轨迹点，但插值样条的形状由控制器内部决定，不能由用户自主控制，导致平滑轨迹在两个轨迹点之间具有不可控的弦高差。
基于以上问题，本发明提出了一种误差可控的的局部插值优化算法，该方法可以实现位置点和姿态点的几何同步插值，该方法具有多轨迹适用性，平顺后的轨迹形状良好，能够插值线性轨迹，并且与线性轨迹之间的弦高误差可控。
发明内容
针对现有技术存在的问题，本发明拟构造满足插值精度、弦高差约束、保型约束、对称约束的多维优化轨迹，优化轨迹通过对“虚拟线性轨迹”进行拐角过渡得到，该算法的核心是采用几何迭代法构造“虚拟线性轨迹”并计算满足以上约束的“过渡参数”。
一种误差可控的机器人轨迹同步优化方法，其特征在于：
步骤1、通过定义多维轨迹点来表示多种机器人的位置和姿态轨迹，该多维轨迹点能够同时表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹；
步骤2、基于多维轨迹点的定义，建立多维轨迹点的统一运算规则和多维曲线，多维轨迹点的多维运算包括多维距离、多维加法、多维数乘和多维减法，所述多维曲线包括多维线段和多维B样条曲线；
步骤3、基于多维轨迹点和多维运算建立基于凸组合表示的机器人轨迹的高连续同步过渡方法，该方法可针对不同的情况扩展、客制化，实现不同类型机器人及不同连续性要求的机器人轨迹过渡，同步过渡可采用圆弧、抛物线、B样条等曲线作为过渡曲线，不同过渡类型的区别仅在于基函数的不同；
设一系列机器人线性轨迹点表示为
Figure 106374DEST_PATH_IMAGE002
，其中
Figure 501583DEST_PATH_IMAGE004
为多维轨迹点，遍历
Figure 784797DEST_PATH_IMAGE006
，基于凸组合的
Figure 200735DEST_PATH_IMAGE004
点处的过渡轨迹表示为：
Figure DEST_PATH_IMAGE008A
其中
Figure 936610DEST_PATH_IMAGE010
为曲线的参数，
Figure 728985DEST_PATH_IMAGE012
，
Figure 397864DEST_PATH_IMAGE014
，式中的加法、数乘和减法均表示多维轨迹点的多维运算，
Figure 391228DEST_PATH_IMAGE016
和
Figure 614399DEST_PATH_IMAGE018
为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到；
步骤4、建立多维机器人线性轨迹的位姿同步高精度插值优化方法，首先对下文中需要用到的符号进行说明，
Figure 351411DEST_PATH_IMAGE020
为优化前的多维机器人线性轨迹，
Figure 468271DEST_PATH_IMAGE022
表示优化前多维线性轨迹点
Figure 898116DEST_PATH_IMAGE024
的索引
Figure 139741DEST_PATH_IMAGE026
的上界，
Figure 883706DEST_PATH_IMAGE028
为待求解的虚拟线性轨迹，
Figure 527177DEST_PATH_IMAGE030
和
Figure 455819DEST_PATH_IMAGE032
为待求解的过渡参数，其中
Figure 184740DEST_PATH_IMAGE034
，
Figure 732396DEST_PATH_IMAGE036
为待求解的优化曲线，
Figure 230374DEST_PATH_IMAGE038
为过渡轨迹
Figure 2021DEST_PATH_IMAGE036
的参数中点：
Figure 557853DEST_PATH_IMAGE040
，其中
Figure 440359DEST_PATH_IMAGE042
、
Figure 261684DEST_PATH_IMAGE044
、
Figure 204232DEST_PATH_IMAGE046
分别表示第
Figure 235642DEST_PATH_IMAGE026
个，第
Figure 656259DEST_PATH_IMAGE048
个和第
Figure 597670DEST_PATH_IMAGE050
个待求解的虚拟线性轨迹点；
步骤4.1：构造第一个迭代（迭代次数
Figure 711120DEST_PATH_IMAGE052
）的虚拟线性轨迹和过渡参数，其中初始虚拟线性轨迹
Figure 636351DEST_PATH_IMAGE054
与输入的线性轨迹
Figure 454134DEST_PATH_IMAGE020
相同，过渡参数
Figure 515631DEST_PATH_IMAGE056
和
Figure 268823DEST_PATH_IMAGE058
根据弦高差阈值
Figure 681350DEST_PATH_IMAGE060
、保形约束上界
Figure 302824DEST_PATH_IMAGE062
和位置轨迹对称约束计算，计算方法为求解以下线性优化问题：
Figure DEST_PATH_IMAGE064AA
其中
Figure 546724DEST_PATH_IMAGE066
表示两个位置点之间三维距离的运算函数，即
Figure 736397DEST_PATH_IMAGE068
指两个位置点
Figure 105061DEST_PATH_IMAGE070
和
Figure 671172DEST_PATH_IMAGE072
之间的三维距离，
Figure 972840DEST_PATH_IMAGE074
指两个位置点
Figure 926890DEST_PATH_IMAGE070
和
Figure 314009DEST_PATH_IMAGE076
之间的三维距离；
步骤4.2：计算过渡曲线
Figure 683810DEST_PATH_IMAGE078
与目标点
Figure 43247DEST_PATH_IMAGE024
的位姿误差，该误差通过
Figure 840302DEST_PATH_IMAGE080
与目标点
Figure 449138DEST_PATH_IMAGE024
的误差评估，其中
Figure 216105DEST_PATH_IMAGE082
，
Figure 226787DEST_PATH_IMAGE080
与目标点
Figure 194743DEST_PATH_IMAGE024
之间的误差采用多维距离
Figure 759716DEST_PATH_IMAGE084
计算，判断当前计算的位置误差
Figure 471320DEST_PATH_IMAGE086
和姿态误差
Figure 336508DEST_PATH_IMAGE088
是否满足输入的位置插值误差阈值
Figure 68841DEST_PATH_IMAGE090
和姿态插值误差阈值
Figure 652269DEST_PATH_IMAGE092
，输入轨迹为三维线性轨迹时不需判断姿态误差，若
Figure 370826DEST_PATH_IMAGE094
且
Figure 356100DEST_PATH_IMAGE096
，则令
Figure 665858DEST_PATH_IMAGE098
,
Figure 330058DEST_PATH_IMAGE100
,
Figure 383465DEST_PATH_IMAGE102
,转步骤4.5，否则转步骤4.3。
步骤4.3：根据目标点误差计算虚拟线性轨迹的调整向量和步长，令
Figure 223245DEST_PATH_IMAGE104
，新的虚拟线性轨迹为：
Figure 172746DEST_PATH_IMAGE106
，其中
Figure 465187DEST_PATH_IMAGE108
为步长，
Figure 915760DEST_PATH_IMAGE110
为调整向量，首尾目标点不调整，即：
Figure 610047DEST_PATH_IMAGE112
，每个迭代的步长与调整向量的选取方法有关；
所述调整向量的确定方法有两种：方法1为变系数的加权迭代法：该方法的几何意义是：对第
Figure 261608DEST_PATH_IMAGE026
个迭代点
Figure 775766DEST_PATH_IMAGE114
进行调整时，只考虑当前点与待插值轨迹点
Figure 170975DEST_PATH_IMAGE024
之间的误差向量：
Figure 719768DEST_PATH_IMAGE116
；方法2为变系数的最小二乘迭代法，该方法的几何意义是：对第
Figure 401285DEST_PATH_IMAGE026
个迭代点
Figure 668318DEST_PATH_IMAGE114
进行调整时，不仅考虑当前点与待插值轨迹点
Figure 867218DEST_PATH_IMAGE024
之间的误差向量，也考虑前后两个点的误差向量；调整向量的计算如下：
Figure 739360DEST_PATH_IMAGE118
，在上述公式中，
Figure 998303DEST_PATH_IMAGE120
为调整向量的集合，
Figure 346107DEST_PATH_IMAGE122
，
Figure 83119DEST_PATH_IMAGE124
,
Figure 606504DEST_PATH_IMAGE126
,
Figure 505190DEST_PATH_IMAGE128
，
根据调整向量估算步长的范围：设某一矩阵为
Figure 100002_DEST_PATH_IMAGE130
，对于方法1：
Figure 100002_DEST_PATH_IMAGE132
；对于方法2：
Figure 100002_DEST_PATH_IMAGE134
，步长
Figure 74712DEST_PATH_IMAGE108
的取值范围为：
Figure 100002_DEST_PATH_IMAGE136
，其中
Figure 100002_DEST_PATH_IMAGE138
是矩阵
Figure 677732DEST_PATH_IMAGE130
的最大特征值，特殊的，可取
Figure 100002_DEST_PATH_IMAGE140
作为步长的取值，计算完调整向量后转步骤4.4；
步骤4.4：根据调整向量计算虚拟线性轨迹和过渡参数，虚拟线性轨迹的计算公式为：
Figure 321202DEST_PATH_IMAGE106
，过渡参数
Figure 100002_DEST_PATH_IMAGE142
和
Figure 100002_DEST_PATH_IMAGE144
的计算与步骤4.1类似，除了弦高差阈值
Figure 718686DEST_PATH_IMAGE060
、保形约束上界
Figure 447607DEST_PATH_IMAGE062
和位置轨迹对称约束，还需考虑迭代算法的收敛性约束，计算方法为求解以下线性优化问题：
Figure 100002_DEST_PATH_IMAGE146
其中目标函数的含义是目标点位置插值，前两个约束条件为弦高差约束和保型约束，第三个约束条件为对称性约束，最后两个约束条件分别为位置和姿态的收敛性约束，该优化问题为带约束的二次优化问题，能够很容易的求解出满足条件的
Figure 100002_DEST_PATH_IMAGE148
和
Figure 100002_DEST_PATH_IMAGE150
，该步骤计算完成后转步骤4.2；
步骤4.5：根据虚拟线性轨迹
Figure 57580DEST_PATH_IMAGE028
和过渡参数
Figure 100002_DEST_PATH_IMAGE152
,
Figure 100002_DEST_PATH_IMAGE154
构造
Figure 149033DEST_PATH_IMAGE024
点处的优化轨迹
Figure 655101DEST_PATH_IMAGE036
:
Figure 100002_DEST_PATH_IMAGE156
其中基函数
Figure 340160DEST_PATH_IMAGE016
和
Figure 816141DEST_PATH_IMAGE018
与
Figure 168625DEST_PATH_IMAGE152
,
Figure 111173DEST_PATH_IMAGE154
的取值有关，具体与选取的过渡曲线类型有关，遍历除首尾外的所有目标点
Figure 100002_DEST_PATH_IMAGE158
，进行轨迹优化，最终的优化轨迹由线性部分和过渡曲线部分组成。
所述步骤1中多维轨迹点采用统一参数表示，三维位置轨迹、SCARA（SelectiveCompliance Assembly Robot Arm）机器人的位姿轨迹和六关节机器人的位姿轨迹，其多维轨迹点形式如下：
Figure 100002_DEST_PATH_IMAGE160
其中
Figure 100002_DEST_PATH_IMAGE162
表示三维位置点，
Figure 100002_DEST_PATH_IMAGE164
表示SCARA机器人的位置和姿态，
Figure 100002_DEST_PATH_IMAGE166
表示SCARA机器人的姿态采用轴角法表示时的旋转角，SCARA机器人姿态的旋转轴固定，只有旋转角度为变量，
Figure 100002_DEST_PATH_IMAGE168
表示六关节机器人的位置和姿态点，
Figure 100002_DEST_PATH_IMAGE170
是六关节机器人姿态的四元数表示。
所述步骤2多维距离是指两个多维轨迹点之间的抽象距离，设
Figure 100002_DEST_PATH_IMAGE172
，定义多维距离运算
Figure 100002_DEST_PATH_IMAGE174
：
Figure 100002_DEST_PATH_IMAGE176
当
Figure 142583DEST_PATH_IMAGE162
时，
Figure 100002_DEST_PATH_IMAGE178
指两个位置点
Figure 100002_DEST_PATH_IMAGE180
和
Figure 156675DEST_PATH_IMAGE024
之间的三维距离；当
Figure 98086DEST_PATH_IMAGE164
或
Figure 945957DEST_PATH_IMAGE168
时，多维距离包括三维位置距离
Figure 464663DEST_PATH_IMAGE178
和姿态夹角距离
Figure 100002_DEST_PATH_IMAGE182
，其中
Figure 100002_DEST_PATH_IMAGE184
表示两个旋转角的差的绝对值，
Figure 100002_DEST_PATH_IMAGE186
表示从四元数
Figure 100002_DEST_PATH_IMAGE188
表示的姿态旋转到
Figure 100002_DEST_PATH_IMAGE190
表示的姿态所旋转的角度。
所述步骤2中多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，其形式如下式，当
Figure 751287DEST_PATH_IMAGE164
时的加法运算与线性空间的加法一致，
Figure 78364DEST_PATH_IMAGE168
时的加法运算由线性空间的加法和三维旋转空间的四元数乘法组成：
Figure 100002_DEST_PATH_IMAGE192
其中
Figure 100002_DEST_PATH_IMAGE194
表示四元数
Figure 425031DEST_PATH_IMAGE188
与
Figure 837558DEST_PATH_IMAGE190
的乘法运算，设
Figure 100002_DEST_PATH_IMAGE196
,
Figure 100002_DEST_PATH_IMAGE198
，四元数的乘法表示为：
Figure 100002_DEST_PATH_IMAGE200
，其中s为一个一维变量，
Figure 100002_DEST_PATH_IMAGE202
为一个三维向量。
所述步骤2中多维数乘是一个多维向量和一个常数的数乘运算，为位置数乘和姿态数乘的组合，表示如下式：
Figure 100002_DEST_PATH_IMAGE204
当
Figure 100002_DEST_PATH_IMAGE206
时的数乘运算与线性空间的数乘一致，
Figure 724612DEST_PATH_IMAGE168
时的数乘运算由线性空间的数乘和姿态空间的指数运算组成，
Figure 100002_DEST_PATH_IMAGE208
表示四元数
Figure 906194DEST_PATH_IMAGE190
的指数运算，m为用于数乘的常数。
所述步骤2中多维减法采用多维加法和多维数乘表示：
Figure 100002_DEST_PATH_IMAGE210
。
所述步骤2中多维线段由线性空间的线性插值和旋转空间(姿态)的球面线性插值（SLERP）组合得到，设
Figure 830288DEST_PATH_IMAGE172
为两个多维轨迹点，两点之间的多维线段
Figure 100002_DEST_PATH_IMAGE212
可表示为下式：
Figure 100002_DEST_PATH_IMAGE214
，
式中的加法、数乘和减法均表示多维轨迹点的多维运算。
所述步骤2中多维B样条曲线由线性空间的B样条曲线和旋转空间(姿态)的四元数B样条曲线组合得到，其具体定义如下：给定控制顶点
Figure DEST_PATH_IMAGE216
，节点向量
Figure 100002_DEST_PATH_IMAGE218
，次数
Figure 100002_DEST_PATH_IMAGE220
，多维B样条曲线
Figure DEST_PATH_IMAGE222
可表示为：
Figure DEST_PATH_IMAGE224
其中：
Figure DEST_PATH_IMAGE226
为累积B样条基函数，累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数
Figure DEST_PATH_IMAGE228
表示为：
Figure DEST_PATH_IMAGE230
累积B样条基函数表示如下：
Figure DEST_PATH_IMAGE232
。
所述步骤4.5当采用G2连续的B样条对
Figure DEST_PATH_IMAGE234
过渡，多维B样条曲线控制点的分布和基函数的具体形式如下，
Figure DEST_PATH_IMAGE236
Figure DEST_PATH_IMAGE238
式中
Figure DEST_PATH_IMAGE240
为B样条的5个控制点，
Figure DEST_PATH_IMAGE242
为过渡起点，
Figure DEST_PATH_IMAGE244
为过渡终点，
Figure 100002_DEST_PATH_IMAGE246
为具有5个控制点的三次B样条的基函数，取
Figure 100002_DEST_PATH_IMAGE248
,根据
Figure 100002_DEST_PATH_IMAGE250
计算出
Figure 100002_DEST_PATH_IMAGE252
, 根据
Figure 100002_DEST_PATH_IMAGE254
计算出
Figure DEST_PATH_IMAGE256
，进而计算出5个控制点；遍历除首尾外的所有目标点
Figure 635171DEST_PATH_IMAGE158
，进行轨迹优化，最终的优化轨迹由线性部分和过渡曲线部分组成，在上述案例中，最终的优化后的轨迹为线性部分
Figure DEST_PATH_IMAGE258
，曲线部分
Figure DEST_PATH_IMAGE260
,线性部分
Figure DEST_PATH_IMAGE262
，曲线部分
Figure DEST_PATH_IMAGE264
,直到曲线部分
Figure DEST_PATH_IMAGE266
线性部分
Figure DEST_PATH_IMAGE268
组成，线性轨迹与优化曲线之间具有G2连续性。
本发明具有如下优点：
1、本发明提出的误差可控的机器人轨迹同步优化方法能够实现位置和姿态的高精度插值，平顺后的轨迹形状良好，能够插值线性轨迹，并且与线性轨迹之间的弦高误差可控；
2、本发明提出的机器人轨迹优化方法能够实现位置和姿态几何参数同步的高连续性轨迹平顺；算法计算效率高且数值计算稳定；
3、本发明突出的多维轨迹点及其运算的方法具有多轨迹适用性：能够同时适用于三维位置轨迹、SCARA机器人位姿轨迹、及六关节机器人位姿轨迹的几何平顺；建立统一的计算框架，能够涵盖任意过渡曲线格式和任意连续性要求，可以很容易针对不同的情况扩展、客制化。
附图说明
图1为六关节机器人位置轨迹过渡示意图；
图2为六关节机器人姿态轨迹过渡示意图；
图3为本发明提出的轨迹优化前后对比图；
图4为本发明提出的轨迹优化算法流程图。
具体实施方式
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
一种误差可控的机器人轨迹同步优化方法，该方法可以实现多种机器人位姿（位置和姿态）轨迹的几何同步过渡或插值优化，优化后的位置和姿态同时满足高连续性和误差可控。本发明的发明内容主要包含以下两部分：
（1）定义多维轨迹点及多维机器人轨迹的拐角过渡算法：多维轨迹点采用统一参数表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹，其形式如下：
Figure DEST_PATH_IMAGE160A
其中
Figure 556658DEST_PATH_IMAGE162
表示三维位置点，
Figure 186223DEST_PATH_IMAGE164
表示SCARA机器人的位置和姿态，
Figure 812376DEST_PATH_IMAGE166
表示SCARA机器人的姿态采用轴角法表示时的旋转角，SCARA机器人姿态的旋转轴固定，只有旋转角度为变量，
Figure 668337DEST_PATH_IMAGE168
表示六关节机器人的位置和姿态点，
Figure 772559DEST_PATH_IMAGE170
是六关节机器人姿态的四元数表示。
多维轨迹点的运算主要包括多维距离、多维加法、多维数乘和多维减法。
多维距离是指两个多维轨迹点之间的抽象距离，设
Figure 928734DEST_PATH_IMAGE172
，定义多维距离运算
Figure 319264DEST_PATH_IMAGE174
：
Figure DEST_PATH_IMAGE176A
当
Figure 396941DEST_PATH_IMAGE162
时，
Figure 570434DEST_PATH_IMAGE178
指两个位置点
Figure 174590DEST_PATH_IMAGE180
和
Figure 142546DEST_PATH_IMAGE024
之间的三维距离；当
Figure 238678DEST_PATH_IMAGE164
或
Figure 153545DEST_PATH_IMAGE168
时，多维距离包括三维位置距离
Figure 284312DEST_PATH_IMAGE178
和姿态夹角距离
Figure 16644DEST_PATH_IMAGE182
，其中
Figure 600072DEST_PATH_IMAGE184
表示两个旋转角的差的绝对值，
Figure 849788DEST_PATH_IMAGE186
表示从四元数
Figure 303903DEST_PATH_IMAGE188
表示的姿态旋转到
Figure 613662DEST_PATH_IMAGE190
表示的姿态所旋转的角度。
多维轨迹点的多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，其形式如下式，当
Figure 277861DEST_PATH_IMAGE164
时的加法运算与线性空间的加法一致，
Figure 331268DEST_PATH_IMAGE168
时的加法运算由线性空间的加法和三维旋转空间的四元数乘法组成：
Figure DEST_PATH_IMAGE192A
其中
Figure 639890DEST_PATH_IMAGE194
表示四元数
Figure 714025DEST_PATH_IMAGE188
与
Figure 6466DEST_PATH_IMAGE190
的乘法运算，设
Figure 863564DEST_PATH_IMAGE196
,
Figure 26692DEST_PATH_IMAGE198
，四元数的乘法表示为：
Figure 678253DEST_PATH_IMAGE200
，其中s为一个一维变量，
Figure DEST_PATH_IMAGE270
为一个三维向量。
多维轨迹点的数乘表示如下式，当
Figure 317045DEST_PATH_IMAGE271
时的数乘运算与线性空间的数乘一致，
Figure DEST_PATH_IMAGE272
时的数乘运算由线性空间的数乘和姿态空间的指数运算组成：
Figure DEST_PATH_IMAGE204A
Figure 774571DEST_PATH_IMAGE208
表示四元数
Figure 323364DEST_PATH_IMAGE190
的指数运算，m为用于数乘的常数。
多维减法可采用多维加法和多维数乘表示：
Figure 411405DEST_PATH_IMAGE273
。
基于以上多维运算可建立多维线段和多维B样条曲线，其中多维线段由线性空间的线性插值和旋转空间(姿态)的球面线性插值（SLERP）组合得到，设
Figure DEST_PATH_IMAGE274
为两个多维轨迹点，两点之间的多维线段
Figure 147280DEST_PATH_IMAGE275
可表示为下式：
Figure DEST_PATH_IMAGE276
。
多维B样条曲线由线性空间的B样条曲线和旋转空间(姿态)的四元数B样条曲线组合得到，其具体定义如下：给定控制顶点
Figure 674077DEST_PATH_IMAGE277
，节点向量
Figure 342955DEST_PATH_IMAGE218
，次数
Figure 70740DEST_PATH_IMAGE220
，多维B样条曲线
Figure DEST_PATH_IMAGE278
可表示为：
Figure DEST_PATH_IMAGE224A
其中：
Figure 152965DEST_PATH_IMAGE279
为累积B样条基函数，累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数
Figure DEST_PATH_IMAGE280
表示为：
Figure DEST_PATH_IMAGE230A
累积B样条基函数可表示如下：
Figure 217873DEST_PATH_IMAGE281
。
基于多维轨迹点的定义和多维运算，本发明提出基于凸组合来描述多维机器人轨迹的位姿同步过渡方法，设一系列机器人线性轨迹的目标点表示为
Figure DEST_PATH_IMAGE282
，遍历
Figure 210100DEST_PATH_IMAGE283
，基于凸组合的描述，在
Figure 374365DEST_PATH_IMAGE004
点处的过渡轨迹可表示为：
Figure DEST_PATH_IMAGE285AAAA
式中的加法、数乘和减法均表示多维轨迹点的多维运算，其中
Figure DEST_PATH_IMAGE286
和
Figure 678308DEST_PATH_IMAGE287
称为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到。
接下来介绍两种过渡曲线及基函数表示。
第一种为G1连续的拐角过渡，过渡轨迹采用4个控制点的三次Bezier曲线，节点向量为
Figure 219010DEST_PATH_IMAGE289
，控制点和基函数的表示如下式，其中
Figure 190377DEST_PATH_IMAGE242
位于多维线段
Figure 791123DEST_PATH_IMAGE291
上，
Figure 254465DEST_PATH_IMAGE293
位于多维线段
Figure 67701DEST_PATH_IMAGE295
上，
Figure 565678DEST_PATH_IMAGE297
和
Figure 71746DEST_PATH_IMAGE299
与
Figure 881439DEST_PATH_IMAGE004
重合：
Figure DEST_PATH_IMAGE301AA
Figure DEST_PATH_IMAGE303A
Figure 826261DEST_PATH_IMAGE242
、
Figure 178745DEST_PATH_IMAGE297
、
Figure 324556DEST_PATH_IMAGE305
、
Figure 762490DEST_PATH_IMAGE293
为4个多维控制点，
Figure 448686DEST_PATH_IMAGE307
，
Figure 249152DEST_PATH_IMAGE309
为具有四个控制点的三次B样条的基函数，
Figure 362602DEST_PATH_IMAGE311
为两个比例参数。
第二种为G2连续的拐角过渡，过渡轨迹采用5个控制点的三次B-spline曲线，节点向量为
Figure 287832DEST_PATH_IMAGE313
，控制点和基函数的表示如下式，其中
Figure 980982DEST_PATH_IMAGE242
,
Figure 308058DEST_PATH_IMAGE297
位于多维线段
Figure 185884DEST_PATH_IMAGE291
上，
Figure 332832DEST_PATH_IMAGE315
位于多维线段
Figure 360831DEST_PATH_IMAGE295
上，
Figure 11255DEST_PATH_IMAGE299
与
Figure 466507DEST_PATH_IMAGE004
重合。
Figure DEST_PATH_IMAGE317AA
Figure DEST_PATH_IMAGE238A
其中
Figure 428647DEST_PATH_IMAGE242
、
Figure 588233DEST_PATH_IMAGE297
、
Figure 624322DEST_PATH_IMAGE305
、
Figure 250475DEST_PATH_IMAGE319
为5个多维控制点，
Figure DEST_PATH_IMAGE320
为具有5个控制点的三次B样条的基函数，
Figure 840856DEST_PATH_IMAGE311
为两个比例参数。
图1和图2分别为采用5个控制点的三次B-spline对位置和姿态进行拐角过渡的示意图。图1中虚线轨迹为三维线性空间的线性插值线段（黑色圆点
Figure DEST_PATH_IMAGE322
表示位置点），三角形点
Figure DEST_PATH_IMAGE324
表示三维位置优化的5个控制点，实线轨迹为位置轨迹优化后平滑曲线。图2中虚线轨迹为三维旋转空间的球面线性线性插值（黑色圆点
Figure DEST_PATH_IMAGE326
表示姿态点），三角形点
Figure DEST_PATH_IMAGE328
表示三维姿态优化的5个控制点，实线轨迹为姿态轨迹优化后平滑曲线。
（2）多维机器人线性轨迹的位姿同步高精度插值优化方法。该方法的目的是构造满足插值精度、弦高差约束、保型约束、对称约束的多维优化轨迹，优化轨迹通过对“虚拟线性轨迹”进行拐角过渡得到，该算法的核心是采用几何迭代法构造“虚拟线性轨迹”并计算满足以上约束的“过渡参数”。
首先对下文中需要用到的符号进行说明。如图3所示，实线带圆点轨迹
Figure DEST_PATH_IMAGE329
为优化前的多维机器人线性轨迹，三角形虚线轨迹
Figure DEST_PATH_IMAGE330
为待求解的虚拟线性轨迹，
Figure DEST_PATH_IMAGE331
和
Figure DEST_PATH_IMAGE332
为待求解的过渡参数，其中
Figure DEST_PATH_IMAGE333
，
Figure DEST_PATH_IMAGE334
为待求解的虚拟线性轨迹和过渡参数构造的过渡曲线，
Figure DEST_PATH_IMAGE335
为过渡轨迹
Figure 335292DEST_PATH_IMAGE334
的参数中点：
Figure DEST_PATH_IMAGE336
。
如图4所示，多维机器人线性轨迹的位姿同步高精度插值优化方法的具体步骤为：
步骤4.1：构造第一个迭代（迭代次数
Figure DEST_PATH_IMAGE337
）的虚拟线性轨迹
Figure DEST_PATH_IMAGE338
和过渡参数
Figure DEST_PATH_IMAGE339
和
Figure DEST_PATH_IMAGE340
，其中初始虚拟线性轨迹
Figure 553783DEST_PATH_IMAGE338
与输入的线性轨迹
Figure 944313DEST_PATH_IMAGE329
相同，过渡参数
Figure 553149DEST_PATH_IMAGE339
和
Figure 461062DEST_PATH_IMAGE340
根据弦高差阈值
Figure DEST_PATH_IMAGE341
、保形约束上界
Figure DEST_PATH_IMAGE342
和位置轨迹对称约束计算，计算方法为求解以下线性优化问题：
Figure DEST_PATH_IMAGE064AAA
其中前两个不等式中，第一项描述弦高差约束，第二项描述保型约束，其中
Figure DEST_PATH_IMAGE344
与具体基函数相关，在上述的第一种过渡类型中，
Figure DEST_PATH_IMAGE346
，第二种类型中
Figure DEST_PATH_IMAGE348
，其中
Figure DEST_PATH_IMAGE350
为两条过渡曲线之间的线性部分占整条线段的比例。
步骤4.2：计算过渡曲线
Figure DEST_PATH_IMAGE351
与目标点
Figure 65219DEST_PATH_IMAGE024
的位姿误差，根据过渡曲线的对称性，该误差可直接通过参数中点
Figure 33175DEST_PATH_IMAGE080
与目标点
Figure 722782DEST_PATH_IMAGE024
的误差评估，其中：
Figure DEST_PATH_IMAGE352
，
Figure 434386DEST_PATH_IMAGE080
与目标点
Figure 33995DEST_PATH_IMAGE024
之间的误差可采用多维距离
Figure DEST_PATH_IMAGE353
计算,多维距离由位置误差
Figure DEST_PATH_IMAGE354
和姿态误差
Figure DEST_PATH_IMAGE355
组成，判断两个误差是否满足输入的位置插值误差阈值
Figure DEST_PATH_IMAGE356
和姿态插值误差阈值
Figure DEST_PATH_IMAGE357
，即判断
Figure DEST_PATH_IMAGE358
且
Figure DEST_PATH_IMAGE359
，当输入轨迹为三维线性轨迹时不需判断姿态误差，若两个误差都满足条件，说明当前的过渡轨迹能够插值目标轨迹点，可以停止循环，令
Figure DEST_PATH_IMAGE360
，
Figure DEST_PATH_IMAGE361
,
Figure DEST_PATH_IMAGE362
，转步骤4.5，否则转步骤4.3.
步骤4.3：根据目标点误差计算虚拟线性轨迹的调整向量和步长，令
Figure DEST_PATH_IMAGE363
，新的虚拟线性轨迹为：
Figure DEST_PATH_IMAGE364
，其中
Figure 297486DEST_PATH_IMAGE108
为步长，
Figure 208810DEST_PATH_IMAGE110
为调整向量。首尾目标点不调整，即：
Figure DEST_PATH_IMAGE365
。每个迭代的步长与调整向量的选取方法有关。
本发明提出了两种确定调整向量的方法：第一种方法称为变系数的加权迭代法，该方法的几何意义是：对第
Figure 724105DEST_PATH_IMAGE026
个迭代点
Figure DEST_PATH_IMAGE366
进行调整时，只考虑当前点与待插值轨迹点
Figure 178220DEST_PATH_IMAGE024
之间的误差向量：
Figure DEST_PATH_IMAGE367
，第二种调整向量的取法命名为变系数的最小二乘迭代法，该方法的几何意义是：对第
Figure 81454DEST_PATH_IMAGE026
个迭代点
Figure 886599DEST_PATH_IMAGE366
进行调整时，不仅考虑当前点与待插值轨迹点
Figure 940006DEST_PATH_IMAGE024
之间的误差向量，也考虑前后两个点的误差向量，调整向量的计算如下：
Figure DEST_PATH_IMAGE368
，在上述公式中，
Figure 248628DEST_PATH_IMAGE120
为调整向量的集合，
Figure DEST_PATH_IMAGE369
，
Figure DEST_PATH_IMAGE370
,
Figure DEST_PATH_IMAGE371
,
Figure DEST_PATH_IMAGE372
。
接下来估算步长的范围。设某一矩阵为
Figure 791604DEST_PATH_IMAGE130
，对于方法1：
Figure DEST_PATH_IMAGE373
；对于方法2：
Figure DEST_PATH_IMAGE374
。步长
Figure 954819DEST_PATH_IMAGE108
的取值范围为：
Figure DEST_PATH_IMAGE375
，其中
Figure 546337DEST_PATH_IMAGE138
是矩阵
Figure 975044DEST_PATH_IMAGE130
的最大特征值。特殊的，可取
Figure DEST_PATH_IMAGE376
作为步长的取值，计算完调整向量后转步骤4.4。
步骤4.4：根据调整向量计算虚拟线性轨迹和过渡参数，虚拟线性轨迹的计算方法为：
Figure 626606DEST_PATH_IMAGE364
，过渡参数
Figure DEST_PATH_IMAGE377
和
Figure DEST_PATH_IMAGE378
的计算与步骤1类似，除了弦高差阈值
Figure 468660DEST_PATH_IMAGE341
、保形约束上界
Figure 863869DEST_PATH_IMAGE342
和位置轨迹对称约束，还需考虑迭代算法的收敛性约束，计算方法为求解以下线性优化问题：
Figure DEST_PATH_IMAGE146A
其中目标函数的含义是目标点位置插值，前两个约束条件为弦高差约束和保型约束，第三个约束条件为对称性约束，最后两个约束条件分别为位置和姿态的收敛性约束。该优化问题为带约束的二次优化问题，能够很容易的求解出满足条件的
Figure 334033DEST_PATH_IMAGE148
和
Figure 625337DEST_PATH_IMAGE150
。计算完该迭代的虚拟线性轨迹和过渡参数后转步骤4.2。
步骤4.5：根据虚拟线性轨迹
Figure 892371DEST_PATH_IMAGE330
和过渡参数
Figure 91271DEST_PATH_IMAGE152
,
Figure 415942DEST_PATH_IMAGE154
构造
Figure 409306DEST_PATH_IMAGE024
点处的优化轨迹
Figure 632477DEST_PATH_IMAGE334
:
Figure DEST_PATH_IMAGE156A
其中基函数
Figure 228543DEST_PATH_IMAGE286
和
Figure 751928DEST_PATH_IMAGE287
与
Figure 916193DEST_PATH_IMAGE152
,
Figure 626660DEST_PATH_IMAGE154
的取值有关，具体与选取的过渡曲线类型有关，具体可参考前文介绍的两种过渡曲线类型。遍历除首尾外的所有目标点
Figure DEST_PATH_IMAGE379
，进行轨迹优化，最终的优化轨迹由线性部分和过渡曲线部分组成，在上述案例中，最终的优化后的轨迹为线性部分
Figure DEST_PATH_IMAGE380
，曲线部分
Figure DEST_PATH_IMAGE381
,线性部分
Figure DEST_PATH_IMAGE382
，曲线部分
Figure DEST_PATH_IMAGE383
,直到曲线部分
Figure DEST_PATH_IMAGE384
线性部分
Figure DEST_PATH_IMAGE385
组成，线性轨迹与优化曲线之间具有G2连续性。
本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (10)

1.一种误差可控的机器人轨迹同步优化方法，其特征在于，包括如下步骤：
步骤1、通过定义多维轨迹点来表示多种机器人的位置和姿态轨迹，该多维轨迹点能够同时表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹；
步骤2、基于多维轨迹点的定义，建立多维轨迹点的统一运算规则和多维曲线，多维轨迹点的多维运算包括多维距离、多维加法、多维数乘和多维减法，所述多维曲线包括多维线段和多维B样条曲线；
步骤3、基于多维轨迹点和多维运算建立基于凸组合表示的机器人轨迹的高连续同步过渡方法，同步过渡采用圆弧、抛物线或B样条曲线作为过渡曲线，不同过渡类型的区别仅在于基函数的不同；
设一系列机器人线性轨迹点表示为
Figure DEST_PATH_IMAGE002
，其中
Figure DEST_PATH_IMAGE004
为多维轨迹点，遍历
Figure DEST_PATH_IMAGE006
，基于凸组合的
Figure 229763DEST_PATH_IMAGE004
点处的过渡轨迹表示为：
Figure DEST_PATH_IMAGE008
其中
Figure DEST_PATH_IMAGE010
为曲线的参数，
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
，式中的加法、数乘和减法均表示多维轨迹点的多维运算，
Figure DEST_PATH_IMAGE016
和
Figure DEST_PATH_IMAGE018
为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到；
步骤4、建立多维机器人线性轨迹的位姿同步高精度插值优化方法：
设
Figure DEST_PATH_IMAGE020
为优化前的多维机器人线性轨迹，
Figure DEST_PATH_IMAGE022
表示优化前多维线性轨迹点
Figure DEST_PATH_IMAGE024
的索引
Figure DEST_PATH_IMAGE026
的上界，
Figure DEST_PATH_IMAGE028
为待求解的虚拟线性轨迹，
Figure DEST_PATH_IMAGE030
和
Figure DEST_PATH_IMAGE032
为待求解的过渡参数，其中
Figure DEST_PATH_IMAGE034
，
Figure DEST_PATH_IMAGE036
为待求解的优化曲线，
Figure DEST_PATH_IMAGE038
为过渡轨迹
Figure 396171DEST_PATH_IMAGE036
的参数中点：
Figure DEST_PATH_IMAGE040
其中
Figure DEST_PATH_IMAGE042
、
Figure DEST_PATH_IMAGE044
、
Figure DEST_PATH_IMAGE046
分别表示第
Figure 249944DEST_PATH_IMAGE026
个，第
Figure DEST_PATH_IMAGE048
个和第
Figure DEST_PATH_IMAGE050
个待求解的虚拟线性轨迹点；
步骤4.1：构造迭代次数
Figure DEST_PATH_IMAGE052
时对应的第一个迭代的虚拟线性轨迹和过渡参数，其中初始虚拟线性轨迹
Figure DEST_PATH_IMAGE054
与输入的线性轨迹
Figure 707470DEST_PATH_IMAGE020
相同，过渡参数
Figure DEST_PATH_IMAGE056
和
Figure DEST_PATH_IMAGE058
根据弦高差阈值
Figure DEST_PATH_IMAGE060
、保形约束上界
Figure DEST_PATH_IMAGE062
和位置轨迹对称约束计算，计算方法为求解以下线性优化问题：
Figure DEST_PATH_IMAGE064
其中
Figure DEST_PATH_IMAGE066
表示两个位置点之间三维距离的运算函数，即
Figure DEST_PATH_IMAGE068
指两个位置点
Figure DEST_PATH_IMAGE070
和
Figure DEST_PATH_IMAGE072
之间的三维距离，
Figure DEST_PATH_IMAGE074
指两个位置点
Figure 708793DEST_PATH_IMAGE070
和
Figure DEST_PATH_IMAGE076
之间的三维距离；
步骤4.2：计算过渡曲线
Figure DEST_PATH_IMAGE078
与目标点
Figure 97DEST_PATH_IMAGE024
的位姿误差，该误差通过
Figure DEST_PATH_IMAGE080
与目标点
Figure 267130DEST_PATH_IMAGE024
的误差评估，其中
Figure DEST_PATH_IMAGE082
，
Figure 59506DEST_PATH_IMAGE080
与目标点
Figure 197226DEST_PATH_IMAGE024
之间的误差采用多维距离
Figure DEST_PATH_IMAGE084
计算，判断当前计算的位置误差
Figure DEST_PATH_IMAGE086
和姿态误差
Figure DEST_PATH_IMAGE088
是否满足输入的位置插值误差阈值
Figure DEST_PATH_IMAGE090
和姿态插值误差阈值
Figure DEST_PATH_IMAGE092
，输入轨迹为三维线性轨迹时不需判断姿态误差，若
Figure DEST_PATH_IMAGE094
且
Figure DEST_PATH_IMAGE096
，则令
Figure DEST_PATH_IMAGE098
,
Figure DEST_PATH_IMAGE100
,
Figure DEST_PATH_IMAGE102
，转步骤4.5，否则转步骤4.3；
步骤4.3：根据目标点误差计算虚拟线性轨迹的调整向量和步长，令
Figure DEST_PATH_IMAGE104
，新的虚拟线性轨迹为：
Figure DEST_PATH_IMAGE106
，其中
Figure DEST_PATH_IMAGE108
为步长，
Figure DEST_PATH_IMAGE110
为调整向量，首尾目标点不调整，即：
Figure DEST_PATH_IMAGE112
；
调整向量的计算如下：
Figure DEST_PATH_IMAGE114
，
Figure DEST_PATH_IMAGE116
为调整向量的集合，
Figure DEST_PATH_IMAGE118
，
Figure DEST_PATH_IMAGE120
,
Figure DEST_PATH_IMAGE122
，
Figure DEST_PATH_IMAGE124
，
根据调整向量估算步长的范围：设某一矩阵为
Figure DEST_PATH_IMAGE126
，步长
Figure 439858DEST_PATH_IMAGE108
的取值范围为：
Figure DEST_PATH_IMAGE128
，其中
Figure DEST_PATH_IMAGE130
是矩阵
Figure 787662DEST_PATH_IMAGE126
的最大特征值，计算完调整向量后转步骤4.4；
步骤4.4：根据调整向量计算虚拟线性轨迹和过渡参数，虚拟线性轨迹的计算公式为：
Figure 524674DEST_PATH_IMAGE106
，过渡参数
Figure DEST_PATH_IMAGE132
和
Figure DEST_PATH_IMAGE134
的计算，除了弦高差阈值
Figure 516901DEST_PATH_IMAGE060
、保形约束上界
Figure 946745DEST_PATH_IMAGE062
和位置轨迹对称约束，还需考虑迭代算法的收敛性约束，具体为求解以下线性优化问题：
Figure DEST_PATH_IMAGE136
其中目标函数的含义是目标点位置插值，前两个约束条件为弦高差约束和保型约束，第三个约束条件为对称性约束，最后两个约束条件分别为位置和姿态的收敛性约束，该优化问题为带约束的二次优化问题，能够很容易的求解出满足条件的
Figure DEST_PATH_IMAGE138
和
Figure DEST_PATH_IMAGE140
，该步骤计算完成后转步骤4.2；
步骤4.5：根据虚拟线性轨迹
Figure 844163DEST_PATH_IMAGE028
和过渡参数
Figure DEST_PATH_IMAGE142
,
Figure DEST_PATH_IMAGE144
构造
Figure 588128DEST_PATH_IMAGE024
点处的优化轨迹
Figure 231599DEST_PATH_IMAGE036
:
Figure DEST_PATH_IMAGE146
其中基函数
Figure DEST_PATH_IMAGE148
和
Figure DEST_PATH_IMAGE150
与
Figure DEST_PATH_IMAGE152
,
Figure DEST_PATH_IMAGE154
的取值有关，具体与选取的过渡曲线类型有关，遍历除首尾外的所有目标点
Figure DEST_PATH_IMAGE156
，进行轨迹优化，最终的优化轨迹由线性部分和过渡曲线部分组成。
2.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤1中多维轨迹点采用统一参数表示，三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹，其多维轨迹点形式如下：
Figure DEST_PATH_IMAGE158
其中
Figure DEST_PATH_IMAGE160
表示三维位置点，
Figure DEST_PATH_IMAGE162
表示SCARA机器人的位置和姿态，
Figure DEST_PATH_IMAGE164
表示SCARA机器人的姿态采用轴角法表示时的旋转角，SCARA机器人姿态的旋转轴固定，只有旋转角度为变量，
Figure DEST_PATH_IMAGE166
表示六关节机器人的位置和姿态点，
Figure DEST_PATH_IMAGE168
是六关节机器人姿态的四元数表示。
3.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2多维距离是指两个多维轨迹点之间的抽象距离，设
Figure DEST_PATH_IMAGE170
，定义多维距离运算
Figure DEST_PATH_IMAGE172
：
Figure DEST_PATH_IMAGE174
当
Figure 347191DEST_PATH_IMAGE160
时，
Figure DEST_PATH_IMAGE176
指两个位置点
Figure DEST_PATH_IMAGE178
和
Figure DEST_PATH_IMAGE180
之间的三维距离；当
Figure 138430DEST_PATH_IMAGE162
或
Figure 217244DEST_PATH_IMAGE166
时，多维距离包括三维位置距离
Figure 715222DEST_PATH_IMAGE176
和姿态夹角距离
Figure DEST_PATH_IMAGE182
，其中
Figure DEST_PATH_IMAGE184
表示两个旋转角的差的绝对值，
Figure DEST_PATH_IMAGE186
表示从四元数
Figure DEST_PATH_IMAGE188
表示的姿态旋转到
Figure DEST_PATH_IMAGE190
表示的姿态所旋转的角度。
4.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2中多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，其形式如下式，当
Figure DEST_PATH_IMAGE192
时的加法运算与线性空间的加法一致，
Figure DEST_PATH_IMAGE194
时的加法运算由线性空间的加法和三维旋转空间的四元数乘法组成：
Figure DEST_PATH_IMAGE196
其中
Figure DEST_PATH_IMAGE198
表示四元数
Figure DEST_PATH_IMAGE200
与
Figure DEST_PATH_IMAGE202
的乘法运算，设
Figure DEST_PATH_IMAGE204
,
Figure DEST_PATH_IMAGE206
，四元数的乘法表示为：
Figure DEST_PATH_IMAGE208
，其中s为一个一维变量，
Figure DEST_PATH_IMAGE210
为一个三维向量。
5.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2中多维数乘是一个多维向量和一个常数的数乘运算，为位置数乘和姿态数乘的组合，表示如下式：
Figure DEST_PATH_IMAGE212
当
Figure DEST_PATH_IMAGE214
时的数乘运算与线性空间的数乘一致，
Figure DEST_PATH_IMAGE215
时的数乘运算由线性空间的数乘和姿态空间的指数运算组成，
Figure DEST_PATH_IMAGE217
表示四元数
Figure DEST_PATH_IMAGE218
的指数运算，m为用于数乘的常数。
6.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2中多维减法采用多维加法和多维数乘表示：
Figure DEST_PATH_IMAGE220
。
7.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2中多维线段由线性空间的线性插值和旋转空间的球面线性插值组合得到，设
Figure DEST_PATH_IMAGE221
为两个多维轨迹点，两点之间的多维线段
Figure DEST_PATH_IMAGE223
表示为下式：
Figure DEST_PATH_IMAGE225
，
式中的加法、数乘和减法均表示多维轨迹点的多维运算。
8.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤2中多维B样条曲线由线性空间的B样条曲线和旋转空间的四元数B样条曲线组合得到，其具体定义如下：给定控制顶点
Figure DEST_PATH_IMAGE227
，节点向量
Figure DEST_PATH_IMAGE229
，次数
Figure DEST_PATH_IMAGE231
，多维B样条曲线
Figure DEST_PATH_IMAGE233
表示为：
Figure DEST_PATH_IMAGE235
其中：
Figure DEST_PATH_IMAGE237
为累积B样条基函数，累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数
Figure DEST_PATH_IMAGE239
表示为：
Figure DEST_PATH_IMAGE241
累积B样条基函数表示如下：
Figure DEST_PATH_IMAGE243
。
9.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤4.3中调整向量的确定方法有两种：方法1为变系数的加权迭代法：该方法的几何意义是：对第
Figure 923087DEST_PATH_IMAGE026
个迭代点
Figure DEST_PATH_IMAGE245
进行调整时，只考虑当前点与待插值轨迹点
Figure DEST_PATH_IMAGE246
之间的误差向量：
Figure DEST_PATH_IMAGE248
；方法2为变系数的最小二乘迭代法，该方法的几何意义是：对第
Figure 936042DEST_PATH_IMAGE026
个迭代点
Figure 818548DEST_PATH_IMAGE245
进行调整时，不仅考虑当前点与待插值轨迹点
Figure 639873DEST_PATH_IMAGE246
之间的误差向量，也考虑前后两个点的误差向量；根据调整向量估算步长的范围：设某一矩阵为
Figure 582421DEST_PATH_IMAGE126
，对于方法1：
Figure DEST_PATH_IMAGE250
；对于方法2：
Figure DEST_PATH_IMAGE252
；步长
Figure DEST_PATH_IMAGE253
的取值范围为：
Figure DEST_PATH_IMAGE254
，其中
Figure DEST_PATH_IMAGE255
是矩阵
Figure 676148DEST_PATH_IMAGE126
的最大特征值。
10.如权利要求1所述的一种误差可控的机器人轨迹同步优化方法，其特征在于：所述步骤4.5当采用G2连续的B样条对
Figure DEST_PATH_IMAGE257
过渡，多维B样条曲线控制点的分布和基函数的具体形式如下，
Figure DEST_PATH_IMAGE259
Figure DEST_PATH_IMAGE261
式中
Figure DEST_PATH_IMAGE263
为B样条的5个控制点，
Figure DEST_PATH_IMAGE265
为过渡起点，
Figure DEST_PATH_IMAGE267
为过渡终点，
Figure DEST_PATH_IMAGE269
为具有5个控制点的三次B样条的基函数，取
Figure DEST_PATH_IMAGE271
，根据
Figure DEST_PATH_IMAGE273
计算出
Figure DEST_PATH_IMAGE275
, 根据
Figure DEST_PATH_IMAGE277
计算出
Figure DEST_PATH_IMAGE279
，进而计算出5个控制点；遍历除首尾外的所有目标点
Figure DEST_PATH_IMAGE281
，进行轨迹优化，最终的优化轨迹由线性部分和过渡曲线部分组成，最终的优化后的轨迹为线性部分
Figure DEST_PATH_IMAGE283
，曲线部分
Figure DEST_PATH_IMAGE285
,线性部分
Figure DEST_PATH_IMAGE287
，曲线部分
Figure DEST_PATH_IMAGE289
,直到曲线部分
Figure DEST_PATH_IMAGE291
线性部分
Figure DEST_PATH_IMAGE293
组成，线性轨迹与优化曲线之间具有G2连续性。
CN202110557131.XA 2021-05-21 2021-05-21 一种误差可控的机器人轨迹同步优化方法 Active CN112975992B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110557131.XA CN112975992B (zh) 2021-05-21 2021-05-21 一种误差可控的机器人轨迹同步优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110557131.XA CN112975992B (zh) 2021-05-21 2021-05-21 一种误差可控的机器人轨迹同步优化方法

Publications (2)

Publication Number Publication Date
CN112975992A CN112975992A (zh) 2021-06-18
CN112975992B true CN112975992B (zh) 2021-08-13

Family

ID=76337115

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110557131.XA Active CN112975992B (zh) 2021-05-21 2021-05-21 一种误差可控的机器人轨迹同步优化方法

Country Status (1)

Country Link
CN (1) CN112975992B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114184206B (zh) * 2021-12-03 2024-04-19 北京车慧达科技有限公司 一种基于车辆轨迹点生成行驶路线的方法和装置
CN114714351B (zh) * 2022-04-06 2023-06-23 上海工程技术大学 用于移动机械臂的抗饱和目标跟踪控制方法及控制***
CN115202293B (zh) * 2022-07-15 2023-04-28 武汉瀚迈科技有限公司 一种工业机器人两段式速度规划方法
CN116909154B (zh) * 2023-09-13 2023-12-08 武汉瀚迈科技有限公司 一种通过特征查表进行前馈补偿的机器人轨迹优化方法
CN117315198B (zh) * 2023-10-09 2024-04-16 中微智创(北京)软件技术有限公司 一种动目标轨迹拐角平缓微调的平滑优化方法和***

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0795804A4 (en) * 1995-08-31 2001-03-28 Fanuc Ltd METHOD FOR CURVE INTERPOLATION IN A SPEED CONTROLLER OF A ROBOT DURING A ROUNDING PROCESS
JP2014208400A (ja) * 2014-06-10 2014-11-06 株式会社デンソーウェーブ ロボットの制御装置及びロボットの姿勢補間方法
CN106826829A (zh) * 2017-02-22 2017-06-13 武汉工程大学 一种可控误差的工业机器人光顺运动轨迹生成方法
CN109571473A (zh) * 2018-12-03 2019-04-05 武汉工程大学 一种误差可控的小线段轨迹光顺方法
CN109664303A (zh) * 2019-02-28 2019-04-23 武汉工程大学 一种误差可控的四轴工业机器人b样条过渡式平顺轨迹生成方法
CN109676613A (zh) * 2019-02-28 2019-04-26 武汉工程大学 一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法
CN110308699A (zh) * 2019-04-28 2019-10-08 威海印刷机械有限公司 一种轨迹规划方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0795804A4 (en) * 1995-08-31 2001-03-28 Fanuc Ltd METHOD FOR CURVE INTERPOLATION IN A SPEED CONTROLLER OF A ROBOT DURING A ROUNDING PROCESS
JP2014208400A (ja) * 2014-06-10 2014-11-06 株式会社デンソーウェーブ ロボットの制御装置及びロボットの姿勢補間方法
CN106826829A (zh) * 2017-02-22 2017-06-13 武汉工程大学 一种可控误差的工业机器人光顺运动轨迹生成方法
CN109571473A (zh) * 2018-12-03 2019-04-05 武汉工程大学 一种误差可控的小线段轨迹光顺方法
CN109664303A (zh) * 2019-02-28 2019-04-23 武汉工程大学 一种误差可控的四轴工业机器人b样条过渡式平顺轨迹生成方法
CN109676613A (zh) * 2019-02-28 2019-04-26 武汉工程大学 一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法
CN110308699A (zh) * 2019-04-28 2019-10-08 威海印刷机械有限公司 一种轨迹规划方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法

Also Published As

Publication number Publication date
CN112975992A (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
CN112975992B (zh) 一种误差可控的机器人轨迹同步优化方法
CN109571473B (zh) 一种误差可控的小线段轨迹光顺方法
CN109664303B (zh) 一种误差可控的四轴工业机器人b样条过渡式平顺轨迹生成方法
CN107263484B (zh) 机器人关节空间点到点运动的轨迹规划方法
CN111791236B (zh) 一种工业机器人笛卡尔空间轨迹过渡方法
CN106647282B (zh) 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN110900612B (zh) 一种位姿同步的六轴工业机器人轨迹平顺方法
Liu et al. Development and implementation of a NURBS interpolator with smooth feedrate scheduling for CNC machine tools
CN109343345B (zh) 基于qpso算法的机械臂多项式插值轨迹规划方法
CN108568817B (zh) 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法
Zhang et al. Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines
Du et al. An error-bounded B-spline curve approximation scheme using dominant points for CNC interpolation of micro-line toolpath
CN105500354A (zh) 一种工业机器人应用的过渡轨迹规划方法
He et al. A tolerance constrained G2 continuous path smoothing and interpolation method for industrial SCARA robots
CN109676613B (zh) 一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法
CN111283683B (zh) 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法
CN115202293B (zh) 一种工业机器人两段式速度规划方法
CN111633668A (zh) 一种用于机器人加工三维自由曲面的运动控制方法
Fang et al. Study on high-speed and smooth transfer of robot motion trajectory based on modified S-shaped acceleration/deceleration algorithm
CN113103240B (zh) 用于实现c2连续的机器人轨迹规划方法、装置及***
CN111515954B (zh) 一种机械臂高质量运动路径生成方法
Li et al. A novel cartesian trajectory planning method by using triple NURBS curves for industrial robots
CN113276116B (zh) 一种误差可控的机器人轨迹同步过渡方法
CN116117796B (zh) 一种工业机器人姿态轨迹过渡与速度规划方法及***
CN113787525B (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
GR01 Patent grant
GR01 Patent grant