发明内容
有鉴于此,本申请实施方式提供了一种路径生成方法、路径生成装置、交通工具及计算机可读存储介质。
本申请提供了一种路径生成方法,包括:
根据螺旋曲线确定关于曲率的目标函数;
根据参考路径上的离散点形成约束条件;
根据所述约束条件优化所述目标函数,以得到所述螺旋曲线的目标曲率;和
根据所述目标曲率和所述螺旋曲线,生成与所述参考路径对应的目标路径。
在某些实施方式中,所述目标函数包括曲率最小化成本、所述曲率的一阶微分最小化成本和曲率的二阶微分最小化成本,所述曲率最小化成本与所述螺旋曲线的曲率相关,所述曲率的一阶微分最小化成本根据所述螺旋曲线的一阶导数的曲率确定,所述曲率的二阶微分最小化成本根据所述螺旋曲线的二阶导数的曲率确定。
在某些实施方式中,所述参考路径为道路中心线。
在某些实施方式中,所述约束条件包括位置约束、角度约束和曲率约束,所述位置约束的上、下限根据所述参考路径上的所述离散点确定,所述曲率约束的上、下限的绝对值为第一阈值。
在某些实施方式中,所述根据所述约束条件优化所述目标函数,以得到所述螺旋曲线的目标曲率,包括:
在满足所述约束条件的情况下,将使得所述目标函数取得最小值时对应的所述目标函数的自变量的取值作为所述螺旋曲线的所述目标曲率。
在某些实施方式中,所述根据所述目标曲率和所述螺旋曲线,生成与所述参考路径对应的目标路径,包括:
采用前向欧拉法离散所述螺旋曲线的位置和角度的参数化方程;
将所述目标曲率带入到离散化后的所述位置和角度的参数化方程以得到位置、角度与目标曲率的对应关系;
根据所述位置、所述角度与目标曲率的对应关系和所述螺旋曲线生成所述参考路径对应的目标路径。
在某些实施方式中,所述将所述目标曲率带入到所述位置和角度的参数化方程以得到位置、角度与目标曲率的对应关系包括:
矩阵化所述位置和角度的参数化方程;
将角度参数方程带入所述位置参数方程以得到位置、角度与目标曲率的对应关系。
本申请还提供了一种路径生成装置,包括:
确定模块,用于根据螺旋曲线确定关于曲率的目标函数;
约束模块,用于根据参考路径上的离散点形成约束条件;
优化模块,用于根据所述约束条件优化所述目标函数,以得到所述螺旋曲线的目标曲率;
生成模块,用于根据所述目标曲率和所述螺旋曲线,生成与所述参考路径对应的目标路径。
本申请还提供了包含一种交通工具,所述交通工具包括存储器和处理器,所述存储器中存储有计算机程序,所述计算机程序被所述处理器执行时,实现上述的路径生成方法。
本申请还提供一种包含计算机程序的非易失性计算机可读存储介质,当所述计算机程序被一个或多个处理器执行时,实现上述的路径生成方法。
本申请实施方式的路径生成方法、装置、交通工具和计算机可度存储介质中,通过设置螺旋曲线的曲率优化目标函数,得到螺旋曲线的目标曲率,进而根据目标曲率和螺旋曲线得到平滑的目标路径,能够提高车辆自动驾驶的稳定性。
本申请实施方式的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本申请的实践了解到。
具体实施方式
下面详细描述本申请的实施方式,实施方式的示例在附图中示出,其中,相同或类似的标号自始至终表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的,仅用于解释本申请的实施方式,而不能理解为对本申请的实施方式的限制。
请参阅图1,本申请提供了一种路径生成方法,包括:
01:根据螺旋曲线确定关于曲率的目标函数;
02:根据参考路径上的离散点形成约束条件;
03:根据约束条件优化目标函数,以得到螺旋曲线的目标曲率;
04:根据目标曲率和螺旋曲线,生成与参考路径对应的目标路径。
相应地,请参阅图2,本申请实施方式还提供了一种路径生成装置100,本申请实施方式的路径生成方法可以由路径生成装置100实现。
路径生成装置100包括确定模块110、约束模块120、优化模块130和生成模块140。步骤01可以由确定模块110实现,步骤02可以由约束模块120实现,步骤03可以由优化模块130实现,步骤04可以由生成模块140实现。
或者说,确定模块110用于根据螺旋曲线确定关于曲率的目标函数。约束模块120用于根据参考路径上的离散点形成约束条件。优化模块130用于根据约束条件优化目标函数,以得到螺旋曲线的目标曲率。生成模块140用于根据目标曲率和螺旋曲线,生成与参考路径对应的目标路径。
本申请实施方式还提供了一种交通工具。交通工具包括存储器和处理器。存储器中存储有计算机程序,处理器用于根据螺旋曲线确定关于曲率的目标函数,根据参考路径上的离散点形成约束条件,根据约束条件优化目标函数,以得到螺旋曲线的目标曲率,根据目标曲率和螺旋曲线,生成与参考路径对应的目标路径。
交通工具可以为但不限于车辆(例如纯电动车、混合动力电动车、增程式电动车、燃油车)、飞行汽车等。以下为便于描述,交通工具以车辆为例进行实施例展开。
可以理解,在相关技术中,基于数值优化生成目标路径的方法中,通常将参考路径离散化,直接调整参考路径上离散点的位置以使得设计的目标函数最小化,并将使得设计的目标函数最小化时离散点的位置作为目标路径的离散点。也即是说,在相关技术中,得到的目标路径由离散点组成,存在平滑性不够的问题。进一步地,如果想要形成平滑的目标路径,需要通过拟和、插值等方式求取相邻的两个离散点中间的点,为了尽可能地平滑,需要对参考路径密集采样才能获得较好的中间插值点,这样又会带来计算量较大、时间成本较高等问题。
而本发明实施方式的路径生成方法、路径生成装置和交通工具中,根据螺旋曲线设计关于螺旋曲线曲率的目标函数,同时根据参考路径的离散点的行程约束条件,利用约束条件对目标函数进行优化,能够得到螺旋曲线较优的目标曲率,进一步地,根据目标曲率和螺旋曲线能够直接生成平滑的目标路径。
在某些实施方式中,在某些实施方式中,参考路径为道路的中心线。道路的中心线可以通过高精地图直接获取到,在获取道路的中心线的同时,还可以获取到道路的左右边界信息。如此,能够快速准确地确定约束条件。进一步地,可在笛卡尔坐标系中绘制道路的中心线和道路的左右边界,这样,中心线和左右边界上的每个位置对应一个坐标,便于计算。同时,为了节省运算量,可以对中心线进行离散化处理,即按照预设路径长度在中心线上选取均匀分布的一系列离散点。
在某些实施方式中,目标函数包括曲率最小化成本、曲率的一阶微分最小化成本和曲率的二阶微分最小化成本。曲率最小化成本根据螺旋曲线的曲率确定,曲率的一阶微分最小化成本根据螺旋曲线的一阶导数的曲率确定,曲率的二阶微分最小化成本根据螺旋曲线的二阶导数的曲率确定。
如此,目标函数与参考路径相关,通过优化曲率最小化成本、曲率的一阶微分最小化成本和曲率的二阶微分最小化成本的目标函数,能够得到参考路径上的离散点对应的螺旋曲线的曲率,且能够使得螺旋曲线更加平滑。可以理解,目标函数的自变量为螺旋曲线的曲率,优化目标函数得到的最优解即螺旋曲线的曲率。
在某些实施方式中,目标函数可由以下公式表示:
其中,第一项为曲率最小化成本,第二项为曲率的一阶微分最小化成本,第三项为曲率的二阶微分最小化成本。
如此,通过优化目标函数能够得到目标曲率。
具体地,目标函数的自变量为:
[κ0,κ1,…,κn-1]
同时也是螺旋曲线中任意控制点Pi的曲率,其中,螺旋曲线任意一个控制点Pi在笛卡尔坐标系中的坐标为(x
i,y
i,θ
i),目标函数中的曲率为可变化的且待确定的,目标函数中的控制点Pi的初始解可以为参考路径上的离散点。参考路径(道路的中心线)上的离散点为
其中,i∈[0,1,...,N-1]。x,y分别表示粗略路径在笛卡尔坐标系离散点的位置;θ表示离散点的航向角;k表示离散点的曲率;s表示曲线的弧长。因此,参考路径点集为
ωcenter为第一项的权重,ωκ为第二项的权重,ωdκ为第三项的权重,各项的权重为已知的预先设定的值,在优化目标函数的过程中,各项的权重保持不变。在某些实施方式中,ωcenter、ωκ、ωdκ依次减小。
在某些实施方式中,约束条件包括位置约束、角度约束和曲率约束。其中,位置约束的上、下限根据参考路径上的离散点确定,角度约束的上、下限的绝对值为第一阈值;曲率约束的上、下限的绝对值为第二阈值,弧长约束的上、下限的绝对值为第三阈值;螺旋曲线约束根据螺旋曲线的控制点确定。
具体地,位置约束、角度约束和曲率约束均为线性约束。位置约束包括x
i坐标的边界约束和y
i坐标的边界约束,其中,x
i坐标的边界约束为:x
i,left≤x
i≤x
i,right,y坐标的边界约束为:y
i,left≤y
i≤y
i,right。x
i,left、y
i,left、x
i,right、y
i,right为根据参考路径上的离散点Pi确定的,确定方式为:过离散点Pi作参考路径的切线,并过离散点Pi作该切线的法线,将该切线的法线与道路的左、右边界的交点分别作为参考路径上的离散点Pi对应的道路的左边界点
和右边界点
在某些实施方式中,角度约束为:
其中,θ
buffer为其中,表示航向角变化范围,θ
buffer可以为
在某些实施方式中,曲率约束为:κmin≤κi≤κmax,曲率约束的上、下限的绝对值为0.2,即,-0.2≤κi≤0.2。
如此,通过多项约束条件对目标函数的自变量进行约束,使得最终得到的目标曲率更利于平滑参考路径。
优选地,请参阅图3,在某些实施方式中,步骤03包括:
032:在满足约束条件的情况下,将使得目标函数取得最小值时对应的目标函数的自变量的取值作为螺旋曲线的目标曲率。
在某些实施方式中,步骤032可以由优化模块130来实现。或者说,优化模块130用于在满足约束条件的情况下,将使得目标函数取得最小值时对应的目标函数的自变量的取值作为螺旋曲线的目标曲率。
在某些实施方式中,处理器用于在满足约束条件的情况下,将使得目标函数取得最小值时对应的目标函数的自变量的取值作为螺旋曲线的目标曲率。
具体地,在约束条件内,调整目标函数的自变量的取值,以使得目标函数的函数值取得最小值,当目标函数的函数值取得最小值时,将使得目标函数取得最小值时对应的目标函数的自变量的取值作为螺旋曲线的曲率。
如此,能够确定合适的目标曲率。可以理解,由于目标函数是基于螺旋曲线的平滑性代价设计的,当目标函数取得最小值时,意味着螺旋曲线的平滑性最好,将此时目标函数的自变量的取值作为螺旋曲线的目标曲率,能够生成平滑的目标路径。
优选地,请参阅图4,在某些实施方式中,步骤04还包括:
042:采用前向欧拉法离散螺旋曲线的位置和角度的参数化方程。
044:将目标曲率带入到离散化后的位置和角度的参数化方程以得到位置、角度与目标曲率的对应关系;
046:根据位置、角度与目标曲率的对应关系和螺旋曲线生成参考路径对应的目标路径。
在某些实施方式中,子步骤042、044和子步骤046可以由生成模块140来实现。或者说,生成模块140用于采用前向欧拉法离散螺旋曲线的位置和角度的参数化方程,将目标曲率带入到离散化后的位置和角度的参数化方程以得到位置、角度与目标曲率的对应关系;生成模块140还用于根据位置、角度与目标曲率的对应关系和螺旋曲线生成参考路径对应的目标路径。
在某些实施方式中,处理器用于采用前向欧拉法离散螺旋曲线的位置和角度的参数化方程,将目标曲率带入到离散化后的位置和角度的参数化方程以得到位置、角度与目标曲率的对应关系;处理器还用于根据位置、角度与目标曲率的对应关系和螺旋曲线生成参考路径对应的目标路径。
具体地,螺旋曲线的位置、角度的参数化方程为:
进一步的,假设起点坐标p1(xinit,yinit,θinit),两个控制点之间的距离Δsi=si+1-si。使用前向欧拉发差分上述螺旋曲线的位置、角度的参数化方程得到:
x0=xinit
xi+1=xi+Δsicos(θi) (4)
y0=yinit
yi+1=yi+Δsisin(θi) (5)
θ0=θinit
θi+1=θi+Δsiκi (6)
然后,矩阵化位置和角度的参数化方程得到如下表达式:
x=x0+ΔScos(θ) (7)
y=y0+ΔSsin(θ) (8)
θ=θ0+ΔSκ (9)
其中:
x=[x0x1x2…xn-1]T∈RN
x0=[xinitxinitxinit…xinit]T∈RN
y0=[y0y1y2…yn-1]T∈RN
y0=[yinityinityinit…yinit]T∈RN
θ=[θ0θ1θ2…θn-1]T∈RN
θ0=[θinitθinitθinit…θinit]T∈RN
κ=[κ0κ1κ2…κn-1]T∈RN
进而,将角度的参数方程带入位置参数方程以得到位置、角度与目标曲率的对应关系:
x(κ)=x0+ΔScos(θ0+ΔSκ) (10)
y(κ)=y0+ΔSsin(θ0+ΔSκ) (11)
如此,得到离散化形式的位置、角度与曲率相关的参数化方程。
其中,k=1,2,…,N-1。
进而,根据位置、角度与目标曲率的对应关系和螺旋曲线进行曲线重构,具体地,请结合图6,对于曲线重构,使用曲率关于弧长的导数恒定的分段螺旋线表示如下:
κi+1=κi+αiΔsi (15)
此外,对于上式积分,由于没有原函数,可以采用高斯—勒让德积分公式,从而根据位置、角度与目标曲率的对应关系和螺旋曲线计算螺旋曲线中的位置和角度值以生成对于的目标路径。
如此,将目标曲率带入螺旋曲线中位置、角度的参数化方程和曲率的多项式表达式中,即可生成与参考路径对应的目标路径。
在一个例子中,请结合图6和图7,其中,图6为本方法得到的目标路径P2与参考路径P1的对比图,图7为本方法得到的目标路径上各点的曲率k2与参考路径的上各点的曲率k1的对比图,通过对比发现,本方法得到的目标路径相较于参考路径,平滑性得到提升。
本申请实施方式还提供了一种计算机可读存储介质。一个或多个存储有计算机程序的非易失性计算机可读存储介质,当计算机程序被一个或多个处理器执行时,实现上述任一实施方式的自动构建方法。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的软件来完成。程序可存储于一非易失性计算机可读存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)等。
在本说明书的描述中,参考术语“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”或“一些示例”等的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。同时,参考术语“第一”、“第二”等的描述意在将同类或相似操作区别开来,“第一”与“第二”之间在某些实施方式中具有前后逻辑关系,在某些实施方式中并不一定具有逻辑或前后关系,需要根据实际实施例进行判定,不应该仅通过字面意思进行判定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
尽管上面已经示出和描述了本申请的实施方式,可以理解的是,上述实施方式是示例性的,不能理解为对本申请的限制,本领域的普通技术人员在本申请的范围内可以对上述实施方式进行变化、修改、替换和变型。