一种平面冗余度机器人避障及避奇异的路径规划方法
【技术领域】
[0001] 本发明涉及机器人运动控制领域，尤其涉及一种平面冗余度机器人避障及避奇异 的路径规划方法。
【背景技术】
[0002] 随着电子产品的小型化和精密化，传统的机器人自动化装配不能很好的完成一些 复杂精细化装配任务，而完全由人工完成此类产品的装配任务会使工作人员的劳动强度大 大增加，从而影响产品的装配质量。目前代工厂商开始引入平面型机器人作为高速生产线 的辅助性设备，协助工作人员完成电子产品的装配作业，降低工作人员的劳动强度并且提 升装配质量。在流水线这种狭小空间内协助工人完成装配任务，人机碰撞的概率大大增加。 这不仅对工人的生命安全存在很大的威胁，而且对产品的装配质量也会有一定的影响。
[0003] 平面冗余度机器人，是指关节数大于2的平面关节型机器人，在不影响末端位姿的 情况下可以通过其自运动特性实现多种运动优化目标。在人机协作装配过程中，由于装配 流水线空间狭小，同一流水线上的协作者和协作机器人的工作空间大面积重叠，再加之此 类机器人的工作速度较大，使得人机协作的风险大大增加。再者奇异构形附近冗余度机器 人容易出现关节速度非常大，导致机器人末端执行器偏离预期轨迹，这将会对装配质量有 一定的影响。
[0004] 所以用于小型电子产品的人机协作机器人需要一种可以同时完成避障、避奇异的 路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异 构型的回避从而降低人机协作的风险并提高装配质量。目前，平面冗余度机器人运动控制 领域内，尚无有关同时完成避障、避奇异的路径规划方法的报道。
【发明内容】
[0005] 本发明基于如下理论：
[0006] 1、避障的路径规划理论
[0007] 首先计算障碍物和机器人连杆之间的最短距离，针对人机协作装配这类对路径规 划方法实时性要求较高的应用场合，提出了一种改进的实时最小距离计算法。
[0008] 针对人机协作完成装配任务的过程，协作者可考虑成随机的障碍物。平面冗余度 机器人各关节上装有测量距离的传感器，可以实时测量机器人关节和障碍物之间的距离。 在机器人运动的平面上，可以将障碍物等效成一个半径为R的圆盘，以平面三自由度冗余机 器人为例，机器人和障碍物位置示意图如图1所示。
[0009] (1)采用本发明所述方法计算实时最小距离
[0010] 先筛选出不能和障碍物发生碰撞的杆件
[0011] 本发明所举的例子中连杆是由杆件1、杆件2和杆件3构成；
[0012] 已知障碍物中心在坐标系{〇}中的位置(XtQ，ytQ)，连杆截面为边长为a的正方形， 障碍物半径为R，根据坐标系{1}相对于坐标系{〇}的旋转矩阵，得出障碍物中心在坐标系 {1}中的位置(Xtl，ytl)，即
[0014] 其中，θχ为关节1的转角；
[0015] 若(Xtl，ytl)满足
[0018] 杆件1为安全杆件，不用计算其和障碍物距离；
[0019] 若(Xtl，ytl)满足
[0021] 其中，xA1，yA1为杆件1上A点在坐标系{1}中的位置坐标，χ Β1为杆件1上B点在坐标系 {1}中的位置横坐标；
[0022]则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离；
[0023]定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志 点S在坐标系{1}中的坐标为(Xtl，0)，标志点S和障碍物之间的距离为：
[0024] di= | yAi-yti
[0025] 同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离办和山， 并取dl、d2和d3的最小值，即得到实时最小距离dmin。
[0026] (2)采用传统方法计算实时最小距离
[0027] -般都在基座坐标系下完成，障碍物在基座坐标系中的坐标为(Xt，yt )，杆件1的方 程可以写成
[0029]其中，XA，yA为杆件1上A点在基座坐标系中的坐标;XB，y B为杆件1上B点在基座坐标 系中的坐标；
[0030]过障碍物圆心点(Xt，yt)并垂直于杆件1的直线方程为
[0032]将公式(5)和(6)联立解出碰撞点(xs，ys)，进一步得出碰撞点到障碍物距离cU，
[0034] 同理计算出杆件2和杆件3上碰撞点到障碍物距离山和出，进而计算出实时最小距 1? dmin 〇
[0035] 本发明采用改进的距离计算方法，与传统的实时最小距离计算方法相比，提高了 计算效率，更好地满足了动态避障对实时性的要求，主要体现在两个方面：
[0036]①在计算杆件距离之前，先筛选出可能发生碰撞的连杆，只计算此类杆件和障碍 物的距离，大大减小了计算量；
[0037]②改进的距离计算方法采用侧坐标变换的方法计算距离，舍弃了传统方法中解方 程组的繁琐计算，大大减少了计算量，提高了计算效率。
[0038] (3)避障的路径规划
[0039] 在人机协作完成产品装配任务时，人与机器人的工作空间相互重叠，碰撞发生的 可能性大大增加，再加之机器人运动至奇异位置附近时，关节速度会突然增大甚至超出关 节速度限制，对协同工作的人员造成极大的安全隐患。避障避奇异路径规划方法可以在对 障碍物回避的同时完成对奇异构型的回避，从而可以消除这些安全隐患，进一步提高人机 协作的安全性。
[0040] 机器人运动过程中的某一时刻，通过上述判断危险杆件的方法，筛选出危险杆件 并求出杆件上的标志点S的位置，且该标志点S处的雅可比矩阵Js满足
[0041] J sq = ts (8)
[0042] 其中，JSeRmXn为最靠近障碍物的标志点处的雅可比矩阵，扣iT1为关节速度向 量，4为标志点的运动速度向量；
[0043] 根据梯度投影法，冗余度机器人逆解为
[0044] p + {l-.rf)H (9)
[0045] 其中，声为笛卡尔空间中机器人末端速度，I为单位矩阵，机器人的雅克 比矩阵，J+￡RnXm为雅可比矩阵的广义逆，Η为冗余度机器人零空间避障函数；
[0046] 冗余度机器人零空间避障函数可由下式求得
[0047] Η -[./,((7-.；/)) ](W±) no)
[0048] 其中，*eiTxl为机器人末端的运动速度向量；
[0049] 那么基于梯度投影法的冗余度机器人避障运动学逆解为
[0050] q = J+x + (I - l - J+J )Jj+fx:; - J,rkj (11)
[0051 ]由于(I-J+J)的特殊性，引入避障系数a和避障增益b，公式(11)可写成
[0052] q ^J+x+a\js ((J- ./+·/))?+ (bxs -JsJ+x) (12)
[0053] 上式中系数a，b和实时最小距离dmin有关；
[0054] 定义两个距离dKcb，当dmin〈d2时为危险区域，启动避障策略，当cUXcU时杆件为极 度危险区域，开启避障增益策略；
[0057]对公式(12)求积分后，换算成关节转角。
[0058] 2、避奇异的路径规划理论
[0059] 机器人在奇异位置附近时关节速度容易超限，故引入DLS法对其进行优化；
[0060] 首先对机器人雅可比矩阵进行奇异值分解：
[0061] J=ABCT (15)
[0062] 其中，矩阵A和矩阵C都是正交矩阵，矩阵B由机器人雅可比矩阵的奇异值组成的对 角矩阵：
[0064] 其中0^02，··· ^为机器人雅克比矩阵的奇异值，且ο: 2 σ2 2…2 om，om是雅可比矩 阵的最小奇异值，可以体现机器人接近奇异构形的程度；
[0065] 针对冗余度机器人求逆解一般使用伪逆法，伪逆法的实质就是在保证末端轨迹误 差最小的如提下求解关节速度的最小范数解，即末端跟踪精度的优先级尚于关机速度的优 先级;伪逆法为了保证跟踪精度最小，而没有考虑关节速度，为了使机械臂在奇异构型附近 的关节速度不至于过大，使用DLS法，引入阻尼系数C，用以调节末端跟踪精度和关节速度的 优先级别，即在保证
[0066] |^-,/^~| + (c||4||) (17)
[0067] 最小的前提下，求解关节速度的最小范数解，
[0068] 则机器人运动学逆解可以写成
[0069] ^ = = 十Γ:/) 1 尸 (18)
[0070] 其中，1 =