技术领域
[0001] 本
发明涉及机器人行走轨迹规划技术领域，尤其是一种
串联机器人轨迹规划方法。
背景技术
[0002] 机器人是一个复杂动态体系，能够用在不同领域。其中，六
自由度机器人在工业生产中被广泛使用，机器人的串联机构是机器人的重要组成部分，机器人串联结构典型组成为底座、腰部、大臂(上臂)、小臂(前臂)、腕部和手部。由于串联机器人采用的是开环结构，即从底座到
末端执行器间各关节依次串接，只有一条
运动链，这使得它具有以下优点：(1)负载能
力强，能够实现负重较大的工作；(2)运动空间大，操作广度好，能够对大规模目标进行操作；(3)控制对象明确，各关节之间关联性好，控制简便；(4)灵活性强，延展性高；(5)技术成熟，成本低，便于维护，但是机器人串联机构的轨迹规划使机器人运行的效率还比较低，如果路径规划不科学可能导致机器人经过奇异
位姿，或发生碰撞损坏机器人；如果机器人的速度规划不合理会使机构震动，运动不平稳，也可能损坏
电机。
发明内容
[0004] 本发明
实施例所要解决的一个技术问题是：提供一种串联机器人轨迹规划方法，以解决现有技术存在的问题，所述串联机器人轨迹规划方法包括：
[0005] 得到机器人运动任务空间中机器人的
姿态控制
节点Ti的序列，和与之对应的时间节点ti的序列；
[0006] 计算各个姿态控制节点的
角序列 在机器人运动任务空间中构造连续的曲线插值姿态控制节点角的序列，所述角序列 与时间节点ti的序列对应，j为节点序列号，j＝1,2,3,……n；
[0007] 规划在相邻两个姿态控制节点之间机器人运动的轨迹；
[0008] 根据机器人运动的轨迹计算在相邻两个姿态控制节点之间机器人运动的速度及
加速度。
[0009] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述根据机器人运动的轨迹计算在相邻两个姿态控制节点之间机器人运动的速度及加速度包括：
[0010] 机器人控制单元接收到机器人移动的命令，设定机器人的运行时间和运行速度，计算机器人运动的加速度，将加速度指令赋给机器人运动控制电机；
[0011] 机器人运动控制电机按照机器人控制单元的指令进行加速度运行；
[0012] 设定时间参数t1，t2，t3，在0-t1时间内机器人加速度上升，在t1-t2时间内机器人做匀加速运动，在t2-t3时间内加速度减少。
[0013] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述设定时间参数t1，t2，t3，在0-t1时间内机器人加速度上升，在t1-t2时间内机器人做匀加速运动，在t2-t3时间内加速度减少的速度、
加速度计算模型包括梯形模型和正弦模型。
[0014] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述梯形模型的数学表达式包括：
[0015] 机器人运动加速度的数学表达式：
[0016]
[0017] 对公式(1)进行时间积分得到机器人运动速度的函数，设置机器人运动的初始速度和结束速度为零，则机器人运动速度的数学表达式为：
[0018]
[0019] 对公式(2)进行时间积分得到机器人位移的函数，设置机器人的初始时位移为零，则机器人位移的数学表达式为：
[0020]
[0021] 令T1＝t1，T2＝t2-t1，T3＝t3-t2，则公式(3)中
[0022] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述正弦模型的数学表达式包括：
[0023] 机器人运动加速度的数学表达式：
[0024]
[0025] 为保证最大加速度和最大速度的约束，以及加速度边界条件，式中
[0026] 将式(4)进行时间积分得到机器人运动的速度函数，设置机器人运动的初始速度和末速度为零，则机器人运动的速度函数表达式为：
[0027]
[0028] 对公式(5)进行时间积分，得到机器人运动的位移函数表达式：
[0029]
[0030] 式中： S02＝S01+vmaxT2，T2＝t2-t1。
[0031] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述规划在相邻两个姿态控制节点之间机器人运动的轨迹包括：
[0032] 对机器人运动的控制节点进行规划；
[0033] 对相邻两个姿态控制节点之间机器人运动的直线运动轨迹规划；
[0034] 对相邻两个姿态控制节点之间机器人运动的运动姿态规划。
[0035] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述对机器人运动的控制节点进行规划采用采用三次样条规划。
[0036] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述三次样条规划包括：
[0037] 令Pi(ti,si(ti))和Pi+1(ti+1,si(ti+1))为机器人运动曲线上的相邻的两个控制节点，在这两个控制节点之间的第i个三次多项式为：
[0038] si(t)＝ai(t-ti)3+bi(t-ti)2+ci(t-ti)+di；(ti≤t≤ti+1) (7)
[0039] si(t)的一阶导数为：
[0040] si′(t)＝3ai(t-ti)2+2bi(t-ti)+ci (8)
[0041] si(t)的二阶导数为：
[0042]
[0043] 令pi＝si(ti)，pi'＝si'(ti)，pi”＝si”(ti)，Δti＝ti+1-ti，Δpi＝pi+1-pi，得：
[0044]
[0045] 三次样条曲线在各节点处连续且一阶导数、二阶导数均连续，则：
[0046] si(ti+1)＝si+1(ti+1) (11)
[0047] si′(ti+1)＝si+1′(ti+1) (12)
[0048] si″(ti+1)＝si+1″(ti+1) (13)
[0049] 由式(11)和(13)得：
[0050]
[0051] 联立公式(8)、(12)、(13)、(14)得：
[0052]
[0053] 联立公式(9)和(13)，得：
[0054] 3ai(Δti)2+2bi(Δti)+ci＝ci+1 (16)
[0055] 将式(11)、(15)、(16)代入公式(7)得
[0056] Δti+1pi+2″+2(Δti+Δti+1)pi+1″+Δtipi″＝6(Δpi+1/Δti+1-Δpi/Δti) (17)[0057] 式中i＝1,2,3...n-2，因此将式(17)写成矩阵的模式：
[0058] Ap”＝6Bp
[0059] 式中：p＝[p1,p2,...,pn]T，p”＝[p1”,p2”,...,pn”]T；
[0060] A和B为(n-2)×n阶矩阵：
[0061]
[0062]
[0063] 其中：αi＝Δti,αij＝αi+αj,βi＝1/Δti,βij＝βi+βj；
[0064] 设置两个约束条件，p1″＝pn″＝0；
[0065] 将p”,p均变为n-2维，矩阵A和B也相应的变为(n-2)×(n-2)矩阵：
[0066]
[0067]
[0068] 从而求出各控制节点的加速度矢量：p″＝6A-1Bp；
[0069] 将节点加速度矢量p”和节点位移矢量p带入式(11)(14)(15)求出所有的三次多项式系数，已知系数利用式(8)即求出三次样条轨迹。
[0070] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述对相邻两个姿态控制节点之间机器人运动的直线运动轨迹规划包括：
[0071] 机器人控制单元接收到机器人从某一控制节点移动到下一个控制节点的命令；
[0072] 机器人控制单元将机器人的直线运动分解为X、Y、Z三个轴的运动；
[0073] 应用加速度控制方法，根据在进行运动规划时规定的加速度amax和速度vmax，得到位移时间函数S(t)；
[0074] 将位移时间函数S(t)在X、Y、Z三个轴上投影，控制X、Y、Z三个轴的运动，实现机器人末端沿直线运动。
[0075] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述机器人控制单元将机器人的直线运动分解为X、Y、Z三个轴的运动包括：
[0076] 设某一控制节点P的坐标为[x1,y1,z1]，与其相邻的下一个控制节点Q的坐标为[x2,y2,z2]，则有：
[0077]
[0078] 式中： X(t)、Y(t)、Z(t)分别为机器人在X、Y、Z轴方向上的位移。
[0079] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述对相邻两个姿态控制节点之间机器人运动的运动姿态规划包括：
[0080] 设置机器人在一个控制节点的初始运动姿态为R1，在其下一个相邻的控制节点的末端运动姿态为R2，获得旋转矩阵R，所述初始运动姿态为R1为机器人
机械臂开始的
位置，末端运动姿态为R2为机器人机械臂完成任务后用户需要的姿态；
[0081] 使用罗德里格斯变换公式，计算出机器人运动的等效旋
转轴和旋转角度；
[0082] 进行机器人运动的加减速控制运算。
[0083] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述使用罗德里格斯变换公式，计算出机器人运动的等效
旋转轴和旋转角度包括：
[0084] 设置机器人在一个控制节点的初始运动姿态为R1，在其下一个相邻的控制节点的末端运动姿态为R2，获得旋转矩阵R，R1通过旋转矩阵R变换得到R2，即R2＝RR1，R＝R2R1-1；
[0085] 使用罗德里格斯变换公式推导可得：
[0086]
[0087]
[0088] 由式(20)求出旋转轴，观察通用旋转矩阵得到：
[0089]
[0090] 通过式(21)确定旋转轴的方向，求出cosθ、sinθ，在此
基础上求出等效旋转角：
[0091] θ＝atan(cosθ,sinθ) (22)
[0092] 其中θ为相邻姿态之间的等效旋转总角度。
[0093] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述规划在相邻两个姿态控制节点之间机器人运动的轨迹还包括姿态的四元数规划，所述姿态的四元数规划包括：
[0094] 所述四元数由实数加上三个虚数单位i,j,k构造，四元数的表达式为q＝[s,(x,y,z)]＝s+xi+yj+zk，其中s、x、y、z为实数，虚数单位k表示绕Z轴旋转，虚数单位j代表绕Y轴旋转，虚数单位i旋转代表绕X轴旋转，-i,-j,-k分别代表虚数单位i,j,k旋转的反向旋转。
[0095] 与现有技术相比，本发明具有以下优点：
[0096] 本发明提出了一种串联机器人轨迹规划方法，保证了机器人在各关节位置的速度、加速度连续；采用三次样条插值的方法，得到机器人运动
关节角度的平滑函数，使机器人规划运行轨迹更加平滑；采用梯形模式和正弦模式进行机器人加速度规划，保证了机器人加速度的连续；采用四元数规划方法，对机器人运动任务空间的位置进行直线规划和姿态规划；采用三次样条规划，对机器人空间关节进行规划，实现机械臂的加速减速，空间运动的效果，本发明通过对机械臂
算法进行规划，实现了机械臂的准确控制。
[0097] 下面通过
附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
[0098] 构成
说明书的一部分的附图描述了本发明的实施例，并且连同描述一起用于解释本发明的原理。
[0099] 参照附图，根据下面的详细描述，可以更加清楚地理解本发明，其中：
[0100] 图1为本发明的串联机器人轨迹规划方法的一个实施例的
流程图；
[0101] 图2为本发明的串联机器人轨迹规划方法的另一个实施例的流程图；
[0102] 图3为本发明的串联机器人轨迹规划方法的又一个实施例的流程图。
具体实施方式
[0103] 现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。
[0104] 对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。
[0105] 如图1所示，所述串联机器人轨迹规划方法包括：
[0106] 10，得到机器人运动任务空间中机器人的姿态控制节点Ti的序列，和与之对应的时间节点ti的序列；
[0107] 20，计算各个姿态控制节点的角序列 在机器人运动任务空间中构造连续的曲线插值姿态控制节点角的序列，所述角序列 与时间节点ti的序列对应，j为节点序列号，j＝1,2,3,……n；
[0108] 30，规划在相邻两个姿态控制节点之间机器人运动的轨迹；
[0109] 40，根据机器人运动的轨迹计算在相邻两个姿态控制节点之间机器人运动的速度及加速度。
[0110] 所述根据机器人运动的轨迹计算在相邻两个姿态控制节点之间机器人运动的速度及加速度包括：
[0111] 机器人控制单元接收到机器人移动的命令，设定机器人的运行时间和运行速度，计算机器人运动的加速度，将加速度指令赋给机器人运动控制电机；
[0112] 机器人运动控制电机按照机器人控制单元的指令进行加速度运行；
[0113] 设定时间参数t1，t2，t3，在0-t1时间内机器人加速度上升，在t1-t2时间内机器人做匀加速运动，在t2-t3时间内加速度减少。
[0114] 所述设定时间参数t1，t2，t3，在0-t1时间内机器人加速度上升，在t1-t2时间内机器人做匀加速运动，在t2-t3时间内加速度减少的速度、加速度计算模型包括梯形模型和正弦模型。
[0115] 在基于本发明上述串联机器人轨迹规划方法的另一个实施例中，所述梯形模型的数学表达式包括：
[0116] 梯形运动控制是加减速控制方法中简单的模式。加减速过程分为加速、匀速、减速三个阶段，机器人运动加速度的数学表达式：
[0117]
[0118] 对公式(1)进行时间积分得到机器人运动速度的函数，设置机器人运动的初始速度和结束速度为零，则机器人运动速度的数学表达式为：
[0119]
[0120] 对公式(2)进行时间积分得到机器人位移的函数，设置机器人的初始时位移为零，则机器人位移的数学表达式为：
[0121]
[0122] 令T1＝t1，T2＝t2-t1，T3＝t3-t2，则公式(3)中
[0123] 梯形模式的优点是算法容易，有助于提高机器人算法实时性，但是在轨迹的始末端其加速度不连续，容易使运动系统振动，会对机器人产生冲击。
[0124] 所述正弦模型的数学表达式包括：
[0125] 机器人运动加速度的数学表达式：
[0126]
[0127] 为保证最大加速度和最大速度的约束，以及加速度边界条件，式中
[0128]
[0129] 将式(4)进行时间积分得到机器人运动的速度函数，设置机器人运动的初始速度和末速度为零，则机器人运动的速度函数表达式为：
[0130]
[0131] 对公式(5)进行时间积分，得到机器人运动的位移函数表达式：
[0132]
[0133] 式中： S02＝S01+vmaxT2，T2＝t2-t1。
[0134] 正弦模式的优点是速度、加速度函数连续，机器人运动平稳。但是在相同的最大加速度和最大速度约束下，其运动时间较长，响应不够快。
[0135] 如图2所示，所述规划在相邻两个姿态控制节点之间机器人运动的轨迹包括：
[0136] 101，对机器人运动的控制节点进行规划；
[0137] 102，对相邻两个姿态控制节点之间机器人运动的直线运动轨迹规划；
[0138] 103，对相邻两个姿态控制节点之间机器人运动的运动姿态规划。
[0139] 所述对机器人运动的控制节点进行规划采用采用三次样条规划。
[0140] 所述三次样条规划包括：
[0141] 令Pi(ti,si(ti))和Pi+1(ti+1,si(ti+1))为机器人运动曲线上的相邻的两个控制节点，在这两个控制节点之间的第i个三次多项式为：
[0142] si(t)＝ai(t-ti)3+bi(t-ti)2+ci(t-ti)+di；(ti≤t≤ti+1) (7)
[0143] si(t)的一阶导数为：
[0144] si′(t)＝3ai(t-ti)2+2bi(t-ti)+ci (8)
[0145] si(t)的二阶导数为：
[0146]
[0147] 令pi＝si(ti)，pi'＝si'(ti)，pi”＝si”(ti)，Δti＝ti+1-ti，Δpi＝pi+1-pi，得：
[0148]
[0149] 三次样条曲线在各节点处连续且一阶导数、二阶导数均连续，则：
[0150] si(ti+1)＝si+1(ti+1) (11)
[0151] si′(ti+1)＝si+1′(ti+1) (12)
[0152] si″(ti+1)＝si+1″(ti+1) (13)
[0153] 由式(11)和(13)得：
[0154]
[0155] 联立公式(8)、(12)、(13)、(14)得：
[0156]
[0157] 联立公式(9)和(13)，得：
[0158] 3ai(Δti)2+2bi(Δti)+ci＝ci+1 (16)
[0159] 将式(11)、(15)、(16)代入公式(7)得
[0160] Δti+1pi+2″+2(Δti+Δti+1)pi+1″+Δtipi″＝6(Δpi+1/Δti+1-Δpi/Δti) (17)[0161] 式中i＝1,2,3...n-2，因此将式(17)写成矩阵的模式：
[0162] Ap”＝6Bp
[0163] 式中：p＝[p1,p2,...,pn]T，p”＝[p1”,p2”,...,pn”]T；
[0164] A和B为(n-2)×n阶矩阵：
[0165]
[0166]
[0167] 其中：αi＝Δti,αij＝αi+αj,βi＝1/Δti,βij＝βi+βj；
[0168] 设置两个约束条件，p1″＝pn″＝0；
[0169] 将p”,p均变为n-2维，矩阵A和B也相应的变为(n-2)×(n-2)矩阵：
[0170]
[0171]
[0172] 从而求出各控制节点的加速度矢量：p″＝6A-1Bp；
[0173] 将节点加速度矢量p”和节点位移矢量p带入式(11)(14)(15)求出所有的三次多项式系数，已知系数利用式(8)即求出三次样条轨迹。
[0174] 如图3所示，所述对相邻两个姿态控制节点之间机器人运动的直线运动轨迹规划包括：
[0175] 201，机器人控制单元接收到机器人从某一控制节点移动到下一个控制节点的命令；
[0176] 202，机器人控制单元将机器人的直线运动分解为X、Y、Z三个轴的运动；
[0177] 203，应用加速度控制方法，根据在进行运动规划时规定的加速度amax和速度vmax，得到位移时间函数S(t)；
[0178] 204，将位移时间函数S(t)在X、Y、Z三个轴上投影，控制X、Y、Z三个轴的运动，实现机器人末端沿直线运动。
[0179] 所述机器人控制单元将机器人的直线运动分解为X、Y、Z三个轴的运动包括：
[0180] 设某一控制节点P的坐标为[x1,y1,z1]，与其相邻的下一个控制节点Q的坐标为[x2,y2,z2]，则有：
[0181]
[0182] 式中：X(t)、Y(t)、Z(t)分别为机器人在X、Y、Z轴方向上的位
移。
[0183] 分别将 和代入式(18)可得到机器人在x、y、z方向上的位移变化
方程，机器人的主
控制器根据 控制机
器人在X、Y、Z这三个方向的运动，实现机器人末端沿直线运动。
[0184] 所述对相邻两个姿态控制节点之间机器人运动的运动姿态规划包括：
[0185] 设置机器人在一个控制节点的初始运动姿态为R1，在其下一个相邻的控制节点的末端运动姿态为R2，获得旋转矩阵R，所述初始运动姿态为R1为机器人机械臂开始的位置，末端运动姿态为R2为机器人机械臂完成任务后用户需要的姿态；
[0186] 使用罗德里格斯变换公式，计算出机器人运动的等效旋转轴和旋转角度；
[0187] 进行机器人运动的加减速控制运算。
[0188] 所述使用罗德里格斯变换公式，计算出机器人运动的等效旋转轴和旋转角度包括：
[0189] 设置机器人在一个控制节点的初始运动姿态为R1，在其下一个相邻的控制节点的末端运动姿态为R2，获得旋转矩阵R，R1通过旋转矩阵R变换得到R2，即R2＝RR1，R＝R2R1-1；
[0190] 使用罗德里格斯变换公式推导可得：
[0191]
[0192]
[0193] 由式(20)求出旋转轴，观察通用旋转矩阵得到：
[0194]
[0195] 通过式(21)确定旋转轴的方向，求出cosθ、sinθ，在此基础上求出等效旋转角：
[0196] θ＝atan(cosθ,sinθ) (22)
[0197] 其中θ为相邻姿态之间的等效旋转总角度，然后对等效旋转角进行加减速控制，可得到机器人姿态的插补点序列。
[0198] 所述规划在相邻两个姿态控制节点之间机器人运动的轨迹还包括姿态的四元数规划，所述姿态的四元数规划包括：
[0199] 所述四元数由实数加上三个虚数单位i,j,k构造，四元数的表达式为q＝[s,(x,y,z)]＝s+xi+yj+zk，其中s、x、y、z为实数，虚数单位k表示绕Z轴旋转，虚数单位j代表绕Y轴旋转，虚数单位i旋转代表绕X轴旋转，-i,-j,-k分别代表虚数单位i,j,k旋转的反向旋转。
[0200] 四元数有如下性质：
[0201] (1)设 则
[0202]
[0203] 当||q||＝1时，称q为单位四元数，一定存在单位矢量 和角度θ使
[0204] 因此四元数可以更直观的描述三维空间刚体的姿态以及其旋转，在三维空间中绕轴 旋转θ角可以用四元数 来描述。设 为三维空间中的矢量，其对应的四-1
元数为 p绕 旋转θ角后的可得四元数为p'，p′＝qpq ；
[0205] (2)四元数的叉乘：设四元数 则
[0206]
[0207] 为了求出四元数和齐次变换矩阵的关系，可以把四元数的左乘和右乘等效为矩阵的左乘。即p×q＝TLq，p×q＝TRp；
[0208] 其中：
[0209]
[0210]
[0211] 若用齐次变换矩阵和四元数同时操作一个三维矢量，假设其变化是等价的，即[0212] 由四元数性质可推导得：
[0213] T＝TLTR
[0214]
[0215] 根据机器人的位姿求出与之对应的两个互补的四元数：
[0216] q＝[w,(x,y,z)]
[0217]
[0218] x＝(ay-oz)/4w
[0219] y＝(nz-ax)/4w
[0220] z＝(ny-ox)/4w
[0221] 这样对w,x,y,z进行插补便可得到平滑的姿态变化。
[0222] 本说明书中各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
[0223] 本发明的描述是为了示例和描述起见而给出的，而并不是无遗漏的或者将本发明限于所公开的形式。很多
修改和变化对于本领域的普通技术人员而言是显然的。选择和描述实施例是为了更好说明本发明的原理和实际应用，并且使本领域的普通技术人员能够理解本发明从而设计适于特定用途的带有各种修改的各种实施例。
