clc
clear all
close all
%% 定义优化问题实例和优化变量
opti = casadi.Opti();
waypoints = [0 15 10];
M=size(waypoints,2);
N =4;
tN=opti.variable(1);
dt=tN/N;
u = opti.variable(N,1);
x = opti.variable(N+1,2);
mu = opti.variable(N+1,M);
%=N+1 判断靠近的公式好计算
v = opti.variable(N+1,1);
lamda = opti.variable(N+1,M);
%% 优化目标
opti.minimize(tN);
%% 初始化
opti.set_initial(tN, 0);% tN初始化为0/2可以解出来,初始化成1或5解不出来
opti.set_initial(x, 0);
opti.set_initial(mu, 0);
opti.set_initial(u, 0);
%
% lamda_ini = 0.5*ones(N+1,1);
% lamda_ini(1:10)=1;
% lamda_ini(11:end)=0;
opti.set_initial(lamda, 1);
opti.set_initial(v, 0);
%% 定义G,即constraints
f = @(x,u) [x(2),u-x(2)];
for k=1:N-1 % loop over control intervals
% Runge-Kutta 4 integration
k1 = f(x(k,:), u(k,:));
k2 = f(x(k,:)+dt/2*k1, u(k,:));
k3 = f(x(k,:)+dt/2*k2, u(k,:));
k4 = f(x(k,:)+dt*k3, u(k,:));
x_next = x(k,:) + dt/6*(k1+2*k2+2*k3+k4);
opti.subject_to(x(k+1,:)==x_next); % close the gaps
end
%公式13第一个式子对应的约束
for j =1:N
opti.subject_to(mu(j,:) - lamda(j,:) + lamda(j+1,:) == 0);
end
%公式13最后一行第二个式子对应的约束
for i =1:N
for j =1:M
opti.subject_to(lamda(i,j) - lamda(i+1,j) >= 0);
end
end
%输入的限制
for j =1:N
opti.subject_to(-5 <= u(j) <= 5);
end
opti.subject_to( u(1) == 0);
%公式13最后一行第一个式子对应的约束
% 这个准确讲应该改成0/1
for j =1:N
opti.subject_to(0<=mu(j));
end
%公式14对应的约束
% for j =1:N
% opti.subject_to(mu(j)*((x(j)-10)*(x(j)-10)-v(j))==0);
% end
for i =1:N+1
for j =1:M
opti.subject_to(mu(i,j)*((x(i,1)-waypoints(j))*(x(i,1)-waypoints(j))-v(i))==0);
end
end
% for j =1:N
% opti.subject_to(lamda(j)>=0);
% end
d = opti.parameter();
for j =1:N
opti.subject_to(0 <= v(j) <= d);
end
opti.set_value(d,0.02);
opti.subject_to([x(1,1);x(1,2)] == [waypoints(1);0]);
% opti.subject_to(x(1,1)==waypoints(1));
% opti.subject_to(x(1,2)==0);
opti.subject_to([x(N,1);x(N,2)] == [waypoints(end);0]);
% opti.subject_to(x(N,1)==waypoints(end));
% opti.subject_to(x(N,2)==0);
opti.subject_to([lamda(1);lamda(N+1)] == [1;0]);
% opti.subject_to(lamda(1)==1);
% opti.subject_to(lamda(N+1)==0);
opti.subject_to(0 <= tN);
% opti.option.up();
opti.solver('ipopt',struct('print_time',false),struct('print_level',0,'max_iter',10000));
sol = opti.solve();
%% 后处理
tN2=sol.value(tN);
m=tN2/(N-1);
t = 0: m:tN2;
subplot(2,2,1);
hold on
plot(t,sol.value(u),'-o');
% plot(t,sol.value(mu(:,1)));
% plot(t,sol.value(mu(:,2)));
% plot(t,sol.value(mu(:,3)));
legend('u');
m2=tN2/(N);
t = 0: m2:tN2;
subplot(2,2,2);
hold on
plot(t,sol.value(mu(:,1)));
plot(t,sol.value(mu(:,2)));
plot(t,sol.value(mu(:,3)));
legend('mu1','mu2','mu3');
subplot(2,2,3);
plot(m2,sol.value(v));
plot(m2,sol.value(x(:,1)));
plot(m2,sol.value(x(:,2)));
plot(t,sol.value(lamda(:,1)));
plot(t,sol.value(lamda(:,2)));
plot(t,sol.value(lamda(:,3)));
legend('v','x2','lamda');
subplot(2,2,4);
plot(m2,sol.value(x(:,1)));
plot(m2,sol.value(x(:,2)));
legend('x1','x2');
没有合适的资源?快使用搜索试试~ 我知道了~
被动铰接多四旋翼平台的几何约束轨迹优化matlab代码.zip
![preview](https://csdnimg.cn/release/downloadcmsfe/public/img/white-bg.ca8570fa.png)
共6个文件
m:5个
opt:1个
![preview-icon](https://csdnimg.cn/release/downloadcmsfe/public/img/scale.ab9e0183.png)
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 47 浏览量
2024-05-03
23:22:42
上传
评论
收藏 11KB ZIP 举报
温馨提示
1.版本:matlab2014/2019a/2021a 2.附赠案例数据可直接运行matlab程序。 3.代码特点:参数化编程、参数可方便更改、代码编程思路清晰、注释明细。 4.适用对象:计算机,电子信息工程、数学等专业的大学生课程设计、期末大作业和毕业设计。
资源推荐
资源详情
资源评论
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![7z](https://img-home.csdnimg.cn/images/20210720083312.png)
收起资源包目录
![package](https://csdnimg.cn/release/downloadcmsfe/public/img/package.f3fc750b.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
共 6 条
- 1
资源评论
![avatar-default](https://csdnimg.cn/release/downloadcmsfe/public/img/lazyLogo2.1882d7f4.png)
![avatar](https://profile-avatar.csdnimg.cn/864ffdc5a26342a6add0026479aef1e5_matlab_dingdang.jpg!1)
![avatar-vip](https://csdnimg.cn/release/downloadcmsfe/public/img/user-vip.1c89f3c5.png)
matlab科研助手
- 粉丝: 2w+
- 资源: 2416
![benefits](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-1.c8e153b4.png)
下载权益
![privilege](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-2.ec46750a.png)
C知道特权
![article](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-3.fc5e5fb6.png)
VIP文章
![course-privilege](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-4.320a6894.png)
课程特权
![rights](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-icon.fe0226a8.png)
开通VIP
上传资源 快速赚钱
我的内容管理 展开
我的资源 快来上传第一个资源
我的收益
登录查看自己的收益我的积分 登录查看自己的积分
我的C币 登录后查看C币余额
我的收藏
我的下载
下载帮助
![voice](https://csdnimg.cn/release/downloadcmsfe/public/img/voice.245cc511.png)
![center-task](https://csdnimg.cn/release/downloadcmsfe/public/img/center-task.c2eda91a.png)
安全验证
文档复制为VIP权益,开通VIP直接复制
![dialog-icon](https://csdnimg.cn/release/downloadcmsfe/public/img/green-success.6a4acb44.png)