在这里分享了运动规划方面的一些基本的算法原理和伪代码实现，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

A、OMPL编译与安装

1. 方式一：通过官网下载install-ompl-ubuntu.sh文件，运行后，自动编译安装 https://ompl.kavrakilab.org/installation.html，此方法在使用库文件时，cmakeconfig文件可能有问题，还是建议方法二。
2. 方式二：通过Github下载源码，自己编译安装， https://github.com/ompl/ompl，
3. 检查安装成功 及 ROS中使用 https://blog.csdn.net/zghforever/article/details/106688410
4. 官方教程 https://ompl.kavrakilab.org/tutorials.html

B、OMPL使用

B1、基本定义

1. 求解方式
   求解问题的过程中，根据是否使用ompl::geometric::SimpleSetup类，有两种方式，简单规划和完整规划，见代码部分C1。
2. 求解过程：利用OMPL提供的类和方法来规划，基本过程包括以下几个部分
   ♥ 确认实际问题的状态空间 （如SE(3)）
   ♥ 从OMPL提供的类中构建一个状态空间（如ompl::base::SE3StateSpace is appropriate）
   ♥ 设置状态空间的边界
   ♥ 定义状态有效性的判断方法
   ♥ 定义起点和终点的状态
   ♥ 求解和可视化

3. 状态与状态空间

   //状态与状态空间的基本定义
   ompl::base::StateSpacePtr space(new T());
   ompl::base::ScopedState<> state1(space);    //直接通过状态空间StateSpace初始化
   ompl::base::SpaceInformationPtr si(space);
   ompl::base::ScopedState<T> state2(si);      //通过SpaceInformation初始化
   //举个栗子
   ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());
   ompl::base::ScopedState<ompl::base::SE2StateSpace> state(space);
       state->setX(0.1);
       state->setY(0.2);
       state->setYaw(0.0);
   ompl::base::ScopedState<> backup = state;   //state == backup, backup本质是State*, 作用是备份参数， 因此setX()等函数无效
   //状态空间的合并
   //合并方式1
   ompl::base::CompoundStateSpace *cs = new ompl::base::CompoundStateSpace();  //利用CompoundStateSpace类来添加不同状态空间
   cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.0);
   cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);
   ompl::base::StateSpacePtr space(cs);
   //合并方式2
   // define the individual state spaces
   ompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());
   ompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());
   ompl::base::StateSpacePtr space = so2 + so3;
   
   //虽然空间可以混合，但对于状态仍然需要指定具体的类型
   ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
   state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

4. 有效性检查
   OMPL不提供具体检查方式，需要根据具体的问题具体确定。用户指定有效性检查的方式有两种，见代码部分C2
   ♥ 继承OMPL定义的两个抽象类
   ♥ 直接定义有效性检查函数

   // 方法2：直接定义判断函数
   bool myStateValidityCheckerFunction(const base::State *state)
   {
    return ...;
   }
   //方法2：使用
   base::SpaceInformationPtr si(space);
   si->setStateValidityChecker(myStateValidityCheckerFunction);
   si->setStateValidityCheckingResolution(0.03); // 3%
   si->setup();

5. 采样
   采样的两种方式：基于ompl::base::StateSampler和基于ompl::base::ValidStateSampler。
   ♥ 基于ompl::base::StateSampler：生成状态空间的一个状态，生成状态附近的一个状态，生成高斯分布的一组状态。
   ♥ 基于ompl::base::ValidStateSampler：以ompl::base::StateSampler为基础，不断采样，直到采到一个有效状态或者到达迭代最大值，有效性通过ompl::base::SpaceInformation::isValid判断。
   ♥ OMPL提供了几个继承自ompl::base::ValidStateSampler的类，如下，可以使用这些类或继承ompl::base::ValidStateSampler重新编写采样器，基本使用方法如下，具体代码与解释见代码部分C3。

//基本使用方式
//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

B2、路径可视化

1. OMPL不提供路径可视化工具，直接一点的方式是作为矩阵打印输出，保存成文本文件。
2. OMPL提供两种路径：ompl::geometric::PathGeometric 和 ompl::control::PathControl，两个类可用成员函数printAsMatrix()打印，前者每行打印一个状态，后者每行除了状态，还有控制指令和控制间隔时间。

3. 打印方式如下：

   bool solved = ss.solve(20.0);
   if (solved)
   {
   // if ss is a ompl::geometric::SimpleSetup object
   ss.getSolutionPath().printAsMatrix(std::cout);
   
   // if ss is a ompl::control::SimpleSetup object
   ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
   }

