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

最佳路径寻找及其方法--曲率法2
一．路径规划方法的选择
我拟考虑使用一条曲线进行路径规划。

我选择曲率法而不选择其他方法有如下考虑：
1，从难易程度上面考虑，曲率法不是最简单的方法，但是它有其它方法不能达到的好处，我们通过计算车子每一点的曲率，首先可以反映跑道每一点的弯曲程度，而且，曲率还能反映我们车子经过该点的最大速度和最大向心加速度。

所以即便是我们采用其他方法进行控制，最后还是要回归到求跑道的曲率上面来。

二．最佳路径的寻找
最佳路径的寻找不是随便找一条曲线作为运行路径，而是特定的那一条曲线，在任何赛道情况下，只能找到一条这样的路径，下面我就通过各种赛道的图像来寻找最佳路径：
情况一：弯道
红色曲线是规划出来的最佳路径，θ为重建出来的跑道所转过的角度，θ>0表示向左转，θ<0表示向右转。

红色曲线与车子起始方向相切，且在满足不压两边跑道的情况下半径最短。

设规划出来的路径半径为r ，车子需要跑过θ角度，车子起点为（CarX,CarY ）,
车子目前的速度为v ，以半径r 为规划路径行进时的最大速度为m ax V ，车子的向心加速度
R
v
a 2
0 （这个是在车子硬件，机械确定以后提前测出来的，为固定值保存在程序中，意思
就是通过半径为R 的跑道时，允许的最大速度为0v ）。

所以车子在规划路径上跑时，也就是在上图中红色路径上跑的时候，允许的最大速度为：
R
r v r
a V *
*0max == 在此段路程中花费的总时间为：
r R v R
r v r v
s t ***
*0
0θ
θ=
==
所以得出r 越小，总时间花的就越短。

故车子应该尽量切内道跑。

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

由图中标注：
CarX r a +=θcos * CarY r b +=θsin *
a ，
b 应满足：
]19[]19[RX a LX <<
]19[]19[RY b LY <<
在计算出上面的参数过后，就给舵机和电机赋值，舵机赋值为1/r ，r 可以反映出舵机偏转角的大小，r 越大，路径越平缓，舵机偏转就应该越小，r 越小，路径弯度越大，舵机偏转就应该越大。

然后在1/r 的基础上再乘以一个比例系数K ，K 是通过实验调节出的一个合适的值，参数调节合适后，是会适应所有弯道的，并不只是适 用于当前弯道。

电机的赋值采用控速的闭环控制法，保证车子以通过此弯道允许的最大速度m ax V 跑过去。

补充：路径规划的时候还需要考虑到车子相对于跑道的偏向，因为规划出来的路径必须与车子当前的方向相切，否则会是车子强制产生一个偏向角，这种规划出来的路径是不可实现的，并且在这种情况下也有可能会产生侧滑等等。

所以我们要计算出车身相对于跑道的偏向角度。

通过几何关系我们知道计算这个量的前提是找到每一段规划路径的圆心坐标。

情况二：小S 或大S
和情况一中的弯道处理方式一样。

情况三：直道
直道情况一般是弯进直，所以他的路径一般是靠近弯进直那个内侧弯道的一边。

处理情况和弯道相同。

如图所示：
三．算法实现
1.θ计算
Θ是我们路径规划中其中之一的已知量，它的计算我们采用扇形的几何关系求出。

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

如图所示：
t r r =-2
cos
*θ
2
sin
**2θ
r d =
所以：
t d d =-
2cos *2
sin *22
sin
*2θ
θθ
令2
sin θ
=n
得：
t n n
d n d =--21**2*2 所以有：
0*4*)4(2222=--+d n td n d t
求解上面的方程得：
)4(*2)
4(41642
222222d t d t d d t td n ++++=（舍） )
4(*2)
4(41642222222d t d t d d t td n +++-=
故：
)arcsin(*2n =θ； n
d
r *2=
；
2.向心加速度计算
对特定弯道进行测试，以不同的速度通过半径R 固定的弯道，当在速度为V 时开
始出现侧滑，这最大速度V 对应R ，向心加速度R
V a 2
=。

因为车子轮胎相对于跑道的
摩擦系数不变，故在任何位置a 的值不变，保存a 的值，在其他跑道中，电机所给的最大速度R a V *max =。

3.寻找圆心坐标
我们需要求解我们规划出来路径的圆心坐标，这样我们就能求得我们规划路径的方程，然后就能方便的判断我们的规划路径是否压线（两边跑道）。

而我们现在的已知量只有起始点和终止点的坐标，设为（x1,y1）;（x2,y2）。

还有一个已知量就是我们规划路径的半径r ，通过这三个量我们需要求解出规划路径的圆心坐标。

由于规划出来的路径是理想的，所以圆心坐标也是相当准确的。

我们采用的方法是通过过两个点作以半径为r 的两个圆，相交的两个点即为圆心坐标，如下图所示：
联立方程组：
222)1()1(r y y x x =-+-
222)2()2(r y y x x =-+-
令
222)21()21(y y x x d -+-=
求解上式，得：
1. x1!=x2 && y1!=y2
2
)
21(21)12(*2))21(()21(*42)21(2))21(()21(*42
4222422x x x x y y d d x x x x r x y y d d x x x x r y ++
--+---±
=++
+---±=
2. x1=x2
12
42)
21(2
2
x d
r x y y y +-±
=+=
3. y1=y2
12
42)21(2
2
y d
r y x x x +-±
=+=
4.求直线方程
车身方向直线：
)
(*)(1phi Tan CarX
CarY X phi Tan Y ++-
=
与车身方向垂直的直线：
CarX phi Tan CarY X phi Tan Y *)(*)(-+=。