CN107443373B - 基于关节臂机器人的避碰轨迹规划方法和装置 - Google Patents

基于关节臂机器人的避碰轨迹规划方法和装置 Download PDF

Info

Publication number
CN107443373B
CN107443373B CN201710594150.3A CN201710594150A CN107443373B CN 107443373 B CN107443373 B CN 107443373B CN 201710594150 A CN201710594150 A CN 201710594150A CN 107443373 B CN107443373 B CN 107443373B
Authority
CN
China
Prior art keywords
collision prevention
duration
length
rotation
track
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
CN201710594150.3A
Other languages
English (en)
Other versions
CN107443373A (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.)
Guangzhou Zsrobot Intelligent Equipment Co ltd
Wuhan Zhongshe Robot Technology Co ltd
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN201710594150.3A priority Critical patent/CN107443373B/zh
Publication of CN107443373A publication Critical patent/CN107443373A/zh
Application granted granted Critical
Publication of CN107443373B publication Critical patent/CN107443373B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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
    • B25J9/1666—Avoiding collision or forbidden zones
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39082—Collision, real time collision avoidance

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种基于关节臂机器人的避碰轨迹规划方法和装置，根据作业任务要求在规定的时间之内生成候补避碰轨迹序列，考虑路径长度，关节转动总量，以及作业用时这三方面因素，从中选择最佳的避碰轨迹，从而实现自主避碰路径规划，避免在作业过程之中的人工干预。

Description