B3、API Overview

C、代码附录

C1、Geometric Planning for a Rigid Body in 3D

//注：这是在ROS里面写的，虽然这一部分跟ROS暂时没关系。
#include <ros/ros.h>
#include <iostream>

#include <ompl/base/spaces/SE3StateSpace.h>

//使用ompl::geometric::SimpleSetup类
#include <ompl/geometric/SimpleSetup.h>
//不使用ompl::geometric::SimpleSetup类
#include <ompl/base/SpaceInformation.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;

//状态检查函数
bool isStateValid(const ob::State *state)
{
    // cast the abstract state type to the type we expect
    // 将抽象状态类型转换为我们期望的类型
    const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

    // extract the first component of the state and cast it to what we expect 
    // 提取状态的第一个组件并将其转换为我们所期望的
    const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

    // extract the second component of the state and cast it to what we expect
    // 提取状态的第二个组件并将其转换为我们所期望的
    const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

    // check validity of state defined by pos & rot

    // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
    // 返回一个始终为true但使用我们定义的两个变量的值
    return (const void*)rot != (const void*)pos;
}

//简单规划：使用ompl::geometric::SimpleSetup类
void planWithSimpleSetup()
{
    // 状态空间：构建
    auto space(std::make_shared<ob::SE3StateSpace>());

    // 状态空间：边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    space->setBounds(bounds);

    // 配置类：构建
    og::SimpleSetup ss(space);

    // 配置类：设置状态有效性检查器
    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

    // 配置类：设置起终点 
    ob::ScopedState<> start(space);
    start.random();
    ob::ScopedState<> goal(space);
    goal.random();
    ss.setStartAndGoalStates(start, goal);

    // this call is optional, but we put it in to get more output information
    ss.setup();
    ss.print();

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        // print the path to screen
        ss.simplifySolution();
        ss.getSolutionPath().print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

//完整规划：不使用ompl::geometric::SimpleSetup类
void plan()
{
    // 状态空间：构建
    auto space(std::make_shared<ob::SE3StateSpace>());

    // 状态空间：边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    space->setBounds(bounds);

    // 空间信息：构建
    auto si(std::make_shared<ob::SpaceInformation>(space));

    // 空间信息：设置状态有效性检查器
    si->setStateValidityChecker(isStateValid);

    // 问题实例：构建
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));

    // 问题实例：设置起终点
    ob::ScopedState<> start(space);
    start.random();
    ob::ScopedState<> goal(space);
    goal.random();
    pdef->setStartAndGoalStates(start, goal);

    // 规划方法：构建
    auto planner(std::make_shared<og::RRTConnect>(si));

    // 规划方法：实际问题
    planner->setProblemDefinition(pdef);

    // 规划方法：初始化
    planner->setup();

    // 打印空间信息和问题实例
    si->printSettings(std::cout);
    pdef->print(std::cout);

    // 求解
    ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);

    if (solved)
    {
        // get the goal representation from the problem definition (not the same as the goal state)
        // and inquire about the found path
        ob::PathPtr path = pdef->getSolutionPath();
        std::cout << "Found solution:" << std::endl;

        // print the path to screen
        path->print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "rigid_body_planning");
    ros::NodeHandle nh;

    std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
    plan();
    std::cout << std::endl << std::endl;
    planWithSimpleSetup();  

    return 0;
}

C2、有效性检查方式

//方法1：继承OMPL定义的抽象类 base::StateValidityChecker
class myStateValidityCheckerClass : public base::StateValidityChecker
{
public:
     myStateValidityCheckerClass(const base::SpaceInformationPtr &si) :
       base::StateValidityChecker(si)
        {
     }

