Планирование траектории мобильного робота

При управлении мобильными роботами встаёт задача планирования их траектории. Например, роботу нужно попасть из пункта А в пункт Б, объехав несколько препятствий. Для этой цели окружающее робота пространство можно представить как плоский граф. Для поиска пути использовать классические алгоритмы как A* или D*. Но писать такие алгоритмы самому довольно утомительное занятие, поэтому я предлагаю использовать Open Motion Planning Library, написанную на платформе boost специально для планирования траекторий мобильных роботов и манипуляторов.
Чтобы использовать эту библиотеку нужно самому реализовать класс карты местности и классы, проверяющие допустимость нахождения робота в конкретной точке карты, допустимость перемещения робота из точки А в точку Б по прямой. Сначала я рассмотрю основные классы этой библиотеки, а потом покажу возможную реализацию указанных классов. Все примеры написаны для версии OMPL 0.93.

Чтобы спланировать траекторию мобильного робота, предварительно придётся создать несколько объектов.

namespace ob = ompl::base;
namespace og = ompl::geometric;
 
struct VTrajectoryPlanner::Private
{
    ob::ProblemDefinitionPtr pdef;
    ob::PlannerPtr planner;
    ob::StateManifoldPtr manifold;
    ob::SpaceInformationPtr si;
    og::PathSimplifierPtr simplifier;
};
Private *d;
d = new Private;

Карту местности, по которой будет передвигаться мобильный робот, представим с помощью изображения в формате OpenCV( cv::Mat ). На карте нулевой пиксель означает, что в этой точке препятствия нет. Ненулевой - наличие препятствия. Карта - одно канальное, восьми битное изображение.

При написании собственного планировщика движения сначала необходимо указать, какой тип траектории нужно будет планировать. Для этого нужно будет создать один из классов, производных от StateManifold. StateManifold – класс, описывающий пространство обобщённых координат робота. В случае плоской траектории мобильного робота нужно создать экземпляр SE2StateManifold. Положение нашего робота будет описываться тремя координатами: x, y и угол поворота робота относительно оси x.

// construct the manifold we are planning in
d->manifold = ob::StateManifoldPtr( new ob::SE2StateManifold );

Следующим шагом нужно ограничить область, в которой будет перемещаться робот:

// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds( 2 );
bounds.setLow( robotRadius );
bounds.setHigh( 0, map.cols - robotRadius );
bounds.setHigh( 1, map.rows - robotRadius );
d->manifold->as< ob::SE2StateManifold >( )->setBounds( bounds );

Теперь нужно создать экземпляр класса SpaceInformation, который будет описывать взаимодействия алгоритма планирования и пространства состояний:

// construct an instance of  space information from this manifold
d->si = ob::SpaceInformationPtr( new ob::SpaceInformation( d->manifold ) );

Ему необходимо передать указатель на экземпляр класса, реализующий проверку допустимости нахождения робота в конкретной точке карты:

// set state validity checking for this space
d->si->setStateValidityChecker( ob::StateValidityCheckerPtr( new VStateValidityChecker( d->si, map, robotRadius ) ) );

И указатель на экземпляр класса, реализующий проверку допустимости перемещения робота из точки А в точку Б по прямой:

d->si->setMotionValidator( ob::MotionValidatorPtr( new VMotionValidator( d->si, map, robotRadius ) ) );
d->si->setup( );

Далее создаём указатель на объект, реализующий алгоритм планирования. Доступно несколько различных алгоритмов. За подробностями смотрите документацию.

// create a planner for the defined space
d->planner = ob::PlannerPtr( new og::RRTConnect( d->si ) );

Путь, найдённый алгоритмом планирования, будет не самым оптимальным. Для его оптимизации нам понадобиться объект, реализующий оптимизацию пути:

d->simplifier = og::PathSimplifierPtr( new og::PathSimplifier( d->si ) );

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

// create a problem instance
d->pdef = ob::ProblemDefinitionPtr( new ob::ProblemDefinition( d->si ) );

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

// create a start state
ob::ScopedState< > start( d->manifold );
start[ 0 ] = initialPoint.x;
start[ 1 ] = initialPoint.y;
start[ 2 ] = initialYaw;
// create a goal state
ob::ScopedState< > goal( d->manifold );
goal[ 0 ] = destination.x;
goal[ 1 ] = destination.y;
goal[ 2 ] =  yaw;
// set the start and goal states
d->pdef->setStartAndGoalStates( start, goal );

Передадим указатель на задачу алгоритму планирования.

// set the problem we are trying to solve for the planner
d->planner->setProblemDefinition( d->pdef );
// perform setup steps for the planner
d->planner->setup( );

Попытаемся решить задачу:

// attempt to solve the problem
bool solved = d->planner->solve( maxTimeForCalculating );

Если траекторию найти удалось, упростим её.

if ( solved ) {
    og::PathGeometric path( dynamic_cast< const og::PathGeometric& >( * d->pdef->getGoal( )->getSolutionPath( ).get( ) ) );
    d->simplifier->simplifyMax( path );
}

Класс VMotionValidator наследует от MotionValidator. Он реализует проверку допустимости перемещения робота из точки А в точку Б по прямой. Его методы должны быть потоко защищёнными. Класс VStateValidityCheckerнаследует от StateValidityChecker. Он реализует проверку допустимости нахождения робота в конкретной точке карты. Его методы тоже должны быть потоко защищёнными. Детали реализации этих классов смотрите в приложенных исходниках.

Результаты работы алгоритма планирования траектории:

Проложена траектория из центра тёмно серого круга в центр светло серого. Белым обозначены препятствия.

Скачать исходные коды.

Leave a Reply

You must be logged in to post a comment.