Описание проблемы

В этом примере вы используете пользовательскую функцию стоимости, чтобы изучить навигацию робота в среде симуляции. Цель задачи навигации состоит в том, чтобы достигнуть заданной цели при предотвращении препятствий. Направление к цели представлено как модульный вектор силы ($\overrightarrow{{\mathit{F}}_{\mathit{t}}}$) направленный от робота до целевого местоположения. Направление предотвращения препятствия представлено модульным вектором силы ($\overrightarrow{{\mathit{F}}_{\mathit{o}}}$) направленный к роботу от самого близкого местоположения препятствия.

Робот, цель и препятствие показываются кругами с радиусом на 0,5 м в среде симуляции на 25 м x 25 м. Задача навигации состоит в том, чтобы объединиться, сила векторизовала таким образом что направление $\theta$ из результирующего вектора силы $\overrightarrow{\mathit{F}}$ обеспечивает направление без коллизий для робота.

$\overrightarrow{\mathit{F}}=\mathit{w}\overrightarrow{{\mathit{F}}_{\mathit{o}}}+\left(1-\mathit{w}\right)\overrightarrow{{\mathit{F}}_{\mathit{t}}}$, где $0\le \mathit{w}\le 1$

Этот пример принимает робота с дифференциальной кинематикой для симуляции. Другими словами, робот может вращаться на его центре без любых ограничений. Однако, чтобы избежать крутых поворотов, изменение на временной шаг в направлении робота ограничивается $\left[-\frac{\pi }{4},\frac{\pi }{4}\right]$. Поэтому, если текущее направление заголовка робота ${\theta }_{\mathit{r}}\left(\mathit{k}\right)$, следующее направление заголовка вычисляется как:

$${\theta }_{\mathit{r}}\left(\mathit{k}+1\right)={\theta }_{\mathit{r}}\left(\mathit{k}\right)+\min \left(\max \left(\theta -{\theta }_{\mathit{r}}\left(\mathit{k}\right),\frac{\pi }{4}\right),-\frac{\pi }{4}\right)$$.

Вес $\mathit{w}$ из вектора силы $\overrightarrow{{\mathit{F}}_{\mathit{o}}}$ вычисляется с помощью функции ${\mathit{f}}_{\mathit{w}}$:

где

Достигнуть задачи навигации, функции ${\mathit{f}}_{\mathit{w}}$ должен сгенерировать высоко $\mathit{w}$ значения, то есть, особое внимание при предотвращении препятствия, когда:

В противном случае, ${\mathit{f}}_{\mathit{w}}$ должен сгенерировать низко $\mathit{w}$ значения, то есть, особое внимание при достижении цели.

Цель этого примера состоит в том, чтобы спроектировать FIS, который изучает нечеткие правила и оптимизирует параметры FIS, чтобы смоделировать функцию ${\mathit{f}}_{\mathit{w}}$ для навигации робота без коллизий в среде симуляции.

Предположения

Следующие предположения применяются к симуляции робота:

Создайте нечеткую систему

К функции модели ${\mathit{f}}_{\mathit{w}}$, создайте FIS как показано в следующем рисунке. В данном примере используйте Mamdani FIS.

fisin = mamfis;

Добавьте следующие два входных параметров как показано на предыдущем рисунке.

Установите область значений первого входа к [0,2], который указывает на это $\alpha$ способствует предотвращению препятствия, когда расстояние препятствия меньше чем или равно дважды целевому расстоянию.

Установите область значений второго входа к [0,pi/2], который указывает на это ${\theta }_{\mathit{t},\mathit{o}}$ способствует предотвращению препятствия, когда различие между целью и направлениями препятствия меньше чем или равно pi/2.

fisin = addInput(fisin,[0 2],'Name','alpha');
fisin = addInput(fisin,[0 pi/2],'Name','theta_t_o');

Чтобы минимизировать количество правил, которое соответствует количеству комбинаций входных функций принадлежности, добавляют, две функции принадлежности (MFS) к каждому входу. Чтобы сгенерировать подобные значения членства вне входных диапазонов, используйте zmf (Z-образная функция принадлежности кривой) и smf (S-образная функция принадлежности кривой) MFS. Настраивающий процесс оптимизирует значения параметров входа MF.

Добавьте функции принадлежности в первый вход.

fisin = addMF(fisin,'alpha','zmf',[0 2],'Name','low');
fisin = addMF(fisin,'alpha','smf',[0 2],'Name','high');

Добавьте функции принадлежности во второй вход.

fisin = addMF(fisin,'theta_t_o','zmf',[0 pi/2],'Name','low');
fisin = addMF(fisin,'theta_t_o','smf',[0 pi/2],'Name','high');

Добавьте выход в FIS или силу препятствия векторный вес, ограничив значения веса областью значений [0,1].

fisin = addOutput(fisin,[0 1],'Name','w');

Добавьте два MFS в выход. Можно добавить больше MFS в выход для более прекрасной гранулярности выходных значений. Однако выполнение так увеличивает число настраивающихся параметров. Выход MFs также использует zmf и smf сгенерировать подобные значения членства вне входных диапазонов. Настраивающий процесс оптимизирует значения параметров выхода MF.

fisin = addMF(fisin,'w','zmf',[0 1],'Name','low');
fisin = addMF(fisin,'w','smf',[0 1],'Name','high');

Просмотрите структуру FIS. Первоначально, FIS имеет нулевые правила. Настраивающий процесс находит правила для нечеткой системы.

figure
plotfis(fisin)

Изучите правила и оптимизируйте параметры FIS

Поскольку у вас нет обучающих данных для этого примера, вы симулируете навигацию робота с помощью пользовательской функции стоимости. Настраивающий процесс использует эту пользовательскую функцию стоимости при оптимизации параметров FIS.

Для оптимизации параметров управления получите установки параметров из FIS.

[in,out] = getTunableSettings(fisin);

Затем создайте настраивающиеся опции с OptimizationType установите на learning. Этот пример использует генетический алгоритм (ga) метод оптимизации для настраивающего процесса. Чтобы улучшить скорость настраивающего процесса, установите UseParallel опция к true, который требует программного обеспечения Parallel Computing Toolbox™. Если у вас нет программного обеспечения Parallel Computing Toolbox, установите UseParallel к false.

options = tunefisOptions('Method','ga','OptimizationType','learning');

Установите численность населения генетического алгоритма для 200. Большая численность населения увеличивает вероятность генерации лучшего решения в меньшем количестве поколений.

options.MethodOptions.PopulationSize = 200;

Определите максимальный номер поколений к 25. Чтобы настроить параметры далее, можно определить более высокий номер поколений. Однако выполнение так увеличивает длительность настраивающего процесса и может сверхнастроить значения параметров.

options.MethodOptions.MaxGenerations = 25;

Установите местоположения препятствия, цели и робота для учебных сред. Установите первоначальный заголовок робота к pi/2 для учебной среды. Чтобы изучить навигацию и с и без предотвращения препятствия на пути к целевому местоположению, используйте две учебных задачи с различными местоположениями препятствия.

trnObstacle = [3 12;13 18];
trnTarget = [13 22;13 22];
trnRobot = [13 2 pi/2;13 2 pi/2;];

showSimulationEnvironmentsForTraining(trnObstacle,trnTarget,trnRobot)

Задайте пользовательскую функцию стоимости с помощью указателя на функцию.

costFunction = @(fis)navigationCostFcn(fis,trnObstacle,trnTarget,trnRobot);

В функции стоимости навигация робота симулирована в учебных средах с помощью каждого FIS от населения. Каждая задача навигации запущена для 100 итераций, где каждая итерация эквивалентна циклу решения длины 1 с. Робот использует фиксированную скорость 0,5 м/с в задаче навигации. Для получения дополнительной информации симуляции смотрите getNavigationResults функция.

function cost = navigationCostFcn(fis,obstacle,target,robot)

cost = 0;

