一种冗余双臂服务机器人实时避碰的轨迹规划方法及平稳运
行方法
(一)技术领域：
[0002] 随着机器人技术的不断发展，目前中国已成为全球最大的机器人市场，为应对工业生产成本的不断上升、促进产业结构的智能调整，从而进一步提高工业生产效率，
工业机器人在工业生产方面具有无与伦比的优势，正逐渐取代人类生产劳动。
[0003] 伴随全球自动化
进程的加快，人们对机器人提出了更高的要求，而不限于在工业生产上面，特别是在中国，自20世纪90年代以来，我国已经逐步进入人口老龄化的状态，我们更希望机器人能融入我们的生活，特别是在家庭服务行业，帮助老人减轻生活负担。
[0004] 由于家庭环境及操作任务的日益复杂，若使机器人在家庭环境中替代人类完成生活护理，比如实现端茶倒
水，搀扶老人等任务，传统单臂机器人就难以满足家庭服务要求，如想完成家庭护理等服务任务，那就对它的
自由度提出了更高的要求，同时一条手臂也无法满足家庭服务任务，需要双臂机器人实现。
[0005] 相比于单臂机器人，冗余双臂机器人具有无与伦比的优越性：(1) 适用范围广：冗余双臂协调作业可以完成单臂无法完成的任务要求，如端茶倒水等；(2)作业空间大：冗余双臂机器人的作业空间是单臂机器人的二倍，作业任务进一步提高；(3)容错性好：冗余双臂机器人作业时当，当其中一只
机械臂出现故障时，可利用另一只手臂完成任务；(4)可二次优化：由于冗余双臂机器人具有冗余特性，可在实现操作任务的同时进行运动学及动
力学上进行二次优化，比如避奇异位型，避关节极限，及获得最小关节力矩等。
[0006] 虽然双臂机器人具有以上优势，但家庭环境及操作任务的日益复杂，若想使冗余双臂服务机器走进家庭，实现家庭护理等服务任务，其安全性就是首要解决的问题，使机器人在执行任务不与周边环境发生碰撞的同时双臂协调避自碰就显得尤为重要。传统的基于最大距离检测的避碰方法虽然可以实现机器人的实时避碰，但避碰任务的成败主要取决于机械臂上关键点的选取，特别对于冗余双臂空间机器人，这种方法的缺点就被进一步放大，同时现有的基于最大距离检测避障
算法检测的距离是机械臂轴线上的点到障碍物之间的距离，并没有考虑机械臂各个
连杆的半径，从而导致避障任务的失败，再者就是现有的避障算法在检测到机械臂上定义的关键点与障碍物之间的距离小于设定的
阈值后会随机产生一个避障动作，这无疑会增大算法的计算效率，不易于实时避障。再有就是对于避障问题，大多算法只考虑了机械臂与周边环境之间的障碍，或者双臂之间的避自碰，并没有同时考虑双臂与周边作业环境之间的障碍和双臂互为障碍的这种情况，显然以上所提出的避障算法并不能实现实时避障的要求，存在特别大的
风险，尤其是在面向于家庭服务任务，以上这种算法弊端就尤为突出。
[0007] 一种冗余双臂机器人，整体结构示意图如图1，其左右臂各关节参数相同，且均有七个自由度，双臂在协调搬运时，可以保证双臂末端执行操作任务的同时，再利用空间机械臂的冗余特性，实现双臂的避自碰和双臂与环境之间的避碰。
[0008] 冗余双臂机器人的关节坐标矢量图如图2所示，其中O0-X0Y0Z0为冗余双臂机器人参考
坐标系，双臂的末端
位姿是基于该系表达的，用下标l表示主臂，用下标f表示从臂，则：O1l为主臂的肩部，O4l为主臂的肘部，O6l为主臂的腕部，O7l为主臂的末端，O1f为从臂的肩部，O4f为从臂的肘部，O6f为从臂的腕部，O7f为从臂的末端参考坐标系，冗余双臂
机器人手臂的D-H(Denavit-Hartenberg)参数如表1 所示。
[0009] 表1
[0010]
[0011] 建立机械臂的
运动学模型即可实时获得主臂各关节末端的
位置随关节转
角变化的位姿函数，通过视觉信息获得机器人在工作时周边的环境信息，实时获得障碍物的位置信息。
[0012] 由于现有的基于距离函数的避碰轨迹规划算法大都没有考虑双臂的实际尺寸，只是单纯的把机械臂视为一条线段，以线段上的标志点和障碍物之间的长度视为距离指标，来进行避障从而不能安全有效的进行实时避障。为求得障碍物与机械臂连杆之间的实际距离，就得把机械臂的实际模型考虑进去，因此需要对机械臂的模型进行简化，常用的方法有椭球体包围简化法、长方体包围简化法以及胶囊体包围简化法，其中胶囊体包围简化法与机械臂的模型最为贴近，且计算量小。故本发明将采用胶囊体包围简化法对冗余双臂机器人进行处理，冗余双臂机器人的胶囊体简化模型如图3所示，冗余双臂机器人的胶囊体简化模型参数如表2所示。
[0013] 表2
[0014]
[0015] 此时，双臂避碰时的实际距离为机械臂轴线上关键点到障碍物之间的距离减去机械臂各连杆相对应的半径或者障碍物的半径。
[0016] 对于现有算法在选取关键点上存在的
缺陷，本发明
专利采用一种新的基于梯度投影的方法来确定主臂各连杆与从臂各连杆、主从机械臂与任务空间中的障碍物间的最小距离。(三)发明内容：
[0017] 本发明的目的在于提供一种冗余双臂服务机器人实时避碰的轨迹规划方法，它可以克服现有避障轨迹规划方法的不足，是一种安全有效且容易实现和推广应用的避碰轨迹规划方法，特别适合用于复杂作业环境且对安全性能要求较高的操作任务，如家庭服务环境等，可以有效解决冗余双臂机器人的避碰问题。
[0018] 本发明的技术方案：一种冗余双臂服务机器人实时避碰的轨迹规划方法，其特征在于取主臂的第i个连杆、从臂的第j个连杆以及环境障碍物k为研究对象，分析主臂的第i个连杆与环境障碍物k之间的最短距离，分析从臂的第j个连杆视为主臂第i个连杆的障碍物时，主臂的第i个连杆与障碍物之间的几何关系，分析从臂的第j个连杆与环境障碍物k之间的几何关系，分析主臂的第i个连杆视为从臂第j个连杆的障碍物时，从臂的第j个连杆与障碍物之间的几何关系。
[0019] 具体分析如下：
[0020] (1)分析主臂的第i个连杆与环境障碍物k之间的最短距离：
[0021] 用点 分别代表主臂第i连杆的起始关节与末端关节，其在基坐标系O0-x0y0z0下的坐标分别用 表示， 为双臂胶囊体包围模型中主臂的第i连杆的长度，则该主臂的第i连杆的单位向量为：
[0022]
[0023] 其中， 代表此刻主臂的第i个连杆轴线上与环境障碍物k相距最近的点， 的位置与 值的正负有关； 为 的转置， 为
机器人视觉信息定位的环境障碍物k在冗余双臂机器人参考坐标系O0-x0y0z0下的位置坐标，则主臂的第i连杆的轴线上距离环境障碍物k的最近点在基坐标系下的位置 为：
[0024]
[0025] 其中，i代表主臂的第i个连杆，k代表第k个环境障碍物，主臂的第i连杆轴线上距离环境障碍物k的最近点记为 则环境障碍物k 的中心与主臂的第i个连杆轴线之间的最短距离为：
[0026]
[0027] 考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上环境障碍物k与主臂的第i个连杆间的最短距离为：
[0028]
[0029] 其中， 代表冗余双臂机器人的胶囊体简化模型中主臂第i个连杆的半径， 为环境障碍物k的半径。
[0030] (2)分析主臂与从臂在协调搬运物体时，从臂的第j个连杆20 视为主臂第i个连杆的障碍物，用从臂第j个连杆的起始关节和末端关节代替，主臂的第i个连杆与障碍物之间的几何关系：
[0031] 2.1用点 表示从臂第j连杆的起始关节，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标用 表示，将从臂第j连杆的起始关节视为主臂第i个连杆的障碍物，此时同步骤(1)一样可求得主臂第i连杆的轴线上距离障碍物的最近距离为：
[0032]
[0033] 考虑到实际的机械臂模型不能简单的用一条线段来表示，因此实际上障碍物与主臂第i个连杆之间的最短距离为：
[0034]
[0035] 主臂第i连杆轴线上距离障碍物的最近点记为
[0036] 其中， 代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆的半径；
[0037] 2.2用点 表示从臂第j连杆的末端关节，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标用 表示，将从臂第j个连杆的末端关节视为主臂第i个连杆的障碍物，此时 同步骤(1)一样可求得主臂的第i个连杆轴线上与障碍物间的最近距离为：
[0038]
[0039] 主臂的第i连杆轴线上距离障碍物最近的点记为 进而求得实际上主臂第i个连杆与障碍物间的最近距离为：
[0040]
[0041] 利用步骤(1)和步骤(2)即可求得环境障碍物k在主臂各个连杆上的最近距离d*l1以及从臂的各连杆起始关节和末端关节视为主臂的障碍物时，其分别到主臂各个连杆上的* *最近距离d l2，d l3，进一步可得到环境障碍物k以及从臂上各连杆的关节(包括起始关节与末端关节)，视为主臂的障碍物时，障碍物包括环境障碍物k及从臂上各连杆的关节，与主臂的最近距离为：
[0042]
[0043] 此时，对应的主臂的连杆轴线上与障碍物(包括环境障碍物k19 及从臂7上各连杆的关节)相距最近的点记为xl0，一般称为标志点；
[0044] (3)分析从臂的第j连杆与环境障碍物k之间的最短距离：
[0045] 用点 分别代表从臂第j连杆的起始关节与末端关节，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标分别用 表示， 为双臂的胶囊体包围模型中从臂的第j连杆的长度，则该从臂的第j连杆的单位向量为：
[0046]
[0047] 其中， 代表此刻从臂7的第j连杆轴线上与环境障碍物k相距最近的点， 的位置与 值的正负有关， 为 的转置，则从臂的第j连杆轴线上距离环境障碍物k的最近点在基坐标系下的位置 为：
[0048]
[0049] 其中，j代表从臂的第j个连杆，k代表第k个环境障碍物，则从臂第j个连杆轴线上与环境障碍物k间的最短距离则为：
[0050]
[0051] 考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上从臂的第j个连杆与环境障碍物k间的最短距离为：
[0052]
[0053] 从臂的第j个连杆的轴线上距离环境障碍物k的最近点记为
[0054] 其中， 代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆的半径；
[0055] (4)分析主臂与从臂在协调搬运物体时，主臂的第i个连杆15 视为从臂第j个连杆的障碍物，用主臂第i个连杆的起始关节和末端关节代替，从臂的第j个连杆与障碍物之间的几何关系：
[0056] 4.1将主臂第i连杆的起始关节视为从臂第j个连杆的障碍物时，利用步骤2的原理可求实际上从臂的第j个连杆与障碍物间最近距离为：
[0057]
[0058] 从臂的第j个连杆轴线上距离障碍物最近的点记为
[0059] 4.2将主臂第i连杆的末端关节视为从臂的第j个连杆障碍时，利用步骤2的原理求得从臂第j个连杆与障碍物间的最近距离为：
[0060]
[0061] 从臂第j个连杆的轴线上距离障碍物的最近点记为
[0062] 利用步骤(3)和步骤(4)即可求得环境障碍物k在从臂各个连杆上的最近距离d*f1以及主臂各连杆的起始关节和末端关节视为从臂的障碍物时，其分别到从臂各个连杆上的最近距离d*f2，d*f3，进一步可得到环境障碍物k以及主臂各连杆的关节，包括起始关节与末端关节，视为从臂的障碍物时，障碍物，包括环境障碍物k及主臂上各连杆的关节，与从臂的最近距离为：
[0063]
[0064] 此时，对应的从臂的连杆轴线上与障碍物，包括环境障碍物k及主臂上各连杆的关节，相距最近的点记为xf0，一般称为标志点；
[0065] (5)利用连杆变换矩阵实时求解出机械臂各关
节点的位置信息，当检测到障碍物与标志点的距离d0小于设定的阈值时，标志点将产生一个避障速度，则关节
角速度函数为：
[0066]
[0067] 其中，J为末端雅克比矩阵，J+为J的广义逆， 为末端速度，I为单位矩阵，J0为改进前的标志点的雅克比矩阵，当标志点的位置确定后，其值可由运动学方程直接求出； 为预先设置的三维避障速度，等式右边的第一项为满足机械手末端轨迹
跟踪所需的速度，第二项是在不影响末端轨迹的前提下标志点的避障速度；
[0068] 由于机械臂处于三维空间状态，则避障速度也将是三维的，为简化计算量，提高轨迹规划算法效率，利用改进的缩减避障运动操作空间的方法得到机器人实时避障的轨迹。
[0069] 所述步骤(5)中的连杆变换矩阵为：
[0070]
[0071] 其中， 为连杆i相对于连杆i-1的变换矩阵，n是自然数。
[0072] 所述步骤(5)中利用改进的缩减避障运动操作空间的方法得到机器人实时避障的轨迹，具体方法如下：
[0073] 设障碍物与机械臂上的单位距离为:
[0074] 则标志点的雅克比矩阵J0及其避障速度 可分别改写为：
[0075]
[0076] 在未对 J0∈Rmxn处理之前，当标志点在进行障碍物躲避的时候，其速度的维度是mx1，标志点雅克比J0的维度是mxn，经改进后与标志点避障速度 的维度缩减为1x1，标志点雅克比 的维度缩减为1xn，使轨迹规划方法的计算效率进一步得到提高。
[0077] 一种能够使冗余双臂机器人在避障运动过程中运行平稳的方法，其特征在于它是在所述一种冗余双臂服务机器人实时避碰的轨迹规划方法的
基础上引入权重因子αi、γi，(i＝f,l)，均是关于机械臂上的标志点与障碍物之间距离d的函数，分别记作αi(d)和γi(d)，式中，αi(d)是表示躲避障碍物速度大小的权重因子，所述γi(d)是关节角速度的增益。
[0078] 当障碍物与机械臂之间的距离d大于安全阈值d3时，此时为安全区域，可令αi＝γi＝0，即不改变由伪逆解所给出的机器人的位姿此时各关节角速度值；当障碍物与机械臂之间的距离d处于安全阈值d3与预警阈值d2之间时，此时为预警区域，可以不引入标志点的速度，即αi＝0，但γi要均匀的变化，此时关节角速度的齐次解由零开始逐渐变化，使冗余机械臂开始避碰动作，以保证机械臂平稳运行的同时远离障碍物，随着d的减小，达到预警阈值d2时，则γi达到了额定的增益值；当障碍物与机械臂之间的距离d处于预警阈值d2与危险阈值 d1之间时，此时为危险区域，随着d的减小，标志点的速度增益αi将由零迅速增大，此时关节角速度的齐次解将进一步加快变化，以保证机械臂平稳运行的同时，加快机械臂的避碰动作；当障碍物与机械臂之间的距离d等于危险阈值d1时，标志点的速度增益达到最大；此方法可以保证冗余双臂服务机器人在执行操作任务的同时双臂各连杆间及连杆与环境障碍物间的最近距离始终大于设定的安全距离且各关节运动连续、平稳。
[0079] 本发明的优越性：采用胶囊体包围简化法对冗余双臂服务机器人进行简化，利用梯度投影法实时求得环境障碍物中心与主从机械臂连杆轴线间的最近距离及主从机械臂各连杆轴线间的最近距离。为使机械臂躲避障碍物的运动连续而平稳引入了‘
门槛距离’d来评估障碍物与机械臂间发生碰撞的危险程度，从而实时的改变机械臂的避障速度。保证冗余双臂服务机器人在执行操作任务的同时双臂各连杆间及连杆与环境障碍物间的最近距离始终大于设定的安全距离且各关节运动连续、平稳。(四)
附图说明：
[0080] 图1为本发明所涉背景技术中一种冗余双臂机器人的整体结构示意图。
[0081] 图2为本发明所涉背景技术中一种冗余双臂机器人的关节坐标矢量图。
[0082] 图3为本发明所涉背景技术中一种冗余双臂机器人的胶囊体简化模型。
[0083] 图4为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中冗余双臂服务机器人抓持物体时的结构状态示意图。
[0084] 图5为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中主臂第i个连杆15、从臂第j个连杆20以及环境障碍物k19 间的示意图。
[0085] 图6为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中冗余双臂机器人主臂第i连杆15与环境障碍物k19之间的几何关系示意图。
[0086] 图7为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中冗余双臂机器人从臂第j连杆20视为主臂第i连杆15的障碍物时，从臂第j个连杆20与障碍物之间的几何关系示意图。
[0087] 图8为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中冗余双臂机器人从臂第j连杆20与环境障碍物k19之间的几何关系示意图。
[0088] 图9为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法中冗余双臂机器人主臂第i连杆15视为从臂第j连杆20的障碍物时，从臂第j个连杆20与障碍物之间的几何关系示意图。
[0089] 图10为本发明所涉一种能够使冗余双臂机器人在避障运动过程中运行平稳的方法中α0与γn随障碍物与标志点之间的距离d的变化关系示意图。
[0090] 图11为本发明所涉一种冗余双臂服务机器人实时避碰的轨迹规划方法的工作流程示意图。
[0091] 图中，1为主臂，2为主臂的肩部，3为主臂的大臂，4为主臂的肘部，5为主臂的小臂，6为主臂的腕部，7为从臂，8为从臂的肩部， 9为从臂的大臂，10为从臂的肘部，11为从臂的小臂，12为从臂的腕部，13为双臂
工作空间中的环境障碍物，14为被搬运的物体，15 为主臂的第i个连杆，16为主臂第i个连杆的始端，17为主臂第i个连杆的末端，18为主臂第i个连杆的轴线，19为主臂的第i个连杆与从臂的第j个连杆间的环境障碍物k，20为从臂的第j个连杆，21为从臂第j个连杆的始端，22为从臂第j个连杆的末端，23为从臂第j个连杆的轴线。
(五)具体实施方式：
[0092]
实施例：一种冗余双臂服务机器人实时避碰的轨迹规划方法，其特征在于，冗余双臂机器人抓持物体时的结构示意图如图4所示，取主臂的第i个连杆15、从臂的第j个连杆20以及环境障碍物k19为研究对象，如图5所示；分析主臂的第i个连杆15与环境障碍物k19之间的最短距离，如图6所示；分析从臂的第j个连杆20视为主臂第i个连杆15的障碍物(用从臂第j个连杆20的起始关节21和末端关节22 代替)时，主臂的第i个连杆15与障碍物21、22之间的几何关系如图7所示；分析从臂的第j个连杆20与环境障碍物k19之间的几何关系如图8所示；分析主臂的第i个连杆15视为从臂第j个连杆20的障碍物(用主臂第i个连杆15的起始关节16和末端关节17代替)时，从臂的第j个连杆20与障碍物16、17之间的几何关系如图9所示。
[0093] 具体分析如下：
[0094] (1)分析主臂的第i个连杆15与环境障碍物k19之间的最短距离：
[0095] 主臂的第i个连杆15与环境障碍物k19之间的几何关系如图6所示，用点分别代表主臂第i连杆15的起始关节16与末端关节 17，其在基坐标系O0-x0y0z0下的坐标分别用 表示， 为双臂胶囊体包围模型中主臂的第i连杆15的长度，则该主臂的第i连杆 15的单位向量为：
[0096]
[0097] 其中， 代表此刻主臂的第i个连杆轴线18上与环境障碍物k19 相距最近的点，的位置与 值的正负有关； 为 的转置， 为机器人视觉信息
定位的环境障碍物k在冗余双臂机器人参考坐标系O0-x0y0z0下的位置坐标，则主臂的第i连杆15的轴线上距离环境障碍物k19的最近点在基坐标系下的位置 为：
[0098]
[0099] 其中，i代表主臂的第i个连杆15，k代表第k个环境障碍物19，主臂的第i连杆轴线18上距离环境障碍物k19的最近点记为 则环境障碍物k19的中心与主臂的第i个连杆轴线18之间的最短距离为：
[0100]
[0101] 考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上环境障碍物k19与主臂的第i个连杆15间的最短距离为：
[0102]
[0103] 其中， 代表冗余双臂机器人的胶囊体简化模型中主臂第i个连杆15的半径，为环境障碍物k19的半径。
[0104] (2)分析主臂1与从臂7在协调搬运物体时，从臂的第j个连杆 20视为主臂第i个连杆15的障碍物(用从臂第j个连杆20的起始关节 21和末端关节22代替)时，主臂的第i个连杆15与障碍物21、22 之间的几何关系如图7所示；
[0105] 2.1用点 表示从臂第j连杆20的起始关节21，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标用 表示，将从臂第j连杆的起始关节21视为主臂第i个连杆15的障碍物，此时 同步骤(1)一样可求得主臂第i连杆的轴线18上距离障碍物 21的最近距离为：
[0106]
[0107] 考虑到实际的机械臂模型不能简单的用一条线段来表示，因此实际上障碍物21与主臂第i个连杆15之间的最短距离为：
[0108]
[0109] 主臂第i连杆轴线18上距离障碍物21的最近点记为
[0110] 其中， 代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆20的半径；
[0111] 2.2用点 表示从臂第j连杆20的末端关节22，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标用 表示，将从臂第j个连杆的末端关节22视为主臂第i个连杆15的障碍物，此时 同步骤(1)一样可求得主臂的第i个连杆轴线18上与障碍物22间的最近距离为：
[0112]
[0113] 主臂的第i连杆轴线18上距离障碍物22最近的点记为 进而求得实际上主臂第i个连杆15与障碍物22间的最近距离为：
[0114]
[0115] 利用步骤(1)和步骤(2)即可求得环境障碍物k19在主臂1各个连杆上的最近距离d*l1以及从臂7的各连杆起始关节和末端关节视为主臂1的障碍物时，其分别到主臂1各个连杆上的最近距离d*l2， d*l3，进一步可得到环境障碍物k19以及从臂上各连杆的关节(包括起始关节与末端关节)，视为主臂1的障碍物时，障碍物(包括环境障碍物k19及从臂7上各连杆的关节)与主臂1的最近距离为：
[0116]
[0117] 此时，对应的主臂1的连杆轴线上与障碍物(包括环境障碍物k19 及从臂7上各连杆的关节)相距最近的点记为xl0，一般称为标志点。
[0118] (3)分析从臂的第j连杆20与环境障碍物k19之间的最短距离：
[0119] 从臂的第j个连杆20与环境障碍物k19之间的几何关系如图8所示，用点分别代表从臂7第j连杆20的起始关节21与末端关节22，其在冗余双臂机器人参考坐标系O0-x0y0z0下的坐标分别用 表示， 为双臂的胶囊体包围模型中从臂7的第j连杆20的长度，则该从臂7的第j连杆20的单位向量为：
[0120]
[0121] 其中， 代表此刻从臂7的第j连杆轴线23上与环境障碍物k19 相距最近的点，的位置与 值的正负有关， 为 的转置，则从臂7的第j连杆轴线23上距离环境障碍物k19的最近点在基坐标系下的位置 为：
[0122]
[0123] 其中，j代表从臂7的第j个连杆20，k代表第k个环境障碍物19，则从臂7第j个连杆轴线23上与环境障碍物k19间的最短距离则为：
[0124]
[0125] 考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上从臂7的第j个连杆20与环境障碍物k19间的最短距离为：
[0126]
[0127] 从臂7的第j个连杆的轴线23上距离环境障碍物k19的最近点记为
[0128] 其中， 代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆20的半径。
[0129] (4)分析主臂1与从臂7在协调搬运物体时，主臂的第i个连杆 15视为从臂第j个连杆20的障碍物(用主臂第i个连杆15的起始关节16和末端关节17代替)时，从臂的第j个连杆20与障碍物16、17 之间的几何关系如图9所示。
[0130] 4.1将主臂第i连杆的起始关节16视为从臂第j个连杆20的障碍物时，利用步骤2的原理可求实际上从臂的第j个连杆20与障碍物16 间最近距离为：
[0131]
[0132] 从臂的第j个连杆轴线23上距离障碍物16最近的点记为
[0133] 4.2将主臂第i连杆的末端关节17视为从臂的第j个连杆20障碍时，利用步骤2的原理求得从臂第j个连杆20与障碍物17间的最近距离为：
[0134]
[0135] 从臂第j个连杆的轴线23上距离障碍物17的最近点记为
[0136] 利用步骤(3)和步骤(4)即可求得环境障碍物k19在从臂7各个连杆上的最近距离d*f1以及主臂1各连杆的起始关节和末端关节视为从臂7的障碍物时，其分别到从臂7各个连杆上的最近距离d*f2， d*f3，进一步可得到环境障碍物k19以及主臂1各连杆的关节(包括起始关节与末端关节)视为从臂7的障碍物时，障碍物(包括环境障碍物k19及主臂1上各连杆的关节)与从臂7的最近距离为：
[0137]
[0138] 此时，对应的从臂7的连杆轴线上与障碍物(包括环境障碍物k19 及主臂1上各连杆的关节)相距最近的点记为xf0，一般称为标志点。
[0139] (5)利用连杆变换矩阵实时求解出机械臂各关节点的位置信息，当检测到障碍物与标志点的距离d0小于设定的阈值时，标志点将产生一个避障速度，则关节角速度函数为：
[0140]
[0141] 其中，J为末端雅克比矩阵，J+为J的广义逆， 为末端速度，I为单位矩阵，J0为改进前的标志点的雅克比矩阵，当标志点的位置确定后，其值可由运动学方程直接求出； 为预先设置的三维避障速度，等式右边的第一项为满足机械手末端轨迹跟踪所需的速度，第二项是在不影响末端轨迹的前提下标志点的避障速度；
[0142] 由于机械臂处于三维空间状态，则避障速度也将是三维的，为简化计算量，提高轨迹规划算法效率，利用改进的缩减避障运动操作空间的方法得到机器人实时避障的轨迹。
[0143] 所述步骤(5)中的连杆变换矩阵为：
[0144]
[0145] 其中， 为连杆i相对于连杆i-1的变换矩阵，n是自然数。
[0146] 所述步骤(5)中利用改进的缩减避障运动操作空间的方法得到机器人实时避障的轨迹，具体方法如下：
[0147] 设障碍物与机械臂上的单位距离为:
[0148] 则标志点的雅克比矩阵J0及其避障速度 可分别改写为：
[0149]
[0150] 在未对 J0∈Rmxn处理之前，当标志点在进行障碍物躲避的时候，其速度的维度是mx1，标志点雅克比J0的维度是mxn，经改进后与标志点避障速度 的维度缩减为1x1，标志点雅克比 的维度缩减为1xn，使轨迹规划方法的计算效率进一步得到提高。
[0151] 一种能够使冗余双臂机器人在避障运动过程中运行平稳的方法，其特征在于它是在所述一种冗余双臂服务机器人实时避碰的轨迹规划方法的基础上引入权重因子αi、γi，(i＝f,l)，均是关于机械臂上的标志点与障碍物之间距离d的函数，分别记作αi(d)和γi(d)，式中，αi(d)是表示躲避障碍物速度大小的权重因子，所述γi(d)是关节角速度的增益。
[0152] 所述αi(d)和γi(d)与距离d之间的关系如图10所示，当d>d3时，此时为安全区域，表明障碍物与机械臂相距较远，可令αi＝γi＝0，即不改变由伪逆解所给出的机器人的位姿此时各关节角速度值；当 d2
[0153] 下面结合附图和实施案例对本发明专利做进一步的描述。本发明是基于新的梯度投影最小距离检测算法对空间冗余机械臂进行实施避障的轨迹规划方法，工作流程如图11所示。
[0154] 冗余双臂服务机器人抓持物体时的结构状态如图4所示，1为冗余双臂服务机器人的主臂，3为主臂的大臂，5为主臂的小臂，2为主臂的肩部，4为主臂的肘关节，6为主臂的腕部，7为冗余双臂服务机器人的从臂，9为从臂的大臂，11为从臂的小臂，8为从臂的肩部，10为从臂的肘部，12为从臂的腕部，13为双臂工作空间中的环境障碍物，14为被搬运的物体。
[0155] 本算法求在进行实时避障的路径规划前，首先要通过D-H法建立冗余双臂机器人各关节位姿与各关节转角的数学模型，求解过程如下：利用连杆变换矩阵:
[0156]
[0157]
[0158] 式中： 分别为机械臂的
姿态矩阵与位置矩阵。
[0159] 这样主臂1与从臂7的各关节位姿信息便可实时获得，再通过机器人的视觉信息获得双臂在工作时周边的环境信息，实时获得环境障碍物13的位置信息，这样，机器人在运动过程中主臂1、从臂7的各关节位置信息及工作空间中的环境障碍13信息就可实时获得。
[0160] 然后利用胶囊体包围法获得冗余双臂机器人的简化模型，基于梯度投影法分别计算环境障碍物13与主臂1上的最近距离、从臂的各个连杆的起始关节和末端关节视为主臂7的障碍时与主臂各连杆的最近距离，取三者之间的最小值记为 与其对应的标志点记为同理可求得此刻环境障碍物13与从臂7的各连杆之间的最短距离及主臂各个连杆的起始关节及末端关节视为从臂的障碍时与主臂各连杆之间的最近距离，取三者之间的最小值记为 与其对应的标志点记为 进而求得主从机械臂在缩减的避障运动操作空间内标志点的雅克比矩阵 及其避障速度
[0161] 接着建立冗余双臂机器人协调搬运的运动学约束方程，冗余双臂机器人抓持物体时的结构意图如图4所示，双臂在协调搬运过程中，被搬运物体14与主臂1的运动学约束方程为：
[0162]
[0163] 主臂1与从臂7之间的运动学约束方程为：
[0164]
[0165] 当被搬运物体14的运动规律已知时，T可以直接求出
[0166] 故双臂在不考虑避障的情况下，双臂运动学约束方程可写成：
[0167]
[0168] 其中：
[0169] 考虑障碍物的情况下，双臂运动学约束方程可为
[0170]
[0171] 其中等式右边的第一项为满足机械手末端轨迹跟踪所需的速度，第二项是在不影响末端轨迹的前提下主臂标志点的避障速度，第三项是在不影响末端轨迹的前提下从臂标志点的避障速度，
[0172] 式中：I为单位矩阵， 分别为缩减避障空间维数后计算得到的主臂1与从臂7标志点的雅克比矩阵， 为缩减避障空间维数后得到的主从臂1与从臂7的避障速度。
[0173] 在冗余双臂机器人的工作空间内计算主臂1与从臂7的各关节角速度随时间的函数表达式，对其积分得到下一时刻各关节的转角信息从而计算下一时刻各关节的位置信息，再计算主臂1与障碍物(包括环境障碍13及从臂7上各连杆的关节)间的最小值记为与其对应的标志点记为xl0，从臂7与障碍物(包括环境障碍13及主臂1上各连杆的关节)间的最小值记为 与其对应的标志点记为 再判断 与d之间的关系(如图10所示)，进而更新J， αi(d)、γi(d)的值，计算下一时刻各关节的
角速度，判断被搬运的物体是否达到目标位置，若还未到达
指定位，则机械臂重复上述步骤，以此循环直到被搬运物体移动到目标位，最终冗余双臂停止运动。