基于关节臂机器人的避碰轨迹规划方法和装置
技术领域
本发明涉及自动轨迹规划领域，特别涉及一种基于关节臂机器人的避碰轨迹规划方法和装置。
背景技术
关节臂机器人具有多自由度关节，并且借鉴人的抓取操作经验进行任务规划和命令设计,因此在对其作业环境的复杂性和执行任务所要求的精细化程度越来越高的工业领域被广泛采用。为了提高关节臂机器人的操作性能和作业水平，自主规划能力成为关节臂机器人所必备的一项基础功能，而自主规划之中最重要的一项功能就是避碰轨迹规划。传统上，避碰轨迹规划的目的是在开始配置和目标配置之间找到避免碰撞的一条轨迹。现有的研究与应用都侧重与在静态障碍物环境下关节臂的避碰轨迹的优化问题，但是这些研究与应用的关注点是避碰轨迹规划的过程必须满足预先设定的约束条件。因此，所提出的技术方案只适用于单一的简单作业任务，远远不能满足越来越复杂的多任务作业，而且需要随着作业任务的变更也必须对避碰轨迹进行重新设定。也有一些研究与应用也有提出多目标优化问题，即同时考虑总行进时间和关节能量消耗这些参数因子，建立最小化的目标函数。不过上述研究与应用之中没有通过关注的轨迹参数经标准化处理得到的函数来评价重复使用基于采样的避碰路径规划得到的轨迹从而获得最优的避碰轨迹的方法研究。因此上述的现有技术方案只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预。
发明内容
本发明的主要目的在于提供一种基于关节臂机器人的避碰轨迹规划方法及装置，旨在解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。
为实现上述目的，本发明提供的一种基于关节臂机器人的避碰轨迹规划方法，包括：
步骤1，从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；
步骤2，在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1to M)，所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N 个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN-1，PN]，所述 P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；
步骤3，利用所述候补避碰轨迹Qi所对应的轨迹长度clength(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述clength(Qi)的最小值clength_min所对应的所述候补避碰轨迹Qi作为最优路径Qoptimal，其中：
所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2to N)|Pj-Pj-1|；
所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离。
优选地，所述步骤3，利用所述候补避碰轨迹Qi所对应的所有关节的行程 crotation(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项 Qi进行评价计算之后，将所述crotation(Qi)的最小值crotation_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述crotation(Qi)的计算公式为：Σ(k＝1to D)Σ(j＝2to N)|θPkj–θPk(j-1)|；
所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数 D的正整数；
所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度；
所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值。
优选地，所述步骤3，利用所述候补避碰轨迹Qi所对应的持续时间cduration(Qi) 作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述cduration(Qi)的最小值cduration_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart；
所述Tstart为所述关节臂在所述起始位姿Pstart的时刻；
所述Tend为所述关节臂在所述终止位姿Pend的时刻。
优选地，所述步骤3，利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+ C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
优选地，所述步骤3，利用评价函数f(Qi)＝A*clength(Qi)＊+B*crotation(Qi) ＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述 clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/ σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi) 对应的标准偏差。
本发明进一步提供一种基于关节臂机器人的避碰轨迹规划装置，包括：
规划任务输入模块，用于从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；
候补避碰轨迹序列生成模块，用于在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1to M)，所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN-1，PN]，所述P1为所述起始位姿Pstart，所述PN为所述终止位姿 Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；
最优避碰轨迹选择模块，用于利用所述候补避碰轨迹Qi所对应的轨迹长度clength(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述clength(Qi)的最小值clength_min所对应的所述候补避碰轨迹Qi作为最优路径Qoptimal，其中：
所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2to N)|Pj-Pj-1|；
所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离。
优选地，所述最优避碰轨迹选择模块，用于利用所述候补避碰轨迹Qi所对应的所有关节的行程crotation(Qi)作为评价系数，对所述候补避碰轨迹序列 {Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述crotation(Qi)的最小值 crotation_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述crotation(Qi)的计算公式为：Σ(k＝1to D)Σ(j＝2to N)|θPkj-θPk(j-1)|；
所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数 D的正整数；
所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度；
所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值。
优选地，所述最优避碰轨迹选择模块，用于利用所述候补避碰轨迹Qi所对应的持续时间cduration(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述cduration(Qi)的最小值cduration_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart；
所述Tstart为所述关节臂在所述起始位姿Pstart的时刻；
所述Tend为所述关节臂在所述终止位姿Pend的时刻。
优选地，所述最优避碰轨迹选择模块，用于利用评价函数f(Qi)＝A*clength(Qi) +B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
优选地，所述最优避碰轨迹选择模块，用于利用评价函数f(Qi)＝A* clength(Qi)＊+B*crotation(Qi)＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi} (i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述 clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/ σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi) 对应的标准偏差。
本发明通过从生成的候补避碰轨迹序列之中，考虑路径，关节转动总量，以及作业用时这三方面因素，根据任务需要，选择最佳的避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径，关节转动总量，作业用时，以及评价函数f(Qi)这些计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
其次，通过正则化处理消除上述路径，关节转动总量，以及作业用时这三个因素各自的偏差，从而能够选择最优的避碰轨迹。
附图说明
图1为实现本发明各个实施例的终端的硬件结构示意图；
图2为本发明方法第一实施例的流程示意图；
图3为本发明方法第一实施例候补避碰轨迹序列的示意图；
图4为本发明方法第一实施例的具体处理流程示意图。
图5为本发明装置第一实施例的功能模块示意图；
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
现在将参考附图描述实现本发明各个实施例的装置设备。在后续的描述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身并没有特定的意义。因此，"模块"与"部件"可以混合地使用。
基于关节臂机器人的避碰轨迹规划方法及装置可以以各种形式来实施。例如，本发明中描述的处理方法及装置可以包括各种设备终端。下面，假设装置是一种固定设备终端。然而，本领域技术人员将理解的是，根据本发明的实施方式的构造也能够应用于其他类似的设备终端。
图1为实现本发明各个实施例的设备终端的硬件结构示意。设备终端100可以包括通信单元110、输入单元120、输出单元130、存储器140、电源单元150、接口单元160和控制器170等等。图1示出了具有各种组件的设备终端，但是应理解的是，并不要求实施所有示出的组件。可以替代地实施更多或更少的组件。将在下面详细描述移动终端的元件。
通信单元110通常包括一个或多个组件，其允许设备终端100与无线通信系统或网络之间进行通信。例如，通信单元可以包括无线互联网模块、短程通信模块和以太网模块中的至少一个。所述无线互联网模块支持设备终端的无线互联网接入。该模块可以内部或外部地耦接到终端。该模块所涉及的无线互联网接入技术可以包括WLAN(无线LAN)(Wi-Fi)、Wibro(无线宽带)、 Wimax(全球微波互联接入)、HSDPA(高速下行链路分组接入)等等。所述短程通信模块是用于支持短程通信的模块。短程通信技术的一些示例包括蓝牙 TM、射频识别(RFID)、红外数据协会(IrDA)、超宽带(UWB)、紫蜂TM等等。
输入单元120可以根据用户输入的命令生成键输入数据以控制移动终端的各种操作。输入单元120允许用户输入各种类型的信息，并且可以包括键盘、锅仔片、触摸板(例如，检测由于被接触而导致的电阻、压力、电容等等的变化的触敏组件)、滚轮、摇杆等等。特别地，当触摸板以层的形式叠加在输出单元所对应的显示模块上时，可以形成触摸屏。
输出单元130被构造为以视觉、音频和/或触觉方式提供输出信号(例如，音频信号、视频信号、警报信号、振动信号等等)。输出单元130可以包括显示模块、音频输出模块、警报模块等等。输出单元130可向所述显示模块输出信息，从而输出在设备终端100中所处理的信息，例如，可以通过在所述显示模块显示与作业相关的用户界面(UI)或图形用户界面(GUI)。当设备终端100处于作业模式时，所述显示模块可以显示捕获的工件与作业的轨迹图像、以及相关功能的UI或GUI等等。同时，当所述显示模块和触摸板以层的形式彼此叠加以形成触摸屏时，所述显示模块可以用作输入装置和输出装置。所述显示模块可以包括液晶显示器(LCD)、薄膜晶体管LCD(TFT-LCD)、有机发光二极管(OLED) 显示器、柔性显示器、三维(3D)显示器等等中的至少一种。这些显示器中的一些可以被构造为透明状以允许用户从外部观看，这可以称为透明显示器，典型的透明显示器可以例如为TOLED(透明有机发光二极管)显示器等等。根据特定想要的实施方式，设备终端100可以包括两个或更多显示模块(或其它显示装置)，例如，设备终端可以包括外部显示模块(未示出)和内部显示模块(未示出)。触摸屏可用于检测触摸输入压力以及触摸输入位置和触摸输入面积。
存储器140可以存储由控制器170执行的处理和控制操作的软件程序等等，或者可以暂时地存储己经输出或将要输出的数据。存储器140可以包括至少一种类型的存储介质，所述存储介质包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等等)、随机访问存储器(RAM)、静态随机访问存储器 (SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等等。而且，设备终端100可以与通过网络连接执行存储器140的存储功能的网络存储装置协作。
电源单元150在控制器170的控制下接收外部电力或内部电力并且提供操作各元件和组件所需的适当的电力。
接口单元160用作至少一个外部装置与设备终端100连接可以通过的接口。例如，外部装置可以包括外部电源(或电池充电器)端口、有线或无线数据端口、存储卡端口、用于连接具有识别模块的装置的端口、数据输入/输出(I/O)端口、视频I/O端口等等。识别模块可以是存储用于验证用户使用设备终端100的各种信息并且可以包括用户识别模块(UIM)、客户识别模块(SIM)、通用客户识别模块(USIM)等等。另外，具有识别模块的装置(下面称为"识别装置")可以采取智能卡的形式，因此，识别装置可以经由端口或其它连接装置与设备终端100 连接。接口单元160可以用于接收来自外部装置的输入(例如，数据信息、电力等等)并且将接收到的输入传输到设备终端100内的一个或多个元件或者可以用于在设备终端和外部装置之间传输数据。
另外，当设备终端100与外部底座连接时，接口单元160可以用作允许通过其将电力从底座提供到设备终端100的路径或者可以用作允许从底座输入的各种命令信号通过其传输到移动终端的路径。从底座输入的各种命令信号或电力可以用作用于识别移动终端是否准确地安装在底座上的信号。
控制器170通常控制设备终端的总体操作。例如，控制器170执行与数据通信等等相关的控制和处理。控制器170可以执行模式识别处理，以将在触摸屏上执行的手写输入或者图片绘制输入识别为字符或图像。
这里描述的各种实施方式可以以使用例如计算机软件、硬件或其任何组合的计算机可读介质来实施。对于硬件实施，这里描述的实施方式可以通过使用特定用途集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理装置 (DSPD)、可编程逻辑装置(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器、被设计为执行这里描述的功能的电子单元中的至少一种来实施，在一些情况下，这样的实施方式可以在控制器170中实施。对于软件实施，诸如过程或功能的实施方式可以与允许执行至少一种功能或操作的单独的软件模块来实施。软件代码可以由以任何适当的编程语言编写的软件应用程序(或程序)来实施，软件代码可以存储在存储器140中并且由控制器170执行。
至此，己经按照其功能描述了设备终端。下面，为了简要起见，将描述诸如折叠型、直板型、摆动型、滑动型设备终端等等的各种类型的终端中的滑动型终端作为示例。因此，本发明能够应用于任何类型的设备终端，并且不限于滑动型终端。
如图1中所示的设备终端100可以被构造为利用经由帧或分组发送数据的诸如有线和无线通信系统以及基于其他类型的通信系统来操作。基于上述设备终端硬件结构以及相关系统，提出本发明基于关节臂机器人的避碰轨迹规划方法和装置的各个实施例。
参照图2，图2为本发明基于关节臂机器人的避碰轨迹规划方法的第一实施例的流程示意图。如图2所示的实施例，所述基于关节臂机器人的避碰轨迹规划方法包括：
步骤S10、输入规划任务。
即从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D。
步骤S20、生成候补避碰轨迹序列。
即在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1to M)，所述候补避碰轨迹序列 {Qi}(i＝1to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N 个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN-1，PN]，所述 P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数。
步骤S30、选择最优避碰轨迹。
即利用所述候补避碰轨迹Qi所对应的轨迹长度clength(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述 clength(Qi)的最小值clength_min所对应的所述候补避碰轨迹Qi作为最优路径 Qoptimal，其中：
所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2to N)|Pj-Pj-1|；
所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离。
根据在所述步骤S10所获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，在所述步骤S20，使用如快速扩展随机树法(Rapidly-exploring Random Tree，RRT)这样的随机方法生成的规划路径，然后通过如包围盒算法(Axis-Aligned Bounding Box，AABB)这样的碰撞检测算法来对所述生成的规划路径进行碰撞检测，如果检测结果为所述生成的规划路径符合避碰规则，则将所述生成的规划路径加入候补避碰轨迹序列。在经过所述预设时间T之后重复执行上述的，形成如图3所示的候补避碰轨迹序列。在图3之中，CO1，CO2，COn表示环境中的不同障碍物，Q1，Q2，Q3，Qn分别表示生成的候补避碰轨迹， C-Space相当于配置空间C，qstart相当于起始位姿Pstart，qend相当于终止位姿 Pend。
所述步骤S20的伪代码可以具体描述如下：
其中，所述Planning time为规划时间，所述PlannerID为采样的规划器类型，所述成功次数success的初始值设置为0。如果路径规划path_plan成功则启动轨迹规划trajectory_plan和碰撞检测collision_detect，否则重复path_plan；如果trajectory_plan和collision_detect同时成功则保存当前的避碰轨迹数据Q 到容器并使成功规划的统计值success加1。
通过所述步骤S30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择路径最短的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径的计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
所述Planning time，即所述预设时间T设置为8秒，可以为生成所述候补避碰轨迹序列{Qi}(i＝1to M)确保必要的时间，从而可以提供足够的候补避碰轨迹，从而确保能获得极佳的最优避碰轨迹。同时也确保所述基于关节臂机器人的避碰轨迹规划方法能够在允许的工作时间内执行完毕。
进一步，基于上述图2的实施例，在所述步骤S30选择最优避碰轨迹，利用所述候补避碰轨迹Qi所对应的所有关节的行程crotation(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述crotation(Qi)的最小值crotation_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述crotation(Qi)的计算公式为：Σ(k＝1to D)Σ(j＝2to N)|θPkj–θPk(j-1)|；
所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数 D的正整数；
所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度；
所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值。
通过所述步骤S30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择所述关节臂机器人的所有关节转动总量最少的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述关节转动总量计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图2的实施例，在所述步骤S30选择最优避碰轨迹，利用所述候补避碰轨迹Qi所对应的持续时间cduration(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述 cduration(Qi)的最小值cduration_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart；
所述Tstart为所述关节臂在所述起始位姿Pstart的时刻；
所述Tend为所述关节臂在所述终止位姿Pend的时刻。
通过所述步骤S30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择所述关节臂机器人的用时最少的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述作业用时这些计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图2的实施例，在所述步骤S30选择最优避碰轨迹，利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
如图4所示，在Step1：Get planning tasks，获取所述规划任务Task1，Task2，Task3，Task4任务列表之中各个规划任务Task的起始位姿Pstart和终止位姿 Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数之后，通过 Step2：Re-planning获得候补避碰轨迹序列{Qi}(i＝1to M)之后，在Step3，即所述步骤S30综合考虑路径，关节转动总量，以及作业用时这三方面因素，选择最佳的避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径，关节转动总量，作业用时，以及评价函数f(Qi)这些计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图2的实施例，在所述步骤S30选择最优避碰轨迹，利用评价函数f(Qi)＝A*clength(Qi)＊+B*crotation(Qi)＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi) 最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述 clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/ σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi) 对应的标准偏差。
通过所述步骤S30，除了综合考虑路径，关节转动总量，以及作业用时这三方面因素之外，还考虑通过正则化处理消除各个因素的偏差，从而选择最优的避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径，关节转动总量，作业用时的正则化计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
上述本发明基于关节臂机器人的避碰轨迹规划方法的第一实施例中的基于关节臂机器人的避碰轨迹规划方法可以由本发明基于关节臂机器人的避碰轨迹规划装置的第一实施例所提供的基于关节臂机器人的避碰轨迹规划装置来实现。
参照图5，图5为本发明基于关节臂机器人的避碰轨迹规划装置的第一实施例提供一种基于关节臂机器人的避碰轨迹规划装置100，所述装置包括：
规划任务输入模块10，用于从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；
候补避碰轨迹序列生成模块20，用于在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列 {Qi}(i＝1to M)，所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N个路径点所构成的向量[P1，P2，…，Pj－1， Pj，Pj+1，…，PN-1，PN]，所述P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；
最优避碰轨迹选择模块30，用于利用所述候补避碰轨迹Qi所对应的轨迹长度clength(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述clength(Qi)的最小值clength_min所对应的所述候补避碰轨迹Qi作为最优路径Qoptimal，其中：
所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2to N)|Pj-Pj-1|；
所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离。
根据在所述规划任务输入模块10所获取所述规划任务Task的起始位姿 Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，在所述候补避碰轨迹序列生成模块20，使用如快速扩展随机树法 (Rapidly-exploring Random Tree，RRT)这样的随机采样方法生成的规划路径，然后通过如包围盒算法(Axis-Aligned BoundingBox，AABB)这样的碰撞检测算法来对所述生成的规划路径进行碰撞检测，如果检测结果为所述生成的规划路径符合避碰规则，则将所述生成的规划路径加入候补避碰轨迹序列。在经过所述预设时间T之后重复执行上述的，形成如图3所示的候补避碰轨迹序列。在图3之中，CO1，CO2，COn表示环境中的不同障碍物，Q1，Q2，Q3， Qn分别表示生成的候补避碰轨迹，C-Space相当于配置空间C，qstart相当于起始位姿Pstart，qend相当于终止位姿Pend。
所述候补避碰轨迹序列生成模块20所对应的处理步骤的伪代码可以具体描述如下：
其中，所述Planning time为规划时间，所述PlannerID为采样的规划器类型，所述成功次数success的初始值设置为0。如果路径规划path_plan成功则启动轨迹规划trajectory_plan和碰撞检测collision_detect，否则重复path_plan；如果trajectory_plan和collision_detect同时成功则保存当前的避碰轨迹数据Q 到容器并使成功规划的统计值success加1。
通过所述最优避碰轨迹选择模块30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择路径最短的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径的计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
所述Planning time，即所述预设时间T设置为8秒，可以为生成所述候补避碰轨迹序列{Qi}(i＝1to M)确保必要的时间，从而可以提供足够的候补避碰轨迹，从而确保能获得极佳的最优避碰轨迹。同时也确保所述基于关节臂机器人的避碰轨迹规划方法能够在允许的工作时间内执行完毕。
进一步，基于上述图5的实施例，在所述最优避碰轨迹选择模块30，用于利用所述候补避碰轨迹Qi所对应的所有关节的行程crotation(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述crotation(Qi)的最小值crotation_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述crotation(Qi)的计算公式为：Σ(k＝1to D)Σ(j＝2to N)|θPkj-θPk(j-1)|；
所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数 D的正整数；
所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度；
所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值。
通过所述所述最优避碰轨迹选择模块30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择所述关节臂机器人的所有关节转动总量最少的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述关节转动总量计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图5的实施例，在所述最优避碰轨迹选择模块30，利用所述候补避碰轨迹Qi所对应的持续时间cduration(Qi)作为评价系数，对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述 cduration(Qi)的最小值cduration_min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart；
所述Tstart为所述关节臂在所述起始位姿Pstart的时刻；
所述Tend为所述关节臂在所述终止位姿Pend的时刻。
通过所述所述最优避碰轨迹选择模块30，可以根据从所述候补避碰轨迹序列{Qi}(i＝1to M)之中选择所述关节臂机器人的用时最少的避碰轨迹作为最优避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述作业用时这些计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图5的实施例，在所述最优避碰轨迹选择模块30，利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值 fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
如图4所示，在Step1：Get planning tasks对应规划任务输入模块10，获取所述规划任务Task1，Task2，Task3，Task4任务列表之中各个规划任务Task 的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数之后，通过候补避碰轨迹序列生成模块20执行Step2： Re-planning获得候补避碰轨迹序列{Qi}(i＝1to M)之后，在最优避碰轨迹选择模块30执行Step3，即综合考虑路径，关节转动总量，以及作业用时这三方面因素，选择最佳的避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径，关节转动总量，作业用时，以及评价函数f(Qi)这些计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
进一步，基于上述图5的实施例，在所述最优避碰轨迹选择模块30，利用评价函数f(Qi)＝A*clength(Qi)＊+B*crotation(Qi)＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi) 的最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述 clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/ σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi) 对应的标准偏差。
除了综合考虑路径，关节转动总量，以及作业用时这三方面因素之外，还考虑通过正则化处理消除各个因素的偏差因素，从而选择最优的避碰轨迹，既可以提高作业路径效率，节约作业时间，也可以解决现有关节臂机器人只适用于相对简单的作业任务，而且由于没有进行优化，其作业的综合效率不高，并且为了避免碰撞以及错误，需要在作业过程之中进行人工干预的技术问题。上述路径，关节转动总量，作业用时的正则化计算公式，具有计算复杂度低，运算量少，成熟可靠，响应速度快，易于工程实现的优点。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
显然，本领域的技术人员应该明白，上述的本发明的各模块单元或各步骤可以用通用的计算装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘) 中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (4)