for i = 1:size(obstacle,1)

results = getNavigationResults(fis,obstacle(i,:),target(i,:),robot(i,:));

cost = cost + getNavigationCost(results);

end

end

Стоимость каждой задачи навигации является общим расстоянием, путешествовавшим роботом. Если робот не достигает цели или сталкивается с препятствием, дорогостоящее значение (200) присвоено для симуляции.

function cost = getNavigationCost(results)

if results.notSafe || ~results.reachedTarget

cost = 200;

else

cost = results.travelledDistance;

end

end

Поскольку оптимизация генетического алгоритма использует случайный поиск, чтобы получить восстанавливаемые результаты, инициализировать генератор случайных чисел к его настройке по умолчанию.

rng('default')

Изучение правил с помощью tunefis функционируйте занимает приблизительно 10 минут. В данном примере позвольте настроиться установкой runtunefis к true. Загружать предварительно обученные результаты, не запуская tunefis, можно установить runtunefis к false.

runtunefis = false;

Настройте FIS использующий заданных учебных окружений и настройка опций.

if runtunefis
    fisout = tunefis(fisin,[in;out],costFunction,options); %#ok<UNRCH>
else
   tunedfis = load('tunedfisnavigation.mat'); 
   fisout = tunedfis.fisout;
end

Настроенный FIS производит следующие траектории робота в средах симуляции.

showNavigationTrajectories(fisout,trnObstacle,trnTarget,trnRobot)

В первом случае робот достигает цели с минимальным расстоянием, поскольку препятствие не находится на пути к цели. Во втором случае робот успешно избегает препятствия и достигает цели.

Просмотрите структуру настроенного FIS, fisout.

figure
plotfis(fisout)

Настраивающий процесс производит набор новых правил для FIS.

fisout.Rules

ans = 
  1x5 fisrule array with properties:

    Description
    Antecedent
    Consequent
    Weight
    Connection

  Details:
                         Description                 
         ____________________________________________

    1    "alpha==low & theta_t_o==high => w=low (1)" 
    2    "alpha==low & theta_t_o==low => w=high (1)" 
    3    "theta_t_o==high => w=low (1)"              
    4    "alpha==high & theta_t_o==high => w=low (1)"
    5    "alpha==low => w=low (1)"                   

Правила описаны можно следующим образом относительно ожидаемых поведений ${\mathit{f}}_{\mathit{w}}$:

Удалите избыточные правила.

fisoutpruned = fisout;
fisoutpruned.Rules([1 4 5]) = [];
fisoutpruned.Rules

ans = 
  1x2 fisrule array with properties:

    Description
    Antecedent
    Consequent
    Weight
    Connection

  Details:
                         Description                
         ___________________________________________

    1    "alpha==low & theta_t_o==low => w=high (1)"
    2    "theta_t_o==high => w=low (1)"             

fisout и fisoutpruned сгенерируйте подобные поверхности управления. Поэтому только два правила необходимы для предотвращения препятствия в среде симуляции.

figure
subplot(1,2,1)
gensurf(fisout)
title('Output surface of fisout')
subplot(1,2,2)
gensurf(fisoutpruned)
title('Output surface of fisoutpruned')

Проверяйте эффективность

Подтвердите эффективность настроенного FIS с различными положениями препятствия, робота и цели. В следующих случаях валидации робот успешно избегает препятствия, чтобы достигнуть целевого положения с помощью настроенного FIS.

vldObstacle = [13 5;10 10;8 8];
vldRobot = [5 12 0;5 20 -pi/2;19 19 -pi];
vldTarget = [23 12;15 4;5 5];

showNavigationTrajectories(fisoutpruned,vldObstacle,vldTarget,vldRobot)

Заключение

