论文名称:基于路径参数一致的水下机器人编队与避障控制
主要内容:水下机器人;编队控制;避障控制;虚拟结构;路径参数一致
一、代码及文献获取
1.复现代码链接:哔哩哔哩工房
2.复现最终效果视频:水下机器人/无人车/欠驱无人船 编队 避障 变换队形_哔哩哔哩_bilibili
二、文章主要内容
第一步:个体跟踪控制,基于欠驱水下机器人与虚拟参考点之间的位置和速度误差反馈实现个体路径跟踪控制(等价于欠驱无人船,欠驱无人车可通用)
第二步:编队控制,考虑邻居节点状态信息,设计一致性编队控制器
第三步:避障,利用人工势函数设计基于速度修正的避障控制律,实现机器人编队与避障控制
首先,构建基于虚拟结构的机器人编队误差模型,将路径引导下的编队控制解耦为路径跟踪控制与路径参数同步。
三、部分复现结果展示
四、主要代码
x0=[x1(i),x2(i),x3(i)];y0=[y1(i),y2(i),y3(i)];
psi0=[pi/2,pi/2,pi/2]-[psi1(i),psi2(i),psi3(i)];%x0,y0,psi0为航迹中的某一确定点
l=8; %船长
b=4; %船宽
x10=x0-(l/2)*sin(psi0);
y10=y0-(l/2)*cos(psi0);
x7=x0+(l/4)*sin(psi0);
y7=y0+(l/4)*cos(psi0);
x20=x10-(b/2)*cos(psi0);
y20=y10+(b/2)*sin(psi0);
x30=x7-(b/2)*cos(psi0);
y30=y7+(b/2)*sin(psi0);
x4=x0+(l/2)*sin(psi0);
y4=y0+(l/2)*cos(psi0);
x5=x7+(b/2)*cos(psi0);
y5=y7-(b/2)*sin(psi0);
x6=x10+(b/2)*cos(psi0);
y6=y10-(b/2)*sin(psi0);
x11=[ x10(1) x20(1) x30(1) x4(1) x5(1) x6(1) x10(1)];
y11=[ y10(1) y20(1) y30(1) y4(1) y5(1) y6(1) y10(1)];
x22=[ x10(2) x20(2) x30(2) x4(2) x5(2) x6(2) x10(2)];
y22=[ y10(2) y20(2) y30(2) y4(2) y5(2) y6(2) y10(2)];
x33=[ x10(3) x20(3) x30(3) x4(3) x5(3) x6(3) x10(3)];
y33=[ y10(3) y20(3) y30(3) y4(3) y5(3) y6(3) y10(3)];
plot(x11,y11,'g','HandleVisibility','off');
plot(x22,y22,'r','HandleVisibility','off');
plot(x33,y33,'b','HandleVisibility','off');
fill(x11,y11,'g','HandleVisibility','off');
fill(x22,y22,'r','HandleVisibility','off');
fill(x33,y33,'b','HandleVisibility','off');
i=2400;
% psi=-0.1:0.1:2*pi;
% ox1=30+12*cos(psi);
% oy1=45+12*sin(psi);
% plot(ox1,oy1,'black','LineWidth',1.5,'HandleVisibility','off');
% fill(ox1,oy1,'black','HandleVisibility','off');
% ox2=200+8*cos(psi);
% oy2=150+8*sin(psi);
% plot(ox2,oy2,'black','LineWidth',1.5,'HandleVisibility','off');
% fill(ox2,oy2,'black','HandleVisibility','off');
x0=[x1(i),x2(i),x3(i)];y0=[y1(i),y2(i),y3(i)];
psi0=[pi/2,pi/2,pi/2]-[psi1(i),psi2(i),psi3(i)];%x0,y0,psi0为航迹中的某一确定点
l=8; %船长
b=4; %船宽
x10=x0-(l/2)*sin(psi0);
y10=y0-(l/2)*cos(psi0);
x7=x0+(l/4)*sin(psi0);
y7=y0+(l/4)*cos(psi0);
x20=x10-(b/2)*cos(psi0);
y20=y10+(b/2)*sin(psi0);
x30=x7-(b/2)*cos(psi0);
y30=y7+(b/2)*sin(psi0);
x4=x0+(l/2)*sin(psi0);
y4=y0+(l/2)*cos(psi0);
x5=x7+(b/2)*cos(psi0);
y5=y7-(b/2)*sin(psi0);
x6=x10+(b/2)*cos(psi0);
y6=y10-(b/2)*sin(psi0);
x11=[ x10(1) x20(1) x30(1) x4(1) x5(1) x6(1) x10(1)];
y11=[ y10(1) y20(1) y30(1) y4(1) y5(1) y6(1) y10(1)];
x22=[ x10(2) x20(2) x30(2) x4(2) x5(2) x6(2) x10(2)];
y22=[ y10(2) y20(2) y30(2) y4(2) y5(2) y6(2) y10(2)];
x33=[ x10(3) x20(3) x30(3) x4(3) x5(3) x6(3) x10(3)];
y33=[ y10(3) y20(3) y30(3) y4(3) y5(3) y6(3) y10(3)];
plot(x11,y11,'g','HandleVisibility','off');
plot(x22,y22,'r','HandleVisibility','off');
plot(x33,y33,'b','HandleVisibility','off');
fill(x11,y11,'g','HandleVisibility','off');
fill(x22,y22,'r','HandleVisibility','off');
fill(x33,y33,'b','HandleVisibility','off');
标签:cos,机器人,sin,x10,HandleVisibility,psi0,无人,y10,编队
From: https://blog.csdn.net/weixin_43789589/article/details/144666251