     virtual bool isValid(const base::State *state) const
     {
             return ...;
     }
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(std::make_shared<myStateValidityCheckerClass>(si));
si->setup();

//方法1：继承OMPL定义的抽象类 base::MotionValidator
class myMotionValidator : public base::MotionValidator
{
public:
    // implement checkMotion()
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setMotionValidator(std::make_shared<myMotionValidator>(si));
si->setup();

// 方法2：直接定义判断函数
bool myStateValidityCheckerFunction(const base::State *state)
{
     return ...;
}
//方法2：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(myStateValidityCheckerFunction);
si->setStateValidityCheckingResolution(0.03); // 3%
si->setup();

C3、采样方式

1 已有采样器

不能直接在SimpleSetup或者SpaceInformation类中配置采样方式，需要通过定义下面类型的函数，输入ompl::base::SpaceInformation，返回ompl::base::ValidStateSamplerPtr。

//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

2 自定义采样器

继承ompl::base::ValidStateSampler重新编写采样器，继承后的类与OMPL提供的类的使用方法相同。

//***************************定义子类***************************
namespace ob = ompl::base;
namespace og = ompl::geometric;

// This is a problem-specific sampler that automatically generates valid
// states; it doesn't need to call SpaceInformation::isValid. This is an
// example of constrained sampling. If you can explicitly describe the set valid
// states and can draw samples from it, then this is typically much more
// efficient than generating random samples from the entire state space and
// checking for validity.
class MyValidStateSampler : public ob::ValidStateSampler
{
public:
    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "my sampler";
    }
    // Generate a sample in the valid part of the R^3 state space
    // Valid states satisfy the following constraints:
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    bool sample(ob::State *state) override
    {
        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);

        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
                case 0: val[0]=x-1;  val[1]=y-1;  break;
                case 1: val[0]=x-.8; val[1]=y+.8; break;
                case 2: val[0]=y-1;  val[1]=x-1;  break;
                case 3: val[0]=y+.8; val[1]=x-.8; break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        assert(si_->isValid(state));
        return true;
    }
    // We don't need this in the example below.
    bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
protected:
    ompl::RNG rng_;
};
//***************************定义状态分配器（函数）***************************
ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
{
    return std::make_shared<MyValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);

实际问题

Dubins曲线基础

问题关键

CSC型

CCC型

Dubins曲线的一些讨论

下面是写的一段MATLAB代码，只是简单实现，没有考虑最优。
1、CSC

%Dubins Curve CSC型
%目标定义
    %定义起终点[x y dir]
    I=[1 1 7*pi/4];
    G=[6 8 3*pi/4];
    %定义或计算转弯半径
    ri=1;
    rg=2;
    %组合
    k=-1;    % 1:RSX,-1:LSX
    i=-1;   % 1:XSR,-1:XSL
    j=i*k;  % 1:RSR/LSL, -1:RSL/LSR
%计算首尾圆心坐标
    xi=I(1,1)-ri*k*cos(I(1,3));
    yi=I(1,2)+ri*k*sin(I(1,3));  
    xg=G(1,1)-rg*i*cos(G(1,3));
    yg=G(1,2)+rg*i*sin(G(1,3));
%计算法向量
    %首尾圆圆心之间的向量V1
    v1x=xg-xi;              %向量坐标x
    v1y=yg-yi;              %向量坐标y
    D=sqrt(v1x*v1x+v1y*v1y);%向量模长 
    v1x=v1x/D;              %单位化
    v1y=v1y/D;
    %计算法向量n
    c=(j*ri-rg)/D;            %定义中间量
    nx=v1x*c-i*v1y*sqrt(1-c*c);
    ny=v1y*c+i*v1x*sqrt(1-c*c);
%计算切点
    xit=xi+j*ri*nx;
    yit=yi+j*ri*ny;
    xgt=xg+rg*nx;
    ygt=yg+rg*ny;
%绘图
    %初始方向
    xiDir=[I(1,1)-ri*cos(I(1,3)-pi/2),I(1,1)];
    yiDir=[I(1,2)+ri*sin(I(1,3)-pi/2),I(1,2)];
    xgDir=[G(1,1)-rg*cos(G(1,3)-pi/2),G(1,1)];
    ygDir=[G(1,2)+rg*sin(G(1,3)-pi/2),G(1,2)];
    %切线
    xxx=[xit xgt];
    yyy=[yit ygt];
    %首尾圆
    t=0:0.01:2*pi;
    %t=I(1,3)-pi/2:0.01:3*pi/2-atan(ny/nx);
    xxi=xi+ri*sin(t);
    yyi=yi+ri*cos(t);
    %t=-pi/2-atan(ny/nx):0.01:G(1,3)-pi/2;
    xxg=xg+rg*sin(t);
    yyg=yg+rg*cos(t);
    %法向量
    nxi=[xi xi+ri*nx];
    nyi=[yi yi+ri*ny];
    nxg=[xg xg+rg*nx];
    nyg=[yg yg+rg*ny];
    %绘图
    plot(I(1,1),I(1,2), 'go' ,G(1,1),G(1,2),'go',...      %初始位置
            xiDir,yiDir,'-g',xgDir,ygDir,'-g',...       %初始方向
            xi,yi,'k*',xg,yg,'k*',...                   %圆心
            xxi,yyi,'-r',xxg,yyg,'-r',...               %首尾圆
            nxi,nyi,'-b',nxg,nyg,'-b',...               %法向量
            xxx,yyy,'-ro')                              %切线
    axis([-3 15 -3 15])

