坐标，χΒ1为杆件1上B点在坐标系 {1}中的位置横坐标；
[0153] 则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离。
【具体实施方式】 [0154] 四、本实施方式是对一至三之一所述的一种平面冗余 度机器人避障及避奇异的路径规划方法的进一步说明，步骤三的具体过程为：
[0155] 定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志 点S在坐标系{1}中的坐标为(Xtl，0)，标志点S和障碍物之间的距离为：
[0156] di= | yAi-yti
[0157] 同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离办和山， 并取dl、d2和d3的最小值，即得到实时最小距离dmin。
【具体实施方式】 [0158] 五、本实施方式是对一至四之一所述的一种平面冗余 度机器人避障及避奇异的路径规划方法的进一步说明，步骤六的具体过程为：
[0159] 标志点S处的雅可比矩阵Js满足
[0160] 人4 二（
[0161] 其中，JSeRmXn为最靠近障碍物的标志点处的雅可比矩阵jeiT1为关节速度向 量，4为标志点的运动速度向量；
[0162] 根据梯度投影法，冗余度机器人逆解为
[0163] q^r p-^{! -J+J)H
[0164] 其中，#为笛卡尔空间中机器人末端速度，I为单位矩阵，机器人的雅克 比矩阵，J+￡RnXm为雅可比矩阵的广义逆，Η为冗余度机器人零空间避障函数；
[0165] 冗余度机器人零空间避障函数可由下式求得
[0166] ((/-JV))1+(i^ -JSJ^
[0167] 其中，細iTxl为机器人末端的运动速度向量。
【具体实施方式】 [0168] 六、本实施方式是对一至五之一所述的一种平面冗余 度机器人避障及避奇异的路径规划方法的进一步说明，步骤七的具体过程为：
[0169] 基于梯度投影法的冗余度机器人避障运动学逆解为
[0170] q ^ J+x+(l - JVMJs((I-JVM+(h -
[0171 ]由于(I-J+J)的特殊性，引入避障系数a和避障增益b，上式可写成
[0172] q^J+x+a\js{iyl- J+J)) + (bxs - JsJ+x)
[0173] 对上式求积分后，换算成关节转角，公式中系数a，b和实时最小距离cUn有关；
[0174] 定义两个距离dKcb，当dmin〈d2时为危险区域，启动避障策略，当cUXcU时杆件为极 度危险区域，开启避障增益策略；
[0177] 弓丨入DLS法对上面公式# = Λ + ，·/))]+(Κ-以4)提出的避障算法进行 避奇异优化，则避障避奇异协同优化解为
[0178] q = J}x+a -{bxs-J,J+x^
[0179] 对上式求积分后，换算成关节转角；
[0180] 其中，1 =
[0181] 【具体实施方式】七、本实施方式是对【具体实施方式】二至六之一所述的一种平面冗余 度机器人避障及避奇异的路径规划方法的进一步说明，步骤一中所述的连杆由4、5、6或7个 杆件构成。
【主权项】
1. 一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于它按W下步骤进 行： 所述方法设及两个同时进行的过程： 过程一:避障的路径规划步骤如下： 步骤一:建立各杆件坐标系，并将各障碍物坐标投影到各杆件坐标系中； 步骤二:对杆件是否为安全杆件进行判断； 步骤二.一:如杆件为安全杆件，则剔除此杆件，不作任何处理； 步骤二.二:如杆件不是安全杆件，则转至步骤Ξ; 步骤Ξ:求出连杆和障碍物间的实时最小距离W及各杆件上标志点的坐标； 步骤四：对实时最小距离是否处于安全距离进行判断； 步骤四.一:如实时最小距离大于或等于安全距离，则转至步骤八； 步骤四.二:如实时最小距离小于安全距离，则转至步骤五； 步骤五:根据实时最小距离处于危险距离或极度危险距离，分别启动避障策略或启动 避障增益策略； 步骤六:计算机器人雅可比矩阵和标志点处雅可比矩阵； 步骤屯:逆运动学求解，积分得到关节的运动角度； 步骤八:不启动避障策略，继续执行机器人末端运动； 步骤九:判断机器人末端是否到达终点； 步骤九.一:机器人末端到达终点，结束 过程二:避奇异的路径规划步骤如下： 步骤A:判断最小奇异值是否低于奇异值下限； 步骤A. - :最小奇异值大于或等于奇异值下限，转至步骤B; 步骤A.二:最小奇异值小于奇异值下限，转至步骤C; 步骤B:不启动避奇异策略，继续执行机器人末端运动，再转至步骤九； 步骤C:启动避奇异策略，再转至步骤屯。2. 如权利要求1所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征 在于步骤一的具体过程为： 所述连杆是由杆件1、杆件2和杆件3构成； 已知障碍物中屯、在坐标系{〇}中的位置(xt〇，yt〇)，连杆截面为边长为a的正方形，障碍物 半径为R，根据坐标系{1}相对于坐标系{〇}的旋转矩阵，得出障碍物中屯、在坐标系{1}中的 位置(xti'yti)，即3. 如权利要求1或2所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特 征在于步骤二的具体过程为： 若(xti，yti)满足杆件1为安全杆件； 若(xti，yti)满足(4) 其中，XA1，yA功杆件1上A点在坐标系{1}中的位置坐标，XB1为杆件1上B点在坐标系{1} 中的位置横坐标； 则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离。4. 如权利要求3所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征 在于步骤Ξ的具体过程为： 定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志点S 在坐标系{1}中的坐标为(xti，0)，标志点S和障碍物之间的距离为： di= I YAi-yti 同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离cb和d3,并取 山、d2和d3的最小值，即得到实时最小距离dmin。5. 如权利要求4所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征 在于步骤六的具体过程为： 标志点S处的雅可比矩阵Js满足其中，JsERmX哨最靠近障碍物的标志点处的雅可比矩阵，扣iTl为关节速度向量，电 为标志点的运动速度向量； 根据梯度投影法，冗余度机器人逆解为其中，兵为笛卡尔空间中机器人末端速度，I为单位矩阵，jERmXn为机器人的雅克比矩 阵，J+ERnXm为雅可比矩阵的广义逆，Η为冗余度机器人零空间避障函数. 冗余度机器人零空间避障函数可由下式求得其中，托化-1为机器人末端的运动速度向量。6. 如权利要求5所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征 在于步骤屯的具体过程为： 基于梯度投影法的冗余度机器人避障运动学逆解为由于(I-J+J)的特殊性，引入避障系数a和避障增益b，上式可写成对上式求积分后，换算成关节转角，公式中系数a, b和实时最小距离cUin有关； 定义两个距离dl<cb，当cUin<d2时为危险区域，启动避障策略，当dmin<dl时杆件为极度危 险区域，开启避障增益策略；引入DLS法对上面公式令= /+* + "[Λ批-八^；1)了 (砖-.V叫提出的避障算法进行避奇 异优化，则避障避奇异协同优化解为对上式求积分后，换算成关节转角； 其中：7.如权利要求6所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征 在于步骤一所述的连杆由4、5、6或7个杆件构成。
【专利摘要】一种平面冗余度机器人避障及避奇异的路径规划方法，涉及机器人运动控制领域，尤其涉及一种平面冗余度机器人避障及避奇异的路径规划方法。本发明为解决现有平面冗余度机器人在协作过程中存在对工作人员伤害的风险和机器人末端运动至奇异位置时，关节速度会超出机器人允许范围的问题。本发明按以下步骤进行：本发明所述方法涉及两个同时进行的线程：线程一述及避障的路径规划各步骤；线程二述及避奇异的路径规划各步骤。本发明所述方法可以同时完成避障、避奇异的路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异构型的回避从而降低人机协作的风险并提高装配质量。本发明所述方法可应用于机器人运动控制领域。
【IPC分类】G05D1/02
【公开号】CN105700527
【申请号】CN201610052247
【发明人】刘玉斌, 戴骞, 张赫, 赵杰, 李戈
【申请人】哈尔滨工业大学
【公开日】2016年6月22日
【申请日】2016年1月26日