這篇是基于粒子群算法的牙齒正畸路徑規(guī)劃研究的python實(shí)現(xiàn)，參考的是徐曉強(qiáng)等人的《基于改進(jìn)粒子群算法的牙齒正畸路徑規(guī)劃方法》露泊，與這篇文章的區(qū)別在于：
1.徐等的文章設(shè)計(jì)了一種改進(jìn)的粒子群算法贱除，此篇使用的是普通的粒子群算法和敬。
2.徐等的文章是雖然是對(duì)三維進(jìn)行建模视粮，但是對(duì)二維的仿真，而此篇是直接做三維仿真土浸。
3.徐等的文章利用matlab實(shí)現(xiàn)在基準(zhǔn)函數(shù)進(jìn)行了模型測(cè)試罪针，此篇用python實(shí)現(xiàn)簡(jiǎn)單的路徑規(guī)劃仿真。
此篇涉及的知識(shí)點(diǎn)有：
1.粒子群算法：如何直觀形象地理解粒子群算法黄伊？
2.obb包圍盒實(shí)現(xiàn)：Python實(shí)現(xiàn)obb包圍盒及包圍框
3.python如何讀取stl格式數(shù)據(jù)
主函數(shù)
import numpy as np
from sympy import *
from matplotlib import cm
import math
import pylab as pl
import scipy as sp
from stl import stl
from numpy.linalg import solve
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as a3
import matplotlib.colors as colors
from mpl_toolkits.mplot3d import Axes3D
from orthodontic import Tooth_dot,Tooth_obb_box,Tooth_obb_plot,Tooth_obb_plot2,Orthodontic,Particle_generation
from orthodontic import PSO
fig = plt.figure()
ax = Axes3D(fig)
#加載牙齒的stl數(shù)據(jù)
stl_list=[0,1,2,3,4,5,6,7,8,9,10,11,12,13]
# stl_list=[13]
z=5#30個(gè)粒子
m=len(stl_list)#牙齒個(gè)數(shù)
n=5#七個(gè)階段
iter_max=10
#通過隨機(jī)平移和旋轉(zhuǎn)生成z個(gè)粒子
pt=Particle_generation(z,m,n)
# print(pt)
#得到最好的粒子
best_pt=PSO(pt,z,m,n,iter_max)
# 獲取牙齒的stl點(diǎn)坐標(biāo),理想位置
for k in range(n):
for i in range(0,1):
dot=Tooth_dot(stl_list[i])
b,x,ans,les,cos=Tooth_obb_box(dot)
# diag_b=b.diagonal()
# for i in range(3):
# print(x,math.acos(diag_b[i])*180/math.pi)
color_bar=['purple','y','orange','g','k','gray','purple']
#矯正位置
orth_x=best_pt[i,k,0:3]
orth_ang=best_pt[i,k,3:6]
orth_b,orth_ans=Orthodontic(b,cos,orth_ang,orth_x,les)
if k==0:
Tooth_obb_plot(ax,dot,orth_b,orth_x,orth_ans,'gp')
Tooth_obb_plot(ax,dot,orth_b,orth_x,orth_ans,color_bar[k])
if k==n-1:
Tooth_obb_plot2(ax,dot,b,x,ans,'rp')
plt.pause(0.000001)
plt.show()
#obb碰撞檢測(cè)參考：http://www.doc88.com/p-5953424737378.html
obb包圍盒實(shí)現(xiàn)及粒子群算法仿真：
import math
import copy
import numpy as np
from stl import stl
from math import sin,cos,log,pi
from numpy.linalg import solve
import matplotlib.pyplot as plt
from scipy.optimize import fsolve
import mpl_toolkits.mplot3d as a3
import matplotlib.colors as colors
from mpl_toolkits.mplot3d import Axes3D
#獲取牙齒的數(shù)據(jù)
def Tooth_dot(stl_list_i):
mesh = stl.StlMesh('./牙齒正畸/牙齒切分模型/'+str(stl_list_i)+'.stl')
dot_orignal=np.zeros((1,3))
for i in range(len(mesh)):
s1=mesh.points[i]
s2=s1.reshape(3,3)
dot_orignal=np.vstack((dot_orignal,s2))
return dot_orignal[1:,:]
#求解obb包圍盒
def Tooth_obb_box(dot):
#畫出obb包圍盒
def Tooth_obb_plot(ax,dot,b,x,ans,colorr):
#畫出所有點(diǎn)
# kwargs = {'alpha': 0.0001,'color':'blue'}
# ax.plot3D(dot[:,0].tolist(),dot[:,1].tolist(),dot[:,2].tolist(),'.',linewidth=0.05,**kwargs)
#畫出三個(gè)新的坐標(biāo)軸
for i in range(3):
ax.plot3D([x[0],x[0]+b[0,i]],[x[1],x[1]+b[1,i]],[x[2],x[2]+b[2,i]])
x_ans=[ans[0,0],ans[1,0],ans[3,0],ans[2,0],ans[0,0],ans[4,0],ans[6,0],ans[7,0],ans[5,0],ans[4,0]]
y_ans=[ans[0,1],ans[1,1],ans[3,1],ans[2,1],ans[0,1],ans[4,1],ans[6,1],ans[7,1],ans[5,1],ans[4,1]]
z_ans=[ans[0,2],ans[1,2],ans[3,2],ans[2,2],ans[0,2],ans[4,2],ans[6,2],ans[7,2],ans[5,2],ans[4,2]]
#畫出obb盒子
ax.plot3D(x_ans,y_ans,z_ans,colorr,linewidth=0.8)
for i in range(3):
ax.plot3D([ans[i+1,0],ans[i+5,0]],[ans[i+1,1],ans[i+5,1]],[ans[i+1,2],ans[i+5,2]],colorr,linewidth=0.8)
def Tooth_obb_plot2(ax,dot,b,x,ans,colorr):
#畫出所有點(diǎn)
kwargs = {'alpha': 0.003,'color':'blue'}
ax.plot3D(dot[:,0].tolist(),dot[:,1].tolist(),dot[:,2].tolist(),'.',linewidth=0.05,**kwargs)
#畫出三個(gè)新的坐標(biāo)軸
for i in range(3):
ax.plot3D([x[0],x[0]+b[0,i]],[x[1],x[1]+b[1,i]],[x[2],x[2]+b[2,i]])
x_ans=[ans[0,0],ans[1,0],ans[3,0],ans[2,0],ans[0,0],ans[4,0],ans[6,0],ans[7,0],ans[5,0],ans[4,0]]
y_ans=[ans[0,1],ans[1,1],ans[3,1],ans[2,1],ans[0,1],ans[4,1],ans[6,1],ans[7,1],ans[5,1],ans[4,1]]
z_ans=[ans[0,2],ans[1,2],ans[3,2],ans[2,2],ans[0,2],ans[4,2],ans[6,2],ans[7,2],ans[5,2],ans[4,2]]
#畫出obb盒子
ax.plot3D(x_ans,y_ans,z_ans,colorr,linewidth=0.8)
for i in range(3):
ax.plot3D([ans[i+1,0],ans[i+5,0]],[ans[i+1,1],ans[i+5,1]],[ans[i+1,2],ans[i+5,2]],colorr,linewidth=0.8)
#求解變換后的局部坐標(biāo)系向量
def f(x):
x1 = float(x[0])
y1 = float(x[1])
z1 = float(x[2])
x2 = float(x[3])
y2 = float(x[4])
z2 = float(x[5])
x3 = float(x[6])
y3 = float(x[7])
z3 = float(x[8])
return [x1*x2+y1*y2+z1*z2,
x1*x3+y1*y3+z1*z3,
x2*x3+y2*y3+z2*z3,
x1*x1+y1*y1+z1*z1-1,
x2*x2+y2*y2+z2*z2-1,
x3*x3+y3*y3+z3*z3-1,
x1-float(x[9]),
y2-float(x[10]),
z3-float(x[11]),
0.,
0.,
0.]
#牙齒移動(dòng)后的相關(guān)坐標(biāo)：局部坐標(biāo)系,八個(gè)頂點(diǎn)的坐標(biāo)
def Orthodontic(ob,cos,orth_ang,orth_x,les):
b = ob.T.flatten().tolist()
orth_ang = np.cos(orth_ang/180*pi)
orth_ang = orth_ang.tolist()
x=b+orth_ang
result = fsolve(f,x)
orth_b = np.array(result[0:9]).reshape(3,3)
orth_b=orth_b.T
orth_ans = np.mat(orth_b.T).I*cos.T
for i in range(8):
orth_ans[:,i]=orth_ans[:,i]*les[i]+np.mat(orth_x).T
return orth_b,orth_ans.T
def Particle_generation(z,m,n):
pt=np.zeros((z,m,n,6))
#三顆牙齒的理想位置
for i in range(z):
for j in range(m):
pt[i,0,0,:]=np.array([23.61006943,17.82747056,-20.16727971,69.4602916066788,118.19793758099148,28.944969967151017])
pt[i,1,0,:]=np.array([22.00154474,22.23109873,-10.56274203,82.20503306251742,102.23797429234898,22.659750787501384])
pt[i,2,0,:]=np.array([20.21602939,24.55590149,-2.56155887,19.59300214465621,160.00846123132308,9.34366689686702])
pt[i,3,0,:]=np.array([17.4970468,27.10152147,2.91609252,40.59623501981751,139.67459285911806,6.191516712578595])
pt[i,4,0,:]=np.array([12.69758738,29.13274355,8.5260505,78.95655178930328,100.28096565608264,33.931521943543544])
pt[i,5,0,:]=np.array([8.16643294,30.28463427,11.42548952,89.19138292409808,98.71517063028655,41.944118961376994])
pt[i,6,0,:]=np.array([3.55807655,31.58355968,12.26046366,83.50554859236546,103.41535763991074,53.69547623532445])
pt[i,7,0,:]=np.array([-2.04687096,32.17083736,12.52742242,85.46722651861216,84.94579068682329,58.91289971101213])
pt[i,8,0,:]=np.array([-7.12704824,32.10145805,11.43092301,98.08610437351948,104.41711898593356,59.05840552581135])
pt[i,9,0,:]=np.array([-12.15524467,31.13939936,8.31407426,100.21243845803976,69.79845402078593,37.36596015162017])
pt[i,10,0,:]=np.array([-17.12435839,30.17595961,2.86044776,150.92175547611808,162.41497137576253,60.810690756858506])
pt[i,11,0,:]=np.array([-20.04271334,29.57043377,-4.32045092,152.3966623284993,26.461495168995285,9.154096620848087])
pt[i,12,0,:]=np.array([-22.0431927,26.59292723,-12.05594014,103.35215185282235,79.9218652295453,21.802210187730434])
pt[i,13,0,:]=np.array([-23.77424033,22.90435583,-21.39226861,73.461423996978,79.07185113689994,113.40844315750385])
for k in range(1,n):
pt[i,j,k,0:3]=pt[i,j,k-1,0:3]+((-1 + 2*np.random.random((1,3)))*0.28)
pt[i,j,k,3:6]=pt[i,j,k-1,3:6]+(-1 + 2*np.random.random((1,3)))
for i in range(z):
for j in range(m):
# print(np.flipud(pt[i,j]))
pt[i,j]=copy.deepcopy(np.flipud(pt[i,j]))
# print(pt[i,j])
return pt
#碰撞檢測(cè)
def Collision_detection(pt_i):
return 0
#其他限制條件
def Constraint(p1,p2):
gc1=sum(np.power(p1[0:3]-p2[0:3],2))
gc2=sum(abs(p1[3:6]-p2[3:6]))
if gc1<=0.25 and gc2<=3:
return 1
else:
return 0
#適應(yīng)度函數(shù)
def Fitness(pt_i,m,n):
f1=0;f2=0;f3=0;g1=0;g2=0;gc1=0;gc2=0;cons=0;Cons=1
for j in range(m):
for k in range(1,n):
#距離評(píng)價(jià)函數(shù)
f1=f1+np.sqrt(sum(np.power(pt_i[j,k,0:3]-pt_i[j,k-1,0:3],2)))
#角度評(píng)價(jià)函數(shù)
f2=f2+np.sum(abs(pt_i[j,k,3:6]-pt_i[j,k-1,3:6]))
#碰撞檢測(cè)約束條件
f3=Collision_detection(pt_i[j,k,:])
#距離和角度約束條件
cons=Constraint(pt_i[j,k,:],pt_i[j,k-1,:])
Cons=Cons*cons
if Cons==1:
fitness=f1+f2+f3
else:
fitness=(f1+f2+f3)*100
return fitness
# #PSO算法
def PSO(pt,z,m,n,iter_max):
#初始化粒子群算法相關(guān)參數(shù)
w=0.8;c1=2;c2=2;r1=np.random.random(1);r2=np.random.random(1)
v=np.zeros((m,6))
sbf=np.ones(z)*500#單個(gè)粒子最小適應(yīng)度值
sb=np.zeros((z,m,n,6))#單個(gè)粒子最優(yōu)值
ap=np.zeros((z,m,n,6))#平均值
sd=np.zeros((z,m,n,6))#標(biāo)準(zhǔn)差
wb=np.ones((iter_max,m,n,6))#全局最優(yōu)值
for iter in range(iter_max):
for i in range(z):#粒子個(gè)數(shù)
sf=Fitness(pt[i],m,n)
if sf<sbf[i]:
# print("第",iter,"循環(huán)泪酱，","第",i,"個(gè)粒子的適應(yīng)度值：",sf)
sbf[i]=sf
sb[i]=pt[i] #更新單個(gè)粒子目前為止最優(yōu)值
# print("第",iter,"循環(huán)后，單個(gè)粒子的最小適應(yīng)度值:",sbf)
wb[iter]=sb[np.argmin(sbf)] #更新目前為止全局最優(yōu)值
# print("第",iter,"循環(huán)后还最，全局最小適應(yīng)度值對(duì)應(yīng)的粒子:",wb[iter])
#進(jìn)行更新
for i in range(z):
for j in range(m):
for k in range(1,n-1):
ap[i,j,k,:]=(pt[i,j,k,:]+sb[i,j,k,:]+wb[iter,j,k,:])/3#公式5
sd[i,j,k,:]=np.sqrt((np.power((pt[i,j,k,:]-ap[i,j,k,:]),2)#公式6
+np.power(sb[i,j,k,:]-ap[i,j,k,:],2)
+np.power(wb[iter,j,k,:]-ap[i,j,k,:],2))/3)
K=np.sqrt(-2*log(np.random.random(1)))*cos(2*pi*np.random.random(1))#公式7
# p1=w*pt[i,j,k,:]
# p2=c1*r1*(w*(sb[i,j,k,:]+wb[iter,j,k,:])/2-pt[i,j,k,:])
# p3=c2*r2*((sb[i,j,k,:]-wb[iter,j,k,:])/2-pt[i,j,k,:])
# p4=K*sd[i,j,k,:]
# pt[i,j,k,:]=p1+p2+p3+p4#公式4
v[j,:]=w*v[j,:]+c1*r1*(sb[i,j,k,:]-pt[i,j,k,:])+c2*r2*(wb[iter,j,k,:]-pt[i,j,k,:])
pt[i,j,k,:]=pt[i,j,k,:]+v[j,:]#公式4
# print(i,j,k,pt[i,j,k,:])
return wb[-1]
效果圖（圖有點(diǎn)丑墓阀，但這不是重點(diǎn)）：
單顆牙齒正畸過程.png