2、CCC

%Dubins Curve CCC型

%目标定义
    %定义起终点[x y dir] 
    I=[1 1 7*pi/4];
    G=[4 5 3*pi/4];
    %定义或计算转弯半径
    ri=1;
    rg=1;
    rmid=4;
    %组合
    i=1;    % 1:RLR  -1:LRL

%三圆心相关量
    %计算首尾圆心坐标及其连线向量V12
    xi=I(1,1)-ri*i*cos(I(1,3));
    yi=I(1,2)+ri*i*sin(I(1,3));
    xg=G(1,1)-rg*i*cos(G(1,3));
    yg=G(1,2)+rg*i*sin(G(1,3));

    V12=[xg-xi,yg-yi];
    angleV12=atan(V12(1,2)/V12(1,1));

    %计算中间圆坐标及三圆心连线向量V13、V32
    d12=sqrt((xg-xi)^2+(yg-yi)^2);
    rmid=max([rmid (d12-ri-rg)*0.5]);
    d13=ri+rmid;
    d32=rmid+rg;
    angleP213=acos((d12^2+d13^2-d32^2)/(2*d12*d13));

    xmid=xi+d13*cos(angleV12-angleP213);
    ymid=yi+d13*sin(angleV12-angleP213);

    V13=[xmid-xi,ymid-yi];
    V32=[xg-xmid,yg-ymid];

    V13=V13/d13;    %单位化
    V32=V32/d32;

%计算切点坐标
    xt1=xi+ri*V13(1,1);
    yt1=yi+ri*V13(1,2);
    xt2=xmid+rmid*V32(1,1);
    yt2=ymid+rmid*V32(1,2);

%绘图
    %初始方向
    xiDir=[I(1,1)-ri*cos(I(1,3)-pi/2),I(1,1)];
    yiDir=[I(1,2)+ri*sin(I(1,3)-pi/2),I(1,2)];
    xgDir=[G(1,1)-rg*cos(G(1,3)-pi/2),G(1,1)];
    ygDir=[G(1,2)+rg*sin(G(1,3)-pi/2),G(1,2)];
    %首尾圆
    t=0:0.01:2*pi;
    xxi=xi+ri*sin(t);
    yyi=yi+ri*cos(t);
    xxg=xg+rg*sin(t);
    yyg=yg+rg*cos(t);
    xxmid=xmid+rmid*sin(t);
    yymid=ymid+rmid*cos(t);
    %绘图
    plot(I(1,1),I(1,2),'go',G(1,1),G(1,2),'go',...          %初始位置
            xiDir,yiDir,'-g',xgDir,ygDir,'-g',...           %初始方向
            xi,yi,'k*',xg,yg,'k*',xmid,ymid,'k*',...        %圆心
            xt1,yt1,'bo',xt2,yt2,'bo',...                   %切点
            xxi,yyi,'-r',xxg,yyg,'-r',xxmid,yymid,'-r')     %三圆
    axis([-3 15 -3 15])

参考文献

1、http://planning.cs.uiuc.edu/node821.html
2、http://www.banbeichadexiaojiubei.com/index.php/2020/03/15/自动驾驶运动规划-dubins曲线
3、https://gieseanw.files.wordpress.com/2012/10/dubins.pdf

在这里分享了运动规划方面的一些基本的算法原理和伪代码实现，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

基于采样

这一部分是基于采样的路径规划算法，依次介绍RRT、RRT-connect、RRT*。另外，有关RRT*的其他衍生的算法，日后更新…

RRT

1、RRT (Rapidly-exploring Random Trees)，经典的RRT是单树RRT，算法示意图如下：


2、算法伪代码解释如下：


3、RRT的不足
①收敛慢，目标性不强，这一点相似于BFS，为了整体上向目标点方向生长，一种思路是根据随机的概率，采样点随机地选择通过随机采样还是直接使用目标点作为x_rand，即 x_rand = chooseTarget(sample(map),x_goal,p)，另一种思路是双树RRT。
②没有最优解，→RRT*渐进最优解
③采样范围为整个空间

RRT-connect

1、为提高计算效率，可以同时从起点和终点“生长树枝”。就像这样：

2、基本的过程，如图所示：

3、伪代码，这个还比较详细，基本过程如上图，不再细标

RRT*

