[Condução automática] planejamento de caminho - resumo da curva ReedsShepp (implementação python | implementação c++)

Referências

• Introdução às curvas de Reeds-Shepp e Dubins

• Planejamento de caminho - Curvas ReedsShepp

• 原论文: CAMINHOS ÓTIMOS PARA UM CARRO QUE VAI PARA FRENTE E PARA TRÁS

• Planejamento de Reeds Shepp

• Um exemplo para entender o planejamento do caminho da direção automática - (4) Explicação detalhada da curva RS no algoritmo HybridA*

• Pesquisa sobre planejamento de rotas e controle de rastreamento de sistema de estacionamento automático para carros de passageiros_Tao Peng

---

Preâmbulo

• Derivação da curva de Dubins (método baseado em vetores)
• Resumo da fórmula da curva de Dubins e implementação do código python (com base no método de geometria)

---

1. Curva Reeds-Shepp

1.1 Conceitos básicos

O algoritmo Reeds-Shepp é conhecido como RS , um artigo publicado por JAReeds e LAShepp em 1990 (caminho ideal para um carro que anda para frente e para trás) . Este método é aprimorado com base no algoritmo de Dubins , e o movimento reverso (o carro pode andar para trás, engatar marcha à ré) é adicionado ao planejamento, o que permite obter uma solução melhor que a curva de Dubins em alguns casos .

Um exemplo de curva é o seguinte:

Fonte da imagem: https://blog.csdn.net/robinvista/article/details/95137143

---

Como pode ser visto na figura acima, no caminho da curva de Reeds-Shepp , é permitida a existência da válvula cúspide.

1.2 Combinação de campos

Adicione um sobrescrito à letra no campo para indicar a direção do movimento, como segue:

símbolo	significado	em torno do círculo unitário
${eu}^{+}$	vire à esquerda	sentido anti-horário
${eu}^{-}$	atrás à esquerda	sentido horário
$R^{+}$	vire à direita	sentido anti-horário
$R^{-}$	vire à direita de volta	sentido horário
$S^{+}$	siga em frente	/
$S^{-}$	volte direto	/

Use $Os\; caracteres\; CeS$ podem receber os seguintes 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 )}$$

O novo campo pode ser obtido invertendo o sinal da equação (1). Na fórmula acima (1), $C_{p/2}^{}$Indica o $L$ ouRRO ângulo girado de$R$$\pi /2$ ,$C_{}{C}_{}$Uma combinação significa que os arcos correspondentes têm comprimentos iguais.