1.基于关节臂机器人的避碰轨迹规划方法，其特征在于，包括：
步骤1，从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；
步骤2，在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1 to M)，所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN-1，PN]，所述P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；
步骤3，利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)为所述候补避碰轨迹Qi所对应的轨迹长度，所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2 to N)|Pj-Pj-1|，所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离；
所述crotation(Qi)为所述候补避碰轨迹Qi所对应的所有关节的行程，所述crotation(Qi)的计算公式为：Σ(k＝1 to D)Σ(j＝2 to N)|θPkj–θPk(j-1)|，所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数D的正整数，所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度，所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值；
所述cduration(Qi)为所述候补避碰轨迹Qi所对应的持续时间，所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart，所述Tstart为所述关节臂在所述起始位姿Pstart的时刻，所述Tend为所述关节臂在所述终止位姿Pend的时刻；
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
2.如权利要求1所述的基于关节臂机器人的避碰轨迹规划方法，其特征在于，所述步骤3，利用评价函数f(Qi)＝A*clength(Qi)＊+B*crotation(Qi)＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi)对应的标准偏差。
3.基于关节臂机器人的避碰轨迹规划装置，其特征在于，包括：
规划任务输入模块，用于从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；
候补避碰轨迹序列生成模块，用于在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1 to M)，所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN-1，PN]，所述P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；
最优避碰轨迹选择模块，用于利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)为所述候补避碰轨迹Qi所对应的轨迹长度，所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2 to N)|Pj-Pj-1|，所述|Pj-Pj-1|为所述关节臂从所述路径点Pj-1至所述路径点Pj的移动距离；
所述crotation(Qi)为所述候补避碰轨迹Qi所对应的所有关节的行程，所述crotation(Qi)的计算公式为：Σ(k＝1 to D)Σ(j＝2 to N)|θPkj–θPk(j-1)|，所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数D的正整数，所述|θPkj–θPk(j-1)|为所述关节臂的所述第k个关节从所述路径点Pj-1至所述路径点Pj的旋转角度，所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值；
所述cduration(Qi)为所述候补避碰轨迹Qi所对应的持续时间，所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend-Tstart，所述Tstart为所述关节臂在所述起始位姿Pstart的时刻，所述Tend为所述关节臂在所述终止位姿Pend的时刻；
所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；
所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；
所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；
所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。
4.如权利要求3所述的基于关节臂机器人的避碰轨迹规划装置，其特征在于，所述最优避碰轨迹选择模块，用于利用评价函数f(Qi)＝A*clength(Qi)＊+B*crotation(Qi)＊+C*cduration(Qi)＊对所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值f＊ min所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：
所述clength(Qi)＊为对所述clength(Qi)进行z-score标准化处理所获得的所述clength(Qi)对应的标准正态分布，即clength(Qi)＊＝(clength(Qi)-μlength)/σlength，所述μlength为所述clength(Qi)对应的均值，所述σlength为所述clength(Qi)对应的标准偏差；
所述crotation(Qi)＊为对所述crotation(Qi)进行z-score标准化处理所获得的所述crotation(Qi)对应的标准正态分布，即crotation(Qi)＊＝(crotation(Qi)-μrotation)/σrotation，所述μrotation为所述crotation(Qi)对应的均值，所述σrotation为所述crotation(Qi)对应的标准偏差；
所述cduration(Qi)＊为对所述cduration(Qi)进行z-score标准化处理所获得的所述cduration(Qi)对应的标准正态分布，即cduration(Qi)＊＝(cduration(Qi)-μduration)/σduration，所述μduration为所述cduration(Qi)对应的均值，所述σduration为所述cduration(Qi)对应的标准偏差。
CN201710594150.3A 2017-07-20 2017-07-20 基于关节臂机器人的避碰轨迹规划方法和装置 Active CN107443373B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710594150.3A CN107443373B (zh) 2017-07-20 2017-07-20 基于关节臂机器人的避碰轨迹规划方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710594150.3A CN107443373B (zh) 2017-07-20 2017-07-20 基于关节臂机器人的避碰轨迹规划方法和装置

