[Conducción automática] planificación de ruta: resumen de curva de ReedsShepp (implementación de Python | implementación de C++)

Referencias

• Introducción a las curvas de Reeds-Shepp y Dubins

• Planificación de rutas: curvas de ReedsShepp

• 原论文：CAMINOS ÓPTIMOS PARA UN COCHE QUE VA TANTO HACIA ADELANTE COMO HACIA ATRÁS

• Planificación de Reeds Shepp

• Un ejemplo para entender la planificación de trayectoria de la conducción automática - (4) Explicación detallada de la curva RS en el algoritmo HybridA*

• Investigación sobre planificación de rutas y control de seguimiento del sistema de estacionamiento automático para turismos_Tao Peng

---

Preámbulo

• Derivación de la curva de Dubins (método basado en vectores)
• Resumen de la fórmula de la curva de Dubins y la implementación del código Python (basado en el método de geometría)

---

1. Curva de Reeds-Shepp

1.1 Conceptos básicos

El algoritmo de Reeds-Shepp se conoce como RS , un artículo publicado por JAReeds y LAShepp en 1990 (camino óptimo para un automóvil que avanza y retrocede) . Este método se mejora en base al algoritmo de Dubins , y se agrega a la planificación el movimiento inverso (se permite que el automóvil retroceda, se pone marcha atrás), lo que permite obtener una mejor solución que la curva de Dubins en algunos casos. .

Una curva de ejemplo es la siguiente:

Fuente de la imagen: https://blog.csdn.net/robinvista/article/details/95137143

---

Como se puede ver en la figura anterior, en la trayectoria de la curva de Reeds-Shepp , se permite que exista la válvula de cúspide.

1.2 Combinación de campos

Agregue un superíndice a la letra en el campo para indicar la dirección del movimiento, de la siguiente manera:

símbolo	significado	alrededor del círculo unitario
$l^{+}$	girar a la izquierda	en sentido anti-horario
$l^{-}$	atrás izquierda	agujas del reloj
$R^{+}$	girar a la derecha	en sentido anti-horario
$R^{-}$	girar a la derecha	agujas del reloj
$S^{+}$	sigue recto	/
$S^{-}$	volver directamente	/

Utilice $A\; los\; caracteres\; CyS$ se les pueden asignar los siguientes conjuntos:

