本人前段时间需要用到粒子群优化算法来求取全局最优解,这个问题用PySwarms来解决。奈何没有搜索到什么中文文档讲解使用方法,只能抱着英文文档慢慢啃了QAQ。所以这里把个人见解写了下来,希望能和有需求的朋友交流。
这里将主要解析Inverse Kinematics Problem这个官方例子,以及从中学到如何使用GlobalBestPSO。
PySwarms的使用:GlobalBestPSO例子解析
Inverse Kinematics Problem(运动学逆问题)
问题官网网站如下(此问题所有代码均来自官网):https://pyswarms.readthedocs.io/en/latest/examples/usecases/inverse_kinematics.html
问题背景
逆运动学是机器人技术中最具挑战性的问题之一。IK坐标难以计算且多解,所以特别困难。现在可以为3自由度机械手找到简单的解决方案,但是尝试解决6自由度甚至更多自由度的问题可能会出现富有挑战性的代数问题。
在此例中,我们将使用具有5个旋转关节和1个棱柱关节的6自由度斯坦福机械臂。这些关节的活动范围如下所示:
现在,如果给定了末端位置(在本例中为xyz坐标),则需要找到具有上表约束的最佳参数。这些条件足以将这个问题视为优化问题。我们将参数向量X定义如下:
对于末端位置,我们将目标向量T定义为:
下面就开始解决具体问题吧!
原文后续就开始大致讲解粒子群优化算法的原理,不明白的朋友可以看这篇博客,讲解得很清楚。
最优化算法之粒子群算法(PSO)
有关粒子群优化算法的原理这里就不再赘述了。
准备工作
首先导入相关包:
# Import modules
import numpy as np
# Import PySwarms
import pyswarms as ps
我们以点[−2,2,3]作为目标,希望该点为最佳机械手姿势。首先定义一个函数来获取从当前位置到目标位置的距离(可以看出此距离函数就是求两位置间的欧氏距离):
def distance(query, target):
x_dist = (target[0] - query[0])**2
y_dist = (target[1] - query[1])**2
z_dist = (target[2] - query[2])**2
dist = np.sqrt(x_dist + y_dist + z_dist)
return dist
我们将使用此距离函数来计算代价,显然距离越远,距离代价就越高。
下面开始设置算法需要的参数:
swarm_size = 20
dim = 6 # Dimension of X
epsilon = 1.0
options = {'c1': 1.5, 'c2':1.5, 'w':0.5}
constraints = (np.array([-np.pi , -np.pi/2 , 1 , -np.pi , -5*np.pi/36 , -np.pi]),
np.array([np.pi , np.pi/2 , 3 , np.pi , 5*np.pi/36 , np.pi]))
d1 = d2 = d3 = d4 = d5 = d6 = 3
swarm_size为粒子个数;dim为待求参数的维度;epsilon后续也没有用到,我也不太明白放在这里的用意,如有大佬告知,不胜感激;options为重要的参数列表,c1为粒子的个体认知系数,c2为群体系数系数,w为惯性因子;constraints为待求参数取值限制;d1~d6为关节长度,均取3。(有关API参数的含义和注意事项后面会详细介绍)
为了获得当前位置,我们需要计算每个关节的旋转和平移矩阵。为此,这里我们使用Denvait-Hartenberg参数。我们定义了一个计算这些矩阵的函数。该函数使用旋转角度和棱柱形关节的延伸量d作为输入(不用担心不明白这是啥参数矩阵,不用明白,只需要知道输入一些参数返回一个矩阵就好):
def getTransformMatrix(theta, d, a, alpha):
T = np.array([[np.cos(theta) , -np.sin(theta)*np.cos(alpha) , np.sin(theta)*np.sin(alpha) , a*np.cos(theta)],
[np.sin(theta) , np.cos(theta)*np.cos(alpha) , -np.cos(theta)*np.sin(alpha) , a*np.sin(theta)],
[0 , np.sin(alpha) , np.cos(alpha) , d ],
[0 , 0 , 0 , 1 ]
])
return T
现在我们可以计算变换矩阵以获得末端的位置。为此,我们创建了另一个函数,该函数将向量X与关节变量作为输入(同样不需要知道怎么算的,只需要知道输入和输出是什么就好):
def get_end_tip_position(params):
# Create the transformation matrices for the respective joints
t_00 = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
t_01 = getTransformMatrix(params[0] , d2 , 0 , -np.pi/2)
t_12 = getTransformMatrix(params[1] , d2 , 0 , -np.pi/2)
t_23 = getTransformMatrix(0 , params[2] , 0 , -np.pi/2)
t_34 = getTransformMatrix(params[3] , d4 , 0 , -np.pi/2)
t_45 = getTransformMatrix(params[4] , 0 , 0 , np.pi/2)
t_56 = getTransformMatrix(params[5] , d6 ,0 , 0)
# Get the overall transformation matrix
end_tip_m = t_00.dot(t_01).dot(t_12).dot(t_23).dot(t_34).dot(t_45).dot(t_56)
# The coordinates of the end tip are the 3 upper entries in the 4th column
pos = np.array([end_tip_m[0,3],end_tip_m[1,3],end_tip_m[2,3]])
return pos
为了运行算法,我们需要准备的最后一件事就是我们要优化的函数。我们只需要计算每个群体粒子的位置与目标点之间的距离即可(这里就是运用到前面定义的函数get_end_tip_position进行计算X[i]的末端位置,再运用distance函数计算此位置与目标点之间的距离,返回这个距离数组):
def opt_func(X):
n_particles = X.shape[0] # number of particles
target = np.array([-2,2,3])
dist = [distance(get_end_tip_position(X[i]), target) for i in range(n_particles)]
return np.array(dist)
运行算法
经过上述准备,我们终于可以开始实现算法了!
# Call an instance of PSO
optimizer = ps.single.GlobalBestPSO(n_particles=swarm_size,
dimensions=dim,
options=options,
bounds=constraints)
# Perform optimization
cost, joint_vars = optimizer.optimize(opt_func, iters=1000)
很简单的样子!?(并没有,自己写待优化函数的时候呢QAQ)代入上面说的参数实例化一个全局最优器对象,再调用其优化方法便可以得到我们上面设定的代价值和待求参数值。
print(get_end_tip_position(joint_vars))
[-2.09012905 2.07694604 3.01641479]
再查看我们求取的参数值计算得到的末端坐标,几乎就是我们的目标坐标值。成功啦!
下面开始最关键的使用部分啦!
GlobalBestPSO的使用
我举上面的官网例子其实也是因为我就是看那个例子学会的。下面就开始介绍GlobalBestPSO的使用方法吧!
参数(这里只讲常用的参数)
- n_particles :int,粒子群中的粒子数。一般取 20–40。如果是比较难的问题或者特定类别的问题,粒子数可以取到100 - 200。
- dimensions:int,待求取参数的维度。
- options:dict,参数c1,c2,w的设置。
使用带有关键字{‘c1’, ‘c2’, ‘w’}的字典 。这里再重复一遍各关键字的含义:c1为粒子的个体认知系数,c2为群体系数系数,w为惯性因子。
c1和c2取值通常相等,且常取值为2,当然取值0-4也有。
w较大时全局搜索能力强,较小时局部搜索能力强。 - bounds:numpy.ndarray,待求参数的取值范围。
两行的ndarray,第一行为待求参数的最小值,第二行为最大值。很重要的一点是,列的大小要和输入的参数维度匹配,并且还要注意每一个参数范围的对齐,不要错位了。 - init_pos:numpy.ndarray,初始位置的设置。
不设置的话,粒子初始位置则是随机的。
方法
其实我认为最重要的是这个方法里待优化方程的写法QAQ。下面就开始了!
optimize参数
- objective_func :callable,求取最优解的方程。
objective_func怎么写
说实话API文档并没有明确给出这个函数怎么写:)
这里我们以一个简单的二元二次为例,来看这个函数到底怎么写。
显然取全局最优也就是方程取最小值,此时x1=m,x2=n。
最最重要的一点就是待求参数要放在第一个,可以取名x,y等等等等,后面参数放多少都可以(当然夸张了)。其实就是类似于可变参数函数的参数规则。第一个参数是我们现在需要求的参数,所以自然不能传递值。其余参数就必须要有确定的值。
还有一点也是需要特别注意的是:放在第一个位置的参数在多维时,在使用第i个参数时,需要采用 x[: ,i] 这样的形式,否则会出错。
明确完这两点之后就可以开始写函数了:
import pyswarms as ps
swarm_size = 20
dim = 2 # Dimension of paramater vetcor X
options = {'c1': 0.5, 'c2': 0.3, 'w': 0.9}
optimizer = ps.single.GlobalBestPSO(n_particles=swarm_size,
dimensions=dim,
options=options)
def func(x,m=2,n=2):
f = (x[:,0]-m)**2+(x[:,1]-n)**2
return f
cost, pos = optimizer.optimize(func,iters=1000,m=3,n=1)
运行结果如下:
- iters:int,迭代次数。这个参数可以根据待解决问题的大小来选择。
- kwargs :dict,上述方程的输入参数。也可以在后面按参数名传递参数值。
optimize返回值
全局最小代价和全局最优位置。
总结
只要设定了合适的PSO优化算法参数,写好了待优化函数,理论上就能运用GlobalBestPSO求得全局最小代价和全局最优位置。