CN103991074B - 一种基于粒子滤波算法的空间机械臂控制方法及系统 - Google Patents
一种基于粒子滤波算法的空间机械臂控制方法及系统 Download PDFInfo
- Publication number
- CN103991074B CN103991074B CN201410217148.0A CN201410217148A CN103991074B CN 103991074 B CN103991074 B CN 103991074B CN 201410217148 A CN201410217148 A CN 201410217148A CN 103991074 B CN103991074 B CN 103991074B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- space
- space manipulator
- manipulator
- 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.)
- Expired - Fee Related
Links
- 239000002245 particle Substances 0.000 title claims abstract description 83
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 28
- 238000001514 detection method Methods 0.000 claims description 21
- 238000001914 filtration Methods 0.000 claims description 6
- 230000000737 periodic effect Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 238000011156 evaluation Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 4
- 238000005315 distribution function Methods 0.000 description 3
- 238000012423 maintenance Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013398 bayesian method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000005486 microgravity Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
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
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Feedback Control In General (AREA)
- Manipulator (AREA)
Abstract
本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法,包括:任务的第一控制周期结束后,使用粒子滤波算法获得空间机械臂末端的空间位置误差概率分布;依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;依据所述使用可靠度和预设的可靠度阈值,获得调整量;依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的空间位置信息;本发明实施例还提供一种基于粒子滤波算法的空间机械臂控制系统。根据本发明实施例提供的技术方案,可以保证在空间机械臂末端空间位置误差观测噪声为任意分布的条件下提高空间机械臂使用可靠性。
Description
【技术领域】
本发明涉及自动化控制技术,尤其涉及一种基于粒子滤波算法的空间机械臂控制方法及系统。
【背景技术】
目前,空间站设备的组装、回收、维修及舱外活动都需要依赖空间机械臂来完成,由于空间机械臂处于微重力、高低温交变、高辐射等恶劣的太空环境中,维修和维护十分困难,因此空间机械臂的可靠性成为世界航天技术的主要难点。空间机械臂的可靠性包括固有可靠性和使用可靠性,其中,固有可靠性在空间机械臂的设计和生产过程中就已经确定,因此提高空间机械臂的使用可靠性是一个经济、有效地提高空间机械臂的任务完成质量的方法。
空间机械臂都是按照预先设置好的任务规划,执行任务,需要在预设的时间到达预设的空间位置,然而,由于空间机械臂关节上的齿轮间隙或者传感器噪声等原因,空间机械臂在执行任务后,实际到达的空间位置与预期的空间位置之间存在一定的误差,如果空间机械臂继续执行任务,任务的实际执行结果与预期的执行结果之间的误差将越来越大,因此空间机械臂的使用可靠性较低。
由于实际的空间机械臂系统可能存在各种内部结构或环境上的影响,空间机械臂的位置误差的实际分布情况可能不会为简单的高斯分布,而可能是各种难以进行解析描述的情况,这时若使用经典的贝叶斯理论来处理时就会出现如难以进行近似计算以及计算结果与实际有很大偏差等问题。
【发明内容】
有鉴于此,本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法及系统,以实现提高空间机械臂的使用可靠性。
本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法,包括:
任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布;
依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;
依据所述使用可靠度和预设的可靠度阈值,获得调整量;
依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。
上述方法中,获得空间机械臂末端的空间位置误差概率分布的方法为:根据粒子滤波算法,获得表示空间机械臂末端空间位置误差可能取值的粒子,并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值,然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布;使用上述方法获得的空间机械臂末端的空间位置误差概率分布函数为:
其中,θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差;i为大于或者等于1的整数;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;θi|yi为表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差;f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数;Ns为粒子总数,是整数值;为第i控制周期的样本集即粒子集;为粒子所对应的权值;其中,利用如下公式获得δ(x):
这里,x表示δ(x)函数中的变量;
其中,利用如下方法获得
上式中,i为大于或者等于1的整数;为第i-1控制周期的粒子;xi-1为第i-1控制周期空间机械臂末端空间位置误差的调整量;ei为一随机数,满足在0和之间的均匀分布;其中,利用如下方法获得
其中,Ns为粒子总数;yi为第i控制周期空间机械臂末端空间位置误差的观测值;为对应的粒子;h(x)为预设的权值函数,其中x表示变量;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;表示自变量等于检测误差yi与粒子的差值时,函数h(x)的取值。
上述方法中,所述依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度,包括:
依据所述粒子分布所描述的空间机械臂末端的空间位置误差概率密度函数f(θi|yi),并利用如下公式,获得空间机械臂的使用可靠度:
其中,P{θi1≤θi≤θi2}为空间机械臂的使用可靠度;θi1为预设的第一阈值;θi2为预设的第二阈值,且θi2≥θi1。
上述方法中,所述依据所述使用可靠度和预设的可靠度阈值,获得调整量,包括:
比较所述使用可靠度与所述可靠度阈值的大小;
若所述使用可靠度小于所述可靠度阈值,依据所述空间机械臂末端的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量。
上述方法中,所述依据所述空间机械臂的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量,包括:
利用如下公式获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μi:
其中, 为第i控制周期的粒子, 为粒子所对应的权值,Ns为粒子总数。
依据所述空间机械臂的空间位置误差概率分布的期望,并利用如下公式,获得所述空间机械臂末端的调整量:
其中,xi(μi)为空间机械臂末端的调整量;ki为调整系数;Li(μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;Li((1+ki)μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+ki)μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;c为归一化的调整成本;其中,调整系数ki由以下公式得出:
其中,μi为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望;α为控制系统噪声的期望,通过对噪声样本统计获得,为已知量。
上述方法中,所述第一目标空间位置信息包括以下信息中至少一个:空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角;
所述依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位值信息,包括:
依据所述第一目标空间位置信息与所述调整量的和值,获得所述第二目标空间位置信息。
本发明实施例还提供了一种基于粒子滤波算法的空间机械臂控制系统,包括:
处理单元,用于在任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布;
评估单元,用于依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;
生成单元,用于依据所述使用可靠度和预设的可靠度阈值,获得调整量;
控制单元,用于依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。
由以上技术方案可以看出,本发明实施例具有以下有益效果:
本发明实施例的技术方案中,依据空间位置误差概率分布,使用粒子滤波算法,获得空间机械臂的使用可靠度,进而依据使用可靠度获得调整量,依据调整量对下一个控制周期的目标空间位置进行调整,从而可以控制空间机械臂在下一个控制周期的任务执行,因此能够实现在空间机械臂的任务执行结果出现误差的情况下,及时地对空间机械臂的目标空间位置进行调整,以避免空间机械臂的任务执行结果与预期结果之间的误差越来越大,可以在空间机械臂末端空间位置误差观测噪声为任意分布的条件下提高空间机械臂的使用可靠性,提高任务执行成功率,保证空间机械臂的任务执行成功率高。
【附图说明】
为了更清楚地说明本发明实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其它的附图。
图1是本发明实施例所提供的基于粒子滤波算法的空间机械臂控制方法的流程示意图;
图2是本发明实施例提供的目标空间位置观测噪声分布曲线;
图3是利用本发明实施例提供的方法对目标空间位置信息进行调整的过程示意图;
图4是利用本发明实施例所提供的方法对目标空间位置信息进行调整的过程中使用可靠度的曲线图;
图5是基于现有技术的空间机械臂的控制过程的误差示意图;
图6是本发明实施例的执行结果的精度分布示意图;
图7是本发明实施例所提供的基于粒子滤波算法的空间机械臂控制系统的结构示意图。
【具体实施方式】
为了更好的理解本发明的技术方案,下面结合附图对本发明实施例进行详细描述。
应当明确,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
空间机械臂的控制主要包括三个部分,即任务规划、路径规划和运动控制。其中,任务规划指的是输入空间机械臂需要执行的任务的任务规划约束,如任务目标(如任务的起始位置、空间机械臂末端的空间位置)、空间机械臂的约束(如空间机械臂中每个臂杆的长度、每个关节的角度)、环境障碍(如障碍的尺寸和位置)等;然后,依据任务规划约束,对一个完整的任务进行任务拆分,从而获得至少两个子任务;然后,依据任务规划约束,确定每个子任务的中间点;每个子任务的中间点可以是一个或多个。依据中间点和预设的路径规划约束,为空间机械臂生成每两个中间点之间的执行路径,所述执行路径为空间机械臂在两个中间点之间的移动路径,依据该执行路径,空间机械臂就可以按照规划的执行路径进行移动,以完成指定的任务。
使用可靠性是指空间机械臂执行规定的任务时,采用规定的控制方法成功完成任务的能力。使用可靠性通过使用可靠度进行量化,使用可靠度是采用规定的控制方法执行规定的任务,成功完成任务的概率。任务成功的标准由预设的任务执行的精度决定,只要在任务执行的精度范围内,就认为任务成功。执行相同的任务,控制方法不同,所表现出的使用可靠性不同。
本发明实施例中,预先设置至少两个控制周期,在控制空间机械臂执行上述任务过程中,在每个控制周期结束后,都获得空间机械臂的使用可靠度,然后依据使用可靠度获得调整量,依据调整量对下一个控制周期结束后空间机械臂末端的空间位置进行调整,以减小空间机械臂末端的空间位置的误差。为了方便说明,本发明实施例以第一控制周期和第二控制周期为例进行说明,即任务的第一控制周期结束后,可以获得空间机械臂的使用可靠度,依据使用可靠度,获得调整量,然后依据调整量对第二控制周期的目标空间位置信息进行调整。
本发明实施例给出一种基于粒子滤波算法的空间机械臂控制方法,请参考图1,其为本发明实施例所提供的基于粒子滤波算法的空间机械臂控制方法的流程示意图,如图1所示,该方法包括以下步骤:
步骤101,任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布。
具体的,首先,针对空间机械臂闭环控制系统,建立以下状态空间方程模型,用以描述空间机械臂的控制过程:
其中,θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差,即空间机械臂末端的空间位置的真实误差;i为大于或者等于1的整数,yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差,即检测到的带有噪声的空间位置误差;xi-1为第i-1个控制周期结束后空间机械臂的调整量;vi为噪声,vi服从分布Ф,其期望为α,其方差为β,概率密度函数曲线如图2所示。
由于空间机械臂的控制系统中各控制环节存在扰动和噪声,若空间机械臂在执行任务的过程中,空间机械臂末端的实际空间位置处于预设的精度范围内,过度地控制调整将会带来更多的扰动,将由此带来的精度损失定义为调整成本。依据空间机械臂末端的空间位置的实际误差和调整成本,构建如下绩效函数,用以描述空间机械臂执行任务的质量:
其中,L表示空间机械臂执行任务的质量,E[·]表示数学期望运算,yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差,c为归一化的调整成本;xi-1为第i-1个控制周期结束后获得的调整量,T为矩阵yi的转置。
其中,利用如下公式获得δ(x):
这里,x表示δ(x)函数中的变量。
因此,在同时考虑空间机械臂末端的空间位置的检测误差和调整成本的情况下,空间机械臂的最优控制策略为:使空间机械臂按照序列{xi}进行调整,i为大于或者等于1的整数,xi为第i个控制周期结束后获得的调整量。
依据每个控制周期结束后采集的空间机械臂末端的空间位置的检测误差yi,通过粒子滤波算法对控制过程中的空间机械臂末端的空间位置的实际误差θi的分布估计。
其中,采用 和 两个集合来对θi的分布进行描述。其中是用来表示θi可能取值的一组样本数据,称为粒子,Ns为粒子数量;是粒子对应的权值,表示粒子与θi的相关程度,且规定
采用如下的正态分布模型作为空间机械臂末端的空间位置的实际误差θi的先验概率分布:
θ0~N(μ0,Λ0)
其中,θ0为空间机械臂末端的空间位置的实际误差的初始值,μ0为预设的空间机械臂末端的空间位置的实际误差初始值θ0的先验概率分布的期望,Λ0为预设的空间机械臂末端的空间位置的实际误差初始值θ0的先验概率分布的方差。
根据系统状态方程和先验分布实现粒子滤波,其主要步骤包括:
1)初始化,对于第一控制周期,根据θ0的先验概率分布产生数量为Ns的随机量,将这些随机量集合作为初始粒子集并使其对应权值对于第二控制周期及以后的控制周期,根据上一控制周期的粒子 和其权值 利用如下公式获得初始粒子集:
其中,ei为一随机数,满足在0和之间的均匀分布。
2)根据重要性函数更新权值,重要性函数与噪声分布Ф有关,设Ф的概率密度函数为h(ρ),其中ρ为变量,则重要性函数为即
3)权值归一化,即令
根据以上方法得到空间机械臂末端的空间位置的实际误差的概率分布函数为:
其中θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差,θi|yi为空间机械臂末端的空间位置误差概率分布,f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数,为第i控制周期的样本集即粒子集,为粒子所对应的权值权值,且Ns为粒子总数。
其中,利用如下公式获得δ(x):
这里,x表示δ(x)函数中的变量。
步骤102,依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度。
具体的,使用可靠度可以依据第i个控制周期结束后空间机械臂末端的空间位置的实际误差θi的后验概率进行评估,使用可靠度是空间机械臂的使用可靠性的量化,使用可靠度指的是第i个控制周期结束后械臂末端的空间位置的实际误差θi落在预设的精度范围内的概率。
首先,利用如下公式,获得所述空间机械臂末端的空间位置误差概率密度函数f(θi|yi):
其中θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差,θi|yi为空间机械臂末端的空间位置误差概率分布,f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数,为第i控制周期的样本集即粒子集,为粒子所对应的权值权值,且Ns为粒子总数。
然后,依据所述空间机械臂末端的空间位置误差概率密度函数f(θi|yi),并利用如下公式,获得空间机械臂的使用可靠度:
其中,θi1≤θi≤θi2为预设的精度范围,P{θi1≤θi≤θi2}为第i个控制周期结束后空间机械臂的使用可靠度,θi1为精度范围的第一阈值,即精度范围的下限值,θi2为精度范围的第二阈值,即精度范围的上限值,且θi2≥θi1。
步骤103,依据所述使用可靠度和预设的可靠度阈值,获得调整量。
具体的,比较获得的使用可靠度与预设的可靠度阈值的大小;若所述使用可靠度大于或者等于预设的可靠度阈值,则不进行目标空间位置信息的调整,因此,不需要计算调整量,保持空间机械臂当前运行状态继续运行;若所述使用可靠度小于预设的可靠度阈值,则需要依据空间机械臂末端的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量。
本发明实施例中,依据空间机械臂末端的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量的方法包括:
1)将第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μi作为第i个控制周期结束后空间机械臂末端的空间位置的估计值。
即:
其中, 为第i控制周期的粒子, 为粒子所对应的权值,Ns为粒子总数。
2)确定调整量取值:
若为在第i个控制周期结束后空间机械臂末端的空间位置的误差估计值为μi时,对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期最小期望损失。Li(μi)为在第i个控制周期结束后空间机械臂末端空间位置误差的估计值为μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期最小期望损失。为简化计算,设调整量xi=kiμi,ki为一维量。
根据定义有:
根据状态方程
得:
即:
令
可知,当ki=0时,有:
R(μi)=‖μi‖2+μi Tα+αTμi
此时有:
当ki≠0时,有:
相当于求关于ki的二次函数的最小值。
由于‖μi‖2>0,所以最小值在对称轴处,即:
此时有,
即:
所以:
依据所述空间机械臂的空间位置误差概率分布的期望,并利用如下公式,获得所述空间机械臂末端的调整量:
其中,xi(μi)为空间机械臂末端的调整量;ki为调整系数;Li(μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;Li((1+ki)μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+ki)μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;c为归一化的调整成本。
其中,调整系数ki由以下公式得出:
其中,μi为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望;α为观测噪声的期望值,为已知量。
步骤104,依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目的空间位置信息。
具体的,预设的第一目标空间位置信息包括以下信息中至少一个:空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角。预设的第一目标空间位置信息指的是为第i个控制周期空间机械臂执行任务预先规划好的目标空间位置信息,表示在第i个控制周期结束后,空间机械臂应该到达该目标空间位置信息所指示的位置。
本发明实施例中,依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位值信息的方法可以是:依据所述第一目标空间位置信息与所述调整量的和值,获得所述第二目标空间位置信息。
例如,第一目标空间位置信息包括6个维度,其中3个维度表示空间机械臂末端在空间坐标系的坐标值,另3个维度表示空间机械臂末端与空间坐标系中坐标轴的夹角,若第一目标空间位置信息包括空间机械臂末端在空间坐标系的坐标值g1,则可以依据所述空间机械臂末端在空间坐标系的坐标值g1与所述调整量xi(μi)的和值,获得所述第二目标空间位置信息g1′,即:
g1′=g1+xi(μi)
其中,第二目标空间位置信息g1′中空间机械臂末端与空间坐标系中坐标轴的夹角,等于第一目标空间位置信息g1中空间机械臂末端与空间坐标系中坐标轴的夹角,即仅对空间机械臂末端在空间坐标系的坐标值g1进行调整,不调整空间机械臂末端与空间坐标系中坐标轴的夹角。
再例如,若第一目标空间位置信息包括空间机械臂末端与空间坐标系中坐标轴的夹角g2,则可以依据所述空间机械臂末端与空间坐标系中坐标轴的夹角g2与所述调整量xi(μi)的和值,获得所述第二目标空间位置信息g′2,即:
g′2=g2+xi(μi)
其中,第二目标空间位置信息g′2中空间机械臂末端在空间坐标系的坐标值,等于第一目标空间位置信息g2中空间机械臂末端在空间坐标系的坐标值,即仅对空间机械臂末端与空间坐标系中坐标轴的夹角g2进行调整,不调整空间机械臂末端在空间坐标系的坐标值。
再例如,若第一目标空间位置信息中包括空间机械臂末端在空间坐标系的坐标值g1和空间机械臂末端与空间坐标系中坐标轴的夹角g2,则可以依据所述第一目标空间位置与调整量xi(μi)的和值,获得第二目标空间位置信息g′3,即:
g′3=g3+xi(μi)
依据本发明实施例提供的上述方法,对空间机械臂的控制进行了仿真,为一个具有9个控制周期的空间机械臂的空载转移任务,规定任务精度范围是±1mm,精度范围的第一阈值θi1等于-1mm,精度范围的第二阈值θi2等于+1mm,可靠度阈值可以为0.9。设置空间机械臂末端的空间位置的实际误差为7mm,空间机械臂末端的空间位置的实际误差的先验概率服从先验分布θ0~N(0,1)。在每个控制周期结束后获得空间机械臂末端的空间位置的检测误差,引入噪声v0,噪声v0服从分布Ф,其概率密度函数曲线如图2所示。分别使用本发明实施例的技术方案和不使用本发明实施例的技术方案对上述任务进行单次仿真,仿真结果如图3~图5所示。其中图3和图4中的横坐标轴表示控制周期。
请参考图3,其为利用本发明实施例提供的方法对目标空间位置信息进行调整的过程示意图。如图3所示,实线11表示空间机械臂末端的空间位置的实际误差,虚线12表示空间机械臂末端的空间位置的误差的估计值,虚线13表示空间机械臂末端的空间位置的检测误差,空间机械臂末端的空间位置的误差的估计值逐渐向空间机械臂末端的空间位置的实际误差收敛;整个控制过程分别在第1控制周期、第2控制周期和第7控制周期对相应的目的空间位置信息进行了调整;空间机械臂末端的空间位置的实际误差逐渐向0收敛,收敛过程平缓且没有波动。
请参考图4,其为利用本发明实施例所提供的方法对目标空间位置信息进行调整的过程中使用可靠度的曲线图,如图4所示,可以控制方法中依据使用可靠度进行控制的过程中使用可靠度呈现增加趋势,且收敛于1。
请参考图5,其为基于现有技术的空间机械臂的控制过程的误差示意图,如图5中的实线,空间机械臂末端的空间位置的检测误差中存在噪声,即使在每个控制周期都对目的空间位置信息进行调整,如图5中的虚线,空间机械臂末端的空间位置的实际误差仍然持续波动,且不能收敛。
请参考图6,其为本发明实施例的执行结果的精度分布示意图,如图6所示,在重复完成1000次任务后,获得执行结果精度分布,图6中的横坐标轴是精度值,纵坐标轴是执行结果的精度落在横坐标轴对应精度的次数,规定执行结果的精度位于±1.5mm范围内。如图6中实线所示,统计得到使用本发明实施例的技术方案的执行任务的成功概率为73.6%,即利用粒子滤波法对空间机械臂进行控制后,空间机械臂的使用可靠度达到73.6%;如图6中虚线所示,使用贝叶斯法对空间机械臂进行控制,空间机械臂的使用可靠度为35.5%。由图6的两条曲线看出,未使用本发明所述控制方法执行任务会结果产生明显的偏差,使用本发明所述控制方法执行任务的精度更高,有效的提高了空间机械臂的使用可靠性。
本发明实施例进一步给出实现上述方法实施例中各步骤及方法的装置实施例。
请参考图7,其为本发明实施例所提供的基于粒子滤波算法的空间机械臂控制系统的结构示意图。如图所示,该系统包括:
处理单元701,用于在任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布;
评估单元702,用于依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;
生成单元703,用于依据所述使用可靠度和预设的可靠度阈值,获得调整量;
控制单元704,用于依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。
上述系统中,所述处理单元701具体用于:
使用如下方法获得空间机械臂末端的空间位置误差的概率分布:根据粒子滤波算法,获得表示空间机械臂末端空间位置误差可能取值的粒子,并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值,然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布。
使用上述方法获得的空间机械臂末端的空间位置误差概率分布函数为:
其中,θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差;i为大于或者等于1的整数;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;θi|yi为表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差;f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数;Ns为粒子总数,是整数值;为第i控制周期的样本集即粒子集;为粒子所对应的权值。
其中,利用如下公式获得δ(x):
这里,x表示δ(x)函数中的变量。
其中,利用如下方法获得
上式中,i为大于或者等于1的整数;为第i-1控制周期的粒子;xi-1为第i-1控制周期空间机械臂末端空间位置误差的调整量;ei为一随机数,满足在0和 之间的均匀分布。
其中,利用如下方法获得
其中,Ns为粒子总数;yi为第i控制周期空间机械臂末端空间位置误差的观测值;为对应的粒子;h(x)为预设的权值函数,其中x表示变量;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;表示自变量等于检测误差yi与粒子的差值时,函数h(x)的取值。
上述系统中,所述评估单元702具体用于:
利用如下公式,获得所述空间机械臂末端的空间位置误差概率密度函数f(θi|yi):
其中θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差,θi|yi为空间机械臂末端的空间位置误差概率分布,f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数,为第i控制周期的样本集即粒子集,为粒子所对应的权值权值,且Ns为粒子总数。
依据所述空间机械臂末端的空间位置误差概率密度函数f(θi|yi),并利用如下公式,获得空间机械臂的使用可靠度:
其中,P{θi1≤θi≤θi2}为空间机械臂的使用可靠度;θi1为预设的第一阈值;θi2为预设的第二阈值,且θi2≥θi1。
上述系统中,所述生成单元703具体用于:
比较所述使用可靠度与所述可靠度阈值的大小;
若所述使用可靠度小于所述可靠度阈值,依据所述空间机械臂末端的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量。
其中,所述生成单元703具体用于:
依据所述空间机械臂的空间位置误差概率分布获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μi:
然后依据所述空间机械臂的空间位置误差概率分布的期望,并利用如下公式,获得所述空间机械臂末端的调整量:
其中,xi(μi)为空间机械臂末端的调整量;ki为调整系数;Li(μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;Li((1+ki)μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+ki)μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;c为归一化的调整成本。
其中,调整系数ki由以下公式得出:
其中,μi为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望;α为观测噪声的期望值,为已知量。
所述生成单元具体703用于:
上述系统中,所述第一目标空间位置信息包括以下信息中至少一个:空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角;
所述控制单元704具体用于:依据所述第一目标空间位置信息与所述调整量的和值,获得所述第二目标空间位置信息。
本发明实施例的技术方案具有以下有益效果:
本发明实施例的技术方案中,依据空间位置误差概率分布,获得空间机械臂的使用可靠度,进而依据使用可靠度获得调整量,依据调整量对下一个控制周期的目标空间位置进行调整,从而可以控制空间机械臂在下一个控制周期的任务执行,因此能够实现在空间机械臂的任务执行结果出现误差的情况下,及时地对空间机械臂的目标空间位置进行调整,以避免空间机械臂的任务执行结果与预期结果之间的误差越来越大,因此可以提高空间机械臂的使用可靠性,提高任务执行成功率,保证空间机械臂的任务执行成功率高。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明保护的范围之内。
Claims (7)
1.一种基于粒子滤波算法的空间机械臂控制方法,其特征在于,所述方法包括:
任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布;
依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;
依据所述使用可靠度和预设的可靠度阈值,获得调整量;
依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。
2.根据权利要求1所述的方法,其特征在于,任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布,包括:根据粒子滤波算法,获得表示空间机械臂末端空间位置误差可能取值的粒子,并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值,然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布;使用上述方法获得的空间机械臂末端的空间位置误差概率密度函数为:
其中,θi为第i个控制周期结束后空间机械臂末端的空间位置的实际误差;i为大于或者等于1的整数;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;θi|yi为表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差;f(θi|yi)表示空间机械臂末端的空间位置的检测误差为yi时,空间机械臂末端的空间位置的实际误差θi的概率密度函数;Ns为粒子总数,是整数值;为第i控制周期的样本集即粒子集;为粒子所对应的权值;其中,利用如下公式获得δ(x):
这里,x表示δ(x)函数中的变量;
其中,利用如下方法获得
上式中,i为大于或者等于1的整数;为第i-1控制周期的粒子;xi-1为第i-1控制周期空间机械臂末端空间位置误差的调整量;ei为一随机数,满足在0和之间的均匀分布;其中,利用如下方法获得
其中,Ns为粒子总数;yi为第i控制周期空间机械臂末端空间位置误差的观测值;为对应的粒子;h(x)为预设的权值函数,其中x表示变量;yi为第i个控制周期结束后空间机械臂末端的空间位置的检测误差;表示自变量等于检测误差yi与粒子的差值时,函数h(x)的取值。
3.根据权利要求1所述的方法,其特征在于,所述依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度,包括:
依据所述空间机械臂末端的空间位置误差概率密度函数f(θi|yi),并利用如下公式,获得空间机械臂的使用可靠度:
其中,P{θi1≤θi≤θi2}为空间机械臂的使用可靠度;θi1为预设的第一阈值;θi2为预设的第二阈值,且θi2≥θi1。
4.根据权利要求1至3中任一项所述的方法,其特征在于,所述依据所述使用可靠度和预设的可靠度阈值,获得调整量,包括:
比较所述使用可靠度与所述可靠度阈值的大小;
若所述使用可靠度小于所述可靠度阈值,依据所述空间机械臂末端的空间位置误差概率分布的期望,获得所述空间机械臂末端的调整量。
5.根据权利要求4所述的方法,其特征在于,首先依据所述空间机械臂的空间位置误差概率分布获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μi:
然后依据所述空间机械臂的空间位置误差概率分布的期望,并利用如下公式,获得所述空间机械臂末端的调整量:
其中,xi(μi)为空间机械臂末端的调整量;ki为调整系数;Li(μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;Li((1+ki)μi)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+ki)μi时,不对空间机械臂末端的目标空间位置信息进行调整的情况下,第i+1个控制周期的期望损失;c为归一化的调整成本;其中,调整系数ki由以下公式得出:
其中,μi为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望;α为控制系统噪声的期望,通过对噪声样本统计获得,为已知量。
6.根据权利要求1所述的方法,其特征在于,
所述第一目标空间位置信息包括以下信息中至少一个:空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角;
所述依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位值信息,包括:
依据所述第一目标空间位置信息与所述调整量的和值,获得所述第二目标空间位置信息。
7.一种基于粒子滤波算法的空间机械臂控制系统,其特征在于,所述系统包括:
处理单元,用于在任务的第一控制周期结束后,使用粒子滤波算法,获得空间机械臂末端的空间位置误差概率分布;
评估单元,用于依据所述空间位置误差概率分布,获得空间机械臂的使用可靠度;
生成单元,用于依据所述使用可靠度和预设的可靠度阈值,获得调整量;
控制单元,用于依据预设的第一目标空间位置信息和所述调整量,获得第二目标空间位置信息;所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410217148.0A CN103991074B (zh) | 2014-05-21 | 2014-05-21 | 一种基于粒子滤波算法的空间机械臂控制方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410217148.0A CN103991074B (zh) | 2014-05-21 | 2014-05-21 | 一种基于粒子滤波算法的空间机械臂控制方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103991074A CN103991074A (zh) | 2014-08-20 |
CN103991074B true CN103991074B (zh) | 2015-10-21 |
Family
ID=51305543
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410217148.0A Expired - Fee Related CN103991074B (zh) | 2014-05-21 | 2014-05-21 | 一种基于粒子滤波算法的空间机械臂控制方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103991074B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104236543B (zh) * | 2014-09-17 | 2017-06-23 | 上海大学 | 用于工业机器人空间位姿精度和轨迹测量的拉线式测量系统和测量方法 |
US10324168B2 (en) * | 2016-09-12 | 2019-06-18 | The Boeing Company | Systems and methods for spatial filtering using data with widely different error magnitudes |
CN109108963B (zh) * | 2018-07-24 | 2020-10-16 | 西北工业大学 | 基于差分演化粒子群算法空间多关节机器人路径规划方法 |
CN108972553B (zh) * | 2018-08-06 | 2021-08-13 | 北京邮电大学 | 一种基于粒子滤波算法的空间机械臂故障检测方法 |
CN109648567B (zh) * | 2019-01-25 | 2021-08-03 | 华侨大学 | 一种具有容噪特性的冗余度机械臂规划方法 |
CN112775976B (zh) * | 2021-02-05 | 2022-05-10 | 深圳市优必选科技股份有限公司 | 任务执行控制方法、装置、控制设备及可读存储介质 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP0546633A2 (en) * | 1991-12-11 | 1993-06-16 | Koninklijke Philips Electronics N.V. | Path planning in an uncertain environment |
JP2533436B2 (ja) * | 1992-08-05 | 1996-09-11 | 株式会社エービーシ | 物体の認識方法 |
JP2004298975A (ja) * | 2003-03-28 | 2004-10-28 | Sony Corp | ロボット装置、障害物探索方法 |
KR100757839B1 (ko) * | 2006-04-04 | 2007-09-11 | 삼성전자주식회사 | 제어시스템, 제어시스템을 갖는 이동로봇장치 및 그제어방법 |
WO2012153629A1 (ja) * | 2011-05-12 | 2012-11-15 | 株式会社Ihi | 運動予測制御装置と方法 |
-
2014
- 2014-05-21 CN CN201410217148.0A patent/CN103991074B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN103991074A (zh) | 2014-08-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103991074B (zh) | 一种基于粒子滤波算法的空间机械臂控制方法及系统 | |
KR101930420B1 (ko) | 샘플링 비율 결정 기법에 의한 계측 샘플링 방법 및 그 컴퓨터 프로그램 제품 | |
JP6359182B2 (ja) | 機械の動作を制御するための方法およびシステム | |
CN104376581B (zh) | 一种采用自适应重采样的高斯混合无迹粒子滤波算法 | |
JP4223894B2 (ja) | Pidパラメータ調整装置 | |
JP5930655B2 (ja) | 制御問題を解決するための方法 | |
Du et al. | Applying particle swarm optimization algorithm to roundness error evaluation based on minimum zone circle | |
CN105678015B (zh) | 一种高超声速三维机翼的非概率可靠性气动结构耦合优化设计方法 | |
KR101234797B1 (ko) | 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법 | |
Urrea et al. | Parameter identification methods for real redundant manipulators | |
CN108247631A (zh) | 一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法 | |
CN110032706A (zh) | 一种低阶时滞系统的两阶段参数估计方法及系统 | |
JP2017177923A5 (zh) | ||
CN104572575B (zh) | 一种特大变形动态网格生成方法 | |
CN106874561B (zh) | 基于牛顿迭代的多学科不确定性传播分析方法 | |
CN103895017B (zh) | 一种基于使用可靠度的空间机械臂控制方法及系统 | |
Kouzoupis et al. | An efficient SQP algorithm for moving horizon estimation with Huber penalties and multi-rate measurements | |
US11207773B2 (en) | Action transfer device, action transfer method, and non-transitory computer-readable medium storing action transfer program | |
JP2009023016A (ja) | 逆運動学計算方法及び逆運動学計算装置 | |
WO2018198298A1 (ja) | パラメータ推定装置、パラメータ推定方法、及びコンピュータ読み取り可能な記録媒体 | |
CN113985887A (zh) | 差速移动机器人运动轨迹生成方法和运动控制装置 | |
CN106845016A (zh) | 一种基于事件驱动的量测调度方法 | |
CN103310073B (zh) | 一种考虑软件测试与运行环境差别的软件成本模型建模方法 | |
Neunert et al. | Adaptive real-time nonlinear model predictive motion control | |
CN110879589A (zh) | 一种基于反步策略与滑膜策略的机械臂容错控制方法及系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20151021 Termination date: 20160521 |