CN114115298A - 一种无人车路径平滑方法及系统 - Google Patents

一种无人车路径平滑方法及系统 Download PDF

Info

Publication number
CN114115298A
CN114115298A CN202210082735.8A CN202210082735A CN114115298A CN 114115298 A CN114115298 A CN 114115298A CN 202210082735 A CN202210082735 A CN 202210082735A CN 114115298 A CN114115298 A CN 114115298A
Authority
CN
China
Prior art keywords
discrete
coordinate system
waypoint
waypoints
constraint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210082735.8A
Other languages
English (en)
Other versions
CN114115298B (zh
Inventor
冯时
席军强
龚建伟
李德润
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210082735.8A priority Critical patent/CN114115298B/zh
Publication of CN114115298A publication Critical patent/CN114115298A/zh
Application granted granted Critical
Publication of CN114115298B publication Critical patent/CN114115298B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种无人车路径平滑方法及系统。该方法包括提取固定步长的离散路点和参考路点；构建第一目标函数；对第一目标函数进行求解，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；构建第二目标函数；对第二目标函数进行求解。本发明能够在全局导航指引线和车辆实时环境信息已知情况下，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线。

Description

一种无人车路径平滑方法及系统
技术领域
本发明涉及路径规划领域，特别是涉及一种无人车路径平滑方法及系统。
背景技术
无人车辆从起点运动到目标点，需要全局导航指引线的引导，全局导航指引线生成是指依据某个或某些性能指标，例如工作代价最小、行走路线最短、行走时间最短等，依据地图信息找到一条最优或者接近最优的路径，属于路径规划问题，常用方法包括基于图搜索和基于采样的规划算法，这些算法目前已经被广泛采用，但得到的路径一般是折线路径，曲率不连续，不利于车辆进行执行；无人车路径平滑是指依据平滑性指标，在全局折线路径的基础上规划出一条连接起始点和目标点、避开障碍物的平滑路径，常用方法一般通过不同曲线模型拟合获得平滑路径，包括多项式曲线、贝塞尔曲线、样条曲线等。
然而上述使用曲线模型拟合的路径平滑方式往往并未考虑车辆实时朝向信息、位置与导航指引线之间的偏差距离信息以及障碍物信息，不能提供带有足够信息的路径平滑曲线。
因此，亟需一种考虑车辆实时朝向及位置偏差的避障平滑路径生成方式，为后续车辆最终运动规划轨迹的生成提供更加准确的参考信息。
发明内容
本发明的目的是提供一种无人车路径平滑方法及系统，能够在全局导航指引线和车辆实时环境信息已知情况下，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线。
为实现上述目的，本发明提供了如下方案：
一种无人车路径平滑方法，包括：
获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。
可选地，所述根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数，具体包括：
利用公式
Figure 300160DEST_PATH_IMAGE001
确定代价函数；
利用公式
Figure 988630DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束；
利用公式
Figure 172487DEST_PATH_IMAGE003
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 73447DEST_PATH_IMAGE004
为代价函数，
Figure 26359DEST_PATH_IMAGE005
、
Figure 620152DEST_PATH_IMAGE006
以及
Figure 28655DEST_PATH_IMAGE007
均为权重，
Figure 264464DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 71883DEST_PATH_IMAGE009
和
Figure 570998DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 463867DEST_PATH_IMAGE011
和
Figure 706630DEST_PATH_IMAGE012
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 634135DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 835309DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 949895DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 261928DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 43939DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 153365DEST_PATH_IMAGE018
和
Figure 20827DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 870971DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 38648DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 316045DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 405224DEST_PATH_IMAGE023
和
Figure 324638DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 550083DEST_PATH_IMAGE025
和
Figure 263961DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 120664DEST_PATH_IMAGE027
和
Figure 312611DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 658142DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
可选地，所述以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数，具体包括：
利用公式
Figure 542921DEST_PATH_IMAGE030
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；
利用公式
Figure 606692DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
利用公式
Figure 602330DEST_PATH_IMAGE032
确定避障约束；
其中，
Figure 67946DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 123627DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 409115DEST_PATH_IMAGE035
为
Figure 208443DEST_PATH_IMAGE034
的一阶导数，
Figure 528566DEST_PATH_IMAGE036
为
Figure 758078DEST_PATH_IMAGE034
的二阶导数，
Figure 796441DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 133882DEST_PATH_IMAGE038
为
Figure 308511DEST_PATH_IMAGE037
的一阶导数，
Figure 705994DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 231654DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 372785DEST_PATH_IMAGE041
为初始累加距离，
Figure 667500DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 970305DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
可选地，所述利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束，具体以下公式：
Figure 251770DEST_PATH_IMAGE044
；
其中，
Figure 462171DEST_PATH_IMAGE045
和
Figure 611393DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 85099DEST_PATH_IMAGE047
为安全性代价，
Figure 319772DEST_PATH_IMAGE048
为平滑性代价，
Figure 802706DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
一种无人车路径平滑系统，包括：
第一拟合曲线确定模块，用于获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
第二拟合曲线确定模块，用于对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
离散路点提取模块，用于对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
第一目标函数构建模块，用于根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
第一优化模块，用于利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
采样点确定模块，用于将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
动态规划模块，用于利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
第二目标函数构建模块，用于以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
第二优化模块，用于利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。
可选地，所述第一目标函数构建模块具体包括：
代价函数第一确定单元，用于利用公式
Figure 806434DEST_PATH_IMAGE001
确定代价函数；
约束条件第一确定单元，用于利用公式
Figure 982200DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束；
约束条件第二确定单元，用于利用公式
Figure 704168DEST_PATH_IMAGE050
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 459635DEST_PATH_IMAGE004
为代价函数，
Figure 383116DEST_PATH_IMAGE005
、
Figure 464204DEST_PATH_IMAGE006
以及
Figure 673469DEST_PATH_IMAGE007
均为权重，
Figure 498205DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 210947DEST_PATH_IMAGE009
和
Figure 462936DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 425076DEST_PATH_IMAGE011
和
Figure 787924DEST_PATH_IMAGE012
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 620751DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 43642DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 953131DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 119670DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 72582DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 931954DEST_PATH_IMAGE018
和
Figure 337527DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 42178DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 849597DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 614291DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 507160DEST_PATH_IMAGE023
和
Figure 549590DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 477095DEST_PATH_IMAGE025
和
Figure 678269DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 792856DEST_PATH_IMAGE027
和
Figure 104888DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 886900DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
可选地，所述第二目标函数构建模块具体包括：
代价函数第二确定单元，用于利用公式
Figure 993396DEST_PATH_IMAGE030
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；
约束条件第三确定单元，用于利用公式
Figure 860858DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
约束条件第四确定单元，用于利用公式
Figure 711002DEST_PATH_IMAGE051
确定避障约束；
其中，
Figure 347520DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 627847DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 982605DEST_PATH_IMAGE035
为
Figure 636440DEST_PATH_IMAGE034
的一阶导数，
Figure 658623DEST_PATH_IMAGE036
为
Figure 638080DEST_PATH_IMAGE034
的二阶导数，
Figure 214555DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 406502DEST_PATH_IMAGE038
为
Figure 17612DEST_PATH_IMAGE037
的一阶导数，
Figure 636812DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 700583DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 699151DEST_PATH_IMAGE041
为初始累加距离，
Figure 430346DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 220448DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
可选地，所述动态规划模块具体以下公式：
Figure 771515DEST_PATH_IMAGE044
；
其中，
Figure 570843DEST_PATH_IMAGE045
和
Figure 625387DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 586390DEST_PATH_IMAGE047
为安全性代价，
Figure 624753DEST_PATH_IMAGE052
为平滑性代价，
Figure 962193DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明所提供的一种无人车路径平滑方法及系统，在全局导航指引线和车辆实时环境信息已知情况下，通过利用二次规划和动态规划的组合优化方式，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线，相比与仅用曲线拟合方式的路径平滑方法，能够生成考虑车辆实时朝向和位置偏差信息的避障曲线，有利于为车辆最终运动规划结果的生成提供更加丰富的信息。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种无人车路径平滑方法流程示意图；
图2为整体流程示意图；
图3为参考路点示意图；
图4为第一次优化后的路点示意图；
图5为连续性约束示意图；
图6为动态规划算法示意图；
图7为第二次优化后的路点示意图；
图8为本发明生成的路径平滑曲线示意图；
图9为依靠三次B样条曲线生成的路径平滑曲线示意图；
图10为本发明所提供的一种无人车路径平滑系统结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种无人车路径平滑方法及系统，能够在全局导航指引线和车辆实时环境信息已知情况下，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明所提供的一种无人车路径平滑方法流程示意图，图2整体流程示意图，如图1和图2所示，本发明所提供的一种无人车路径平滑方法，包括：
S101，获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
S102，对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
S103，对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
即在第一拟合曲线上依据设置步长
Figure 136823DEST_PATH_IMAGE053
在拟合曲线上选取n个待优化离散点笛卡尔坐标系下坐标点，依据拟合曲线上选取n个待优化离散点笛卡尔坐标系下坐标点计算每两个点之间的距离，获得包含
Figure 627034DEST_PATH_IMAGE054
一系列点集，其中
Figure 887114DEST_PATH_IMAGE055
表示点距离的累加，其计算方式为
Figure 28245DEST_PATH_IMAGE056
，并分别进行
Figure 57381DEST_PATH_IMAGE057
关于
Figure 625766DEST_PATH_IMAGE058
和
Figure 373142DEST_PATH_IMAGE059
关于
Figure 52385DEST_PATH_IMAGE058
的三次自然样条差值拟合，然后依据给定步长
Figure 467186DEST_PATH_IMAGE060
提取一系列固定步长的点，并获得每个点对应的角度
Figure 206472DEST_PATH_IMAGE061
和曲率
Figure 706723DEST_PATH_IMAGE062
，获得一系列待优化点集
Figure 458166DEST_PATH_IMAGE063
，如图3所示。
S104，根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
确定优化变量
Figure 461894DEST_PATH_IMAGE064
，优化目标为使得优化后的一系列点集构成的曲线尽可能贴近步骤一获得的拟合曲线的同时，尽可能平滑且曲率变化平缓，如图4所示，故使得以下代价函数最小。
S104具体包括：
利用公式
Figure 372081DEST_PATH_IMAGE001
确定代价函数；
第一项表示使平滑后的曲线尽量贴近初始全局路径，其权重表示为
Figure 94050DEST_PATH_IMAGE065
，第二项表示最小化曲率，使得曲线尽可能平滑，其权重表示为
Figure 115095DEST_PATH_IMAGE066
，第三项表示最小化曲率差，使曲率变化平缓，其权重表示为
Figure 504488DEST_PATH_IMAGE067
。
利用公式
Figure 319997DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束，如图5所示；
利用公式
Figure 529262DEST_PATH_IMAGE003
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 353998DEST_PATH_IMAGE004
为代价函数，
Figure 332319DEST_PATH_IMAGE005
、
Figure 852817DEST_PATH_IMAGE006
以及
Figure 283799DEST_PATH_IMAGE007
均为权重，
Figure 646647DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 745053DEST_PATH_IMAGE009
和
Figure 699103DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 617380DEST_PATH_IMAGE011
和
Figure 49498DEST_PATH_IMAGE012
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 2411DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 596203DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 270286DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 709357DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 782355DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 78208DEST_PATH_IMAGE018
和
Figure 236656DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 744998DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 406924DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 342519DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 722684DEST_PATH_IMAGE023
和
Figure 34717DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 807939DEST_PATH_IMAGE025
和
Figure 914436DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 781897DEST_PATH_IMAGE027
和
Figure 632042DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 268559DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
S105，利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
即计算每两个点之间的距离获得包含
Figure 545957DEST_PATH_IMAGE068
的一系列点集，并分别进行
Figure 166294DEST_PATH_IMAGE057
关于
Figure 554550DEST_PATH_IMAGE058
和
Figure 45574DEST_PATH_IMAGE069
关于
Figure 27961DEST_PATH_IMAGE058
三次样条差值，然后依据给定步长
Figure 135595DEST_PATH_IMAGE070
提取一系列固定步长的点，将其转到Frenet坐标系下。
一次优化后曲线仅能保证车辆朝向偏差、位置偏差的消除，以及曲线平滑性及平顺性的保证，并无法满足避障需求，故采用动态规划算法对其进行重新处理，获得避障曲线。
S106，将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
作为一个具体的实施例，Frenet坐标系下的离散路点
Figure 327542DEST_PATH_IMAGE071
为m个，
Figure 938652DEST_PATH_IMAGE072
，每一层采样点为
Figure 557852DEST_PATH_IMAGE073
，个数为n；
S107，利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
S107具体以下公式：
Figure 621623DEST_PATH_IMAGE044
；
其中，
Figure 882840DEST_PATH_IMAGE045
和
Figure 348456DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 138558DEST_PATH_IMAGE047
为安全性代价，
Figure 489292DEST_PATH_IMAGE052
为平滑性代价，
Figure 288621DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
其中，将安全性代价设置为
Figure 874323DEST_PATH_IMAGE074
，期望的距离障碍物的安全距离设置为
Figure 100905DEST_PATH_IMAGE075
，当前点距离障碍物距离表示为
Figure 404847DEST_PATH_IMAGE076
，则安全性代价设定为：
Figure 476708DEST_PATH_IMAGE077
；
将当前点与对应原始点
Figure 182496DEST_PATH_IMAGE078
的Frenet坐标系下l轴的距离表示
Figure 314400DEST_PATH_IMAGE079
，每两个原始点
Figure 371218DEST_PATH_IMAGE080
之间的距离表示为
Figure 515279DEST_PATH_IMAGE081
，当前点角度为
Figure 809994DEST_PATH_IMAGE082
，则将平滑性代价表示为：
Figure 378379DEST_PATH_IMAGE083
；
偏差性代价是指希望在保证安全和平滑的前提下，位置和朝向能够尽可能贴近原有的参考线，
Figure 125755DEST_PATH_IMAGE084
为
Figure 804998DEST_PATH_IMAGE085
的朝向，将偏差性代价表示为：
Figure 219799DEST_PATH_IMAGE086
；
对每一层进行动态规划搜索，当前层最小代价值的点的上一层对应的点标记为当前层的父节点，直到搜索到最后一层后，依据父节点逐层向上分别找到每一层代价最小的点，作为一系列可通行点。
每一层的代价值最小的点，在其l轴上寻找尽量满足安全距离的上下边界，作为每一层对应的可通行凸边界
Figure 755822DEST_PATH_IMAGE087
，如图6所示。
S108，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
S108具体包括：
利用公式
Figure 724915DEST_PATH_IMAGE088
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；期望函数主要包括与全局路径的偏移程度和曲线的平滑性两个指标，如图7所示。第一个指标可以通过最小化l来完成。对于第二个指标，可以使曲线相对于全局路径的横向变化尽量轻微，即最小化横向距离的一阶导数和二阶导数；
利用公式
Figure 207849DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
利用公式
Figure 491805DEST_PATH_IMAGE089
确定避障约束，并将第一个点的横向位移设置为车辆当前位置的l坐标；
其中，
Figure 401992DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 389540DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 410585DEST_PATH_IMAGE035
为
Figure 268820DEST_PATH_IMAGE034
的一阶导数，
Figure 349908DEST_PATH_IMAGE036
为
Figure 824752DEST_PATH_IMAGE034
的二阶导数，
Figure 649489DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 158967DEST_PATH_IMAGE038
为
Figure 413887DEST_PATH_IMAGE037
的一阶导数，
Figure 110447DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 473296DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 571702DEST_PATH_IMAGE041
为初始累加距离，
Figure 729014DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 912870DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
S109，利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。本发明确定路径平滑曲线考虑车辆实时位置偏差和朝向，如图8所示；仅依靠三次B样条曲线拟合路径如图9所示，由图8和图9的对比可知，本发明路径平滑且避障效果好。
图10为本发明所提供的一种无人车路径平滑系统结构示意图，如图10所示，本发明所提供的一种无人车路径平滑系统，包括：
第一拟合曲线确定模块1001，用于获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
第二拟合曲线确定模块1002，用于对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
离散路点提取模块1003，用于对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
第一目标函数构建模块1004，用于根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
第一优化模块1005，用于利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
采样点确定模块1006，用于将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
动态规划模块1007，用于利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
第二目标函数构建模块1008，用于以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
第二优化模块1009，用于利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。
所述第一目标函数构建模块1004具体包括：
代价函数第一确定单元，用于利用公式
Figure 79409DEST_PATH_IMAGE001
确定代价函数；
约束条件第一确定单元，用于利用公式
Figure 32322DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束；
约束条件第二确定单元，用于利用公式
Figure 626114DEST_PATH_IMAGE050
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 31688DEST_PATH_IMAGE004
为代价函数，
Figure 67164DEST_PATH_IMAGE005
、
Figure 609004DEST_PATH_IMAGE006
以及
Figure 639277DEST_PATH_IMAGE007
均为权重，
Figure 63305DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 571647DEST_PATH_IMAGE009
和
Figure 233572DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 169167DEST_PATH_IMAGE011
和
Figure 549333DEST_PATH_IMAGE012
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 129875DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 177465DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 283961DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 151423DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 1568DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 638085DEST_PATH_IMAGE018
和
Figure 181062DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 270241DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 924076DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 415100DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 651348DEST_PATH_IMAGE023
和
Figure 493402DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 685349DEST_PATH_IMAGE025
和
Figure 30879DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 915659DEST_PATH_IMAGE027
和
Figure 979430DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 240647DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
所述第二目标函数构建模块1008具体包括：
代价函数第二确定单元，用于利用公式
Figure 175105DEST_PATH_IMAGE030
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；
约束条件第三确定单元，用于利用公式
Figure 496365DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
约束条件第四确定单元，用于利用公式
Figure 50361DEST_PATH_IMAGE051
确定避障约束；
其中，
Figure 584111DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 700972DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 255450DEST_PATH_IMAGE035
为
Figure 293813DEST_PATH_IMAGE034
的一阶导数，
Figure 631253DEST_PATH_IMAGE036
为
Figure 71462DEST_PATH_IMAGE034
的二阶导数，
Figure 203366DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 731955DEST_PATH_IMAGE038
为
Figure 607507DEST_PATH_IMAGE037
的一阶导数，
Figure 902222DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 736186DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 14720DEST_PATH_IMAGE041
为初始累加距离，
Figure 693964DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 843185DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
所述动态规划模块1007具体以下公式：
Figure 582471DEST_PATH_IMAGE044
；
其中，
Figure 348302DEST_PATH_IMAGE045
和
Figure 568586DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 837893DEST_PATH_IMAGE047
为安全性代价，
Figure 748081DEST_PATH_IMAGE052
为平滑性代价，
Figure 470049DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种无人车路径平滑方法，其特征在于，包括：
获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。
2.根据权利要求1所述的一种无人车路径平滑方法，其特征在于，所述根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数，具体包括：
利用公式
Figure 441586DEST_PATH_IMAGE001
确定代价函数；
利用公式
Figure 889885DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束；
利用公式
Figure 466359DEST_PATH_IMAGE003
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 658306DEST_PATH_IMAGE004
为代价函数，
Figure 269416DEST_PATH_IMAGE005
、
Figure 623037DEST_PATH_IMAGE006
以及
Figure 689738DEST_PATH_IMAGE007
均为权重，
Figure 685376DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 150992DEST_PATH_IMAGE009
和
Figure 941094DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 492161DEST_PATH_IMAGE011
和
Figure 25910DEST_PATH_IMAGE012
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 80454DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 307036DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 345399DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 417260DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 591890DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 715005DEST_PATH_IMAGE018
和
Figure 240664DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 116216DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 145352DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 713736DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 461113DEST_PATH_IMAGE023
和
Figure 140356DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 289577DEST_PATH_IMAGE025
和
Figure 28863DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 263535DEST_PATH_IMAGE027
和
Figure 215311DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 484618DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
3.根据权利要求2所述的一种无人车路径平滑方法，其特征在于，所述以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数，具体包括：
利用公式
Figure 397735DEST_PATH_IMAGE030
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；
利用公式
Figure 119703DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
利用公式
Figure 140749DEST_PATH_IMAGE032
确定避障约束；
其中，
Figure 998984DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 80072DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 289337DEST_PATH_IMAGE035
为
Figure 114073DEST_PATH_IMAGE034
的一阶导数，
Figure 92393DEST_PATH_IMAGE036
为
Figure 78804DEST_PATH_IMAGE034
的二阶导数，
Figure 509785DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 872633DEST_PATH_IMAGE038
为
Figure 973969DEST_PATH_IMAGE037
的一阶导数，
Figure 131281DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 315138DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 216098DEST_PATH_IMAGE041
为初始累加距离，
Figure 169010DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 762802DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
4.根据权利要求1所述的一种无人车路径平滑方法，其特征在于，所述利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束，具体以下公式：
Figure 902797DEST_PATH_IMAGE044
；
其中，
Figure 873027DEST_PATH_IMAGE045
和
Figure 680446DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 445140DEST_PATH_IMAGE047
为安全性代价，
Figure 338009DEST_PATH_IMAGE048
为平滑性代价，
Figure 849281DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
5.一种无人车路径平滑系统，其特征在于，包括：
第一拟合曲线确定模块，用于获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；
第二拟合曲线确定模块，用于对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；
离散路点提取模块，用于对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；
第一目标函数构建模块，用于根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；
第一优化模块，用于利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；
采样点确定模块，用于将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；
动态规划模块，用于利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；
第二目标函数构建模块，用于以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；
第二优化模块，用于利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。
6.根据权利要求5所述的一种无人车路径平滑系统，其特征在于，所述第一目标函数构建模块具体包括：
代价函数第一确定单元，用于利用公式
Figure 511206DEST_PATH_IMAGE001
确定代价函数；
约束条件第一确定单元，用于利用公式
Figure 509118DEST_PATH_IMAGE002
确定离散路点和参考路点的连续性约束；
约束条件第二确定单元，用于利用公式
Figure 889284DEST_PATH_IMAGE050
确定车辆朝向约束、车辆实时位置偏差约束及终点约束；
其中，
Figure 935737DEST_PATH_IMAGE004
为代价函数，
Figure 717748DEST_PATH_IMAGE005
、
Figure 824245DEST_PATH_IMAGE006
以及
Figure 426127DEST_PATH_IMAGE007
均为权重，
Figure 276272DEST_PATH_IMAGE008
为提取固定步长的离散路点的个数，
Figure 927438DEST_PATH_IMAGE009
和
Figure 204835DEST_PATH_IMAGE010
为第i个离散路点在笛卡尔坐标系下坐标，
Figure 294014DEST_PATH_IMAGE011
和
Figure 947849DEST_PATH_IMAGE051
为第i个参考路点在笛卡尔坐标系下坐标，
Figure 438873DEST_PATH_IMAGE013
为第i个离散路点的曲率，
Figure 887172DEST_PATH_IMAGE014
为第i+1个离散路点的曲率，
Figure 729226DEST_PATH_IMAGE015
为第i个参考路点对应的角度，
Figure 655594DEST_PATH_IMAGE016
为第i个离散路点对应的角度，
Figure 1125DEST_PATH_IMAGE017
为第i个离散路点与第i个参考路点的角度偏差，
Figure 885904DEST_PATH_IMAGE018
和
Figure 949675DEST_PATH_IMAGE019
为第i+1个离散路点在笛卡尔坐标系下坐标，
Figure 948243DEST_PATH_IMAGE020
为第i+1个离散路点与第i+1个参考路点的角度偏差，
Figure 148280DEST_PATH_IMAGE021
为当前车辆实时朝向，
Figure 203961DEST_PATH_IMAGE022
为车辆此时位置距离，
Figure 755028DEST_PATH_IMAGE023
和
Figure 288777DEST_PATH_IMAGE024
为起点在笛卡尔坐标系下坐标，
Figure 608900DEST_PATH_IMAGE025
和
Figure 569903DEST_PATH_IMAGE026
为终点在笛卡尔坐标系下坐标，
Figure 342687DEST_PATH_IMAGE027
和
Figure 680127DEST_PATH_IMAGE028
为第n个参考路点在笛卡尔坐标系下坐标，
Figure 120336DEST_PATH_IMAGE029
为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。
7.根据权利要求6所述的一种无人车路径平滑系统，其特征在于，所述第二目标函数构建模块具体包括：
代价函数第二确定单元，用于利用公式
Figure 252240DEST_PATH_IMAGE052
确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；
约束条件第三确定单元，用于利用公式
Figure 780829DEST_PATH_IMAGE031
确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；
约束条件第四确定单元，用于利用公式
Figure 656381DEST_PATH_IMAGE053
确定避障约束；
其中，
Figure 951096DEST_PATH_IMAGE033
为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，
Figure 253901DEST_PATH_IMAGE034
为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，
Figure 266857DEST_PATH_IMAGE035
为
Figure 680521DEST_PATH_IMAGE034
的一阶导数，
Figure 829742DEST_PATH_IMAGE036
为
Figure 303449DEST_PATH_IMAGE034
的二阶导数，
Figure 538121DEST_PATH_IMAGE037
为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，
Figure 489897DEST_PATH_IMAGE038
为
Figure 493625DEST_PATH_IMAGE037
的一阶导数，
Figure 403812DEST_PATH_IMAGE039
为Frenet坐标系下的第i个离散路点的累加距离，
Figure 128710DEST_PATH_IMAGE040
为Frenet坐标系下的第i+1个离散路点的累加距离，
Figure 884176DEST_PATH_IMAGE041
为初始累加距离，
Figure 7990DEST_PATH_IMAGE042
为车辆此时累加距离，
Figure 823499DEST_PATH_IMAGE043
为凸可行集边界的最大值和最小值。
8.根据权利要求5所述的一种无人车路径平滑系统，其特征在于，所述动态规划模块具体以下公式：
Figure 767185DEST_PATH_IMAGE044
；
其中，
Figure 326342DEST_PATH_IMAGE045
和
Figure 304662DEST_PATH_IMAGE046
均为每一层最小的代价值，
Figure 291073DEST_PATH_IMAGE047
为安全性代价，
Figure 722054DEST_PATH_IMAGE048
为平滑性代价，
Figure 84902DEST_PATH_IMAGE049
为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。
CN202210082735.8A 2022-01-25 2022-01-25 一种无人车路径平滑方法及系统 Active CN114115298B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210082735.8A CN114115298B (zh) 2022-01-25 2022-01-25 一种无人车路径平滑方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210082735.8A CN114115298B (zh) 2022-01-25 2022-01-25 一种无人车路径平滑方法及系统

Publications (2)

Publication Number Publication Date
CN114115298A true CN114115298A (zh) 2022-03-01
CN114115298B CN114115298B (zh) 2022-05-17

Family

ID=80360822

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210082735.8A Active CN114115298B (zh) 2022-01-25 2022-01-25 一种无人车路径平滑方法及系统

Country Status (1)

Country Link
CN (1) CN114115298B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442491A (zh) * 2022-04-08 2022-05-06 杭州国辰机器人科技有限公司 室内机器人的局部路径规划和跟踪控制方法、装置及介质
CN114676939A (zh) * 2022-05-26 2022-06-28 之江实验室 一种多车型参数自适应的参考线平滑方法和系统
WO2023221537A1 (zh) * 2022-05-16 2023-11-23 北京京东乾石科技有限公司 路径规划方法及装置、无人车

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130090802A1 (en) * 2011-10-07 2013-04-11 Southwest Research Institute Waypoint splining for autonomous vehicle following
CN109947101A (zh) * 2019-03-18 2019-06-28 北京智行者科技有限公司 路径平滑处理方法及装置
CN110057362A (zh) * 2019-04-26 2019-07-26 安徽理工大学 有限单元地图的移动机器人路径规划方法
CN110823238A (zh) * 2019-11-05 2020-02-21 湖南大学 一种改进型三次样条插值曲线拟合路径点方法
CN111002580A (zh) * 2019-12-17 2020-04-14 杭州电子科技大学 一种基于Hilbert曲线改进的3D打印路径填充方法
CN111338341A (zh) * 2020-02-24 2020-06-26 北京百度网讯科技有限公司 车辆避障方法和设备、电子设备、车辆及存储介质
CN111791887A (zh) * 2020-07-03 2020-10-20 北京理工大学 一种基于分层式车速规划的车辆节能驾驶方法
CN112068545A (zh) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质
CN112673234A (zh) * 2020-01-17 2021-04-16 华为技术有限公司 路径规划方法和路径规划装置

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130090802A1 (en) * 2011-10-07 2013-04-11 Southwest Research Institute Waypoint splining for autonomous vehicle following
CN109947101A (zh) * 2019-03-18 2019-06-28 北京智行者科技有限公司 路径平滑处理方法及装置
CN110057362A (zh) * 2019-04-26 2019-07-26 安徽理工大学 有限单元地图的移动机器人路径规划方法
CN110823238A (zh) * 2019-11-05 2020-02-21 湖南大学 一种改进型三次样条插值曲线拟合路径点方法
CN111002580A (zh) * 2019-12-17 2020-04-14 杭州电子科技大学 一种基于Hilbert曲线改进的3D打印路径填充方法
CN112673234A (zh) * 2020-01-17 2021-04-16 华为技术有限公司 路径规划方法和路径规划装置
CN111338341A (zh) * 2020-02-24 2020-06-26 北京百度网讯科技有限公司 车辆避障方法和设备、电子设备、车辆及存储介质
CN111791887A (zh) * 2020-07-03 2020-10-20 北京理工大学 一种基于分层式车速规划的车辆节能驾驶方法
CN112068545A (zh) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
JIANGNAN LI 等: "A Hierarchical Trajectory Planning Framework for Autonomous Driving", 《2020 3RD INTERNATIONAL CONFERENCE ON UNMANNED SYSTEMS (ICUS)》 *
王翔: "园区内自动驾驶车辆局部路径规划与控制算法研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442491A (zh) * 2022-04-08 2022-05-06 杭州国辰机器人科技有限公司 室内机器人的局部路径规划和跟踪控制方法、装置及介质
WO2023221537A1 (zh) * 2022-05-16 2023-11-23 北京京东乾石科技有限公司 路径规划方法及装置、无人车
CN114676939A (zh) * 2022-05-26 2022-06-28 之江实验室 一种多车型参数自适应的参考线平滑方法和系统
CN114676939B (zh) * 2022-05-26 2022-09-02 之江实验室 一种多车型参数自适应的参考线平滑方法和系统

Also Published As

Publication number Publication date
CN114115298B (zh) 2022-05-17

Similar Documents

Publication Publication Date Title
CN114115298B (zh) 一种无人车路径平滑方法及系统
CN110749333B (zh) 基于多目标优化的无人驾驶车辆运动规划方法
US11460311B2 (en) Path planning method, system and device for autonomous driving
WO2022193584A1 (zh) 一种面向多场景的自动驾驶规划方法及系统
CN106767860B (zh) 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN110823240A (zh) 一种具有航向约束的跟随机器人路径规划方法及系统
CN112985445A (zh) 基于高精地图的车道级精度实时性运动规划方法
CN106681320A (zh) 一种基于激光数据的移动机器人导航控制方法
CN112269965A (zh) 一种非完整约束条件下的连续曲率路径优化方法
US20230112991A1 (en) Method of high-precision 3d reconstruction of existing railway track lines based on uav multi-view images
CN112526988B (zh) 一种自主移动机器人及其路径导航和路径规划方法、系统
CN109491389A (zh) 一种具有速度约束的机器人轨迹跟踪方法
CN108180921A (zh) 利用gps数据的ar-hud导航系统及其导航方法
CN113448335A (zh) 路径规划方法和装置、车辆和可读存储介质
CN114510057A (zh) 一种室内环境中基于ros的移动机器人自主导航方法
CN115164907B (zh) 基于动态权重的a*算法的森林作业机器人路径规划方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN112000090B (zh) 一种用于非结构化道路的轨迹队列管理方法
CN116185014A (zh) 一种基于动态规划的智能车全局最优轨迹规划方法与系统
CN111814286B (zh) 一种面向自动驾驶的车道级地图几何模型建立方法
CN112327856A (zh) 一种基于改进A-star算法的机器人路径规划方法
CN106469505A (zh) 一种浮动车轨迹纠偏方法和装置
CN115060280B (zh) 一种车辆轨迹预测方法和装置
CN111857148B (zh) 一种非结构化道路车辆路径规划方法
CN111290406B (zh) 一种路径规划的方法、机器人及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant