move_on 函数dot的迭代那两个表达式应该是等价的,但为什么换了一次输出结果就变了
问题背景是运动学迭代方程求轨迹,然后对固定轨迹搜索每一步的参数
from PIL import Image
import numpy as np
import matplotlib.pyplot as plt
#让图表显示出中文
plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus'] = False
import random
import math as m
def new():
x0,y0,vl,vr,theta=0.0,0.0,0.1,0.1,0.0
dot=np.array([x0,y0,vl,vr,theta])
return dot
def move_on(dot,al,ar,dt):
theta=dot[4]
mid=np.mat(np.array([[m.cos(theta),m.sin(theta),0],[0,0,1]]))
mid=np.transpose(mid)
mid1=np.mat(np.array([[0.5,0.5],[1,-1]]))
vm=np.transpose(np.mat([dot[3]+ar*dt,dot[2]+al*dt]))
change=mid*mid1*vm
dot+=np.array([float(change[0])*dt,float(change[1])*dt,al*dt,ar*dt,float(change[2])*dt])
#dot=dot+np.array([float(change[0])*dt,float(change[1])*dt,al*dt,ar*dt,float(change[2])*dt])
return dot
def dif_one(dot1,dot2):
return (dot1[0]-dot2[0])**2+(dot1[1]-dot2[1])**2
def one_p_best(dot_pre,dot,a_max,N,dt):
min_dif=1000000000
best_alr=[0,0,10000]
for i in range(2*N):
for j in range(2*N):
al,ar=a_max*(i-N)/N,a_max*(j-N)/N
#print(1)
if dif_one(move_on(dot_pre,al,ar,dt),dot)<min_dif:
#print(123)
min_dif=dif_one(move_on(dot_pre,al,ar,dt),dot)
best_alr=[a_max*(i-N)/N,a_max*(j-N)/N,min_dif]
print((move_on(dot_pre,best_alr[0],best_alr[1],dt)[0],move_on(dot_pre,best_alr[0],best_alr[1],dt)[1]))
print(best_alr)
print(dif_one(move_on(dot_pre,best_alr[0],best_alr[1],dt),dot))
return best_alr
def one_by_search(dot,x_set,y_set,a_max,N,dt):
n=len(x_set)
l_x=[];l_y=[];l_dif=[]
l_x.append(dot[0]);l_y.append(dot[1])
for i in range(1,n):
dot_true=[x_set[i],y_set[i]]
l=one_p_best(dot,dot_true,a_max,N,dt)
al,ar,dif=l[0],l[1],l[2]
l_x.append(dot[0]);l_y.append(dot[1]);l_dif.append(dif)
dot=move_on(dot,al,ar,dt)
plt.plot(x_set,y_set,label='预设轨迹',color='green')
plt.legend(loc=0)
plt.plot(l_x,l_y,label='实际轨迹',color='red')
plt.legend(loc=0)
plt.show()
plt.plot(x_set[1:],l_dif,label='累积残差')
plt.legend(loc=0)
plt.show()
new()
true_x=[i/10 for i in range(10)]
true_y=[m.sin(i) for i in true_x]
one_by_search(new(),true_x,true_y,1,10,0.1)
#one_p_best(new(),[true_x[1],true_y[1]],1,100,0.1)