CN110850807B - 一种奇异点规避方法、装置、设备及介质 - Google Patents
一种奇异点规避方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN110850807B CN110850807B CN201911226808.0A CN201911226808A CN110850807B CN 110850807 B CN110850807 B CN 110850807B CN 201911226808 A CN201911226808 A CN 201911226808A CN 110850807 B CN110850807 B CN 110850807B
- Authority
- CN
- China
- Prior art keywords
- point
- function
- target
- interpolation point
- motion
- 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
Links
Images
Classifications
-
- 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
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/19—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
-
- 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/35—Nc in input of data, input till input file format
- G05B2219/35349—Display part, programmed locus and tool path, traject, dynamic locus
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明实施例公开了一种奇异点规避方法、装置、设备及介质,所述方法包括:获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划,通过综合考虑候选插补点与奇异区域轴心的距离以及候选插补点的运动速度进行目标插补点的选取,使得插补点的确定更加准确,从而使得机械臂能够平滑稳定的规避奇异点。
Description
技术领域
本发明实施例涉及机器人技术领域,尤其涉及一种奇异点规避方法、装置、设备及介质。
背景技术
在6轴机械臂的研发中,需要将6轴机械臂以壁挂的形式安装在竖直的直线模组上,这种情况下机械臂的运动控制就很大程度会受到肩关节奇异点的影响,可能会导致机械臂在运动过程中丧失了朝某个方向的运动能力。
现有的机械臂奇异点的处理方法有两种:一、阻尼倒数法,通过奇异分离并设置阻尼倒数来帮助机械臂渡过奇异位点,但是在奇异区域附近解算关节角变化时会表现一定的不光滑性,且该方法需要实时读取机械臂关节的角速度和角加速度,对处理器和通讯的实时性有很高要求。二、设定奇异区域,判断路径规划时插补的点是否落在了奇异区域中,如果是则抛弃该点,不是则留下,但是这种方法只能保证不经过奇异区域,有可能会由于机械臂的运动惯性产生较大震颤而导致位姿误差。
发明内容
本发明实施例提供了一种奇异点规避方法、装置、设备及介质,以实现机械臂能够平滑稳定的规避奇异点,保证机械臂的运动准确性。
第一方面,本发明实施例提供了一种奇异点规避方法,包括:
获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
第二方面,本发明实施例还提供了一种奇异点规避装置,包括:
采样点判断模块,用于获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
候选点获取模块,用于若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
代价值计算模块,用于针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
路径重新规划模块,用于根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
第三方面,本发明实施例还提供了一种计算机设备,所述设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如本发明任意实施例所提供的奇异点规避方法。
第四方面,本发明实施例还提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如本发明任意实施例所提供的奇异点规避方法。
本发明实施例通过获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划,通过在规划路径经过奇异区域时,综合考虑候选插补点与奇异区域轴心的距离以及候选插补点的运动速度,从候选插补点中选取一目标插补点,使得插补点的确定更加准确,从而使得机械臂能够平滑稳定的规避奇异点,同时保证了机械臂的运动准确性。
附图说明
图1是本发明实施例一所提供的一种奇异点规避方法的流程图;
图2是本发明实施例二所提供的一种奇异点规避方法的流程图;
图3是本发明实施例三所提供的一种奇异点规避方法的流程示意图;
图4是本发明实施例四所提供的一种奇异点规避装置的结构示意图;
图5是本发明实施例五所提供的计算机设备的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释本发明,而非对本发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本发明相关的部分而非全部结构。
实施例一
图1是本发明实施例一所提供的一种奇异点规避方法的流程图。本实施例可适用于对机械臂进行路径规划时的情形。该方法可以由奇异点规避装置执行,该奇异点规避装置可以采用软件和/或硬件的方式实现,例如,该奇异点规避装置可配置于计算机设备中。如图1所示,所述方法包括:
S110、获取当前规划路径中的采样点,判断采样点是否位于奇异区域内。
在本实施例中,可以通过均匀采点的方式获取当前规划路径中的采样点,或者通过随机采点的方式获取当前规划路径中的采样点。优选的,将当前规划路径进行分段采点,其中,分段规则以及采点数量可以根据实际需求设定,以保证实时性的前提下,采点数量尽量多为最佳标准。
可选的,奇异区域为包括奇异点的区域,具体可以设置为以经过肩关节的垂直线为轴心,设定半径为横截面半径的柱形区域。其中,该柱形区域的横截面半径可根据实际需求设置。可选的,奇异区域的横截面半径可以为200毫米。
一个实施方式中,判断采样点是否位于奇异区域内可以为:获取采样点的位置坐标以及奇异区域的边界位置坐标,根据采样点的位置坐标与奇异区域的边界位置坐标之间的关系判断采样点是否位于奇异区域内。示例性的,若设定奇异区域为由X=(-150,150)Y=(-150,150)Z=(0,700)构成的圆柱型区域,假设获取的采样点的坐标为(15,30,500),则判定-150<15<150,-150<30<150,0<500<700,因此该采样点在奇异区域内,机械臂运动到该点会发生速度和加速度的剧烈变化,应当舍弃该点并设置新的插补点。
在本实施例中,当前规划路径可以为根据机械臂的运动初始位点以及机械臂的运动目标位点规划出的运动路径,其中,机械臂的运动初始位点可以为机械臂的起始位点,也可以为机械臂由起始位点运动至终止位点的运动过程中经过的位点,机械臂的运动目标位点可以为机械臂的终止位点,也可以为机械臂由起始位点运动至终止位点的运动过程中经过的位点。可选的,机械臂的起始位点为执行单次任务过程中机械臂的运动起点,机械臂的终止位点为执行单次任务过程中机械臂的运动终点。
示例性的,假设机械臂执行单次任务时需要由位置A运动至位置B,即机械臂的起始位点为位置A,机械臂的终止位点为位置B,当由位置A运动至位置B时需要顺次经过位置C及位置D时,机械臂的运动初始位点可以为位置A、位置C或位置D。当机械臂的运动初始位点为位置A时,机械臂的运动终止位点可以为位置C、位置D或位置B,当机械臂的运动初始位点为位置C时,机械臂的运动终止位点可以为位置D或位置B,当机械臂的运动初始位点为位置D时,机械臂的运动终止位点可以为位置B。
S120、若采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点。
若采样点位于奇异区域内,则说明当前规划路径经过奇异区域,需要调整当前规划路径,以规避奇异点。在本实施例中,通过重新确定插补点,基于重新确定的插补点重新进行路径规划的方式规避奇异点。可选的,可以预先设定候选插补点区域,当需要重新确定插补点时,从候选插补点区域内选取多个候选插补点,从多个候选插补点中选取出目标插补点,基于目标插补点重新规划路径。在本实施例中,候选插补点区域可以为以经过肩关节的垂直线为轴心的环形柱状区域,且环形柱状区域的内径为奇异区域横截面的直径,环形柱状区域外径可以根据实际需求设置。
可选的,可以在候选插补点区域内进行随机采样,获取随机的多个候选插补点,也可以在候选插补点区域内进行均匀采样,获取均匀分布的多个候选插补点。优选的,所述在候选插补点区域内选取至少一个候选插补点,包括:利用设定函数在所述候选插补点区域内生成均匀分布的候选插补点。为了使确定的目标插补点更加合理,可以利用设定函数在候选插补点区域内生成均匀分布的伪随机点作为候选插补点。可选的,设定函数获取候选插补点的逻辑可以为:分别沿X轴、Y轴和Z轴方向上进行均匀采样,将X轴、Y轴和Z轴方向上获取的均匀采样点组合生成均匀分布的多个候选插补点。
示例性的,假设X轴方向上均匀采样获得的坐标分别为1和2,Y轴方向上均匀采样获得的坐标分别为3和4,Z轴方向上均匀采样获得的坐标分别为5和6,则将X轴、Y轴和Z轴方向上获取的均匀采样点组合得到坐标值(1,3,5)、坐标值(1,3,6)、坐标值(1,4,5)、坐标值(1,4,6)、坐标值(2,3,5)、坐标值(2,3,6)、坐标值(2,4,5)和坐标值(2,4,6),并将上述坐标值对应的坐标点作为候选插补点。
S130、针对每个候选插补点,基于预先设定的代价函数计算候选插补点的代价值,其中,代价函数是基于距离考量函数和运动速度考量函数构建的。
在本实施例中,获取候选插补点后,根据预先设定的代价函数计算各候选插补点的代价值,基于各候选插补点的代价值选取目标插补点。为了解决机械臂以较大速度靠近奇异区域时调整运动方向可能会产生较大震颤从而导致位姿误差的技术问题,本实施例中将候选插补点的运动速度和候选插补点与奇异区域之间的距离共同作为确定目标插补点的关键参数,基于候选插补点的运动速度和候选插补点与奇异区域之间的距离计算候选插补点的代价值。
可选的,距离考量函数用于考量机械臂经过候选插补点时由于候选插补点与奇异区域之间的距离产生的代价值,运动速度考量函数用于考量机械臂经过候选插补点时由于机械臂运动速度产生的代价值。在本实施例中,对距离考量函数以及运动速度考量函数不做限定,只要能保证候选插补点与奇异区域之间的距离越大,距离考量函数输出的距离考量值越大,运动速度越大,运动速度考量函数输出的速度考量值越大即可。
S140、根据候选插补点的代价值确定目标插补点,基于目标插补点重新进行路径规划。
得到候选插补点的代价值后,根据各候选插补点的代价值确定目标插补点,利用预先设定的路径规划算法对初始位点、目标插补点和终止位点进行拟合,得到新的规划路径曲线。
一个实施方式中,可以设定目标插补点的代价值范围,当候选插补点的插补值在预先设定的代价值范围内时,将该候选插补点作为目标插补点。可选的,可以计算每个候选插补点的代价值,从代价值位于预先设定的代价值范围内的候选插补点中随机选取一候选插补点作为目标插补点。可选的,还可以实时根据计算得到的候选插补点的代价值确定目标插补点,示例性的,当计算到存在一候选插补点的代价值在预先设定的代价值范围内时,将其作为目标插补点。
一个实施例中,所述根据所述候选插补点的代价值确定目标插补点,包括:将代价值最小的候选插补点作为所述目标插补点。将代价值最小的候选插补点作为目标插补点可以使得基于目标插补点的路径规划更加合理。示例性的,若候选插补点包括候选插补点A、候选插补点B、候选插补点C和候选插补点D,候选插补点A的代价值为0.022,候选插补点B的代价值为0.023,候选插补点C的代价值为0.021,候选插补点D的代价值为0.024,则选取代价值最小的候选插补点C作为目标插补点。
在本实施例中,得到新的规划路径曲线后,继续以新的规划路径曲线作为当前规划路径,判断新的规划路径曲线是否经过奇异区域,并在新的规划路径经过奇异区域时,使用本实施例提供的奇异点规避方法进行奇异点规避,直到规划出的路径曲线不经过奇异区域为止。
本发明实施例通过获取当前规划路径中的采样点,判断采样点是否位于奇异区域内;若采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;针对每个候选插补点,基于预先设定的代价函数计算候选插补点的代价值,其中,代价函数是基于距离考量函数和运动速度考量函数构建的;根据候选插补点的代价值确定目标插补点,基于目标插补点重新进行路径规划,通过在规划路径经过奇异区域时,综合考虑候选插补点与奇异区域轴心的距离以及候选插补点的运动速度,从候选插补点中选取一目标插补点,使得插补点的确定更加准确,从而使得机械臂能够平滑稳定的规避奇异点,同时保证了机械臂的运动准确性。
实施例二
图2是本发明实施例二所提供的一种奇异点规避方法的流程图。本实施例将代价函数的生成进行了具体化。如图2所示,所述方法包括:
S210、基于考量点与奇异区域轴心的距离生成距离考量函数,基于考量点的运动速度生成运动速度考量函数。
在本实施例中,首先生成距离考量函数和运动速度考量函数,然后基于生成的距离考量函数和运动速度考量函数生成代价函数。
一个实施例中,考虑到考量点与奇异区域之间的距离是非线性的,为了保证距离考量函数的输出也具有非线性特性,可以选取非线性函数作为距离考量函数。可选的,可以将Sigmoid函数作为距离考量函数,将输入的距离值转换为具有曲率的非线性输入以表达更为丰富的代价信息。一个实施例中,考虑到运动速度越缓慢,经过奇异点附近越安全平稳,故而代价越低,但随着运动速度增高时,运动风险会呈指数级增长,可以基于指数函数生成运动速度考量函数。
可选的,距离考量函数可以为:
其中,f(x1)为考量点的距离考量值,d为考量点与奇异区域轴心之间的距离,D为奇异区域的半径,x1=d-D。当d≤D时,表示考量点位于肩关节奇异区域中,机械臂会因为奇异点的影响产生很大的加速度甚至停止损坏,因此将d≤D时的代价函数设置为MAX,即使算法抛弃该点;当d>D时,表示考量点位于肩关节奇异区域外,通过Sigmoid函数进行考量。可选的,考量点与奇异区域轴心之间的距离d可以通过欧式距离计算。
可选的,运动速度考量函数可以为:其中,f(x2)为考量点的运动速度考量值,x2为考量点的运动速度。可选的,可以将机械臂的最大运动速度作为考量点的运动速度。将上述函数作为运动速度考量函数可以保证运动速度越缓慢,运动代价值越低,且随着运动速度的增高,运动代价值呈指数级增长。
S220、基于距离考量函数和运动速度考量函数生成代价函数。
在本实施例中,可以直接将距离考量函数和运动速度考量函数进行叠加,得到代价函数。以上述距离考量函数和运动速度考量函数为例,将其进行叠加,得到的代价函数为:
其中,各参数的含义可参见上述实施例。
一个实施例中,所述基于所述距离考量函数和所述运动速度考量函数生成所述代价函数,包括:分别为所述距离考量函数以及所述运动速度考量函数添加权重值,将所述距离考量函数和所述运动速度考量函数加权叠加,生成所述代价函数。
可选的,考虑到距离考量值和速度考量值对机械臂运动代价的影响程度不同,可以分别为距离考量函数和运动速度考量函数添加权重值,将距离考量函数和运动速度考量函数加权叠加,得到代价函数。其中,距离考量值对机械臂运动代价的影响较速度考量值对机械臂运动代价的影响较大,因此可以设置距离考量函数的权重大于运动速度考量函数的权重。
示例性的,可以设置距离考量函数的权重为0.7,设置运动速度考量函数的权重为0.3,以上述距离考量函数和运动速度考量函数为例,将其加权叠加,得到的代价函数为:
其中,各参数的含义可参见上述实施例。
在上述方案的基础上,还包括:基于考量点的运动时间生成运动时间考量函数;相应的,所述基于所述距离考量函数和所述运动速度考量函数生成所述代价函数,包括:将所述距离考量函数、所述运动速度考量函数和所述运动时间考量函数叠加,生成所述代价函数。
可选的,为了保证机械臂运动的实时性,还可以将考量点的运动时间作为计算考量点代价值的参数之一。在本实施例中,对运动时间考量函数不做限制,只要能保证考量点的运动时间越长,运动时间考量函数输出的时间考量值越大即可。示例性的,运动时间考量函数可以为f(x3)=kx3,其中,f(x3)为考量点的时间考量值,k为恒定系数,x3为考量点的运动时间。
一个实施例中,可以将距离考量函数、运动速度考量函数和运动时间考量函数直接叠加,生成代价函数。示例性的,以上述距离考量函数、运动速度考量函数和运动时间考量函数为例,将其进行叠加,得到的代价函数为:
其中,各参数的含义可参见上述实施例。
一个实施例中,可以将距离考量函数、运动速度考量函数和运动时间考量函数加权叠加,生成代价函数。其中,各考量函数的权重值可以根据各考量值对机械臂运动代价的影响确定。一般的,距离考量值、运动速度考量值和运动时间考量值对机械臂运动代价的影响依次降低,可以设置距离考量值的权重高大于运动速度考量值的权重,运动速度考量值的权重大于运动时间考量值的权重。示例性的,可以设置距离考量函数的权重为0.6,设置运动速度考量函数的权重为0.3,设置运动时间考量函数的权重为0.1,以上述距离考量函数、运动速度考量函数和运动时间考量函数为例,将其加权叠加,得到的代价函数为:
其中,各参数的含义可参见上述实施例。
S230、获取当前规划路径中的采样点,判断采样点是否位于奇异区域内。
S240、若采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点。
S250、针对每个候选插补点,基于预先设定的代价函数计算候选插补点的代价值,其中,代价函数是基于距离考量函数和运动速度考量函数构建的。
S260、根据候选插补点的代价值确定目标插补点,基于目标插补点重新进行路径规划。
本发明实施例在上述实施例的基础上添加了生成代价函数的操作,通过基于考量点与奇异区域轴心的距离生成距离考量函数,基于考量点的运动速度生成运动速度考量函数,基于距离考量函数和运动速度考量函数生成代价函数,使得基于代价函数对候选插补点的代价值计算更加合理,进而使得目标插补点的确认更加准确。
实施例三
图3是本发明实施例三所提供的一种奇异点规避方法的流程示意图。本实施例在上述实施例的基础上进行了进一步优化。如图3所示,所述方法包括:
S310、获取初始运动参数以及目标运动参数,基于初始运动参数以及目标运动参数进行路径规划,得到当前规划路径。
当机械臂需要执行任务时,根据机械臂当前位点以及任务目标位点进行拟合,得到当前规划路径。在本实施例中,初始运动参数可以为机械臂运动初始点的初始位置值、初始姿态值以及初始关节角,目标运动参数可以为机械臂运动目标点的目标位置值、目标姿态值,也可以为机械臂的运动位置偏移量以及运动姿态偏移量。当目标运动参数为机械臂运动位置偏移量以及运动姿态偏移量时,可以根据机械臂的初始位置值以及运动位置偏移量确定机械臂的运动目标点的目标位置值,根据机械臂的初始姿态值以及运动姿态偏移量确定机械臂的运动目标点的目标姿态值。
在本发明的一种实施方式中,所述初始运动参数包括初始点的初始关节角,所述目标运动参数包括目标点的目标位姿参数,所述基于所述初始运动参数以及所述目标运动参数进行路径规划,包括:基于所述目标点的目标位姿参数确定所述目标点的目标关节角;基于所述初始点关节角、所述目标点关节角以及运动约束条件进行路径规划。
在本实施例中,机械臂的目标位姿参数包括机械臂的目标点的目标位置值以及目标姿态值,在确定机械臂的目标位置值和目标姿态值后,根据机械臂逆解算法(代数法或几何法),将机械臂的初始关节角以及目标关节角作为输入,获得机械臂逆解算法输出的目标关节角。可选的,当机械臂为6轴机械臂时,使用6轴机械臂算法计算机械臂的目标关节角。
确定机械臂的目标关节角后,根据机械臂的初始关节角、目标关节角以及机械臂的运动条件进行路径规划,得到当前规划路径。可选的,可以通过路径规划算法拟合出当前规划路径。示例性的,可以通过五次多项式拟合、三次多项式拟合、抛物线拟合或笛卡尔空间规划算法拟合出当前规划路径。
一个实施例中,所述基于所述初始点关节角、所述目标点关节角以及运动约束条件进行路径规划,包括:将所述初始点关节角和所述目标点关节角作为输入,根据五次多项式算法基于预先设定的运动约束条件进行路径规划。可选的,可以添加边界条件,将初始关节角和目标关节角用五次多项式进行拟合,得到当前规划路径。使用五次多项式拟合可以使拟合出的路径曲线更加光滑。
S320、获取当前规划路径中的采样点,判断采样点是否位于奇异区域内。
S330、若采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点。
S340、针对每个候选插补点,基于预先设定的代价函数计算候选插补点的代价值,其中,代价函数是基于距离考量函数和运动速度考量函数构建的。
S350、根据候选插补点的代价值确定目标插补点,基于目标插补点重新进行路径规划。
本发明实施例在上述实施例的基础上添加了拟合路径曲线的操作,通过获取初始运动参数以及目标运动参数,基于初始运动参数以及目标运动参数进行路径规划,得到当前规划路径,使得当前规划路径更加平滑。
实施例四
图4是本发明实施例四所提供的一种奇异点规避装置的结构示意图。该奇异点规避装置可以采用软件和/或硬件的方式实现,例如该奇异点规避装置可以配置于计算机设备中。如图4所示,所述装置包括采样点判断模块410、候选点获取模块420、代价值计算模块430和路径重新规划模块440,其中:
采样点判断模块410,用于获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
候选点获取模块420,用于若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
代价值计算模块430,用于针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
路径重新规划模块440,用于根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
本发明实施例通过采样点判断模块获取当前规划路径中的采样点,判断采样点是否位于奇异区域内;候选点获取模块若采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;代价值计算模块针对每个候选插补点,基于预先设定的代价函数计算候选插补点的代价值,其中,代价函数是基于距离考量函数和运动速度考量函数构建的;路径重新规划模块根据候选插补点的代价值确定目标插补点,基于目标插补点重新进行路径规划,通过在规划路径经过奇异区域时,综合考虑候选插补点与奇异区域轴心的距离以及候选插补点的运动速度,从候选插补点中选取一目标插补点,使得插补点的确定更加准确,从而使得机械臂能够平滑稳定的规避奇异点,同时保证了机械臂的运动准确性。
可选的,在上述方案的基础上,所述装置还包括:
考量函数生成模块,用于基于考量点与奇异区域轴心的距离生成距离考量函数,基于考量点的运动速度生成运动速度考量函数;
代价函数生成模块,用于基于所述距离考量函数和所述运动速度考量函数生成所述代价函数。
可选的,在上述方案的基础上,所述代价函数生成模块具体用于:
分别为所述距离考量函数以及所述运动速度考量函数添加权重值,将所述距离考量函数和所述运动速度考量函数加权叠加,生成所述代价函数。
可选的,在上述方案的基础上,所述考量函数生成模块还用于:
基于考量点的运动时间生成运动时间考量函数;
相应的,所述代价函数生成模块还用于:
将所述距离考量函数、所述运动速度考量函数和所述运动时间考量函数叠加,生成所述代价函数。
可选的,在上述方案的基础上,所述装置还包括:
路径规划模块,用于在获取当前规划路径中的采样点之前,获取初始运动参数以及目标运动参数,基于所述初始运动参数以及所述目标运动参数进行路径规划,得到所述当前规划路径。
可选的,在上述方案的基础上,所述初始运动参数包括初始点的初始关节角,所述目标运动参数包括目标点的目标位姿参数,所述路径规划模块具体用于:
基于所述目标点的目标位姿参数确定所述目标点的目标关节角;
基于所述初始点关节角、所述目标点关节角以及运动约束条件进行路径规划。
可选的,在上述方案的基础上,所述路径规划模块具体用于:
将所述初始点关节角和所述目标点关节角作为输入,根据五次多项式算法基于预先设定的运动约束条件进行路径规划。
可选的,在上述方案的基础上,所述候选点获取模块420具体用于:
利用设定函数在所述候选插补点区域内生成均匀分布的候选插补点。
可选的,在上述方案的基础上,所述路径重新规划模块430具体用于:
将代价值最小的候选插补点作为所述目标插补点。
本发明实施例所提供的奇异点规避装置可执行任意实施例所提供的奇异点规避方法,具备执行方法相应的功能模块和有益效果。
实施例五
图5是本发明实施例五所提供的计算机设备的结构示意图。图5示出了适于用来实现本发明实施方式的示例性计算机设备512的框图。图5显示的计算机设备512仅仅是一个示例,不应对本发明实施例的功能和使用范围带来任何限制。
如图5所示,计算机设备512以通用计算设备的形式表现。计算机设备512的组件可以包括但不限于:一个或者多个处理器516,系统存储器528,连接不同系统组件(包括系统存储器528和处理器516)的总线518。
总线518表示几类总线结构中的一种或多种,包括存储器总线或者存储器控制器,外围总线,图形加速端口,处理器516或者使用多种总线结构中的任意总线结构的局域总线。举例来说,这些体系结构包括但不限于工业标准体系结构(ISA)总线,微通道体系结构(MAC)总线,增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。
计算机设备512典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机设备512访问的可用介质,包括易失性和非易失性介质,可移动的和不可移动的介质。
系统存储器528可以包括易失性存储器形式的计算机系统可读介质,例如随机存取存储器(RAM)530和/或高速缓存存储器532。计算机设备512可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例,存储装置534可以用于读写不可移动的、非易失性磁介质(图5未显示,通常称为“硬盘驱动器”)。尽管图5中未示出,可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器,以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下,每个驱动器可以通过一个或者多个数据介质接口与总线518相连。存储器528可以包括至少一个程序产品,该程序产品具有一组(例如至少一个)程序模块,这些程序模块被配置以执行本发明各实施例的功能。
具有一组(至少一个)程序模块542的程序/实用工具540,可以存储在例如存储器528中,这样的程序模块542包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据,这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块542通常执行本发明所描述的实施例中的功能和/或方法。
计算机设备512也可以与一个或多个外部设备514(例如键盘、指向设备、显示器524等)通信,还可与一个或者多个使得用户能与该计算机设备512交互的设备通信,和/或与使得该计算机设备512能与一个或多个其它计算设备进行通信的任何设备(例如网卡,调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口522进行。并且,计算机设备512还可以通过网络适配器520与一个或者多个网络(例如局域网(LAN),广域网(WAN)和/或公共网络,例如因特网)通信。如图所示,网络适配器520通过总线518与计算机设备512的其它模块通信。应当明白,尽管图中未示出,可以结合计算机设备512使用其它硬件和/或软件模块,包括但不限于:微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。
处理器516通过运行存储在系统存储器528中的程序,从而执行各种功能应用以及数据处理,例如实现本发明实施例所提供的奇异点规避方法,该方法包括:
获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
当然,本领域技术人员可以理解,处理器还可以实现本发明任意实施例所提供的奇异点规避方法的技术方案。
实施例六
本发明实施例六还提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如本发明实施例所提供的奇异点规避方法,该方法包括:
获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
当然,本发明实施例所提供的一种计算机可读存储介质,其上存储的计算机程序不限于如上所述的方法操作,还可以执行本发明任意实施例所提供的奇异点规避方法中的相关操作。
本发明实施例的计算机存储介质,可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括——但不限于无线、电线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码,所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
注意,上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解,本发明不限于这里所述的特定实施例,对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此,虽然通过以上实施例对本发明进行了较为详细的说明,但是本发明不仅仅限于以上实施例,在不脱离本发明构思的情况下,还可以包括更多其他等效实施例,而本发明的范围由所附的权利要求范围决定。
Claims (12)
1.一种奇异点规避方法,其特征在于,包括:
获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
2.根据权利要求1所述的方法,其特征在于,还包括:
基于考量点与奇异区域轴心的距离生成距离考量函数,基于考量点的运动速度生成运动速度考量函数;
基于所述距离考量函数和所述运动速度考量函数生成所述代价函数。
3.根据权利要求2所述的方法,其特征在于,所述基于所述距离考量函数和所述运动速度考量函数生成所述代价函数,包括:
分别为所述距离考量函数以及所述运动速度考量函数添加权重值,将所述距离考量函数和所述运动速度考量函数加权叠加,生成所述代价函数。
4.根据权利要求2所述的方法,其特征在于,还包括:
基于考量点的运动时间生成运动时间考量函数;
相应的,所述基于所述距离考量函数和所述运动速度考量函数生成所述代价函数,包括:
将所述距离考量函数、所述运动速度考量函数和所述运动时间考量函数叠加,生成所述代价函数。
5.根据权利要求1所述的方法,其特征在于,在获取当前规划路径中的采样点之前,还包括:
获取初始运动参数以及目标运动参数,基于所述初始运动参数以及所述目标运动参数进行路径规划,得到所述当前规划路径。
6.根据权利要求5所述的方法,其特征在于,所述初始运动参数包括初始点的初始关节角,所述目标运动参数包括目标点的目标位姿参数,所述基于所述初始运动参数以及所述目标运动参数进行路径规划,包括:
基于所述目标点的目标位姿参数确定所述目标点的目标关节角;
基于所述初始点关节角、所述目标点关节角以及运动约束条件进行路径规划。
7.根据权利要求6所述的方法,其特征在于,所述基于所述初始点关节角、所述目标点关节角以及运动约束条件进行路径规划,包括:
将所述初始点关节角和所述目标点关节角作为输入,根据五次多项式算法基于预先设定的运动约束条件进行路径规划。
8.根据权利要求1所述的方法,其特征在于,所述在候选插补点区域内选取至少一个候选插补点,包括:
利用设定函数在所述候选插补点区域内生成均匀分布的候选插补点。
9.根据权利要求1所述的方法,其特征在于,所述根据所述候选插补点的代价值确定目标插补点,包括:
将代价值最小的候选插补点作为所述目标插补点。
10.一种奇异点规避装置,其特征在于,包括:
采样点判断模块,用于获取当前规划路径中的采样点,判断所述采样点是否位于奇异区域内;
候选点获取模块,用于若所述采样点位于奇异区域内,则在候选插补点区域内选取至少一个候选插补点;
代价值计算模块,用于针对每个所述候选插补点,基于预先设定的代价函数计算所述候选插补点的代价值,其中,所述代价函数是基于距离考量函数和运动速度考量函数构建的;
路径重新规划模块,用于根据所述候选插补点的代价值确定目标插补点,基于所述目标插补点重新进行路径规划。
11.一种计算机设备,其特征在于,所述设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-9中任一所述的奇异点规避方法。
12.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-9中任一所述的奇异点规避方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911226808.0A CN110850807B (zh) | 2019-12-04 | 2019-12-04 | 一种奇异点规避方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911226808.0A CN110850807B (zh) | 2019-12-04 | 2019-12-04 | 一种奇异点规避方法、装置、设备及介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110850807A CN110850807A (zh) | 2020-02-28 |
CN110850807B true CN110850807B (zh) | 2021-04-27 |
Family
ID=69607502
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911226808.0A Active CN110850807B (zh) | 2019-12-04 | 2019-12-04 | 一种奇异点规避方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110850807B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11559893B2 (en) * | 2020-04-02 | 2023-01-24 | Intrinsic Innovation Llc | Robot control for avoiding singular configurations |
CN111590592A (zh) * | 2020-06-05 | 2020-08-28 | 中科新松有限公司 | 机械臂的控制方法及装置、电子设备、存储介质 |
CN112068544B (zh) * | 2020-07-20 | 2024-06-04 | 上海擎朗智能科技有限公司 | 一种自主移动装置的调度方法、装置、设备及存储介质 |
CN118058844A (zh) * | 2020-09-25 | 2024-05-24 | 武汉联影智融医疗科技有限公司 | 手术机器人控制方法、计算机设备及手术机器人系统 |
CN112589797B (zh) * | 2020-12-11 | 2022-04-15 | 中国科学院合肥物质科学研究院 | 一种非球形手腕机械臂奇异点规避方法和系统 |
CN113561175B (zh) * | 2021-07-16 | 2023-04-11 | 珠海格力智能装备有限公司 | 机械臂的路径规划方法、装置、计算机设备和存储介质 |
CN115922684A (zh) * | 2021-08-19 | 2023-04-07 | 广东博智林机器人有限公司 | 用于六轴机器人的奇异点处理方法、装置、设备及介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS61278906A (ja) * | 1985-06-03 | 1986-12-09 | Hitachi Ltd | ロボツトの制御方法 |
US6838904B1 (en) * | 2002-08-13 | 2005-01-04 | Lattice Semiconductor Corporation | Enhanced CPLD macrocell module having selectable bypass of steering-based resource allocation |
CN103512580A (zh) * | 2012-06-21 | 2014-01-15 | 日电(中国)有限公司 | 一种电动汽车路径规划方法及装置 |
CN104635619A (zh) * | 2013-11-12 | 2015-05-20 | 沈阳高精数控技术有限公司 | 基于刀具矢量插补的五轴数控加工方法 |
JP5727116B1 (ja) * | 2015-03-06 | 2015-06-03 | Fuller株式会社 | アンドロイド端末において特定アプリがosから取得する情報をもとにユーザーが起動したアプリの時系列を検知するコンピューティング |
CN205614678U (zh) * | 2016-01-25 | 2016-10-05 | 珠海格力电器股份有限公司 | 一种奇异点区域减速保护系统和工业机器人 |
CN107116542A (zh) * | 2017-06-28 | 2017-09-01 | 华中科技大学 | 一种六关节工业机器人通过姿态奇点的控制方法及系统 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102646148B (zh) * | 2012-04-25 | 2014-09-24 | 浙江大学 | 仿人机器人机械臂避碰运动轨迹规划方法 |
CN103926873B (zh) * | 2014-03-04 | 2017-02-08 | 浙江大学 | 一种在路径规划阶段消除五轴奇异问题的方法 |
CN107127751B (zh) * | 2017-03-21 | 2019-08-16 | 宁波韦尔德斯凯勒智能科技有限公司 | 关节型机械臂驱控一体化控制系统及控制方法 |
-
2019
- 2019-12-04 CN CN201911226808.0A patent/CN110850807B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS61278906A (ja) * | 1985-06-03 | 1986-12-09 | Hitachi Ltd | ロボツトの制御方法 |
US6838904B1 (en) * | 2002-08-13 | 2005-01-04 | Lattice Semiconductor Corporation | Enhanced CPLD macrocell module having selectable bypass of steering-based resource allocation |
CN103512580A (zh) * | 2012-06-21 | 2014-01-15 | 日电(中国)有限公司 | 一种电动汽车路径规划方法及装置 |
CN104635619A (zh) * | 2013-11-12 | 2015-05-20 | 沈阳高精数控技术有限公司 | 基于刀具矢量插补的五轴数控加工方法 |
JP5727116B1 (ja) * | 2015-03-06 | 2015-06-03 | Fuller株式会社 | アンドロイド端末において特定アプリがosから取得する情報をもとにユーザーが起動したアプリの時系列を検知するコンピューティング |
CN205614678U (zh) * | 2016-01-25 | 2016-10-05 | 珠海格力电器股份有限公司 | 一种奇异点区域减速保护系统和工业机器人 |
CN107116542A (zh) * | 2017-06-28 | 2017-09-01 | 华中科技大学 | 一种六关节工业机器人通过姿态奇点的控制方法及系统 |
Non-Patent Citations (1)
Title |
---|
机械臂及其路径规划研究;段梅;《中国优秀硕士学位论文全文数据库 农业科技辑》;20091115(第11期);第D049-152页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110850807A (zh) | 2020-02-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110850807B (zh) | 一种奇异点规避方法、装置、设备及介质 | |
US12038753B2 (en) | Speed planning method and apparatus, electronic device and storage medium | |
CN113256712B (zh) | 定位方法、装置、电子设备和存储介质 | |
EP3559865B1 (en) | Data-efficient reinforcement learning for continuous control tasks | |
CN111209978B (zh) | 三维视觉重定位方法、装置及计算设备、存储介质 | |
CN110471409B (zh) | 机器人巡检方法、装置、计算机可读存储介质及机器人 | |
JP7228623B2 (ja) | 障害物検出方法、装置、設備、記憶媒体、及びプログラム | |
CN114348026B (zh) | 一种车辆控制方法、装置、设备以及存储介质 | |
CN113776520B (zh) | 地图构建、使用方法、装置、机器人和介质 | |
CN112947523B (zh) | 基于非奇异快速终端滑模控制的角度约束制导方法及系统 | |
CN113199481B (zh) | 机器人运动控制方法、装置、电子设备及介质 | |
CN109933058B (zh) | 用于确定可移动设备的移动误差的方法、装置、服务器及存储介质 | |
CN113009908B (zh) | 一种无人设备的运动控制方法、装置、设备及存储介质 | |
CN116993832B (zh) | 一种基于cnn的飞行目标的位置预测方法及装置 | |
CN111338339B (zh) | 轨迹规划方法、装置、电子设备及计算机可读介质 | |
CN117140517A (zh) | 一种高精度的机械臂自动手眼标定方法及系统 | |
CN112598784A (zh) | 轮廓扫描结果的调整方法、装置、电子设备及存储介质 | |
US20230123671A1 (en) | Localization and mapping | |
CN116358525A (zh) | 基于激光雷达的建图与定位方法、系统及工程车辆 | |
CN112612277B (zh) | 车辆轨迹规划方法、装置、设备和计算机可读介质 | |
CN113093715B (zh) | 无人设备的运动控制方法、装置、设备及存储介质 | |
US10620718B2 (en) | Device selection in three-dimensional environments | |
CN112904367B (zh) | 应用于扫地机重定位时的位置打分方法、装置和电子设备 | |
CN114964204A (zh) | 地图构建方法、地图使用方法、装置、设备和存储介质 | |
CN113447913A (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 |