Lập Kế Hoạch Đường Đi Cho Tay Máy Gắn Trên Máy Bay Không Người Lái Bốn Cánh Quạt

Lập Kế Hoạch Đường Đi Cho Tay Máy Gắn Trên Máy Bay Không Người Lái Bốn Cánh Quạt

Ví dụ này trình bày cách sử dụng loại "floating" cho rigidBodyJoint và manipulatorRRT để lập kế hoạch đường đi hình học không va chạm cho hệ thống nền tảng di động. Hệ thống nền tảng di động có một khớp "floating" có thể tự do di chuyển và xoay trong không gian, do đó có sáu bậc tự do. Ví dụ này sử dụng máy bay không người lái (UAV) có gắn tay máy làm ví dụ về hệ thống nền tảng di động.

Để mô hình hóa hệ thống nền tảng di động bằng đối tượng rigidBodyTree nền tảng cố định, bạn cần định nghĩa phần di động là đối tượng rigidBody và kết nối nó với nền tảng cố định thông qua khớp "floating". Lưu ý rằng bạn không thể thực hiện tính toán động học ngược cho hệ thống nền tảng di động được mô hình hóa theo cách này. Để mô hình hóa hệ thống nền tảng di động cho tính toán động học ngược, hãy tham khảo phần Động học ngược cho Robot có nền tảng di động.

Tạo Máy Bay Không Người Làm Nền Tảng Di Động

Đầu tiên, tạo một cây rigidBody rỗng và một rigidBody có khớp floating.

mayBayDrone = rigidBodyTree(DataFormat="row");
mayBay = rigidBody("uav");
mayBay.Joint = rigidBodyJoint("base_floating_joint","floating");

Đọc file STL đa rotor để lấy hình dạng hình học của máy bay không người lái dưới dạng đối tượng triangulation.

tr = stlread("multirotor.stl");

Điều chỉnh tỷ lệ hình học để kích thước phù hợp hơn với kích thước thực của máy bay không người lái.

scaledtr = triangulation(tr.ConnectivityList,tr.Points/200);
trimesh(scaledtr);
title("Do Thi Tam Giac UAV")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
axis equal

Lưu ý rằng hình học va chạm là không lồi. Để mô hình hóa chính xác hình học này cho việc kiểm tra va chạm, hãy sử dụng hàm collisionVHACD để phân hủy nó thành tập hợp các mesh lồi gần đúng.

options = vhacdOptions("IndividualMesh",MaxNumConvexHulls=8,MaxNumVerticesPerHull=50);
compositeMeshes = collisionVHACD(scaledtr,options);

Sử dụng addCollision để thêm từng mesh đã phân hủy vào rigidBody.

for i = 1:length(compositeMeshes)
    addCollision(mayBay,compositeMeshes{i});
end

Thêm rigidBody máy bay vào cây rigidBody nền tảng di động và hiển thị máy bay.

addBody(mayBayDrone,mayBay,mayBayDrone.BaseName);
show(mayBayDrone,Collisions="on");
title("Luoi Va Cham UAV")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view([az,el])

Gắn Tay Máy Vào Máy Bay

Tải cây rigidBody của tay máy Open Manipulator từ Robotis.

tayMay = loadrobot("robotisOpenManipulator",DataFormat="row");

Để gắn tay máy vào đáy máy bay, hãy đặt phép biến đổi cố định cho khâu đầu tiên của tay máy để nó xoay pi radian theo trục y và có một dịch chuyển nhỏ theo trục z. Dịch chuyển theo trục z để tránh tự va chạm. Ngoài ra, bạn có thể chỉ định thuộc tính SkippedSelfCollisions của bộ lập kế hoạch để chỉ định các cặp liên kết bỏ qua khi kiểm tra tự va chạm. Để biết thêm chi tiết, hãy tham khảo ví dụ Bỏ Qua Kiểm Tra Tự Va Chạm Giữa Các Cặp Thân Cụ Thể.

Gắn mô hình cây rigidBody tay máy vào nền tảng di động của máy bay và hiển thị máy bay có gắn tay máy.

addSubtree(mayBayDrone,"uav",tayMay);
show(mayBayDrone,Collisions="on",Frames="off");
title("UAV Voi Tay May Gan Tren")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view(225,15)

Bai Toan Lap Ke Hoach Chuyen Dong

Bài toán lập kế hoạch chuyển động là tìm đường đi không va chạm từ cấu hình khớp bắt đầu đến cấu hình mục tiêu mong muốn. Để làm điều này, cần xác định các khía cạnh sau của bài toán:

  • Cấu hình khớp khởi đầu
  • Cấu hình khớp mục tiêu
  • Mô hình va chạm của máy bay không người lái
  • Chướng ngại vật trong môi trường
  • Ràng buộc động học của robot
Cấu Hinh Khớp Bat Dau Va Muc Tieu