Publications (2)

Publication Number Publication Date
CN107443373A CN107443373A (zh) 2017-12-08
CN107443373B true CN107443373B (zh) 2018-09-28

Family

ID=60487880

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710594150.3A Active CN107443373B (zh) 2017-07-20 2017-07-20 基于关节臂机器人的避碰轨迹规划方法和装置

Country Status (1)

Country Link
CN (1) CN107443373B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108213757B (zh) * 2018-01-16 2019-10-18 华东理工大学 一种用于焊接机器人的碰撞检测方法
CN108326853B (zh) * 2018-01-17 2021-08-24 广东工业大学 一种打磨机器人系统
CN108297105B (zh) * 2018-01-17 2021-07-06 广东工业大学 一种机械臂任务级时间最优轨迹规划方法
DE102018202321A1 (de) * 2018-02-15 2019-08-22 Robert Bosch Gmbh Koordinierungsanlage, Handhabungseinrichtung und Verfahren
CN108782797B (zh) * 2018-06-15 2021-08-27 广东工业大学 臂式炒茶机翻炒茶叶的控制方法和臂式炒茶机
CN108871216B (zh) * 2018-07-12 2020-01-14 湘潭大学 一种基于视觉引导的机器人多孔接触式自动测量方法
CN109910015B (zh) * 2019-04-12 2021-02-23 成都天富若博特科技有限责任公司 一种砂浆喷抹建筑机器人的末端路径规划算法
CN111551178B (zh) * 2020-04-27 2022-02-18 广东工业大学 一种基于最短路径的分段轨迹时间规划方法
CN111531542B (zh) * 2020-05-09 2021-08-17 北京邮电大学 一种基于改进a*算法的机械臂无碰撞路径规划方法
CN112734619B (zh) * 2020-12-21 2022-11-01 上海理工大学 一种面向检测特征扫描不确定度的自由曲面覆盖视点自动采样方法
CN112947424B (zh) * 2021-02-01 2023-04-25 国网安徽省电力有限公司淮南供电公司 配网作业机器人自主作业路径规划方法和配网作业系统
CN114184150A (zh) * 2021-12-10 2022-03-15 凌云科技集团有限责任公司 一种关节臂式坐标测量机的结构参数优化方法和装置
CN114265364B (zh) * 2021-12-21 2023-10-03 江苏师范大学 一种工业物联网的监测数据处理系统及方法
CN116901086B (zh) * 2023-09-13 2023-11-28 千里眼(广州)人工智能科技有限公司 一种基于智能算法的工业机器人轨迹优化的控制方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5061798B2 (ja) * 2007-08-31 2012-10-31 富士通株式会社 移動経路生成方法、移動経路生成装置、移動経路生成プログラム、ロボットアーム制御装置、およびロボットアーム制御プログラム
KR100929927B1 (ko) * 2007-12-24 2009-12-04 삼성중공업 주식회사 선박 화물창 벽면주행 이동로봇의 경로 계획방법
CN102520718B (zh) * 2011-12-02 2013-06-05 上海大学 一种基于物理建模的机器人避障路径规划方法
CN103009389B (zh) * 2012-11-30 2015-07-08 北京控制工程研究所 一种冗余空间机械臂在轨抓捕的轨迹规划方法
CN105005301B (zh) * 2015-05-25 2018-06-26 湘潭大学 一种基于群体智能算法的工业机器人作业点序列及轨迹规划方法
CN106054876B (zh) * 2016-06-02 2019-04-19 西北工业大学 一种针对空间多任务的避障路径最优逐次操作规划方法
CN106003027B (zh) * 2016-06-03 2019-03-01 广州视源电子科技股份有限公司 机械臂运动路径的设置方法和系统
CN106289232B (zh) * 2016-07-24 2019-06-21 广东大仓机器人科技有限公司 一种基于深度传感器的机器人避障方法

