项目要求:
项目分析:
-
整个过程是开环的,意味着我们需要提前计算好小车在每一个段的左右轮的速度与运行的时间。
-
整个问题的关键是计算每一段的速度:
-
于是我们得出解决这个问题的步骤:
项目解决过程:
1.matlab进行数学建模:
思路:
把这个曲线分成20份直线(不能太多:会慢;也不能太少:会掉下去),计算相邻两个直线之间的夹角
我们的小车就相当于走了20次直线运动加18次转向。
theta数组记录的是每次转向的角度。
我们只需计算小车转相应角度的占功比加直线运动的速度。
把计算好的数据导入小车。
理论上就可以完成任务了。
代码实现(matlab):
a=0.4
l=0.4
%单位:米
l=0.4;
a=0.4;
u=linspace(0,pi,20);
rx=[];
ry=[];
theta=[]
for i=1:20
rx(end+1)=-2*a*( ( l-cos(u(i)) ) * cos(u(i)) + (1-l) );
ry(end+1)=2*a*(l-cos(u(i)))*sin(u(i));
end
plot(rx,ry)
%X,Y对u的一阶导数
X_1=[];
Y_1=[];
X_1=diff(rx)./diff(u)
Y_1=diff(ry)./diff(u)
根据:T=
%计算T
T_X=[];
T_Y=[];
for i=1:19
dis=sqrt(X_1(i)*X_1(i)+Y_1(i)*Y_1(i));
T_X(end+1)=X_1(i)/dis;
T_Y(end+1)=Y_1(i)/dis;
end
T_X
T_Y
the torsion:
k=
T_X1=diff(T_X)./diff(u(1:end-1));
T_Y1=diff(T_Y)./diff(u(1:end-1));
k=[];
for i=1:18
disT=sqrt(T_X1(i)*T_X1(i)+T_Y1(i)*T_Y1(i));
disR=sqrt(X_1(i)*X_1(i)+Y_1(i)*Y_1(i));
k(end+1)=disT/disR;
end
k=[1.0./k];
k
rx1=diff(rx)./diff(u);
ry1=diff(ry)./diff(u);
%将轨迹切成一段一段的直线:
for i=2:19
clf;
hold on;
axis equal;
plot(rx,ry)
dx1=rx(i+1)-rx(i);
dy1=ry(i+1)-ry(i);
dx2=rx(i)-rx(i-1);
dy2=ry(i)-ry(i-1);
quiver(rx(i),ry(i),dx1,dy1,'Color',[1.0,0.0,0.0],'LineWidth',3.0);
theta(end+1)=judge(dx2,dy2,dx1,dy1);
theta(end)
legend('路径','直线运动的方向');
drawnow
end
L=[];
tot=0;
for i=1:19
dx=rx(i+1)-rx(i);
dy=ry(i+1)-ry(i);
L(end+1)=sqrt(dx*dx+dy*dy);
tot=tot+L(end);
end
L
rx
ry
theta
tot
function theta=judge(x1,y1,x2,y2)
len1=sqrt(x1*x1+y1*y1);
len2=sqrt(x2*x2+y2*y2);
dc=x1*x2+y1*y2;
theta=acos(dc/(len1*len2));
end
最后我们得到数学建模后的结果:
2.运用切线法计算出每一段的曲率与长度:
长度的计算:
d
l
=
s
q
r
t
(
d
x
2
+
d
y
2
)
dl=sqrt(dx^2+dy^2)
dl=sqrt(dx2+dy2)
曲率的计算: 曲率:k 单位切向量:T 位移方程:r(x)
于是我们由这些公式就得到了曲线各段的曲率与长度:
曲率:
长度:
3.测量小车在不同pwm下的速度:
这里不同的小车的状况不同,我们就只展示一部分数据:
4.将pwm,t的值带入小车单片机中
-
计算pwm与t
根据公式:
我们将数据带入程序中:
代码(C++)
#include<bits/stdc++.h>
using namespace std;
#define H 500005
#define LL long long
const double v=0.93;//米每秒
const double d=0.17;//两轮之间的距离
double Len[20]={
0, 0.0791 , 0.0815 , 0.0859 , 0.0921 , 0.0995 , 0.1076 , 0.1162 , 0.1249 , 0.1336 , 0.1419,
0.1497 , 0.1570 , 0.1636 , 0.1694 , 0.1743 , 0.1783 , 0.1813 , 0.1834 , 0.1844
};
double theta[20]={
0, 0.4354 , 0.4204 , 0.4004 , 0.3795 , 0.3605 , 0.3442 , 0.3308 , 0.3201 , 0.3114 , 0.3046,
0.2991 , 0.2948 , 0.2914 , 0.2887 , 0.2867 , 0.2852 , 0.2842 , 0.2836 , 0
};
double R[19]={
0, 0.1832 , 0.1953 , 0.2161 , 0.2441 , 0.2774 , 0.3142 , 0.3529 , 0.3921 , 0.4306 , 0.4676,
0.5025 , 0.5345 , 0.5633 , 0.5886 , 0.6100 , 0.6273 , 0.6403 , 0.6489
};
double Lv[20],Rv[20];//左轮pwm与右轮pwm
double t[20];//每一段的时间
int main(){
//计算时间
for(int i=1;i<=19;i++)t[i]=Len[i]/v;
//计算pwm
for(int i=1;i<=18;i++){
Lv[i]=v-d/2*(v/R[i]);
Rv[i]=v+d/2*(v/R[i]);
Lv[i]=ceil(Lv[i]*4000);
Rv[i]=ceil(Rv[i]*4000);
}
//输出数据
for(int i=1;i<=18;i++)printf("%.0f ",Lv[i]);
cout<<endl;
for(int i=1;i<=18;i++)printf("%.0f ",Rv[i]);
cout<<endl;
for(int i=1;i<=18;i++)printf("%.0f ",t[i]*1000);//毫秒
}
- 带入单片机中
部分代码(keil5)
while(tnt<=18){
rv=Rv[tnt];
lv=Lv[tnt];
tt=t[tnt];
TIM_SetCompare2(TIM8,rv);//右轮
TIM_SetCompare1(TIM8,lv);//左轮
delay_ms(tt);
stop();
tnt=tnt+1;
}
TIM_SetCompare2(TIM8,rv);
TIM_SetCompare1(TIM8,lv);
delay_ms(tt);
TIM_SetCompare2(TIM8,rv);
TIM_SetCompare1(TIM8,lv);
delay_ms(tt);
stop();