Loading...
برنامه ریزی حرکت پهپاد بال ثابت با الگوریتم RRT در MATLAB
در این مقاله آموزشی متلب مثال پیش رو نحوه برنامه ریزی حرکت یک وسیله نقلیه هوایی بدون سرنشین بال ثابت (UAV) را با استفاده از الگوریتم جستجوی سریع درخت تصادفی (RRT) نشان می دهد که یک نقطه شروع و نقطه هدف روی یک نقشه سه بعدی ارائه می شود. یک پهپاد بال ثابت ماهیت غیرهولونومیک دارد و باید از محدودیت های آیرودینامیکی مانند حداکثر زاویه چرخش، زاویه مسیر پرواز و سرعت هوا هنگام حرکت بین ایستگاه های بین راهی (نقطه شروع و پایان) تبعیت کند. در این مثال شما یک نقشه سه بعدی را به همراه موقعیت شروع حرکت و هدف تنظیم می کنید و سپس مسیری را با روش RRT که یک الگوریتم برای جستجوی کارآمد غیر محدب است، و با استفاده از حرکت های اولیه 3D Dubins (فقط حرکت رو به جلو در محیط سه بعدی) طراحی می کنید، در آخر مسیر به دست آمده را صاف می کنید و شبیه سازی پرواز پهپاد (UAV) را شروع می کنید.
مراحل انجام کار
آماده سازی اولیه
1 2 3 | % Set RNG seed for repeatable result rng(1,"twister"); |
بارگذاری نقشه
نقشه اشغال احتمالاتی سه بعدی را با نام uavMapCityBlock.mat لود کنید که شامل مجموعه ای از موانع از پیش تولید شده است که وارد فضای کاری (workspace) متلب می شود. این نقشه به صورت ENU یا شرق-شمال-بالا می باشد.
1 2 3 4 5 | mapData = load("uavMapCityBlock.mat","omap"); omap = mapData.omap; % Consider unknown spaces to be unoccupied omap.FreeThreshold = omap.OccupiedThreshold; |
با استفاده از نقشه برای مرجع، موقعیت شروع اولیه و موقعیت هدف نهایی را انتخاب کنید.
1 2 3 4 5 6 7 8 9 | startPose = [12 22 25 pi/2]; goalPose = [150 180 35 pi/2]; figure("Name","StartAndGoal") hMap = show(omap); hold on scatter3(hMap,startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(hMap,goalPose(1),goalPose(2),goalPose(3),30,"green","filled") hold off view([-31 63]) |
طراحی مسیر با استفاده از روش RRT
در این مرحله با استفاده از حرکت سه بعدی Dubins یک مسیر را به کمک روش RRT طراحی کنید. الگوریتم RRT یک روش جستجوی سریع درخت تصادفی است که درخت جستجو را به صورت تدریجی از نمونه های تصادفی یک فضای حالت داده شده می سازد. درخت در نهایت فضای جستجو را در بر می گیرد و موقعیت شروع و موقعیت هدف پایانی را به هم متصل می کند. با استفاده از یک آبجکت uavDubinsConnection که محدودیت های آیرودینامیکی را برآورده می کند، دو موقعیت را به هم وصل کنید. همچنین از آبجکت validatorOccupancyMap3D برای بررسی برخورد بین پهپاد بال ثابت و محیط استفاده کنید.
تعریف آبجکت فضایی حالت
این مثال یک فضای حالت از پیش تعریف شده با نام ExampleHelperUavStateSpace را برای برنامه ریزی مسیر ارائه می دهد. فضای حالت به صورت [x y z headingAngle] تعریف می شود که در آن [x y z] موقعیت پهپاد و headingAngle زاویه سمت را بر حسب رادیان مشخص می کند. همچنین این مثال از یک آبجکت uavDubinsConnection به عنوان مدل سینماتیک برای پهپاد استفاده می کند که در آن حداکثر زاویه چرخش، سرعت هوا و زاویه مسیر پرواز تعریف می شود. با تعیین حداکثر زاویه چرخش، سرعت هوا، و زاویه مسیر پرواز، ویژگی های پهپاد را به عنوان name-value آبجکت فضای حالت ایجاد کنید. از آرگومان نام-مقدار “Bounds” برای تعیین موقعیت و مرز های جهت گیری پهپاد به عنوان یک ماتریس 4 در 2 استفاده کنید که در آن سه ردیف اول مرزهای محور x-, y- و z را در داخل محیط سه بعدی نشان می دهند. نقشه اشغال سه بعدی و آخرین ردیف نشان دهنده زاویه سمت در محدوده [-pi, pi] رادیان است.
1 2 3 4 | ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/6,... "AirSpeed",6,... "FlightPathAngleLimit",[-0.1 0.1],... "Bounds",[-20 220; -20 220; 10 100; -pi pi]); |
محدوده آستانه فضای کاری را بر اساس موقعیت هدف تعیین کنید. این آستانه مشخص می کند که فضای کاری منطقه هدف مورد نظر چقدر بزرگ باشد.
1 | threshold = [(goalPose-0.5)' (goalPose+0.5)'; -pi pi]; |
از تابع setWorkspaceGoalRegion برای به روز رسانی موقعیت هدف و منطقه اطراف آن استفاده کنید.
1 | setWorkspaceGoalRegion(ss,goalPose,threshold) |
تعریف آبجکت اعتبار سنجی وضعیت
آبجکت validatorOccupancyMap3D تعیین می کند که اگر مکان xyz روی نقشه اشغال شده باشد، یک حالت نامعتبر است. حرکت بین دو حالت فقط در صورتی معتبر است که همه حالت های میانی معتبر باشند، به این معنی که پهپاد از هیچ مکان اشغال شده روی نقشه عبور نمی کند. با مشخص کردن آبجکت فضایی حالت و نقشه ارتفاع بلند (مکان هایی روی نقشه در آن ساختمان ها و مکان ها ارتفاع بلندی دارند)، یک آبجکت validatorOccupancyMap3D ایجاد کنید. سپس فاصله اعتبارسنجی را بر حسب متر برای درونیابی بین حالت ها تنظیم کنید.
1 2 | sv = validatorOccupancyMap3D(ss,"Map",omap); sv.ValidationDistance = 0.1; |
تنظیم برنامه ریزی مسیر با RRT
یک آبجکت plannerRRT را با تعیین فضای حالت و اعتبار سنج حالت به عنوان ورودی ایجاد کنید. ویژگی های MaxConnectionDistance، GoalBias و MaxIterations آبجکت planner را تنظیم کنید و سپس یک تابع هدف سفارشی را مشخص کنید. این تابع هدف تعیین می کند که اگر فاصله اِقلیدسی تا هدف زیر آستانه 5 متر باشد، مسیر به هدف رسیده است.
1 2 3 4 5 | planner = plannerRRT(ss,sv); planner.MaxConnectionDistance = 50; planner.GoalBias = 0.10; planner.MaxIterations = 400; planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 5); |
اجرای برنامه ریزی مسیر
برنامه ریزی مسیر مبتنی بر RRT را در فضای سه بعدی انجام دهید. این برنامه ریزی، مسیری را پیدا می کند که بدون برخورد است و برای پرواز پهپاد بال ثابت UAV مناسب است.
1 | [pthObj,solnInfo] = plan(planner,startPose,goalPose); |
شبیه سازی پهپاد در مسیر برنامه ریزی شده
مسیر برنامه ریزی شده را مصور سازی کنید. مسیر برنامه ریزی شده را بر اساس اتصالات UAV Dubins درون یابی کنید. حالت های درون یابی شده را به صورت یک خط سبز ترسیم کنید. پرواز پهپاد را با استفاده از تابع کمکی ارائه شده با نام exampleHelperSimulateUAV شبیه سازی کنید که به نقاط بین راهی (نقطه شروع و پایان)، سرعت هوایی و زمان برای رسیدن به هدف (بر اساس سرعت هوایی و طول مسیر) نیاز دارد. تابع کمکی از مدل هدایت پهپاد بال ثابت برای شبیه سازی رفتار پهپاد بر اساس ورودی های کنترل شده از ایستگاه های بین راهی استفاده می کند. حالت های شبیه سازی شده را به صورت خط قرمز ترسیم کنید. توجه داشته باشید که پرواز پهپاد شبیه سازی شده به دلیل خطاهای ردیابی کوچک، کمی از مسیر برنامه ریزی شده منحرف می شود. همچنین مسیر 3D Dubins تغییرات آنی را در زاویه چرخش پهپاد اعمال می کند، اما دینامیک واقعی واکنش کندتری به دستورات رول دارد. یکی از راه های جبران این تاخیر، برنامه ریزی مسیرهایی با محدودیت های آیرودینامیکی مستقل است.
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 | if (solnInfo.IsPathFound) figure("Name","OriginalPath") % Visualize the 3-D map show(omap) hold on scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled") interpolatedPathObj = copy(pthObj); interpolate(interpolatedPathObj,1000) % Plot the interpolated path based on UAV Dubins connections hReference = plot3(interpolatedPathObj.States(:,1), ... interpolatedPathObj.States(:,2), ... interpolatedPathObj.States(:,3), ... "LineWidth",2,"Color","g"); % Plot simulated UAV trajectory based on fixed-wing guidance model % Compute total time of flight and add a buffer timeToReachGoal = 1.05*pathLength(pthObj)/ss.AirSpeed; waypoints = interpolatedPathObj.States; [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal); hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r"); legend([hReference,hSimulated],"Reference","Simulated","Location","best") hold off view([-31 63]) end |
شبیه سازی مسیر حرکت پهپاد
مسیر برنامه ریزی شده اولیه در حین حرکت به سمت هدف، چرخش های غیرضروری ایجاد می کند. مسیر 3D Dubins را با استفاده از الگوریتم هموار سازی مسیر ارائه شده با نام exampleHelperUAVPathSmoothing ساده کنید. این تابع موقعیت های میانی سه بعدی Dubins را بر اساس یک استراتژی تکراری حذف می کند. برای اطلاعات بیشتر در مورد استراتژی هموارسازی به مقاله معرفی شده در مرجع [1] مراجعه کنید. عملکرد هموارسازی موقعیت های غیر متوالی 3D Dubins را به یکدیگر متصل می کند تا زمانی که انجام این کار منجر به برخورد نشود. مسیرهای صاف ایجاد شده توسط این فرآیند، ویژگی های ردیابی را برای مدل شبیه سازی UAV بهبود می بخشد. مدل پهپاد بال ثابت را با این نقاط جدید و هموار شبیه سازی کنید.
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 | if (solnInfo.IsPathFound) smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss,sv,pthObj); figure("Name","SmoothedPath") % Plot the 3-D map show(omap) hold on scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled") scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled") interpolatedSmoothWaypoints = copy(smoothWaypointsObj); interpolate(interpolatedSmoothWaypoints,1000) % Plot smoothed path based on UAV Dubins connections hReference = plot3(interpolatedSmoothWaypoints.States(:,1), ... interpolatedSmoothWaypoints.States(:,2), ... interpolatedSmoothWaypoints.States(:,3), ... "LineWidth",2,"Color","g"); % Plot simulated flight path based on fixed-wing guidance model waypoints = interpolatedSmoothWaypoints.States; timeToReachGoal = 1.05*pathLength(smoothWaypointsObj)/ss.AirSpeed; [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal); hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r"); legend([hReference,hSimulated],"SmoothedReference","Simulated","Location","best") hold off view([-31 63]); end |
مسیر هموار شده بسیار کوتاهتر است و به طور کلی مسیریابی بهبود یافته را نشان می دهد. تصویر متحرک زیر اجرای شبیه سازی را نشان می دهد.
نکته پایانی: این مقاله آموزشی MATLAB از مثال های آماده خود نرم افزار MATLAB است که اگر جعبه ابزار پهپاد (UAV Toolbox) و جعبه ابزار ناوبری (Navigation Toolbox) بر روی متلب شما نصب باشد، می توانید با تایپ دستور زیر در پنجره کامند متلب به این مدل دسترسی داشته باشید.
1 | openExample('uav/MotionPlanningWithRRTForAFixedWingUAVExample') |
منابع:
- Beard, Randal W., and Timothy W. McLain. Small Unmanned Aircraft: Theory and Practice. Princeton, N.J: Princeton University Press, 2012.
- Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, and Wolfram Burgard. “OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees.” Autonomous Robots 34, no. 3 (April 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.
- mathworks.com
با سلام من میخوام برای یک شناور زیرآبی این طرح را پیاده سازی کنم. میشه بگین چه مواردی رو باید مدنظر قرار بدم و منو راهنمایی کنید. با تشکر