Этот пример использует пользовательскую функцию стоимости, которая симулирует навигацию робота в различных учебных средах, чтобы изучить нечеткие правила и оптимизировать значения параметров FIS для навигации без коллизий. Можно Включать больше учебных сред, чтобы изучить предотвращение препятствия в других сценариях, например, узкий коридор и препятствия U-формы. В этих сценариях роботу, возможно, понадобятся дополнительные подзадачи навигации, такие как стена после и подцель (промежуточный ориентир) достижение, для успешной навигации без коллизий к цели. Сложные среды также требуют дополнительных условий в расчете стоимости для безопасности плавания. Например, в узком коридоре робот должен остаться в середине; то есть, расстояния до препятствий слева и права должны быть тем же самым.

Используя пользовательскую функцию стоимости с tunefis обеспечивает гибкость симуляции пользовательской системы с пользовательским расчетом стоимости. Однако компромисс является долгим настраивающим процессом из-за количества требуемых симуляций (для каждого набора оптимизированных значений параметров). Поэтому, если это возможно, чтобы ускорить настраивающий процесс, обучающие данные использования. Например, настраивающий процесс в этом примере запустился бы быстрее, если бы данные о решении ввода/вывода человеческого оператора были доступны для настройки FIS.

Локальные функции

function showSimulationEnvironment(obstacle,target,robot,navigationResults)

% Show the robot trajectory in the simulation environment.

% Radius of the robot, target, and obstacle.
radius = 1; % 1m

% Use 25mx25m simulation environment.
axis([0 25 0 25]);

% Set equal aspect ratio.
pbaspect([1 1 1])

% Temporary plots to enable legends.
hold on
plot(robot(1),robot(2)+radius,'ob','LineWidth',radius*1,'MarkerFaceColor','b')
plot(robot(1),robot(2)+radius,'or','LineWidth',radius*1,'MarkerFaceColor','r')
plot(robot(1),robot(2)+radius,'og','LineWidth',radius*1,'MarkerFaceColor','g')
hold off

% Draw obstacle.
rectangle('Position',[obstacle(1)-0.5*radius obstacle(2)+0.5*radius radius radius], ...
    'Curvature',[1 1],'FaceColor','b','EdgeColor','b')

% Draw target.
rectangle('Position',[target(1)-0.5*radius target(2)+0.5*radius radius radius], ...
    'Curvature',[1 1],'FaceColor','r','EdgeColor','r')

% Draw robot.
rectangle('Position',[robot(1)-0.5*radius robot(2)+0.5*radius radius radius], ...
    'Curvature',[1 1],'FaceColor','g','EdgeColor','g')

% Add labels, title, and legends.
xlabel('x (m)'),ylabel('y (m)')
title('Simulation Environment for Robot Navigation')
legend(["Obstacle" "Target" "Robot"])

% Plot the robot trajectory if specified.
if nargin == 4
    x = navigationResults.x;
    y = navigationResults.y;
    for i = 1:numel(x)
        rectangle('Position',[x(i)-0.5*radius y(i)+0.5*radius radius radius], ...
            'Curvature',[1 1],'FaceColor','g','EdgeColor','g')
    end
end

end

function showSimulationEnvironmentsForTraining(obstacle,target,robot)

% Show simulation environments for training.
drawEnvironmentAndShowTrajectory(obstacle,target,robot,'Training Task')

end

function showNavigationTrajectories(fis,obstacle,target,robot)

% Show robot trajectories in the simulation environments.
drawEnvironmentAndShowTrajectory(obstacle,target,robot,'Navigation Task',fis)

end

function drawEnvironmentAndShowTrajectory(obstacle,target,robot,plotTitle,varargin)

% Expand figure horizontally to tile the simulation environments.
h = figure;
h.Position = [h.Position(1:2) 3*h.Position(3) h.Position(4)];
numTasks = size(target,1);

% Draw each simulation environment.
for i = 1:numTasks
    o = obstacle(i,:);
    t = target(i,:);
    r = robot(i,:);
    subplot(1,numTasks,i)
    if ~isempty(varargin)
        results = getNavigationResults(varargin{1},o,t,r);
        showSimulationEnvironment(o,t,r,results)
    else
        showSimulationEnvironment(o,t,r)
    end
    title([plotTitle ': ' num2str(i)])
end

end