|
楼主 |
发表于 2018-1-24 18:03:03
|
显示全部楼层
抱歉,代码忘记传了,现在上传,simulink框图传不了,我就截个图吧,拜托诸位了。
- function roll = uav12(rollf)
- % 此函数是跟踪已知二维轨迹上的虚拟目标点
- n=5000; %假设取5000个点
- xm=zeros(n,1);
- ym=zeros(n,1); %初始位置
- vm=ones(n,1)*56.59;
- vmx=ones(n,1)*56.576;
- vmy=ones(n,1)*-1.256; %初始速度方向 速度
- deltat=1/56.59; %采样时间间隔
- i=1;
- xt = zeros(n,1);
- yt = zeros(n,1);
- while(xm(i)<5000)
- [xt(i,1),yt(i,1)]=myplace(xm(i),ym(i)); %求虚拟目标点位置
- lamda = mysight(xm(i),ym(i),xt(i),yt(i)); %无人机当前位置与虚拟目标点间的视线角
- gamma = myspeed(vmx(i),vmy(i)); %当前速度方向
- yita = myangle(lamda,gamma); %速度方向与视线角之间的夹角
- vm(i) = vmx(i)*vmx(i)+vmy(i)*vmy(i); %当前合速度
- ac = myacceleration(vm(i),yita); %非线性导航逻辑算法求加速度指令
- roll = myroll(ac); %由加速度指令转换为滚转角指令
- af = 9.81*tan(rollf); %通过接入无人机横侧向环节simulink框图得到当前滚转角响应值 滚转角响应值再转为加速度值
- vmx(i+1)=vmx(i)+af*sin(gamma)*deltat;
- vmy(i+1)=vmy(i)-af*cos(gamma)*deltat; %由加速度值求下一时刻无人机在xy轴的速度分量
- xm(i+1)=xm(i)+vmx(i)*deltat;
- ym(i+1)=ym(i)+vmy(i)*deltat; %下一时刻无人机位置解算
- i=i+1;
- end
- end
复制代码
|
|