Para representação compacta, evite $\pm$ , também existe uma função infinitesimal:
$$\begin{array}{rl}& CCC\leftarrow \left\{\begin{array}{ccccc}C|C|& C|C& CC|& C{C}_{}|C_{}& C|C{\_}_{}{C}_{}|\end{array}\right\}\\ & CSC\_\_\leftarrow \{\begin{array}{llll}CS& C|C_{p/}SC& CSC\_{\_}_{p/}|& C\left|C_{p/}SC{\_}_{p/}\right|\end{array}\\ & \end{array}\text{\hspace{1em}( 2 )}$$

Entre eles, $|$ indica que a direção do movimento do veículo muda de avanço para ré ou de ré para avanço.

Subscreva as palavras acima, como $C_{}|C{\_}_{}|C{\_}_{}$, entre eles $um,b,\gamma$ representa o ângulo de rotação (em radianos), respectivamente. $S$ com subscrito$d$ significa que a distância percorrida em linha reta é$d$ . Seus intervalos são mostrados na tabela abaixo:

Será $C$ é substituído por$L$ ou$R$ , através de uma transformação simples, existem48combinação de campos. Esta transformação simples inclui time flip (timeflip) , reflexão (reflect) e transformação para trás (backwards) .

1.3 Time flip (timeflip), reflexão (refletir) e transformação para trás (para trás)

1.3.1 Inversão de tempo (timeflip)

A curva calculada é invertida de acordo com sua direção de movimento, e a nova curva obtida é a curva oposta da curva original.

Conforme mostrado na figura, a curva azul ${eu}^{-R}{\_}^{+}{S}^{+}{eu}^{+}$ com a curva vermelha${eu}^{+}{R}^{-S}{\_}^{-}{eu}^{-}$ É simétrico em relação ao eixo Y e os ângulos do rumo do caminho nas duas curvas do caminho são opostos. Do ponto inicial$Ó(0,0,0)$到目标点$A(x,y,\theta )$的路径可以通过起始点$O(0,0,0)$到目标点 $B(-x,-y,-\theta )$的路径取反计算获得。

1.3.2 反射(reflect)

第二种转换关系：“reflect”，即将计算的曲线按照其沿圆周运动方向取反，得到的曲线与原来的曲线长度相同的新曲线。

如图所示，红色曲线 $R^{-}{L}^{+}{S}^{+}{R}^{+}$与蓝色曲线 $L^{-R}{\_}^{+}{S}^{+}{eu}^{+}$ Simétrico em relação ao eixo X, os ângulos do rumo do caminho nas duas curvas do caminho são opostos. Do ponto inicial$Ó(0,0,0)$ para o ponto alvo$UMA(x,sim,\theta )$ pode passar pelo ponto inicial$Ó(0,0,0)$ para o ponto alvo$B(x,-você,-\theta )$ é obtido invertendo a direção circunferencial do caminho.

1.3.3 Transformação para trás (para trás)

A terceira relação de conversão: "para trás", ou seja, o caminho da curva original é convertido na ordem inversa, e a curva obtida é uma nova curva com o mesmo comprimento da curva original.

O diagrama de exemplo "para trás" é mostrado na figura, a curva azul ${eu}^{-S}{\_}^{-R}{\_}^{-}{eu}^{+}$ com a curva vermelha${eu}^{+}{R}^{-S}{\_}^{-}{eu}^{-}$ Não possui as duas relações de empilhamento acima, mas pode ser visto intuitivamente que${eu}^{-S}{\_}^{-R}{\_}^{-}{eu}^{}$A ordem inversa de${}^{+}$${eu}^{+}{R}^{-S}{\_}^{-}{eu}^{-}$ , os caminhos nas duas curvas de caminho são organizados inversamente. Do ponto inicial$Ó(0,0,0)$ para o ponto alvo$UMA(x,sim,\theta )$ pode passar pelo ponto inicial$Ó(0,0,0)$ 到目标点 $B(x\ast \cos \theta +y\ast \sin \theta ,x\ast \sin \theta -y\ast \cos \theta ,\theta )$ 的路径圆周方向取反计算获得。

1.4 48 个字段组合

所有的48个字段组合一一枚举如下：

图中，角标$\beta$代表C旋转的弧度是$\beta$，同理$\pi /2$也是代表旋转弧度，意思就是这个圆必须转$\pi /2$。

所以，Dubins曲线是从6个曲线中选出最短的那条，而Reeds-Shepp曲线是从48个曲线中选出最短的那个（最开始的论文认为最短的曲线一定在48条曲线中，后人研究发现有两条曲线不会是最短的，所以后来搜索范围减小到46条）。

至于这些字段组合具体的推导，请看原论文。

2. RS曲线求解

A fórmula de solução da curva de Reeds-Shepp está na fórmula (8.1) ~ fórmula (8.11) do artigo original . Está resumido aqui.

Suponha que o ponto de partida seja $(0,0,0)$ , o ponto final é$(x,sim,eu)$ .

2.1 Composição de três arcos

• O primeiro tipo de curva básica: $C|C|C$ , ou seja, três arcos e dois arcos em cada arco têm direções opostas. Com base nisso, quatro tipos de curvas podem ser estendidas:${eu}^{+}{R}^{-}{eu}^{+}、L^{-R}{\_}^{+}{eu}^{-}、R^{+}{eu}^{-R}{\_}^{+},R^{-}{eu}^{+}{R}^{-}$。
• O segundo tipo de curva básica: $CC|C$ , ou seja, três arcos e as direções dos arcos dos dois últimos arcos são opostas. Com base nisso, quatro tipos de curvas podem ser estendidas:${eu}^{+}{R}^{+}{eu}^{-}、{eu}^{-R}{\_}^{-}{eu}^{+},R^{+}{eu}^{+}{R}^{-}、R^{-}{eu}^{-R}{\_}^{+}$。
• A terceira curva básica: $C|CC$ , ou seja, três arcos e as direções dos arcos dos dois primeiros arcos são opostas. Com base nisso, quatro tipos de curvas podem ser estendidas:${eu}^{+}{R}^{-}{eu}^{-}、{eu}^{-R}{\_}^{+}{eu}^{+},R^{+}{eu}^{-R}{\_}^{-}、R^{-}{eu}^{+}{R}^{+}$。

Através dos três métodos de transformação de curva de reversão de tempo, reflexão e retrocesso, os símbolos das três fórmulas acima são transformados nos modos de movimento de 12 curvas, que podem ser resolvidos pela fórmula (1).
{ ( você 1 , t 1 ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) { você 1 2 > 4 → L = ∞ você 1 2 ≤ 4 → { A = arco seno ⁡ ( você 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 ∣ onde 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 \ seta para a direita 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}\ direita. \\ \text {onde} A \in\left[\frac{\pi}{2}, \pi\right] \end{array}\right. \end{array}\right.⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧( você1,t1)=R ( x-pecadoeu ,sim-1+porqueeu )⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧você12>4→eu=∞você12≤4→⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧A=arco seno(4você12​)você=M( Uma+t1)( você2,t2)=R ( x-pecadoeu ,sim-1+porqueeu )t=t2v=M ( eu+t-você )eu=∣ t ∣+∣ você ∣+∣v∣ _ _ onde  A∈[2p,p ].​( 1 )

Entre eles $t,ue\; v$ representam o valor do arco de cada volta na fórmula (na verdade, é também o comprimento do arco dos três arcos da curva básica, porque o raio do arco é 1, o círculo unitário)$,$$L$ é o comprimento total do caminho; a função RRna fórmula$R$ é o sistema de coordenadas cartesianas$(x,y)$ no sistema de coordenadas polares$\left({você}_{},t_{}\right)$ , onde os valores das coordenadas radiais são${você}_{}você{\_}_{}$, os valores das coordenadas angulares são $t_{}、t_{}$, a função $M$ é usado para$2\; operação\; módulo\pi$ e limite o valor em radianos a$[-\pi ,p]$ .

função $(r,eu)=R(x,y)$即笛卡尔坐标系中的 $(x,y)$ 与极坐标系$(r,\theta )$的关系可以表示为： $$\begin{array}{r}\{\begin{array}{l}r\cos \theta =x\\ r\sin \theta =y\end{array}\end{array}$$

$\psi =M(\theta )$即表示 $$\begin{array}{r}\{\begin{array}{l}\psi =\theta \mathrm{m}\mathrm{o}\mathrm{d}2\pi \\ -\pi \le \psi <\pi \end{array}\end{array}$$

Além dos símbolos recentemente definidos, os símbolos definidos não serão descritos em detalhe abaixo.

2.2 Composição de dois arcos e retas

• O quarto tipo de forma de curva básica: $CSC$ , que é composto por dois arcos e linhas retas, com base nisso, oito tipos de curvas podem ser estendidas:${eu}^{+}{S}^{+}{eu}^{+}、L^{-S}{\_}^{-}{eu}^{-}、R^{+}{S}^{+}{R}^{+},R^{-S}{\_}^{-R}{\_}^{-}、{eu}^{+}{S}^{+}{R}^{+}、L^{-S}{\_}^{-R}{\_}^{-}、R^{+}{S}^{+}{eu}^{+},R^{-S}{\_}^{-}{eu}^{-}$。

$$\begin{array}{r}\{\begin{array}{l}(u,t)=R(x-\sin \theta ,y-1+\cos \theta )\\ v=M(\theta -t)\\ L=|t|+|u|+|v|\\ \text{ 其中 }A\in [0,\pi ]\end{array}\end{array}\text{\hspace{1em}( 2 )}$$
{ ( você 1 , t 1 ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) { você 1 2 > 4 → L = ∞ você 1 2 ≤ 4 → { você = você 1 2 − 4 ( você 2 , t 2 ) = R ( você , 2 ) t = M ( t 1 + t 2 ) v = M ( t − você ) L = ∣ t ∣ + ∣ você ∣ + ∣ 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 \ seta para a direita\esquerda\{\begin{array}{l} u=\sqrt{u_{1}^{2}-4} \\ \left(u_{2}, t_{2}\right)=R(u , 2) \\ t=M\esquerda(t_{1}+t_{2}\direita) \\ v=M(tu) \\ L=|t|+|u|+|v| \end{array}\right. \end{array}\right. \end{array}\right.\\ \end{aligned}⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧( você1,t1)=R ( x-pecadoeu ,sim-1+porqueeu )⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧você12​>4→L=∞u12​≤4→⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​u=u12​−4 ​(u2​,t2​)=R(u,2)t=M(t1​+t2​)v=M(t−u)L=∣t∣+∣u∣+∣v∣​​​​(3)

通过上述三种曲线变换方式，将第 4 种公式的符号变换成了 8 种曲线，这 8种曲线中前 4 种使用式(2) 进行求解，后 4 种使用式(3) 进行求解。

2.3 四段圆弧组成

• 第五类基础曲线形式: $C{C}_{\beta }\mid C_{\beta }C$, 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相反, 第三段与第四段圆弧的圆弧方向相反, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: $L^{+}{R}_{\beta }{}^{+}{L}_{\beta }{}^{-}{R}^{-}、L^{-}{R}_{\beta }{}^{-}{L}_{\beta }{}^{+}{R}^{+}、R^{+}{L}_{\beta }{}^{+}{R}_{\beta }{}^{-}{L}^{-}、R^{-}{L}_{\beta }{}^{-}{R}_{\beta }{}^{+}{L}^{+}$。

• 第六类基础曲线形式: $C\left|C_{\beta }{C}_{\beta }\right|C$ , que é composto por quatro arcos, o primeiro e o segundo arcos têm a mesma direção do arco, o terceiro e o quarto arcos têm a mesma direção do arco, e os dois primeiros arcos e os dois últimos arcos têm direções opostas, com base nisso, quatro formas de curva podem ser estendidas:${eu}^{+}{R}_{}{}^{-}{eu}_{}{}^{-R}{\_}^{+}、L^{-R}{\_}_{}{}^{+}{eu}_{}{}^{+}{R}^{-}、R^{+}{eu}_{}{}^{-R}{\_}_{}{}^{-}{eu}^{+},R^{-}{eu}_{}{}^{+}{R}_{}{}^{+}{eu}^{-}$。

$$\{\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\le {\rho }_1、{\rho }_2\le 1\to \{\begin{array}{l}{u}_1=\arccos \left({\rho }_1\right)\\ u_2=-\arccos \left({\rho }_2\right)\\ u=u_{1}or{u}_2\\ (t,v)=\tau \omega (u,-u,\xi ,\eta ,\theta )\\ L=|t|+2\ast |u|+|v|\\ \text{ 其中 }u\in [0,p/2\end{array}\\ \text{ Outro }eu=\end{array}\text{\hspace{1em}( 4 )}$$ Esses 8 tipos de curvas podem ser resolvidos pela fórmula (4), ρ 1 , u 1 \rho_{1},u_1
na fórmula acima$R_{},{você}_{}$Para o cálculo do caminho do quinto tipo de fórmula, $R_{},{você}_{}$É utilizado no cálculo do caminho do sexto tipo de fórmula, onde $\tau \omega$ é uma função de cálculo de radianos de curva. Geralmente, o valor de radianos da curva da função$(t,v)=para(você,\_v_{},x,o,\theta )$ é calculado pela fórmula (5).

{ σ = M (u - v 0) ζ = sin ⁡ (u) - sin ⁡ (σ) ψ = cos ⁡ (u) - cos ⁡ (σ) - 1 t 1 = arctan ⁡ η ∗ ζ - ξ ∗ ψ ξ ∗ ζ + η ∗ ψ λ = 2 ∗ ( cos ⁡ ( σ ) − cos ⁡ ( v 0 ) − cos ⁡ ( 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⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧p=M ( você-v0)g=pecado ( você )-sin(σ)ψ=cos(u)−cos(σ)−1t1​=arctanξ∗ζ+η∗ψη∗ζ−ξ∗ψ​λ=2∗(cos(σ)−cos(v0​)−cos(u))+3t={ M(t1​+π),λ<0M(t1​),λ≥0​v=M(t−(u−v0​)−θ)​(5)

2.4 三段圆弧与一条直线段组成

• 第七类基础曲线形式: $C\mid C_{p/}SC$ , que é composto por três arcos e uma reta, o segundo arco é$Arco\; \pi /2$ , a terceira seção é um caminho em linha reta, a primeira seção é oposta aos três últimos arcos, com base nisso, oito tipos de curvas podem ser estendidas: L + R π / 2 −${eu}^{+}{R}_{p/2}^{}{S}^{-R}{\_}^{-}、{eu}^{-R}{\_}_{p/2}^{}{S}^{+}{R}^{+},R^{+}{eu}_{p/2}^{}{S}^{-}{eu}^{-}、R^{-}{eu}_{p/2}^{}{S}^{+}{eu}^{+}、L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}^{-}$、 $L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}^{+}$、 $R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}^{-}$、 $R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}^{+}$。

• 第八类基础曲线形式: $CS{C}_{\pi /2}\mid C$, 即三段圆弧与一段直线组成, 第三段圆弧为 $Arco\; \pi /2$ , o segundo segmento é um caminho reto e o quarto segmento é oposto aos três primeiros arcos.Com base nisso, oito tipos de curvas podem ser estendidos:${eu}^{+}{S}^{+}{eu}_{p/2}^{}{R}^{-}、{eu}^{-S}{\_}^{-}{eu}_{p/2}^{}{R}^{+},R^{+}{S}^{+}{R}_{p/2}^{}{eu}^{-}、R^{-S}{\_}^{-R}{\_}_{p/2}^{}{eu}^{+},R^{+}{S}^{+}{eu}_{p/2}^{}{R}^{-}、R^{-S}{\_}^{-}{eu}_{p/2}^{}{R}^{+}$、${eu}^{+}{S}^{+}{R}_{p/2}^{}{eu}^{-}、{eu}^{-S}{\_}^{-R}{\_}_{p/2}^{}{eu}^{+}$。

$$\{\begin{array}{l}\zeta =x+\sin (\theta )\\ \eta =y-1-\cos (\theta )\\ \left(\rho ,\vartheta \right)=R(-\eta ,\zeta )\\ \{\begin{array}{l}(1)\rho \ge 2\to \{\begin{array}{l}\left(T,{\vartheta }_1\right)=R\left(\sqrt{{\rho }^2-4},-2\right)\\ t=M\left(\vartheta -{\vartheta }_1\right)\\ u=2-{\vartheta }_1\\ v=M(\theta -t-\pi /2)\end{array}\\ (2)\{\begin{array}{l}t=\vartheta \\ u=2-\rho \\ v=M(t+\pi /2-\theta )\end{array}\\ L=|t|+|u|+|v|+\pi /2\\ \text{ 其他，}L=\end{array}\end{array}\text{\hspace{1em}( 6 )}$$

você na cerimônia$u$ representa o comprimento do movimento linear,$tev$ representam o valor em radianos do movimento do arco. Na fórmula (6), quando o caminho do arco é formado por duas curvas no sentido horário, use (1) para calcular. A composição da curva é calculada usando a subfórmula (2).

2.5 Composição de quatro arcos e uma reta

• O nono tipo de forma de curva básica: $C\left|C_{p/}SC{\_}_{p/}\right|C$ , que é composto por quatro arcos e uma linha reta, o segundo arco e o quarto arco são$\pi /2$ arco, o caminho do meio é um caminho reto, e a direção do segundo, terceiro e quarto arcos é oposta à do primeiro e quinto arcos. Com base nisso, quatro tipos de curvas podem ser estendidos: L + R π${eu}^{+}{R}_{p/2}^{}{S}^{-}{eu}_{p/2}^{}{R}^{+}、L^{-R}{\_}_{p/2}^{}{S}^{+}{eu}_{p/2}^{}{R}^{-}$、$R^{+}{eu}_{p/2}^{}{S}^{-R}{\_}_{p/2}^{}{eu}^{+},R^{-}{eu}_{p/2}^{}{S}^{+}{R}_{p/2}^{}{eu}^{-}$。

{ ζ = x + sin ⁡ ( θ ) η = y − 1 − cos ⁡ ( θ ) ( ρ , ϑ ) = R ( ζ , η ) ρ ≥ 2 → { t = M ( ϑ − arccos ⁡ ( − 2 ρ ) ) , t ∈ [ − π 2 , π 2 ] { t ≤ 0 → L = ∞ t > 0 → { u = 4 − ζ + 2 ∗ cos ⁡ ( t ) sin ⁡ ( t ) v = M ( t − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + 2 ∗ π 2 其他,  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\left\{\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{其他, }L=\infty \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧g=x+pecado ( θ )o=sim-1-porque ( θ )( p ,) _=R ( z ,h )R≥2→⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧t=M( ϑ-arcos( -R2) ),t∈[ -2p,2p]⎩⎪⎪⎨⎪⎪⎧t≤0→eu=∞t>0→⎩⎨⎧você=4-pecado ( t )ζ + 2 ∗ cos ( t )v=M ( t-eu )eu=∣ t ∣+∣ você ∣+∣ v ∣+2∗2p​​​outro ,  eu=∞( 7 )

3. Implementação Python

Porque há muitos bugs no código que escrevi. . . Não quero mudar isso, então aqui estão alguns códigos de código aberto do GitHub:

Pitão:

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

4. implementação c++

C++:

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