Reeds-Shepp曲线公式推导及代码实现-第二部分（共三部分）

注：由于CSDN篇幅限制，文章只能拆分成三部分，完整的文章可见Reeds-Shepp曲线公式推导及代码实现

CSDN剩余部分：
Reeds-Shepp曲线公式推导及代码实现-第一部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第二部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第三部分（共三部分）

2. Reeds-Shepp求解

在下面的所有推导中，均是在归一化后的基础上进行的，即车辆的曲率半径为单位长度，并且起始姿态被认为是在原点处。$(t，u，v)$表示所需的每个运动的度量。因此，在转弯机动的情况下，它们表示在转弯圆的中心内切的角度，并且在直线机动的情况下，它们表示要行进的距离。

2.1 $L^{+}{S}^{+}{L}^{+}$

其中

• $\square O_1{O}_{2}BA$是一个矩形
• $\angle S{O}_{1}A=\angle B{O}_{2}E$=t
• $t+v=\phi$
• $O_2$点坐标为$(x-\sin \phi ,y+\cos \phi )$
• $O_1$点的坐标的为$(0,1)$
• $\overrightarrow{O_1{O}_2}$在极坐标下可表示为$(u,t)$，这里$u$对应的是极径，$t$对应的极角。

因此，可得

• $u,t=polar(x-sin\phi ,y+\cos \phi -1)$
• $v=\phi -t$

注：$polar(x,y)$指的是将笛卡尔坐标系中的点$(x,y)$转换为极坐标系中的表示,在极坐标系中，点的位置由半径$r$和角度$\theta$确定。

对于$r,\theta =polar(x,y)$，有$r=\sqrt{x^2+y^2}、\theta =arctan\frac{y}{x}$

2.2 $L^{+}{S}^{+}{R}^{+}$

其中

• $AD=u$
• $O_1{O}_2=u_1$
• $B$是$AD$和$O_1{O}_2$的中点
• $t-v=\phi$
• $O_2$点坐标的$(x+\sin \phi ,y-cos\phi )$
• $O_1$点坐标为$(0,1)$
• $\overrightarrow{O_1{O}_2}$在极坐标下可表示为$(u_1,t_1)$
• 在直角$△O_{1}BA$中有$\frac{{u_1}^2}{4}=1+\frac{u^2}{4}$
• $\theta =\arctan \frac{O_{1}A}{AB}=\arctan \frac{2}{u}$
• $t_1+\theta =t$

因此，可得

• $u_1,t_1=polar(x+\sin \phi ,y-cos\phi -1)$
• $u=\sqrt{u_1^2-4}$
• $\theta =arctan\frac{2}{u}$
• $t=t_1+{\theta }_1$
• $v=t-\phi$

2.3 $L^{+}{R}^{-}{L}^{+}$

其中

• $△O_1{O}_2{O}_3$为等腰三角形
• $O_1{O}_3=u_1$
• $t+u+v=\phi$
• $O_1$点坐标为$(0,1)$
• $O_3$点坐标为$(x-\sin \phi ,y+\cos \phi )$
• $\overrightarrow{O_1{O}_3}$在极坐标下可表示为$(u_1,\theta )$
• $\beta =\arccos \frac{O_1{O}_3/2}{O_1{O}_2}=\arccos \frac{U_1}{4}$
• $u=\pi -2\beta$
• $t=\frac{\pi }{2}+\theta +\beta$

综上，可得

• $u_1,\theta =polar(x-\sin \phi ,y+\cos \phi -1)$
• $\beta =\arccos \frac{u_1}{4}$
• $t=\frac{\pi }{2}+\theta +\beta$
• $u=\pi -2\beta$
• $v=\phi -t-u$

2.4 $L^{+}{R}^{-}{L}^{-}$

其中

• $△O_1{O}_2{O}_3$为等腰三角形
• $O_1{O}_3=u_1$
• $t+u-v=\phi$
• $O_1$点坐标为$(0,1)$
• $O_3$点坐标为$(x-\sin \phi ,y+\cos \phi )$
• O 1 O 3 ⃗ \vec{O_1O_3} O1​O