Also Published As

Publication number Publication date
CN107443373A (zh) 2017-12-08

Similar Documents

Publication Publication Date Title
CN107443373B (zh) 基于关节臂机器人的避碰轨迹规划方法和装置
Li Reinforcement learning applications
CN108734479A (zh) 保险欺诈识别的数据处理方法、装置、设备及服务器
CN108334647A (zh) 保险欺诈识别的数据处理方法、装置、设备及服务器
CN108764046A (zh) 车辆损伤分类模型的生成装置、方法及计算机可读存储介质
CN109313505A (zh) 经由控制点改变渲染对象的属性
CN107533767A (zh) 机器人的增强的配置和控制
CN109447259A (zh) 多任务处理及多任务处理模型训练方法、装置和硬件装置
CN109844777A (zh) 信息处理装置和信息处理方法
CN112001585A (zh) 多智能体决策方法、装置、电子设备及存储介质
CN109771947A (zh) 游戏角色的换装方法、装置、计算机存储介质和电子设备
CN110533723A (zh) 增强现实显示的方法、姿态信息的确定方法及装置
CN105868772A (zh) 图像识别方法及装置
CN108427698A (zh) 预测模型的更新装置、方法及计算机可读存储介质
CN109074368A (zh) 使用异构社交网络的图形框架
CN108236785A (zh) 一种获取对象信息的方法及装置
CN109934247A (zh) 电子装置及其控制方法
CN107280693A (zh) 基于vr交互电子沙盘的心理分析系统及方法
CN109154945A (zh) 基于数据属性的新连接推荐
CN107393502B (zh) 用于多遍渲染的技术
CN109978175A (zh) 用于机器学习模型的并行化坐标下降法
CN108228720A (zh) 识别目标文字内容和原图相关性的方法、系统、装置、终端、及存储介质
CN106707512A (zh) 一种低功耗智能ar系统及智能ar眼镜
CN206584114U (zh) 一种低功耗智能ar装置及智能ar眼镜
CN115588131A (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
TR01 Transfer of patent right

Effective date of registration: 20201208

Address after: Fonda YUNPU Industrial Zone Whampoa District Guangzhou road 510000, Guangdong province No. 6 Room 101

Patentee after: GUANGZHOU ZSROBOT INTELLIGENT EQUIPMENT Co.,Ltd.

Address before: 510000 Panyu District, Guangzhou, Guangzhou University,, West Ring Road, No. 100

Patentee before: GUANGDONG University OF TECHNOLOGY

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230911

Address after: 510530 Room 101, No. 6, Fangda Road, YUNPU Industrial Zone, Huangpu District, Guangzhou, Guangdong

Patentee after: GUANGZHOU ZSROBOT INTELLIGENT EQUIPMENT CO.,LTD.

Patentee after: Wuhan Zhongshe Robot Technology Co.,Ltd.

Address before: Room 101, No.6 Fangda Road, YUNPU Industrial Zone, Huangpu District, Guangzhou City, Guangdong Province 510000

Patentee before: GUANGZHOU ZSROBOT INTELLIGENT EQUIPMENT CO.,LTD.

TR01 Transfer of patent right