Cấu hình khớp của UAV biểu diễn vị thế của nó trong hệ tọa độ thế giới. Do đó, định nghĩa vector cấu hình UAV để mô tả vị trí của nó trong hệ tọa độ thế giới là [q_uav t_uav q_robotic_arm], trong đó:

  • q_uav là hướng của khớp floating trên rigidBody "uav" so với nền tảng cố định, biểu diễn dưới dạng quaternion đơn vị 1×4.
  • t_uav là vị trí của khớp floating trên rigidBody "uav" so với nền tảng cố định, biểu diễn dưới dạng vector 1×3.
  • q_robotic_arm là vector cấu hình khớp của tay máy openmanip, biểu diễn dưới dạng vector 1×6.

Đặt cấu hình bắt đầu: phần di động "uav" ở tọa độ xyz là [0 0 -1], và góc khớp thứ ba của tay máy là -π/3 radian.

cauHinhBatDau = homeConfiguration(mayBayDrone);
cauHinhBatDau(5:7) = [0 0 -1];
cauHinhBatDau(end-3) = -pi/3;

Sử dụng cấu hình bắt đầu và sửa đổi để có cấu hình mục tiêu. Đặt cấu hình mục tiêu cho thân "uav" ở tọa độ xyz là [2 0 2], và đặt hướng của nó theo góc Euler ZYX là [π/2 0 0].

CauHinhDich = cauHinhBatDau;
CauHinhDich(5:7) = [2 0 2];
CauHinhDich(1:4) = eul2quat([pi/2 0 0]);
Them Vat Can Vao Moi Truong

Tạo môi trường bằng các hộp va chạm và đặt vị trí của các hộp này để tạo thành một bức tường giữa cấu hình bắt đầu và cấu hình mục tiêu.

moiTruong={collisionBox(0.5,4,3,Pose=trvec2tform([1.0 0.0 -1.3])),...
     collisionBox(0.5,4,1.5,Pose=trvec2tform([1.0 0.0 1.8]))};

Hiển thị bài toán lập kế hoạch. Hiển thị hình học va chạm và ẩn các khung tọa độ trên máy bay.

figure
show(mayBayDrone,cauHinhBatDau,Collisions="on",Frames="off");
hold on
showCollisionArray(moiTruong);
show(mayBayDrone,CauHinhDich,Collisions="on",Frames="off");
title("Bai Toan Lap Ke Hoach")
view(0,0)
hold off
Rang Buoc Dong Hoc Cua Robot

Trong ví dụ này, đặt giới hạn vị trí PositionLimits của khớp floating được kết hợp với rigidBody "uav" để nó bị giới hạn trong ranh giới Cartesian của khung không gian làm việc (ví dụ: lồng an toàn vật lý), tức là trục X [ -1,3], trục Y [-2,2] và trục Z [-2.5,2.5]. Lưu ý rằng giới hạn xoay của rigidBodyJoint floating không thể đặt và là NaN.

thânUAV=getBody(mayBayDrone,"uav");
thânUAV.Joint.PositionLimits(5:end,:)=[-1,3;-2,2;-2.5,2.5];

Tim Duong Di Khong Va Cham

Tìm đường đi không va chạm giữa cấu hình khớp bắt đầu và cấu hình khớp mục tiêu. Để tìm đường đi như vậy, ví dụ này sử dụng manipulatorRRT để lập kế hoạch đường đi hình học không va chạm.

rrt = manipulatorRRT(mayBayDrone,moiTruong, ...
    MaxConnectionDistance=0.5, ...
    ValidationDistance=0.1, ...
    SkippedSelfCollisions="parent");

Gieo hạt cho bộ tạo số ngẫu nhiên và sử dụng RRT để lập kế hoạch đường đi, rút ngắn đường đi đầu ra và chèn đường đi đã rút ngắn để mượt hơn.

rng(0,"twister");
duongDikeHoach = plan(rrt,cauHinhBatDau,CauHinhDich);
duongDanganRut = shorten(rrt,duongDikeHoach,40);
rrtpath = interpolate(rrt,duongDanganRut,10);

Hien Thi Duong Di Lap Ke Hoach

show(mayBayDrone,cauHinhBatDau,Collisions="on");
title("Hieu Ung Bay UAV")
view(0,0)
hold on
plot3(rrtpath(:,5),rrtpath(:,6),rrtpath(:,7),"k-",LineWidth=1)
showCollisionArray(moiTruong);
rc = rateControl(10);
for i = 1:1:size(rrtpath,1)
    show(mayBayDrone,rrtpath(i,:),...
        PreservePlot=false,...
        FastUpdate=true, ...
        Frames="off", ...
        Collisions="on");
    waitfor(rc);
    view(0,0)
end
hold off

Lưu ý rằng tay máy cũng di chuyển dọc theo đường đi để tránh va chạm với các chướng ngại vật trong môi trường. Vẽ biểu đồ góc khớp của tay máy trên đường đi đã lập kế hoạch.

plot(rrtpath(:,8:end),"-")
title("Goc Khớp Tay May")
xlabel("Thoi Gian (s)")
ylabel("Goc Khop (rad)")
legend("Khop "+string(1:6))

Đăng vào ngày 2 tháng 6 lúc 01:40

Thẻ Phổ Biến

©2026 Thành phố Cuồng loạn