$$\begin{array}{rl}& CCC\leftarrow \left\{C^{+}{C}^{-C}{\_}^{+}{C}^{+}{C}^{-C}{\_}^{-}{C}^{+}{C}^{+}{C}^{-}{C}^{+}{C}_b^{}{C}_b^{}{C}^{-}{C}^{+}{C}_b^{}{C}_b^{}{C}^{+}\right\}\\ & CSC\_\_\leftarrow \{C^{+}{S}^{+}{C}^{+}{C}^{-C}{\_}_{p/2}^{}{S}^{+}{C}^{+}{C}^{+}{S}^{+}{C}_{p/2}^{}{C}^{-}{C}^{-C}{\_}_{p/2}^{}{S}^{+}{C}_{p/2}^{}{C}^{-}\end{array}\text{\hspace{1em}( 1 )}$$

El nuevo campo se puede obtener invirtiendo el signo de la ecuación (1). En la fórmula anterior (1), $C_{p/2}^{}$Indica el $L$ oRREl ángulo girado de$R$$\pi /2$ ,$C_{}{C}_{}$Una combinación significa que los arcos correspondientes tienen longitudes iguales.

Para una representación compacta, evite $\pm$ , también existe una función infinitesimal:
$$\begin{array}{rl}& CCC\leftarrow \left\{\begin{array}{ccccc}C|C|C\_\_\_& C|C& C.C.\_|& C.C.{\_}_{}|C_{}& C|C{\_}_{}{C}_{}|\end{array}\right\}\\ & CSC\_\_\leftarrow \{\begin{array}{llll}Csc\_& C|C_{p/}SC& CSC\_{\_}_{p/}|& C\left|C_{p/}SC{\_}_{p/}\right|\end{array}\\ & \end{array}\text{\hspace{1em}( 2 )}$$

Entre ellos, $|$ indica que la dirección del movimiento del vehículo cambia de adelante a atrás o de atrás a adelante.

Subíndice las palabras anteriores, como $C_{}|C{\_}_{}|C{\_}_{}$， entre ellos $un,segundo,\gamma$ representa el ángulo de rotación (en radianes), respectivamente. $S$ con subíndice$d$ significa que la distancia recorrida en línea recta es$d$ . Sus rangos se muestran en la siguiente tabla:

¿ $C$ es reemplazada por$L$ o$R$ , mediante una simple transformación, existen48combinación de campos. Esta simple transformación incluye cambio de tiempo (timeflip) , reflexión (reflect) y transformación hacia atrás (hacia atrás) .

1.3 Cambio de tiempo (timeflip), reflexión (reflejo) y transformación hacia atrás (hacia atrás)

1.3.1 Inversión de tiempo (inversión de tiempo)

La curva calculada se invierte según la dirección de su movimiento y la nueva curva obtenida es la curva opuesta a la curva original.

Como se muestra en la figura, la curva azul $l^{-R}{\_}^{+}{S}^{+}{l}^{+}$ con la curva roja$l^{+}{R}^{-S}{\_}^{-L}{\_}^{-}$ Es simétrico con respecto al eje Y y los ángulos de rumbo de la trayectoria en las dos curvas de trayectoria son opuestos. Desde el punto de partida$O(0,0,0)$ al punto objetivo$A(x,y,\theta )$ puede pasar por el punto inicial$O(0,0,0)$ al punto objetivo$segundo(-x,-y,-\theta )$ cálculo de inversión de trayectoria.

1.3.2 Reflexionar

La segunda relación de conversión: "reflejar", es decir, invertir la curva calculada según su dirección de movimiento circular y obtener una nueva curva con la misma longitud que la curva original.

Como se muestra en la figura, la curva roja $R^{-L}{\_}^{+}{S}^{+}{R}^{+}$ con la curva azul$l^{-R}{\_}^{+}{S}^{+}{l}^{+}$ Simétrico con respecto al eje X, los ángulos de rumbo de la trayectoria en las dos curvas de trayectoria son opuestos. Desde el punto de partida$O(0,0,0)$ al punto objetivo$A(x,y,\theta )$ puede pasar por el punto inicial$O(0,0,0)$ al punto objetivo$segundo(x,-y,-\theta )$ se obtiene invirtiendo la dirección circunferencial del camino.

1.3.3 Transformación hacia atrás (hacia atrás)

La tercera relación de conversión: "hacia atrás", es decir, la ruta de la curva original se convierte en orden inverso y la curva obtenida es una nueva curva con la misma longitud que la curva original.

El diagrama de ejemplo "al revés" se muestra en la figura, la curva azul $l^{-S}{\_}^{-R}{\_}^{-L}{\_}^{+}$ con la curva roja$l^{+}{R}^{-S}{\_}^{-L}{\_}^{-}$ No tiene las dos relaciones de apilamiento anteriores, pero se puede ver intuitivamente que$l^{-S}{\_}^{-R}{\_}^{-L}{\_}^{}$El orden inverso de${}^{+}$$l^{+}{R}^{-S}{\_}^{-L}{\_}^{-}$ , las trayectorias en las dos curvas de trayectoria están dispuestas de manera inversa. Desde el punto de partida$O(0,0,0)$ al punto objetivo$A(x,y,\theta )$ puede pasar por el punto inicial$O(0,0,0)$ función base$segundo(x\ast \mathrm{porque}i+y\ast \mathrm{pecado}yo,X\ast \mathrm{pecado}i-y\ast \mathrm{porque}yo,\theta )$ se obtiene invirtiendo la dirección circunferencial del camino.

1.4 48 combinaciones de campos

Las 48 combinaciones de campos se enumeran de la siguiente manera:

En la figura, el subíndice $\beta$ representa el radian de rotación C es$\beta$ , la misma razón$\pi /2$ también representa el radian de rotación, lo que significa que el círculo debe girar$p/2$ .

Por lo tanto, la curva de Dubins es la más corta seleccionada de las 6 curvas, y la curva de Reeds-Shepp es la más corta seleccionada de las 48 curvas (el artículo original creía que la curva más corta debe estar entre las 48 curvas, y más tarde Investigación humana encontró que había dos curvas que no serían las más cortas, por lo que el rango de búsqueda se redujo a 46).

En cuanto a la derivación específica de estas combinaciones de campos, consulte el artículo original .

2. Solución de la curva RS

La fórmula de solución de la curva de Reeds-Shepp está en la fórmula (8.1) ~ fórmula (8.11) del artículo original . Está resumido aquí.

Supongamos que el punto de partida es $(0,0,0)$ , el punto final es$(x,y,yo)$ .

2.1 Composición de tres arcos

• El primer tipo de curva básica: $C|C|C$ , es decir, tres arcos y dos arcos en cada arco tienen direcciones opuestas, en base a esto se pueden extender cuatro tipos de curvas:$l^{+}{R}^{-L}{\_}^{+}、L^{-R}{\_}^{+}{l}^{-}、R^{+}{l}^{-R}{\_}^{+}、R^{-L}{\_}^{+}{R}^{-}$。
• El segundo tipo de curva básica: $C.C.\_|C$ , es decir, tres arcos y las direcciones de los arcos de los dos últimos arcos son opuestas, en base a esto se pueden extender cuatro tipos de curvas:$l^{+}{R}^{+}{l}^{-}、L^{-R}{\_}^{-L}{\_}^{+}、R^{+}{l}^{+}{R}^{-}、R^{-L}{\_}^{-R}{\_}^{+}$。
• La tercera curva básica: $C|CC$ , es decir, tres arcos y las direcciones de los arcos de los dos primeros arcos son opuestas, en base a esto se pueden extender cuatro tipos de curvas:$l^{+}{R}^{-L}{\_}^{-}、L^{-R}{\_}^{+}{l}^{+}、R^{+}{l}^{-R}{\_}^{-}、R^{-L}{\_}^{+}{R}^{+}$。

A través de los tres métodos de transformación de curvas de inversión del tiempo, reflexión y retroceso, los símbolos de las tres fórmulas anteriores se transforman en los modos de movimiento de 12 curvas, que se pueden resolver mediante la fórmula (1).
{ ( u 1 , t 1 ) = R ( x − pecado ⁡ θ , y − 1 + porque ⁡ θ ) { u 1 2 > 4 → L = ∞ u 1 2 ≤ 4 → { A = arcsin ⁡ ( u 1 2 4) u = M ( A + t 1 ) ( u 2 , t 2 ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) t = t 2 v = M ( θ + t − u ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ donde A ∈ [ π 2 , π ] (1) \tag{1} \left\{\begin{array}{l} \left(u_{1}, t_ {1}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ \left\{\begin{array}{l} u_{1}^{2}>4 \ flecha derecha L=\infty \\ u_{1}^{2} \leq 4 \rightarrow\left\{\begin{array}{l} A=\arcsin \left(\frac{u_{1}^{2} }{4}\right) \\ u=M\left(A+t_{1}\right) \\ \left(u_{2}, t_{2}\right)=R(x-\sin \theta , y-1+\cos \theta) \\ t=t_{2} \\ v=M(\theta+tu) \\ L=|t|+|u|+|v| \end{array}\ derecha. \\ \text {dónde} A \in\left[\frac{\pi}{2}, \pi\right] \end{array}\right. \end{array}\right.⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧( tú1,t1)=R ( x−pecadoyo ,y−1+porqueyo )⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧tu12>4→l=∞tu12≤4→⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧A=arcosin(4tu12​)tu=METRO( Un+t1)( tú2,t2)=R ( x−pecadoyo ,y−1+porqueyo )t=t2v=M ( yo+t−tú )l=∣ t ∣+∣ tu ∣+∣ v ∣ donde  A∈[2pag,pág ].​( 1 )

Entre ellos $t,uy\; v$ representan el valor del arco de cada giro en la fórmula (en realidad, también es la longitud del arco de los tres arcos de la curva básica, porque el radio del arco es 1, el círculo unitario)$,$$L$ es la longitud total del camino; la función RRen la fórmula$R$ es el sistema de coordenadas cartesiano$(x,y)$ en el sistema de coordenadas polares$\left({tú}_{},t_{}\right)$ , donde los valores de las coordenadas radiales son${tu}_{}、{tú}_{}$, los valores de las coordenadas angulares son $t_{}、t_{}$, la función $M$ se utiliza para$2\; operación\; de\; módulo\pi$ y limitar el valor en radianes a$[-\pi ,p]$ .

función $(r,yo)=R(x,y)$ es ( x , y ) (x, y)en el sistema de coordenadas cartesiano$(x,y)$ y sistema de coordenadas polares$(r,$Defina$\theta$$)$$$\begin{array}{r}\{\begin{array}{l}r\mathrm{porque}i=X\\ r\mathrm{pecado}i=\end{array}\end{array}$$

$pag=M\left(\theta \right)$ función$$\begin{array}{r}\{\begin{array}{l}pag=imodo\_\_2p.m.\\ -pag.\le pag<\end{array}\end{array}$$

Además de los símbolos recién definidos, los símbolos definidos no se describirán en detalle a continuación.

2.2 Composición de dos arcos y rectas

• El cuarto tipo de forma de curva básica: $CSC$ , que se compone de dos arcos y rectas, en base a esto se pueden extender ocho tipos de curvas:$l^{+}{S}^{+}{l}^{+}、L^{-S}{\_}^{-L}{\_}^{-}、R^{+}{S}^{+}{R}^{+}、R^{-S}{\_}^{-R}{\_}^{-}、L^{+}{S}^{+}{R}^{+}、L^{-S}{\_}^{-R}{\_}^{-}、R^{+}{S}^{+}{l}^{+}、R^{-S}{\_}^{-L}{\_}^{-}$。

{ ( u , t ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) v = M ( θ − t ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ y A ∈ [ 0 , π ] (2) \tag{2} \begin{aligned}\left\{\begin{array}{l} (u, t)=R(x-\sin \theta, y-1+\cos \theta ) \\ v=M(\theta-t) \\ L=|t|+|u|+|v| \\ \text { Cuadro } A \in[0, \pi] \end{array}\right. \end{alineado}⎩⎪⎪⎨⎪⎪⎧( tú ,t )=R ( x−pecadoyo ,y−1+porqueyo )v=M ( yo−t )l=∣ t ∣+∣ tu ∣+∣ v ∣ donde  A∈[ 0 ,pág ].​( 2 )
{ ( u 1 , t 1 ) = R ( x − pecado ⁡ θ , y − 1 + porque ⁡ θ ) { u 1 2 > 4 → L = ∞ u 1 2 ≤ 4 → { u = u 1 2 − 4 ( u 2 , t 2 ) = R ( u , 2 ) t = M ( t 1 + t 2 ) v = M ( t − u ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ (3) \tag{3} \begin{aligned} \left\{\begin{array}{l} \left(u_{1}, t_{1}\right)=R(x-\sin \theta, y-1 +\cos \theta) \\ \left\{\begin{array}{l} u_{1}^{2}>4 \rightarrow L=\infty \\ u_{1}^{2} \leq 4 \ flecha derecha\izquierda\{\begin{array}{l} u=\sqrt{u_{1}^{2}-4} \\ \left(u_{2}, t_{2}\right)=R(u , 2) \\ t=M\left(t_{1}+t_{2}\right) \\ v=M(tu) \\ L=|t|+|u|+|v| \end{array}\right. \end{array}\right. \end{array}\right.\\ \end{aligned}⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧( tú1,t1)=R ( x−pecadoyo ,y−1+porqueyo )⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧tu12>4→l=∞tu12≤4→⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧tu=tu12−4 ​( tú2,t2)=R ( tú ,2 )t=METRO( t1+t2)v=METRO ( t−tú )l=∣ t ∣+∣ tu ∣+∣ v ∣​​​( 3 )

A través de los tres métodos de transformación de curvas anteriores, los símbolos de la cuarta fórmula se transforman en 8 tipos de curvas. Entre los 8 tipos de curvas, los primeros 4 tipos de curvas se resuelven mediante la fórmula (2) y los últimos 4 tipos de curvas se resuelven mediante la fórmula (3).

2.3 Composición de cuatro arcos

• El quinto tipo de forma de curva básica: $C.C.{\_}_{}|C_{}C$ , que se compone de cuatro arcos, la dirección del arco del primer segmento es opuesta a la del segundo segmento, la dirección del arco del tercer segmento es opuesta a la del cuarto segmento y la dirección de los dos primeros segmentos es opuesta a la de los dos últimos segmentos, en base a esto se pueden extender cuatro formas de curva:$l^{+}{R}_{}{}^{+}{l}_{}{}^{-R}{\_}^{-}、L^{-R}{\_}_{}{}^{-L}{\_}_{}{}^{+}{R}^{+}、R^{+}{l}_{}{}^{+}{R}_{}{}^{-L}{\_}^{-}、R^{-L}{\_}_{}{}^{-R}{\_}_{}{}^{+}{l}^{+}$。

• El sexto tipo de forma de curva básica: $C|C{\_}_{}{C}_{}|C$ , que se compone de cuatro arcos, el primer y segundo arco tienen la misma dirección de arco, el tercer y cuarto arco tienen la misma dirección de arco, y los dos primeros arcos y los dos últimos arcos tienen direcciones opuestas, en base a esto, cuatro las formas de las curvas se pueden extender:$l^{+}{R}_{}{}^{-L}{\_}_{}{}^{-R}{\_}^{+}、L^{-R}{\_}_{}{}^{+}{l}_{}{}^{+}{R}^{-}、R^{+}{l}_{}{}^{-R}{\_}_{}{}^{-L}{\_}^{+}、R^{-L}{\_}_{}{}^{+}{R}_{}{}^{+}{l}^{-}$。

{ ξ = x + pecado ⁡ ( θ ) η = y − 1 − porque ⁡ ( θ ) ρ 1 = 2 + ξ 2 + η 2 4 ρ 2 = 20 − ξ 2 − η 2 16 0 ≤ ρ 1 , ρ ≤ 1 → { u 1 = arcos ⁡ ( ρ 1 ) u 2 = − arcos ⁡ ( ρ 2 ) u = u 1 o 2 ( t , v ) = τ ω ( u , − u , ξ , η , θ ) L = ∣ t ∣ + 2 ∗ ∣ u ∣ + ∣ v ∣ Sea u ∈ [ 0 , π / 2 ] Sea L = ∞ (4) \tag{4} \left\{\begin{array}{l} \xi = x+\sin(\theta)\\\eta=y-1-\cos(\theta)\\\rho_{1}=\frac{2+\sqrt{\xi^{2}+\eta^{ 2 }}}{4} \\ \rho_{2}=\frac{20-\xi^{2}-\eta^{2}}{16} \\ 0 \leq \rho_{1} 、 \rho_ { 2} \leq 1 \rightarrow\left\{\begin{array}{l} u_{1}=\arccos\left(\rho_{1}\right) \\u_{2}=-\arccos\left ( \rho_{2}\right) \\ u=u_1 \quad o \quad u_2\\ (t, v)=\tau \omega(u,-u, \xi, \eta, \theta) \\ L = |t|+2 *|u|+|v| \\ \text { privado } u \in[0, \pi/2] \end{array}\right. \\ \text { Cuadro } L=\infty \end{array}\right.⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧X=X+pecado ( θ )el=y−1−porque ( θ )r1=42 +X2 +norte2 ​​r2=1 62 0 − ξ2 -h20≤r1、pag2≤1→⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧tu1=arccos( r1)tu2=−arccos( r2)tu=tu1o rtu2( t ,v )=a ( tú , _− tú ,x ,el ,yo )l=∣ t ∣+2∗∣ tu ∣+∣ v ∣ entre ellos  tu∈[ 0 ,p / 2 ] . Otro  l=∞( 4 )
Estos 8 tipos de curvas se pueden resolver mediante la fórmula (4). En la fórmula anterior,$r_{},{tu}_{}$Para el cálculo de la ruta del quinto tipo de fórmula, $r_{},{tu}_{}$Se utiliza en el cálculo de la ruta del sexto tipo de fórmula, donde $\tau \omega$ es una función de cálculo de curvas en radianes. Generalmente, el valor de curvas en radianes de la función$(t,v)=a(tú,\_v_{},x,el,\theta )$ se calcula mediante la fórmula (5).

{ σ = M ( u − v 0 ) ζ = pecado ⁡ ( u ) − pecado ⁡ ( σ ) ψ = cos ⁡ ( u ) − cos ⁡ ( σ ) − 1 t 1 = arctan ⁡ η ∗ ζ − ξ ∗ ψ ξ ∗ ζ + η ∗ ψ λ = 2 ∗ ( porque ⁡ ( σ ) − porque ⁡ ( v 0 ) − porque ⁡ ( u ) ) + 3 t = { M ( t 1 + π ), λ < 0 M ( t 1 ) , λ ≥ 0 v = M ( t − ( u − v 0 ) − θ ) (5) \tag{5}\left\{\begin{array}{l} \sigma=M(u-v_0) \\ \zeta=\sin(u)-\sin(\sigma) \\ \psi=\cos(u)-\cos(\sigma)-1 \\ t_{1}=\arctan \frac{\eta *\zeta-\xi*\psi}{\xi*\zeta+\eta*\psi}\\\lambda=2 *(\cos(\sigma)-\cos(v_0)-\cos(u))+ 3\\t=\left\{\begin{array}{l} M\left(t_{1}+\pi\right), \lambda<0\\M\left(t_{1}\right), \lambda \geq 0 \end{array}\right. \ \ v= M ( t - ( u - v_0 ) - \ theta ) \ end { array } \ right⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧pag=m ( tú−v0)gramo=pecado ( tú )−pecado ( σ )pag=porque ( tú )−porque ( σ )−1t1=arctánξ ∗ ζ + η ∗ ψη ∗ ζ − ξ ∗ ψyo=2∗( porque ( σ )−porque ( v0)−porque ( u ) )+3t={ METRO( t1+pag ),yo<0METRO( t1),yo≥0v=METRO ( t−( tú−v0)−yo ).( 5 )

2.4 Composición de tres arcos y una recta

• La séptima forma de curva básica: $C|C_{p/}SC$ , que se compone de tres arcos y una recta, el segundo arco es$Arco\; \pi /2$ , la tercera sección es un camino en línea recta, la primera sección es opuesta a los últimos tres arcos, en base a esto se pueden extender ocho tipos de curvas:$l^{+}{R}_{p/2}^{}{S}^{-R}{\_}^{-}、L^{-R}{\_}_{p/2}^{}{S}^{+}{R}^{+}、R^{+}{l}_{p/2}^{}{S}^{-L}{\_}^{-}、R^{-L}{\_}_{p/2}^{}{S}^{+}{l}^{+}、L^{+}{R}_{p/2}^{}{S}^{-L}{\_}^{-}$、$l^{-R}{\_}_{p/2}^{}{S}^{+}{l}^{+}$、$R^{+}{l}_{p/2}^{}{S}^{-R}{\_}^{-}$、$R^{-L}{\_}_{p/2}^{}{S}^{+}{R}^{+}$。

• El octavo tipo de forma de curva básica: $CSC\_{\_}_{p/}|C$ , que se compone de tres arcos y una recta, el tercer arco es$\pi /2$ arco, el segundo segmento es un camino recto y el cuarto segmento es opuesto a los primeros tres arcos, en base a esto se pueden extender ocho tipos de curvas:$l^{+}{S}^{+}{l}_{p/2}^{}{R}^{-}、L^{-S}{\_}^{-L}{\_}_{p/2}^{}{R}^{+}、R^{+}{S}^{+}{R}_{p/2}^{}{l}^{-}、R^{-S}{\_}^{-R}{\_}_{p/2}^{}{l}^{+}、R^{+}{S}^{+}{l}_{p/2}^{}{R}^{-}、R^{-S}{\_}^{-L}{\_}_{p/2}^{}{R}^{+}$、$l^{+}{S}^{+}{R}_{p/2}^{}{l}^{-}、L^{-S}{\_}^{-R}{\_}_{p/2}^{}{l}^{+}$。

{ ζ = x + pecado ⁡ ( θ ) η = y − 1 − porque ⁡ ( θ ) ( ρ , ϑ ) = R ( − η , ζ ) { ( 1 ) ρ ≥ 2 → { ( T , ϑ 1 ) = R ( ρ 2 − 4 , − 2 ) t = M ( ϑ − ϑ 1 ) u = 2 − ϑ 1 v = M ( θ − t − π / 2 ) ( 2 ) { t = ϑ u = 2 − ρ v = M ( t + π / 2 − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + π / 2 Además, L = ∞ (6) \tag{6} \left\{\begin{array} {l} \zeta=x+\sin (\theta) \\ \zeta=y-1-\cos (\theta) \\ \left(\rho, \vartheta\right)=R(-\eta, \zeta ) \\ \left\{\begin{array}{l} (1) \rho \geq 2 \rightarrow\left\{\begin{array}{l} \left(T, \vartheta_{1}\right) =R\left(\sqrt{\rho^{2}-4},-2\right) \\ t=M\left(\value-\value_{1}\right) \\ u=2-\value_ { 1 } \\v=M(\theta-t-\pi/2) \end{array}\right. \\(2)\left\{\begin{array}{l} t=\vartheta\\ u=2-\rho \\ v=M(t+\pi / 2-\theta) \end{array}\ bien. \\ L=|t|+|u|+|v|+\pi / 2 \\ \text { también,} L=\infty \end{array}\right. \ final {matriz} \ derecha⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧gramo=X+pecado ( θ )el=y−1−porque ( θ )( p ,) _=R ( − η ,gramo )⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧( 1 ) pag.≥2→⎩⎪⎪⎪⎨⎪⎪⎪⎧( t ,ϑ1)=R(r2−4 ​,− 2 )t=METRO( ϑ−ϑ1)tu=2−ϑ1v=M ( yo−t−pág / 2 ).( 2 )⎩⎨⎧t=ϑtu=2−rv=METRO ( t+p / 2−yo ).l=∣ t ∣+∣ tu ∣+∣ v ∣+p / 2 Otro, L.=∞​( 6 )

uu en la ceremonia$u$ representa la longitud del movimiento lineal,$tyv$ representan el valor en radianes del movimiento del arco. En la fórmula (6), cuando la trayectoria del arco está formada por dos curvas en el sentido de las agujas del reloj, use (1) para calcular. La composición de la curva se calcula usando la subfórmula (2).

2.5 Composición de cuatro arcos y una recta

• El noveno tipo de forma de curva básica: $C\left|C_{p/}SC{\_}_{p/}\right|C$ , que se compone de cuatro arcos y una recta, el segundo arco y el cuarto arco son$Arco\; \pi /2$ , el camino del medio es un camino recto, y la dirección del segundo, tercer y cuarto arco es opuesta a la del primer y quinto arco, en base a esto se pueden extender cuatro tipos de curvas: L + R π$l^{+}{R}_{p/2}^{}{S}^{-L}{\_}_{p/2}^{}{R}^{+}、L^{-R}{\_}_{p/2}^{}{S}^{+}{l}_{p/2}^{}{R}^{-}$、$R^{+}{l}_{p/2}^{}{S}^{-R}{\_}_{p/2}^{}{l}^{+}、R^{-L}{\_}_{p/2}^{}{S}^{+}{R}_{p/2}^{}{l}^{-}$。

{ ζ = x + pecado ⁡ ( θ ) η = y − 1 − cos ⁡ ( θ ) ( ρ , ϑ ) = R ( ζ , η ) ρ ≥ 2 → { t = M ( ϑ − arcos ⁡ ( − 2 ρ ). ) ), t ∈ [ − π 2 , π 2 ] { t ≤ 0 → L = ∞ t > 0 → { u = 4 − ζ + 2 ∗ cos ⁡ ( t ) pecado ⁡ ( t ) v = M ( t − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + 2 ∗ π 2 Además, L = ∞ (7) \tag{7} \left\{\begin{array}{l} \zeta= x+\ sin (\theta) \\ \eta=y-1-\cos (\theta) \\ (\rho, \vartheta)=R(\zeta, \eta) \\ \rho \geq 2 \rightarrow\ izquierda\ {\begin{array}{l} t=M\left(\vartheta-\arccos\left(-\frac{2}{\rho}\right)\right), t \in\left[-\ frac{ \pi}{2}, \frac{\pi}{2}\right] \\ \left\{\begin{array}{l} t \leq 0 \rightarrow L=\infty\\ t>0 \rightarrow \left\{\begin{array}{l} u=4-\frac{\zeta+2*\cos(t)}{\sin(t)}\\v=M(t-\theta) \\ L=|t|+|u|+|v|+2 * \frac{\pi}{2} \end{array}\right. \\\end{array}\right. \\ \end{array}\right.\\ \text{también, }L=\infty \end{array}\right.⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧gramo=X+pecado ( θ )el=y−1−porque ( θ )( p ,) _=R ( z ,h )r≥2→⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧t=METRO( ϑ−arccos( -r2) ),t∈[ -2pag,2pag]⎩⎪⎪⎨⎪⎪⎧t≤0→l=∞t>0→⎩⎨⎧tu=4−pecado ( t )ζ + 2 ∗ porque ( t )v=METRO ( t−yo )l=∣ t ∣+∣ tu ∣+∣ v ∣+2∗2pag​​​Otro ,  l=∞( 7 )

3. Implementación de Python

Porque hay demasiados errores en el código que escribí. . . No quiero cambiarlo, así que aquí hay algunos códigos de github de código abierto:

pitón:

• https://github.com/ghliu/pyReedsShepp
• https://github.com/nathanlct/reeds-shepp-curves
• PythonRobótica

4. implementación de c ++

C++:

• https://ompl.kavrakilab.org/ReedsSheppStateSpace_8cpp_source.html