将轨迹修改为正方形怎么修改具体代码
时间: 2024-03-27 21:41:23 浏览: 16
以下是修改后的具体代码示例:
```matlab
clear all; close all;
ts = 0.001;
% 定义正方形的四个顶点坐标
x1 = 0; y1 = 0;
x2 = 1; y2 = 0;
x3 = 1; y3 = 1;
x4 = 0; y4 = 1;
% 将轨迹点坐标改为正方形顶点坐标
xd = [x1, x2, x3, x4, x1];
yd = [y1, y2, y3, y4, y1];
thd = zeros(1, length(xd));
for k = 1:length(xd)
thd(k) = atan2(yd(k+1)-yd(k), xd(k+1)-xd(k));
end
% 初始化变量
u1 = zeros(1, length(xd));
u2 = zeros(1, length(xd));
e1 = zeros(1, length(xd));
e2 = zeros(1, length(xd));
e3 = zeros(1, length(xd));
y0 = [xd(1); yd(1); thd(1)];
M = 20;
for i = 0:M
pause(0.001);
for k = 1:length(xd)
if k==1
q=y0;
end
xp(k) = q(1);
yp(k) = q(2);
th(k) = q(3);
qd = [xd(k); yd(k); thd(k)];
ce1(k) = qd(1)-q(1);
ce2(k) = qd(2)-q(2);
ce3(k) = qd(3)-q(3);
% 控制器参数调整
K1 = 2.0;
K2 = 2.0;
K3 = 1.0;
u = [u1(k); u2(k)];
B = ts*[cos(q(3)) 0; sin(q(3)) 0; 0 1];
L1 = K1*[cos(q(3)) sin(q(3)) 0; 0 0 1];
L2 = K2*[cos(q(3)+pi/2) sin(q(3)+pi/2) 0; 0 0 1];
cond = norm(eye(2)-L1*B);
U = u + L1*[e1(k);e2(k);e3(k)] + L2*[ce1(k);ce2(k);ce3(k)];
u1(k) = U(1);
u2(k) = U(2);
u = [u1(k);u2(k)];
q = q + B*u;
e1(k) = cos(q(3))-q(1);
e2(k) = sin(q(3))-q(2);
e3(k) = qd(3)-q(3);
end
% 绘制轨迹图和误差曲线
figure(1); hold on;
plot(xd,yd,'r',xp,yp,'b');
xlabel('xd xp'); ylabel('yd,yp');
j = i+1;
times(j) = j-1;
e1i(j) = max(abs(ce1));
e2i(j) = max(abs(ce2));
e3i(j) = max(abs(ce3));
end
% 绘制最终轨迹图和误差曲线
figure(2);
plot(xd,yd,'r',xp,yp,'b');
xlabel('xd xp'); ylabel('yd,yp');
figure(3);
plot(times,e1i,'*-r',times,e2i,'o-b',times,e3i,'o-k');
title('Change of maximum absolute value of e1,e2 and angle with times i');
xlabel('times'); ylabel('e1,e2 and angle');
```
需要注意的是,这份代码中只是将轨迹点坐标修改为了正方形的顶点坐标,并没有进行控制器参数的优化。如果要使控制器能够更好地跟踪正方形轨迹,可能需要根据实际情况进行参数调整和算法优化。
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)