[发明专利]一种无人车启发式自动泊车方法及装置

权利要求书

1.一种无人车启发式自动泊车方法，其特征在于，包括：

构建车辆位姿状态集；

构建多分辨率的状态转换规则集；其中，所述状态转换规则集为从任意位姿状态发散出来的、方向不同的、可以连接相邻两个不同位姿状态的所有分支集合；所述状态转换规则集包括在车辆起始位置和车辆目标泊车点预设范围内使用的第一规则集，以及在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的第二规则集，所述第一规则集的精度大于所述第二规则集的精度；

构建用于泊车路径搜索的启发函数和估计函数；

基于构建的启发函数和估计函数，以及所述状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径；

所述构建车辆位姿状态集，包括：

定义车辆位姿状态，采用三维状态空间点(x,y,θ)来表征车辆特定位姿；其中，(x,y)表示车辆中心在世界坐标系中的位置，θ表示车身朝向角度；

通过离散化方法得到表征车辆位姿的所有状态的集合；

所述构建多分辨率的状态转换规则集，包括：

定义状态转换规则集为从任意位姿状态发散出来的、方向不同的、可连接相邻两个不同位姿状态的所有分支集合，用一系列分支将起始状态、终止状态及两者间多个过渡状态按一定顺序连接起来，便得到泊车过程中的整条路径；

定义第一规则集和第二规则集，使两者位于同一个维度的网络空间中；

使所述第二规则集成为所述第一规则集的一个真子集，以确保所述第一规则集中的路径被充分利用，同时减少图搜索过程中的分支数量，并使得车辆在不同区域拥有不同的决策空间；

所述构建用于泊车路径搜索的启发函数和估计函数，包括：

将车辆当前位置到终点的距离和车辆当前朝向与目标朝向之间的角度差分别进行归一化处理，表达式如下：

L′＝L/L_t

α′＝α/360

其中，L_t表示车辆起点到终点的欧氏距离，L表示车辆当前位置到终点的欧氏距离，L′表示归一化后的距离，α表示车辆当前朝向与目标朝向之间的角度差，且α的单位为度，α′表示归一化后的角度差；

基于归一化的距离和角度差，定义启发函数：

h^*(n)＝w₁·L′+w₂·α′

其中，w₁为距离权值，w₂为角度权值，且w₁和w₂相加为1；

构建状态节点的估计函数f^*(n)：

f^*(n)＝g^*(n)+h^*(n)

其中，g^*(n)为起始点到当前点路径的实际代价值；

g^*(n)采用车辆实际已走过的路径长度及车身转过角度的加权和表示；其中，车辆实际已走过的路径长度及车身转过角度分别进行归一化处理；

所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，包括：

对启发函数进行膨胀，即给启发函数乘上一个大于1的膨胀因子ε，相应地，将估计函数修正为：f^*(n)＝g^*(n)+ε·h^*(n)；

构建OPEN表和CLOSED表；其中，OPEN表用来存储待扩展节点，CLOSED表用来存储已扩展节点；OPEN表中的每一个节点n都有一个估价值g(start,n)+ε·h(n,goal)，其中，g(start,n)为车辆初始位置到车辆当前位置的实际路径代价值，h(n,goal)为启发函数，为当前点到目标点的路径估计代价；ε为大于1的膨胀因子；

路径搜索过程中，每次均选取OPEN表中估价值最小的节点，判断当前选取的节点是否为目标节点，若是，则最优路径搜索过程结束，若不是，则按照所述状态转换规则集对当前选取的节点进行扩展，并将扩展出的且不属于预设禁止区域的新节点放入OPEN表，同时设置这些新节点至其父节点的指针，并将父节点放入CLOSED表，迭代这一过程，直至最优路径搜索过程结束；

在最优路径搜索过程结束后，根据所设的父节点指针从目标节点追溯到起始节点，形成最终的泊车路径，并输出最终的泊车路径；

所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，还包括：

逐步减小膨胀因子ε的值，从而将生成的次优路径不断优化，若膨胀因子ε减小至1，此时生成的路径为最优。

2.一种无人车启发式自动泊车装置，其特征在于，包括：

