智能车最佳路径寻找及其方法--曲率法

最佳路径寻找及其方法－-曲率法2

一.路径规划方法得选择

我拟考虑使用一条曲线进行路径规划。

我选择曲率法而不选择其她方法有如下考虑：

1,从难易程度上面考虑，曲率法不就是最简单得方法,但就是它有其它方法不能达到得好处,我们通过计算车子每一点得曲率,首先可以反映跑道每一点得弯曲程度,而且,曲率还能反映我们车子经过该点得最大速度与最大向心加速度。所以即便就是我们采用其她方法进行控制,最后还就是要回归到求跑道得曲率上面来。

二．最佳路径得寻找

最佳路径得寻找不就是随便找一条曲线作为运行路径，而就是特定得那一条曲线,在任何赛道情况下,只能找到一条这样得路径,下面我就通过各种赛道得图像来寻找最佳路径：情况一:弯道

红色曲线就是规划出来得最佳路径,θ为重建出来得跑道所转过得角度,θ>0表示向左转,θ＜0表示向右转。红色曲线与车子起始方向相切，且在满足不压两边跑道得情况下半径最短。ﻩ设规划出来得路径半径为ｒ，车子需要跑过θ角度,车子起点为(CarＸ,CarY)，

车子目前得速度为v,以半径r为规划路径行进时得最大速度为，车子得向心加速度(这个就是在车子硬件,机械确定以后提前测出来得,为固定值保存在程序中,意思就就是通过半径为Ｒ得跑道时,允许得最大速度为)。

所以车子在规划路径上跑时,也就就是在上图中红色路径上跑得时候，允许得最大速度为:

在此段路程中花费得总时间为:

所以得出r越小,总时间花得就越短。故车子应该尽量切内道跑。

又因为我们规划出来得路径不能压线,由图分析可得,我们只要保证我们规划出来得最远处得那个点不压线切靠近内侧跑道则基本可以保证我们规划出来得跑道不压线。

由图中标注：

ａ,b应满足：

在计算出上面得参数过后，就给舵机与电机赋值,舵机赋值为1/r，r可以反映出舵机偏转角得大小,r越大,路径越平缓，舵机偏转就应该越小,r越小,路径弯度越大,舵机偏转就应该越大。然后在1/r得基础上再乘以一个比例系数K，K就是通过实验调节出得一个合适得值,参数调节合适后,就是会适应所有弯道得,并不只就是适用于当前弯道。电机得赋值采用控速得闭环控制法,保证车子以通过此弯道允许得最大速度跑过去。

补充:路径规划得时候还需要考虑到车子相对于跑道得偏向,因为规划出来得路径必须与车子当前得方向相切,否则会就是车子强制产生一个偏向角,这种规划出来得路径就是不可实现得,并且在这种情况下也有可能会产生侧滑等等。所以我们要计算出车身相对于跑道得偏向角度。通过几何关系我们知道计算这个量得前提就是找到每一段规划路径得圆心坐标。

情况二：小S或大Ｓ

与情况一中得弯道处理方式一样。

情况三：直道

直道情况一般就是弯进直,所以她得路径一般就是靠近弯进直那个内侧弯道得一边。处理情况与弯道相同。如图所示:

三．算法实现

1.θ计算

Θ就是我们路径规划中其中之一得已知量,它得计算我们采用扇形得几何关系求出。

经过测试,这种方法计算出来得θ角较精准,且计算量简便,故予以采用。

如图所示：

所以:

令

得:

所以有:

求解上面得方程得:

(舍）

故:

; ；

2.向心加速度计算

对特定弯道进行测试,以不同得速度通过半径R固定得弯道,当在速度为Ｖ时开始出现侧滑,这最大速度V对应R,向心加速度。因为车子轮胎相对于跑道得摩擦系数不变,故在任何位置a得值不变,保存a得值,在其她跑道中,电机所给得最大速度。

3.寻找圆心坐标

我们需要求解我们规划出来路径得圆心坐标,这样我们就能求得我们规划路径得方程,然后就能方便得判断我们得规划路径就是否压线（两边跑道)。而我们现在得已知量只有起始点与终止点得坐标，设为(ｘ1,y1）；(x２，y2）。还有一个已知量就就是我们规划路径得半径r,通过这三个量我们需要求解出规划路径得圆心坐标。由于规划出来得路径就是理想得,所以圆心坐标也就是相当准确得。我们采用得方法就是通过过两个点作以半径为ｒ得两个圆,相交得两个点即为圆心坐标，如下图所示:

联立方程组:

令

求解上式,得：

1. ｘ1！＝x2 && y1!=y2

2

)

21(21)12(*2))21(()21(*42)21(2))21(()21(*42

4

2

2

2422x x x x y y d d x x x x r x y y d d x x x x r y ++

--+---±

=++

+---±=

2. ｘ1=ｘ2

3. y1=ｙ2

4.求直线方程

车身方向直线:

与车身

方向垂直得直线: