Luận án nghiên cứu điều khiển thích nghi cho robot lặn tự hành

1 MỞ ĐẦU Cho đến ngày nay, phương pháp điều khiển truyền thống để thiết kế hệ thống điều khiển tiên tiến cho phương tiện thủy điển điều khiển tỷ lệ - vi phân – tích phân (PID) cịn phổ biến có cấu trúc đơn giản tính bền vững cao, dễ tổng hợp thiết kế Tuy nhiên, việc cải tiến chất lượng điều khiển PID hệ thống điều khiển ln đề tài nóng hổi cho nhà nghiên cứu Bởi thiết kế điều khiển cho phương tiện thủy, điều khiển PID thường u cầu phải có mơ hình động học phương tiện thủy Nhưng thực tế, tính động học chúng thường mang tính phi tuyến cao chịu ảnh hưởng nhiều yếu tố nhiễu loạn bên Đặc biệt phương tiện lặn tự hành (AUV) môi trường bậc tự Các yếu tố nhiễu loạn bên mang đặc tính phi tuyến khơng xác định cao Điều dẫn đến việc phải xây dựng cấu trúc tham số không xác định đồng thời yêu cầu phải có kỹ thuật điều khiển tiên tiến Vì vậy, nhà khoa học không ngừng nghiên cứu cải tiến chất lượng điều khiển PID theo hai hướng: là, cải tiến cấu trúc điều khiển PID; hai kết hợp lý thuyết lô-gic mờ, mạng nơ-ron nhân tạo, thuật toán di truyền lý thuyết điều khiển thông minh khác với điều khiển PID thông thường để đạt chất lượng điều khiển mong muốn Do vậy, điều khiển kết hợp gọi điều khiển PID thông minh Bộ điều khiển PID thơng minh khơng u cầu phải có mơ hình tốn học đối tượng cách xác, tham số hệ thống có tính bền vững Hiện nay, mạng nơ-ron nhân tạo (Artificial Neural Network) ứng dụng rộng rãi nhiều lĩnh vực, có lĩnh vực điều khiển tự động, mạng nơ-ron giải vấn đề điều khiển phức tạp, với đối tượng điều khiển có tính phi tuyến cao, có mơi trường bên ngồi tác động khơng dự đốn được, làm cho tính đối tượng trở nên khó điều khiển Hơn nữa, khả tính tốn nhanh mạng nơ-ron làm cho chúng trở nên khả thi với ứng dụng điều khiển theo thời gian thực [1], [2], [3], [5], [6], [33] Để giải toán có yếu tố khơng xác định mơ hình động học tàu thủy mặt nước hay phương tiện ngầm nhiễu loạn mơi trường bên ngồi sóng, gió, dịng chảy…, phương pháp sử dụng mạng nơ-ron nhân tạo chứng tỏ hiệu tính khả thi điều khiển [8], [9], [14], [61-64] đề xuất, nghiên cứu phát triển nhiều giới thời gian gần Tính cấp thiết đề tài Với lợi biển, Việt Nam có chủ trương phát triển mạnh ngành vận tải biển cơng nghiệp đóng tàu, quản lý khai thác tài nguyên biển theo hướng hội nhập quốc tế đáp ứng nhu cầu xã hội với mục tiêu “phát triển kinh tế biển phù hợp với Chiến lược biển Việt Nam đến năm 2020, phục vụ nhu cầu phát triển kinh tế - xã hội; góp phần củng cố quốc phịng, an ninh bảo vệ chủ quyền quốc gia vùng biển hải đảo Tổ quốc” Tại Việt Nam, việc nghiên cứu hệ thống điều khiển tiên tiến cho AUV non trẻ chưa ứng dụng rộng rãi Mặc dù nhu cầu ứng dụng AUV cho khảo sát biển, cơng trình ngầm nước, thăm dị bảo trì đường ống hay cáp ngầm, tìm kiếm cứu nạn biển Việt Nam ngày tăng cao Chúng ta chủ yếu thuê mua AUV nước ngồi cho cơng tác Những nghiên cứu nhằm nâng cao chất lượng hệ thống điều khiển AUV, bước tự chủ công nghệ chế tạo, vận hành AUV Việt Nam cần thiết cho ngành khai thác, thăm dò, quản lý biển phục vụ an ninh quốc phòng, bảo vệ biển đảo chủ quyền quốc gia biển Vì nghiên cứu điều khiển AUV vấn đề quan trọng cho nghiệp đại hóa ngành chế tạo điều khiển phương tiện ngầm Việt Nam Từ lý trên, tác giả chọn đề tài “Nghiên cứu điều khiển thích nghi cho robot lặn tự hành” Mục đích nghiên cứu đề tài Trên sở nghiên cứu phương pháp điều khiển AUV thông thường đại, nghiên cứu thuật toán điều khiển với mạng nơ-ron nhân tạo Mục đích nghiên cứu đề tài phát triển ứng dụng điều khiển nơ-ron thích nghi cho hệ thống điều khiển hướng độ sâu AUV, qua cải tiến nâng cao chất lượng điều khiển cho loại phương tiện này, nhằm đưa điều khiển có tính ứng dụng thực tiễn Từ đó, thiết kế thử nghiệm điều khiển mô thực nghiệm Đối tượng phạm vi nghiên cứu Nghiên cứu thuật toán điều khiển nơ-ron điều khiển thích nghi dựa hoạt động mạng nơ-ron nhân tạo Đề xuất phát triển thuật toán ứng dụng vào thiết kế Bộ điều khiển nơ-ron thích nghi cho điều khiển hướng đi, độ sâu tốc độ AUV Sử dụng phần mềm Matlab để mô điều khiển đề xuất, đánh giá chất lượng điều khiển nơ-ron thích nghi phương án điều khiển AUV ứng dụng vào thực tiễn Phương pháp nghiên cứu Phân tích, tổng hợp hệ thống điều khiển AUV dựa mạng nơ-ron thích nghi Nghiên cứu ứng dụng thuật tốn điều khiển nơ-ron thích nghi cho hệ thống điều khiển hướng đi, độ sâu tốc độ AUV Phương pháp mô sử dụng Matlab làm công cụ để mô kiểm chứng lại tính khả thi xác thuật tốn Phương pháp so sánh: so sánh với điều khiển PID thơng thường để chứng minh tính hiệu điều khiển nơ-ron Ý nghĩa khoa học thực tiễn Kết nghiên cứu luận án đóng góp lý thuyết, làm tài liệu tham khảo cho sinh viên, học viên, nghiên cứu sinh người quan tâm đến lĩnh vực điều khiển Thuật tốn điều khiển kết nghiên cứu ứng dụng vào nghiên cứu chế tạo điều khiển có chất lượng tốt phục vụ cho điều khiển AUV thực tế, góp phần vào phát triển kinh tế biển đất nước Đây ý nghĩa khoa học thực tiễn đề tài Những điểm đóng góp luận án Luận án có kết đóng góp cho việc nghiên cứu lý thuyết thực nghiệm điều khiển AUV sau: - Hệ thống hóa lý thuyết thực tiễn điều khiển AUV Việt Nam giới - Đề xuất BĐK nơ-ron thích nghi cho tình điều khiển chuyển động AUV mạng nơ-ron thích nghi trực tuyến - Khảo sát thuật toán đề xuất ứng dụng điều khiển AUV với tình chuyển động thực tế mơ máy tính - Đề xuất hệ thống điều khiển dẫn đường cho AUV sử dụng BĐK nơron, tăng cường khả thích nghi cách nâng cấp hàm mục tiêu hàm huấn luyện mạng nơ-ron Kết cấu luận án Luận án gồm nội dung phân bố thành phần sau: - Mở đầu - Chương 1: Tổng quan điều khiển robot lặn tự hành (Autonomous Underwater Vehicle) - Chương 2: Cơ sở lý thuyết điều khiển nơ-ron thích nghi thiết bị lặn tự hành - Chương 3: Thiết kế hệ thống điều khiển nơ-ron thích nghi cho thiết bị lặn tự hành - Chương 4: Mô điều khiển AUV mạng nơ-ron thích nghi - Kết luận CHƯƠNG TỔNG QUAN VỀ ĐIỀU KHIỂN ROBOT LẶN TỰ HÀNH (AUTONOMOUS UNDERWATER VEHICLE) 1.1 Tổng quan robot lặn tự hành AUV (Autonomous Underwater Vehicle) Nước ta có bờ biển dài 3000 km diện tích lớn ao hồ, đầm lầy… Việc thăm dò khai thức nguồn tài nguyên lòng đại dương gặp phải hạn chế vùng biển sâu Đối với cơng trình biển giàn khoan, đường ống dẫn dầu, đường dây cáp quang… trình xây dựng khai thác nhu cầu thăm dị, khảo sát, tiến hành cơng việc nước tất yếu Trong quân sự, việc rà quét tháo gỡ thủy lơi, mìn làm vùng nước sau chiến tranh chuẩn bị cho việc đổ tác chiến… tiến hành nước Các công việc cứu hộ, cứu nạn, trục vớt biển phát sinh giao thông vận tải ngày phát triển Vấn đề đặt công việc nước thực môi trường khắc nghiệt độ sâu, sóng, gió, nhiễm, nguy hiểm …, với công cụ thô sơ, thợ lặn làm việc nước khả cịn giới hạn tính rủi ro cao Vì vậy, trợ giúp công cụ nghiên cứu, giám sát đại việc làm tất yếu Đó lý quan trọng để nghiên cứu phát triển loại phương tiện, thiết bị ngầm phục vụ cho nhiệm vụ nước phương tiện ngầm điều khiển từ xa, có khả tự hành chọn làm đề tài nghiên cứu luận án Hiện nay, giới có nhiều loại phương tiện ngầm khác để thực công việc nước phổ biến có hai loại sau: phương tiện ngầm điều khiển từ xa (tên tiếng Anh: Remotely Operated Vehicles, tên viết tắt: ROV), phương tiện ngầm tự hành (tên tiếng Anh: Autonomous Underwater Vehicles, tên viết tắt: AUV)…Mỗi loại có tính ưu việt ứng dụng riêng, phạm vi luận án tác giả nghiên cứu điều khiển phương tiện ngầm điều khiển từ xa có khả tự hành - gọi AUV Tuy nhiên, tác giả giới thiệu sơ lược AUV, nhằm cung cấp kiến thức tổng quan loại phương tiện ngầm 1.1.1 Các ứng dụng AUV giới Trên giới có nhiều nước phát triển mạnh điều khiển phương tiện tự hành nước với công nghệ điều khiển tích hợp cao Na Uy, Mỹ, Nga Pháp Các mẫu phương tiện tự hành nước trình bày cụ thể bảng 1.1 [7], [10], [11], [12], [13]: Bảng 1 Một số mẫu AUV giới AUV Mô tả SPURV - SPURV phát triển trường Đại học Washington, Mỹ, 1957 - Lặn sâu 3.000m thời gian lặn liên tục - Có khả đo nhiệt độ độ truyển sử dụng để hỗ trợ nghiên cứu hải dương học, bao gồm nghiên cứu truyền tải âm phát tàu ngầm (SelfPropelled Underwater Research Vehicle) Epaulard - Epaulard chế tạo Viện nghiên cứu đại dương (IFREMER), Pháp, 1980 - Dài 4m nặng 2,9 - Lặn sâu tới 6.000m, sử dụng nghiên cứu đại đại dương AUSS - AUSS chế tạo trung tâm nghiên cứu quân đại dương không gian (SPAWAR), Mỹ, 1983 - Hoạt động độ sâu lên đến 6.000m - Có thể chụp truyền hình ảnh đáy đại dương thơng qua Hình ảnh máy truyền âm tốc độ lên đến 4.800 bít/giây - Được trang bị Sonar quét bên Sonar nhìn phía trước để giúp xác định vị trí đối tượng lạ đại dương REMUS 6000 - REMUS 6000 chế tạo tập đoàn Kongsberg Maritime, Na Uy, 1997 - Có thể lặn sâu tới 6.000m - Phục vụ nghiên cứu giám sát, thăm dò lập đồ đại dương - Dễ dàng tùy biến cho tác vụ dân quân khác trang bị Sonar chức SEAOTE R MKII - SEAOTTER MKII chế tạo tập đoàn Atlas Elektronik, Đức, 2007 - Chiều dài 3,65m,trọng lượng 1000 kg, chiều sâu lặn tới 600m, tải trọng mang thêm đến 160kg thời gian hoạt động lần 20 - Tác vụ: thăm dị khai khống tài ngun biển, chống xâm nhập tàu ngầm, trinh sát giám sát vùng kinh tế biển đảo lập đồ Bluefin-9 - Bluefin-9 phát triển tập đoàn Bluefin Robotics, Mỹ, 2010 - Trọng lượng 60,5kg, Kích thước L x W = 1,65m x 0,24m, - Lặn sâu lớn 200m, tốc độ di chuyển 2m/s, thời gian hoạt động lần 12giờ - Tác vụ: thăm dò khai khống tài ngun biển, theo dõi bảo vệ mơi trường, trinh sát giám sát vùng kinh tế biển đảo, bảo vệ hải cảng giàn khoan Các ứng dụng AUV: Quân sự: AUV kết hợp với công nghệ sonar cho phép phát vật thể không xác định để đảm bảo an toàn cho khu vực biển, điều đặc biệt quan trọng chiến lược chống chiến tranh biển Khai thác: Khả khai thác khoáng sản AUV thể khả dị tìm kim loại quặng lịng đại dương, thay trước phải dị tìm tàu với độ tin cậy thấp Khơng thế, AUV qt vùng có nhiều hải sản cho phép tàu bè đánh bắt với hiệu suất cao Nghiên cứu: Khai phá tầng đại dương, nghiên cứu sống nằm sâu đáy biển, mặt cắt địa chất, mẫu nước, mẫu vật chất gửi lên bờ để phân tích xử lí Mơi trường: Sử dụng công cụ tiến tiến camera kỹ thuật số, camera hồng ngoại, sóng siêu âm… để giám sát tác động bào mòn, tắc nghẽn, vết nứt cơng trình biển Bám theo đường ống dẫn dầu, dẫn khí để kiểm tra có hay khơng rò rỉ, xác định độ bền ống, đảm bảo xử lí cố nhanh Nếu có cố nứt, rị rỉ xảy ra, AUV trang bị cánh tay robot thực chức cắt, hàn, nối… tránh dầu, khí chảy loang biển gây ô nhiễm Theo thống kê [10], loại AUV chức năng, nhiệm vụ kể đến Bảng 1.2 Bảng Thống kê tàu tự hành AUV Quốc gia Năm Mỹ 2000 FENRIR Chuyên chở UUV Khảo sát bảo vệ bến cảng Sentry 2003 SWIMS SeaFox Kiểm tra khả thích ứng kết hợp định vị, điều khiển, dẫn đường 2004 Springer Tên thiết bị C-series AUVs Điều tra mỏ khoáng sản Khảo sát mơi trường, mẫu thí nghiệm giảng dạy Ứng dụng Kiểm tra tính ổn định, khảo sát mơi trường 2011 MUSCL Trinh sát, phòng thủ quân Anh 2000 Barracuda 2008 Blackfish Gầy C –Enduro USV Giám sát hải cảng Ứng dụng lượng mặt trời cho tàu tự hành để giám sát môi trường biển Canada 2000 HammerHead 2004 2005 2007 Gần Italy 2004 2008 SESAMO Charlie ALANIS Kingfisher 2000 DELFIM U-Rangger Quan trắc môi trường Quan trắc môi trường Quan trắc môi trường Phục vụ quân CARVELA Giao tiếp với UUV Phục vụ quân Bồ Đào Nha 2006 Swordfish 2008 Kaasboll 2008 Viknes Israel 2007 Kiểm tra hệ thống định vị điều khiển Kiểm tra nhiều mục đích Silver Marlin Đức 2005 Basil Pháp Singapore 2010 2007 2008 Venus Inspector Tianxiang One Nhiều ứng dụng Trung Quốc 2010 USV-ZhengHe Nhật Bản 2004 UMV series Phát mục tiêu biển Mô lại khả tránh mối đe dọa Lập đồ Quan trắc môi trường Giám sát, trinh sát Khảo sát đường ống khơi Giám sát, trinh sát Khảo sát khí tượng Thu thập liệu mơi trường thủy văn Khảo sát môi trường biển Nguồn: Báo cáo phân tích xu hướng cơng nghệ: “Xu hướng nghiên cứu ứng dụng robot ngầm tự hành phục vụ quan trắc môi trường, khảo sát sông hồ cứu hộ cứu nạn” – Sở KHCN TP Hồ Chí Minh 2019 1.1.2 Các ứng dụng AUV Việt Nam Cùng với phát triển công nghệ, thiết bị di chuyển nước tự động (Autonomous Underwater Vehicle - AUV) ngày trở nên phổ biến Do phương tiện tự hành nước không cần người lái nhằm thực nhiệm vụ khảo sát nghiên cứu quân với nhiều ưu điểm vượt trội thay người nên Việt Nam có nghiên cứu phát 10 triển AUV góp mặt nhiều ứng dụng khác nhau, từ quân đến dân sự, từ kinh tế đến nghiên cứu khoa học [7], [10] Tại Việt Nam, AUV nghiên cứu năm gần Những AUV nhóm nghiên cứu robot phát triển với mục đích đơn xây dựng mơ hình nhỏ để nghiên cứu lý thuyết, với kết cấu tương đối đơn giản, ví dụ robot cá với khả di chuyển sinh học, mơ hình AUV, ROV Hình 1.1 Nghiên cứu AUV mơ hình trường ĐH Bách khoa Hà Nội [7] Hình 1.2 KIAL – AUV ĐH Bách khoa TP Hồ Chí Minh phối hợp nghiên cứu với Hàn Quốc [10] 125 PL2 File mô điều khiển hướng, độ sâu tốc độ AUV % Submarine control test % Decoupled control of speed, depth and course % Date: 2009/May/01st % Author: PHAM VIET ANH – HCM Unv Of Transport %====================================================================== clear; % clear all variables NN = 6000; % number of sample h = 0.1; % sample time (sec) i = 0; % -z = 0; z_s = 0; z_n = 0; delta = 0; delta_s = 0; n = 150; u = 0.3; max_delta = 20*(pi/180); max_delta_s = 20*(pi/180); max_n = 1500; % -% Controller's parameters of the rudder rho = 0.5; lambda = 0.1; sigma1 = 0.2; sigma2 = 0; % -% Controller's parameters of the P&S stern planes rho_s = 0.2; lambda_s = 0.1; sigma1_s = 0.2; sigma2_s = 0.01; % -% Controller's parameters of the propeller rho_n = 1.5; lambda_n = 0.1; sigma1_n = 0.2; sigma2_n = 0.05; % -% ui = [ delta_r delta_s delta_b delta_bp delta_bs n ]' % initial states: x = [ u v w p q r x y z phi theta psi ]' x = [0.3 0 0 0 -10 0 0*pi/180]'; U = 0.3; %*************** START SIMULATION disp('Running ') ******************* % We start executing the controller with ralative small weights: W21 = rand(6,4)*0.0001; % Set random hidden layer weights W32 = rand(1,6)*0.0001; % Set random output layer weights W21_s = rand(6,4)*0.0001; % Set random hidden layer weights W32_s = rand(1,6)*0.0001; % Set random output layer weights W21_n = rand(6,4)*0.0001; % Set random hidden layer weights W32_n = rand(1,6)*0.0001; % Set random output layer weights net_in = zeros(4,1); net_in_s = zeros(4,1); net_in_n = zeros(4,1); % Set initial inputs of neural network while i < NN, i=i+1; time = (i-1)*h; % simulation time in seconds clc; fprintf('Running \n*Counter = %d (%d)\n',round(i/10),round((NNi)/10)); 126 r = w = udot = u = q = theta= x(6); x(3); u-x(1); x(1); x(5); x(11); % Sensor noise for the heading sensor with a uniform distribution on % [-0.1,+0.1] deg % s = 0.1*(pi/180)*(2*rand - 1); s = 0; % This allow us to remove the noise psi = x(12) + s; % Heading of the ship with sensor noise Z = x(9); % % % % psi = x(6); % control system % Phuong an 1: if i=2000, psi_ref(i) = 0*(pi/180); end if i>=4000, psi_ref(i) = 15*(pi/180); end % Phuong an psi_ref(i) = 10*(pi/180); Z_ref(i) = -2; if i=3000, ud(i) = 0.5; end ud(i) = 0.4; err = psi-psi_ref(i); % Do lech huong di so voi huong dat net_in(1) = err; err_s = Z-Z_ref(i); net_in_s(1) = err_s; err_n = -u+ud(i); net_in_n(1) = err_n; ii=round(i/10); if round(i/10)==0,ii=2,end % store data for presentation xout(i,:) = [time,x',U,delta,delta_s,n]; e_out(ii,:) = [err; err_s;err_n]; % % Dau cua BDK duoc tinh toan giay lan (theo bien "ii") if ii>3 % Calculate rudder command: delta = mlnnc(1,6,4,net_in,W21,W32); delta_s = mlnnc(1,6,4,net_in_s,W21_s,W32_s); n = mlnnc(1,6,4,net_in_n,W21_n,W32_n)*1500; if abs(delta)>max_delta,delta=sign(delta)*max_delta;end if abs(delta_s)>max_delta_s,delta_s=sign(delta_s)*max_delta_s;end if abs(n)>max_n,n=sign(n)*max_n;end ui = [delta delta_s 0 127 n ]; [xdot,U] = npsauv(x,ui); % numerical integration x = euler2(xdot,x,h); % Euler integration z = z + h*err; z_s = z_s + h*err_s; z_n = z_n + h*err_n; % Now update the weights of NN using adaptive interaction algorithm % (intensive training): [W21,W32] = annaiTrainIte(h,0.5,50,1,6,4,net_in, err,delta,r,z,rho,lambda,sigma1,sigma2); % Add integration action [W21_s,W32_s] = annaiTrainIte(h,0.5,50,1,6,4,net_in_s, err_s,delta_s,w,theta,rho_s,lambda_s,sigma1_s,sigma2_s); % Add integration action [W21_n,W32_n] = annaiTrainIte(h,0.5,50,1,6,4,net_in_n, err_n,n/1500,udot,z_n,rho_n,lambda_n,sigma1_n,sigma2_n);% Add integration action net_in(2) = e_out(ii-1,1); net_in(3) = e_out(ii-2,1); net_in(4) = e_out(ii-3,1); net_in_s(2) = e_out(ii-1,2); net_in_s(3) = e_out(ii-2,2); net_in_s(4) = e_out(ii-3,2); net_in_n(2) = e_out(ii-1,3); net_in_n(3) = e_out(ii-2,3); net_in_n(4) = e_out(ii-3,3); end % end of "if ii>3" % end % time-series t = xout(:,1); u = xout(:,2); v = xout(:,3); w = xout(:,4); p = xout(:,5); q = xout(:,6); r = xout(:,7)*180/pi; X = xout(:,8); Y = xout(:,9); Z = xout(:,10); phi = xout(:,11); theta = xout(:,12)*180/pi; psi = xout(:,13)*180/pi; U = xout(:,14); delta = -xout(:,15)*180/pi; delta_s = xout(:,16)*180/pi; n = xout(:,17); psi_ref = psi_ref*180/pi; 128 % PLOT THE RESULTS figure(1) clf subplot(211) plot(t,psi_ref,'LineWidth',2,'Color','r','LineStyle',' ') hold on plot(t,psi,'LineWidth',2,'Color','b','LineStyle','-') title('NNC control - Yaw angle \psi (deg)'),ylabel('[Deg]'),grid on legend('Set course','Actual course','Location','Best') %axes([0, -5, 600, 20]) subplot(212) plot(t,delta,'LineWidth',2,'Color','b','LineStyle','-') title('Rudder angle \delta_R (deg)') xlabel('Time (s)'),ylabel('[Deg]'),grid on % legend('ANNAI controller','PID controller') figure(2) clf subplot(311) plot(t,Z_ref,'LineWidth',2,'Color','r','LineStyle',' ') hold on plot(t,Z,'LineWidth',2,'Color','b','LineStyle','-') title('NNC control - Actual depth Z (m)'),ylabel('[m]'),grid on legend('Desired depth','Actual depth','Location','Best') subplot(312) plot(t,theta,'LineWidth',2,'Color','b','LineStyle','-') title('Pitch angle \theta (deg)'),ylabel('[Deg]'),grid on subplot(313) plot(t,delta_s,'LineWidth',2,'Color','b','LineStyle','-') title('Plane angle \delta_S (deg)') xlabel('Time (s)'),ylabel('[Deg]'),grid on figure(3) clf subplot(211) plot(t,ud,'LineWidth',2,'Color','r','LineStyle',' ') hold on plot(t,u,'LineWidth',2,'Color','b','LineStyle','-') title('NNC control - Actual speed U (m/s)'),ylabel('[m/s]'),grid on legend('Desired speed','Actual speed','Location','Best') subplot(212) plot(t,n,'LineWidth',2,'Color','b','LineStyle','-') title('Propeller speed n (rpm)') xlabel('Time (s)'),ylabel('[rpm]'),grid on figure(4) clf plot3(X,Y,Z,'LineWidth',2,'Color','b','LineStyle','-') grid on title('Simulation of the AUV control by FC') xlabel('X(m)'),ylabel('Y(m)'),zlabel('Z(m)') % >>>>>>>>>END y_los = pos_y + n*L; else % delta_y < y_los = pos_y - n*L; end else % delta_x~=0 D = delta_y/delta_x; E = x_k_1; F = y_k_1; G = -D*E + F; A = + D^2; B = 2*(D*G - D*pos_y - pos_x); C = pos_x^2 + pos_y^2 + G^2 - (n*L)^2 - 2*G*pos_y; if delta_x > x_los = (-B + sqrt(B^2 - 4*A*C))/(2*A); else % delta_x < x_los = (-B - sqrt(B^2 - 4*A*C))/(2*A); end y_los = D*(x_los - E) + F; end % End of "if delta_x==0" % Next, we calculate the psi_los (from to 2*pi) if x_los == pos_x if y_los > pos_y 135 psi_los = pi/2; else psi_los = 3*(pi/2); end elseif x_los > pos_x K = (y_los - pos_y)/(x_los - pos_x); if y_los > pos_y psi_los = atan(K); elseif y_los < pos_y psi_los = 2*pi + atan(K); else % if y_los == pos_y psi_los = 0; end else % if x_los < pos_x K = (y_los - pos_y)/(x_los - pos_x); if y_los ~= pos_y psi_los = pi + atan(K); % elseif y_los < pos_y % psi_los = 2*pi + atan2(y_los - pos_y,x_los - pos_x); else % if y_los == pos_y psi_los = pi; end end %>>>>>>>>>>>END