状态集构建模块，用于获取车辆位姿状态，构建并存储车辆位姿状态集；

规则集构建模块，用于构建并存储多分辨率的状态转换规则集；其中，所述状态转换规则集为从任意位姿状态发散出来的、方向不同的、可以连接相邻两个不同位姿状态的所有分支集合；所述状态转换规则集包括在车辆起始位置和车辆目标泊车点预设范围内使用的第一规则集，以及在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的第二规则集，所述第一规则集的精度大于所述第二规则集的精度；

函数构建模块，用于构建并存储用于泊车路径搜索的启发函数和估计函数；

路径规划模块，用于基于所述函数构建模块所构建的启发函数和估计函数，以及所述规则集构建模块所构建的状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径；

所述构建车辆位姿状态集，包括：

定义车辆位姿状态，采用三维状态空间点(x,y,θ)来表征车辆特定位姿；其中，(x,y)表示车辆中心在世界坐标系中的位置，θ表示车身朝向角度；

通过离散化方法得到表征车辆位姿的所有状态的集合；

所述构建多分辨率的状态转换规则集，包括：

定义状态转换规则集为从任意位姿状态发散出来的、方向不同的、可连接相邻两个不同位姿状态的所有分支集合，用一系列分支将起始状态、终止状态及两者间多个过渡状态按一定顺序连接起来，便得到泊车过程中的整条路径；

定义第一规则集和第二规则集，使两者位于同一个维度的网络空间中；

使所述第二规则集成为所述第一规则集的一个真子集，以确保所述第一规则集中的路径被充分利用，同时减少图搜索过程中的分支数量，并使得车辆在不同区域拥有不同的决策空间；

所述构建用于泊车路径搜索的启发函数和估计函数，包括：

将车辆当前位置到终点的距离和车辆当前朝向与目标朝向之间的角度差分别进行归一化处理，表达式如下：

L′＝L/L_t

α′＝α/360

其中，L_t表示车辆起点到终点的欧氏距离，L表示车辆当前位置到终点的欧氏距离，L′表示归一化后的距离，α表示车辆当前朝向与目标朝向之间的角度差，且α的单位为度，α′表示归一化后的角度差；

基于归一化的距离和角度差，定义启发函数：

h^*(n)＝w₁·L′+w₂·α′

其中，w₁为距离权值，w₂为角度权值，且w₁和w₂相加为1；

构建状态节点的估计函数f^*(n)：

f^*(n)＝g^*(n)+h^*(n)

其中，g^*(n)为起始点到当前点路径的实际代价值；

g^*(n)采用车辆实际已走过的路径长度及车身转过角度的加权和表示；其中，车辆实际已走过的路径长度及车身转过角度分别进行归一化处理；

所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，包括：

对启发函数进行膨胀，即给启发函数乘上一个大于1的膨胀因子ε，相应地，将估计函数修正为：f^*(n)＝g^*(n)+ε·h^*(n)；

构建OPEN表和CLOSED表；其中，OPEN表用来存储待扩展节点，CLOSED表用来存储已扩展节点；OPEN表中的每一个节点n都有一个估价值g(start,n)+ε·h(n,goal)，其中，g(start,n)为车辆初始位置到车辆当前位置的实际路径代价值，h(n,goal)为启发函数，为当前点到目标点的路径估计代价；ε为大于1的膨胀因子；

路径搜索过程中，每次均选取OPEN表中估价值最小的节点，判断当前选取的节点是否为目标节点，若是，则最优路径搜索过程结束，若不是，则按照所述状态转换规则集对当前选取的节点进行扩展，并将扩展出的且不属于预设禁止区域的新节点放入OPEN表，同时设置这些新节点至其父节点的指针，并将父节点放入CLOSED表，迭代这一过程，直至最优路径搜索过程结束；

在最优路径搜索过程结束后，根据所设的父节点指针从目标节点追溯到起始节点，形成最终的泊车路径，并输出最终的泊车路径；

所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于A^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，还包括：

逐步减小膨胀因子ε的值，从而将生成的次优路径不断优化，若膨胀因子ε减小至1，此时生成的路径为最优。