这篇是基于粒子群算法的牙齿正畸路径规划研究的python实现，参考的是徐晓强等人的《基于改进粒子群算法的牙齿正畸路径规划方法》，与这篇文章的区别在于：

1.徐等的文章设计了一种改进的粒子群算法，此篇使用的是普通的粒子群算法。

2.徐等的文章是虽然是对三维进行建模，但是对二维的仿真，而此篇是直接做三维仿真。

3.徐等的文章利用matlab实现在基准函数进行了模型测试，此篇用python实现简单的路径规划仿真。

此篇涉及的知识点有：

1.粒子群算法：如何直观形象地理解粒子群算法？

2.obb包围盒实现：Python实现obb包围盒及包围框

3.python如何读取stl格式数据

主函数

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数据

stl_list=[0,1,2,3,4,5,6,7,8,9,10,11,12,13]

# stl_list=[13]

z=5#30个粒子

m=len(stl_list)#牙齿个数

n=5#七个阶段

iter_max=10

#通过随机平移和旋转生成z个粒子

pt=Particle_generation(z,m,n)

# print(pt)

#得到最好的粒子

best_pt=PSO(pt,z,m,n,iter_max)

# 获取牙齿的stl点坐标,理想位置

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碰撞检测参考：http://www.doc88.com/p-5953424737378.html

obb包围盒实现及粒子群算法仿真：

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

#获取牙齿的数据

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):

#画出所有点

# kwargs = {'alpha': 0.0001,'color':'blue'}

# ax.plot3D(dot[:,0].tolist(),dot[:,1].tolist(),dot[:,2].tolist(),'.',linewidth=0.05,**kwargs)

#画出三个新的坐标轴

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):

#画出所有点

kwargs = {'alpha': 0.003,'color':'blue'}

ax.plot3D(dot[:,0].tolist(),dot[:,1].tolist(),dot[:,2].tolist(),'.',linewidth=0.05,**kwargs)

#画出三个新的坐标轴

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 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.]

#牙齿移动后的相关坐标：局部坐标系,八个顶点的坐标

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

#碰撞检测

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

#适应度函数

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):

#距离评价函数

f1=f1+np.sqrt(sum(np.power(pt_i[j,k,0:3]-pt_i[j,k-1,0:3],2)))

#角度评价函数

f2=f2+np.sum(abs(pt_i[j,k,3:6]-pt_i[j,k-1,3:6]))

#碰撞检测约束条件

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):

#初始化粒子群算法相关参数

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#单个粒子最小适应度值

sb=np.zeros((z,m,n,6))#单个粒子最优值

ap=np.zeros((z,m,n,6))#平均值

sd=np.zeros((z,m,n,6))#标准差

wb=np.ones((iter_max,m,n,6))#全局最优值

for iter in range(iter_max):

for i in range(z):#粒子个数

sf=Fitness(pt[i],m,n)

if sf

# print("第",iter,"循环，","第",i,"个粒子的适应度值：",sf)

sbf[i]=sf

sb[i]=pt[i] #更新单个粒子目前为止最优值

# print("第",iter,"循环后，单个粒子的最小适应度值:",sbf)

wb[iter]=sb[np.argmin(sbf)] #更新目前为止全局最优值

# print("第",iter,"循环后，全局最小适应度值对应的粒子:",wb[iter])

#进行更新

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]

效果图(图有点丑，但这不是重点)：

单颗牙齿正畸过程.png