1 简介
水下机器人机械手系统仿真含Matlab源码
2 部分代码
classdef UvmsGraphics
properties
uvms_dynamics;
end
methods
function obj = UvmsGraphics(uvms_dynamics)
if nargin == 1
obj.uvms_dynamics = uvms_dynamics;
else
error('Not enough parameters');
end
end
%% Update UVMS Dynamics
function obj = UpdateUvmsDynamics(obj, uvms_dynamics)
obj.uvms_dynamics = uvms_dynamics;
end
%% Draw Robot
% draw vehicle
function DrawVehicle(obj)
vehicle_vertex = obj.uvms_dynamics.uvms_kinematics.GetVehicleGeometry();
% color definition
orange = [1 0.5 0];
gray = [0.6 0.6 0.6];
blue = [0.25 0.4 0.9];
% draw up & down surfaces
ff1 = fill3(vehicle_vertex(1,[1 2 10 9 1]),vehicle_vertex(2,[1 2 10 9 1]),vehicle_vertex(3,[1 2 10 9 1]),orange);
ff2 = fill3(vehicle_vertex(1,[3 4 12 11 3]),vehicle_vertex(2,[3 4 12 11 3]),vehicle_vertex(3,[3 4 12 11 3]),orange);
ff3 = fill3(vehicle_vertex(1,[13 14 15 16 13]),vehicle_vertex(2,[13 14 15 16 13]),vehicle_vertex(3,[13 14 15 16 13]),orange);
ff4 = fill3(vehicle_vertex(1,[5 6 7 8 5]),vehicle_vertex(2,[5 6 7 8 5]),vehicle_vertex(3,[5 6 7 8 5]),gray);
% draw front & back surfaces
ff5 = fill3(vehicle_vertex(1,[1 5 8 4 12 16 13 9 1]),vehicle_vertex(2,[1 5 8 4 12 16 13 9 1]),vehicle_vertex(3,[1 5 8 4 12 16 13 9 1]),blue);
ff6 = fill3(vehicle_vertex(1,[2 6 7 3 11 15 14 10 2]),vehicle_vertex(2,[2 6 7 3 11 15 14 10 2]),vehicle_vertex(3,[2 6 7 3 11 15 14 10 2]),blue);
% draw left & right surfaces
ff7 = fill3(vehicle_vertex(1,[1 2 6 5 1]),vehicle_vertex(2,[1 2 6 5 1]),vehicle_vertex(3,[1 2 6 5 1]),gray);
ff8 = fill3(vehicle_vertex(1,[3 4 8 7 3]),vehicle_vertex(2,[3 4 8 7 3]),vehicle_vertex(3,[3 4 8 7 3]),gray);
ff9 = fill3(vehicle_vertex(1,[9 10 14 13 9]),vehicle_vertex(2,[9 10 14 13 9]),vehicle_vertex(3,[9 10 14 13 9]),gray);
ff10 = fill3(vehicle_vertex(1,[12 11 15 16 12]),vehicle_vertex(2,[12 11 15 16 12]),vehicle_vertex(3,[12 11 15 16 12]),gray);
% set transparency
% set(ff1,'FaceAlpha',1);
% set(ff2,'FaceAlpha',1);
% set(ff3,'FaceAlpha',1);
% set(ff4,'FaceAlpha',1);
% set(ff5,'FaceAlpha',1);
% set(ff6,'FaceAlpha',1);
% set(ff7,'FaceAlpha',1);
% set(ff8,'FaceAlpha',1);
% set(ff9,'FaceAlpha',1);
% set(ff10,'FaceAlpha',1);
end
% draw manipulator
function DrawManipulator(obj)
[po_1_I, po_2_I, po_3_I, p_L2_left_I, p_L2_right_I, p_L2_start_I, p_L2_end_I] = ...
obj.uvms_dynamics.uvms_kinematics.GetManipulatorGeometry();
% draw link 1
obj.DrawLink(po_1_I, po_2_I, obj.uvms_dynamics.uvms_kinematics.r1);
obj.DrawLink(p_L2_left_I, p_L2_right_I, obj.uvms_dynamics.uvms_kinematics.r1);
% draw link 2
obj.DrawLink(p_L2_start_I, p_L2_end_I, obj.uvms_dynamics.uvms_kinematics.r2);
% draw link 3
obj.DrawLink(p_L2_end_I, po_3_I, obj.uvms_dynamics.uvms_kinematics.r3);
end
% draw desired trajectory of end-effector
function DrawEndEffectorDesiredTrajectory(obj)
cx = obj.uvms_dynamics.uvms_kinematics.p_ee_init(1);
cy = obj.uvms_dynamics.uvms_kinematics.p_ee_init(2);
cz = obj.uvms_dynamics.uvms_kinematics.p_ee_init(3) + obj.uvms_dynamics.uvms_kinematics.r_ee_traj;
alpha = 0:pi/1000:2*pi;
x = 0*alpha + cx;
y = obj.uvms_dynamics.uvms_kinematics.r_ee_traj*cos(alpha) + cy;
z = obj.uvms_dynamics.uvms_kinematics.r_ee_traj*sin(alpha) + cz;
plot3(x,y,z,'Color',[0 0.8 0],'LineWidth',3);
end
%% Plot Variables
% draw end-effector trajectory
function DrawEndEffectorTracjectory(obj, p_ee, p_ee_d)
figure
set(gcf,'outerposition',get(0,'screensize'))
plot3(p_ee_d(1,:),p_ee_d(2,:),p_ee_d(3,:),'b','LineWidth',2)
hold on
plot3(p_ee(1,:),p_ee(2,:),p_ee(3,:),'Color',[0 0.8 0],'LineWidth',4);
hold on
plot3(p_ee(1,1),p_ee(2,1),p_ee(3,1),'ok','MarkerFaceColor','y','MarkerSize',10)
hold on
plot3(p_ee(1,end),p_ee(2,end),p_ee(3,end),'sk','MarkerFaceColor','r','MarkerSize',10)
grid on
title('3D Trajectories')
legend('desired','actual')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')
view(90,0)
axis([0.276-0.3 0.276+0.3 -0.3 0.3 0.37+0.24-0.3 0.37+0.24+0.3])
end
% plot end-effector trajectory
function PlotEndEffectorTracjectory(obj, t_vec, p_ee, p_ee_d)
figure
set(gcf,'outerposition',get(0,'screensize'))
% p_ee_d
subplot(3,2,1)
plot(t_vec,p_ee_d(1,:))
grid on
title('p\_ee\_d')
xlabel('time/s')
subplot(3,2,3)
plot(t_vec,p_ee_d(2,:))
grid on
xlabel('time/s')
subplot(3,2,5)
plot(t_vec,p_ee_d(3,:))
grid on
xlabel('time/s')
% p_ee
subplot(3,2,2)
plot(t_vec,p_ee(1,:))
grid on
title('p\_ee')
xlabel('time/s')
subplot(3,2,4)
plot(t_vec,p_ee(2,:))
grid on
xlabel('time/s')
subplot(3,2,6)
plot(t_vec,p_ee(3,:))
grid on
xlabel('time/s')
end
% plot generalized coordinate
function PlotGeneralizedCoordinate(obj, t_vec, eta)
figure
set(gcf,'outerposition',get(0,'screensize'))
% p
subplot(3,3,1)
plot(t_vec,eta(1,:))
grid on
title('p')
xlabel('time/s')
subplot(3,3,4)
plot(t_vec,eta(2,:))
grid on
xlabel('time/s')
subplot(3,3,7)
plot(t_vec,eta(3,:))
grid on
xlabel('time/s')
% theta
subplot(3,3,2)
plot(t_vec,eta(4,:))
grid on
title('theta')
xlabel('time/s')
subplot(3,3,5)
plot(t_vec,eta(5,:))
grid on
xlabel('time/s')
subplot(3,3,8)
plot(t_vec,eta(6,:))
grid on
xlabel('time/s')
% q
subplot(3,3,3)
plot(t_vec,eta(7,:))
grid on
title('q')
xlabel('time/s')
subplot(3,3,6)
plot(t_vec,eta(8,:))
grid on
xlabel('time/s')
subplot(3,3,9)
plot(t_vec,eta(9,:))
grid on
xlabel('time/s')
end
% plot generalized velocity
function PlotGeneralizedVelocity(obj, t_vec, zeta)
figure
set(gcf,'outerposition',get(0,'screensize'))
% v
subplot(3,3,1)
plot(t_vec,zeta(1,:))
grid on
title('v')
xlabel('time/s')
subplot(3,3,4)
plot(t_vec,zeta(2,:))
grid on
xlabel('time/s')
subplot(3,3,7)
plot(t_vec,zeta(3,:))
grid on
xlabel('time/s')
% omega
subplot(3,3,2)
plot(t_vec,zeta(4,:))
grid on
title('omega')
xlabel('time/s')
subplot(3,3,5)
plot(t_vec,zeta(5,:))
grid on
xlabel('time/s')
subplot(3,3,8)
plot(t_vec,zeta(6,:))
grid on
xlabel('time/s')
% dq
subplot(3,3,3)
plot(t_vec,zeta(7,:))
grid on
title('dq')
xlabel('time/s')
subplot(3,3,6)
plot(t_vec,zeta(8,:))
grid on
xlabel('time/s')
subplot(3,3,9)
plot(t_vec,zeta(9,:))
grid on
xlabel('time/s')
end
% plot control forces
function PlotControlForces(obj, t_vec, tau_c)
figure
set(gcf,'outerposition',get(0,'screensize'))
% vehicle force
subplot(3,3,1)
plot(t_vec,tau_c(1,:))
grid on
title('vehicle force')
xlabel('time/s')
subplot(3,3,4)
plot(t_vec,tau_c(2,:))
grid on
xlabel('time/s')
subplot(3,3,7)
plot(t_vec,tau_c(3,:))
grid on
xlabel('time/s')
% vehicle torque
subplot(3,3,2)
plot(t_vec,tau_c(4,:))
grid on
title('vehicle torque')
xlabel('time/s')
subplot(3,3,5)
plot(t_vec,tau_c(5,:))
grid on
xlabel('time/s')
subplot(3,3,8)
plot(t_vec,tau_c(6,:))
grid on
xlabel('time/s')
% joint torque
subplot(3,3,3)
plot(t_vec,tau_c(7,:))
grid on
title('joint torque')
xlabel('time/s')
subplot(3,3,6)
plot(t_vec,tau_c(8,:))
grid on
xlabel('time/s')
subplot(3,3,9)
plot(t_vec,tau_c(9,:))
grid on
xlabel('time/s')
end
end
methods(Static)
% draw cylinder link
function DrawLink(pa, pb, r)
% input:
% pa dim 3x1 "starting" point
% pb dim 3x1 "ending" point
% r dim 1x1 link radius
% generate points on the cylinder aligned with x
[z, y, x] = cylinder(r*ones(41,1), 40);
x = norm(pb - pa)*x;
x2 = (pb - pa)/norm(pb - pa);
if ((x2(1) == -1) || (x2(1) == 1))
z2 = [0; 0; 1];
else
z2 = cross([1; 0; 0], x2);
z2 = z2/norm(z2);
end
y2 = cross(x2, z2);
% rotate and translate points
R = [x2 y2 z2];
for i = 1:length(x)
for j = 1:length(x)
% rotation
rr = R*[x(i,j); y(i,j); z(i,j)];
x(i,j) = rr(1);
y(i,j) = rr(2);
z(i,j) = rr(3);
% translation
x(i,j) = x(i,j) + pa(1);
y(i,j) = y(i,j) + pa(2);
z(i,j) = z(i,j) + pa(3);
end
end
h = surfl(x, y, z, [0 0 -5]);
set(h, 'facealpha', 0.5)
set(h, 'facecolor', 'interp');
set(h, 'edgecolor', 'none');
colormap(bone)
end
end
end
- 1.
- 2.
- 3.
- 4.
- 5.
- 6.
- 7.
- 8.
- 9.
- 10.
- 11.
- 12.
- 13.
- 14.
- 15.
- 16.
- 17.
- 18.
- 19.
- 20.
- 21.
- 22.
- 23.
- 24.
- 25.
- 26.
- 27.
- 28.
- 29.
- 30.
- 31.
- 32.
- 33.
- 34.
- 35.
- 36.
- 37.
- 38.
- 39.
- 40.
- 41.
- 42.
- 43.
- 44.
- 45.
- 46.
- 47.
- 48.
- 49.
- 50.
- 51.
- 52.
- 53.
- 54.
- 55.
- 56.
- 57.
- 58.
- 59.
- 60.
- 61.
- 62.
- 63.
- 64.
- 65.
- 66.
- 67.
- 68.
- 69.
- 70.
- 71.
- 72.
- 73.
- 74.
- 75.
- 76.
- 77.
- 78.
- 79.
- 80.
- 81.
- 82.
- 83.
- 84.
- 85.
- 86.
- 87.
- 88.
- 89.
- 90.
- 91.
- 92.
- 93.
- 94.
- 95.
- 96.
- 97.
- 98.
- 99.
- 100.
- 101.
- 102.
- 103.
- 104.
- 105.
- 106.
- 107.
- 108.
- 109.
- 110.
- 111.
- 112.
- 113.
- 114.
- 115.
- 116.
- 117.
- 118.
- 119.
- 120.
- 121.
- 122.
- 123.
- 124.
- 125.
- 126.
- 127.
- 128.
- 129.
- 130.
- 131.
- 132.
- 133.
- 134.
- 135.
- 136.
- 137.
- 138.
- 139.
- 140.
- 141.
- 142.
- 143.
- 144.
- 145.
- 146.
- 147.
- 148.
- 149.
- 150.
- 151.
- 152.
- 153.
- 154.
- 155.
- 156.
- 157.
- 158.
- 159.
- 160.
- 161.
- 162.
- 163.
- 164.
- 165.
- 166.
- 167.
- 168.
- 169.
- 170.
- 171.
- 172.
- 173.
- 174.
- 175.
- 176.
- 177.
- 178.
- 179.
- 180.
- 181.
- 182.
- 183.
- 184.
- 185.
- 186.
- 187.
- 188.
- 189.
- 190.
- 191.
- 192.
- 193.
- 194.
- 195.
- 196.
- 197.
- 198.
- 199.
- 200.
- 201.
- 202.
- 203.
- 204.
- 205.
- 206.
- 207.
- 208.
- 209.
- 210.
- 211.
- 212.
- 213.
- 214.
- 215.
- 216.
- 217.
- 218.
- 219.
- 220.
- 221.
- 222.
- 223.
- 224.
- 225.
- 226.
- 227.
- 228.
- 229.
- 230.
- 231.
- 232.
- 233.
- 234.
- 235.
- 236.
- 237.
- 238.
- 239.
- 240.
- 241.
- 242.
- 243.
- 244.
- 245.
- 246.
- 247.
- 248.
- 249.
- 250.
- 251.
- 252.
- 253.
- 254.
- 255.
- 256.
- 257.
- 258.
- 259.
- 260.
- 261.
- 262.
- 263.
- 264.
- 265.
- 266.
- 267.
- 268.
- 269.
- 270.
- 271.
- 272.
- 273.
- 274.
- 275.
- 276.
- 277.
- 278.
- 279.
- 280.
- 281.
- 282.
- 283.
- 284.
- 285.
- 286.
- 287.
- 288.
- 289.
- 290.
- 291.
- 292.
- 293.
- 294.
- 295.
- 296.
- 297.
- 298.
- 299.
- 300.
- 301.
- 302.
- 303.
- 304.
- 305.
- 306.
- 307.
- 308.
- 309.
3 仿真结果
4 参考文献
[1]韩林, 施昕昕. 水下机器人控制系统设计与仿真[J]. 南京工程学院学报：自然科学版, 2021, 19(2):6.
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。
部分理论引用网络文献，若有侵权联系博主删除。
扫一扫
455
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