1、RRT*对RRT的改进
RRT存在一个问题，虽然能找到一条可行路径，但不保证最优。RRT*在RRT的基础上，进行改进，使路径渐进最优，方法是x_near和x_new不会直接连接起来，而是做一个优化处理，令x_new在一定范围内重新选择父节点，同时rewire（随机树重布线）。
2、RRT*示意与伪代码
①示意
重选父节点&随机树重布线的图示点这里，描述的还比较清晰。
②伪代码



（虽然说是伪代码，可能没有很伪代码）

③可参考的更简洁的伪代码：

下面是用QT写的一个简单实现：

参考文献

1、https://blog.csdn.net/weixin_43465857/article/details/96451631 RRT算法原理图解
2、https://blog.csdn.net/gophae/article/details/103231053 全局路径规划：图搜索算法介绍4(RRT/RRT*)
3、https://zhuanlan.zhihu.com/p/30333530 RRT算法是如何实现的？有哪些优缺点？
4、https://zhuanlan.zhihu.com/p/133224593 基于采样的运动规划算法-RRT(Rapidly-exploring Random Trees)
5、https://www.cnblogs.com/21207-iHome/p/7210543.html RRT路径规划算法
6、https://blog.csdn.net/a735148617/article/details/103647804 从零开始的OMPL库算法学习（2）RRT-connect算法
7、https://zhuanlan.zhihu.com/p/51087819 路径规划——改进RRT算法
8、https://www.jianshu.com/p/489b0b39f3f2 运动规划笔记——邱强

在这里分享了运动规划方面的一些基本的算法原理和伪代码实现，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

基于搜索

这一部分是基于搜索的路径规划算法，依次介绍BFS、Dijkstra、A* 和Hybrid A*，从顺序上说，这四种算法也是一个不断迭代和优化的过程。另外，有关A*的其他衍生的算法，日后更新…

BFS

BFS（Breadth-First-Searc），基本思想是以放射状扩散的方式遍历所有点。
图解BFS点这里。
BFS的伪代码解释如下：
1、基本过程

2、为生成路径，引入父节点的定义

3、为避免枚举的问题，引入启发式搜索，以当前点到终点的距离为代价（优先级）

4、路径回放，生成最终路径

5、BFS的一个问题是容易先入为主地认为最先遍历的就是最短路径上的点。

Dijkstra

Dijkstra也采用了BFS类似的方法，其代价的计算方式为以当前点到起点的距离为代价（优先级）。

A*

A*算法结合了BFS和Dijkstra的思想，综合当前点到起点和终点的距离和（代价和/优先级）。

Hybrid A*

A*算法规划的轨迹经过方格中心，现实车辆路径无法满足这一情况，因此提出了Hybrid A*。

1、这里整理了几种车辆的运动学模型：

① 自行车模型的基本假设
·不考虑车辆垂直方向的运动，认为车辆在二维平面上运动
·左右车轮在转速和转角上一致，两个车轮可以合为一个
·车速缓慢，忽略前后轴载荷的转移
·车身和悬架均为刚性
·车辆转向和驱动由前轮完成
② 以后轴中心为原点的车辆运动学模型——输入（a，φ） 输出车辆位姿（x，y，θ）

③ 以质心为中心的车辆运动学模型——输入（a，δ） 输出车辆位姿（x，y，ψ）

④ 前轮驱动的车辆运动学模型——输入（a，δ） 输出车辆位姿（x，y，ψ）

这里假设汽车是前轮驱动的，所以认为方向盘的转角就等于前轮的转角，后轮偏角为0。

2、一种运动学模型下的Hybrid A*算法伪代码实现
①运动学模型：4-D 离散网格中的车辆状态 <x, y, θ, is_foward>，以后轴为中心的车辆运动学模型（离散）。

②伪代码




③与A*的不同
0x00：二维变三维，新增偏航角这一维
0x01：寻找当前位置周围四点，变成寻找当前位置在不同前轮转角下的多个点（range（-35,40,5））

下面是用QT写的一个简单实现：

参考文献

1、https://www.redblobgames.com/pathfinding/a-star/introduction.html
2、https://blog.csdn.net/AdamShan/article/details/79945175 无人驾驶汽车系统入门（十六）——最短路径搜索之A*算法
3、https://zhuanlan.zhihu.com/p/103834150 自动驾驶中的车辆运动学模型
4、https://zhuanlan.zhihu.com/p/139489196 自动驾驶运动规划-Hybird A* 算法
5、https://blog.csdn.net/Rickey_lee09/article/details/104795530 无人驾驶学习笔记–路径规划（一）【Trajectory Generation – Hybrid A*】