一、軌跡規(guī)劃

1.1軌跡平滑與輪跡

• 初始時(shí)，我們有一條由若干坐標(biāo)點(diǎn)構(gòu)成的機(jī)器人運(yùn)行路徑。通過對(duì)這些坐標(biāo)點(diǎn)進(jìn)行插值，我們可以得到一個(gè)更加平滑的軌跡，使得機(jī)器人在移動(dòng)時(shí)能夠更平穩(wěn)地過渡。
• 接著，我們基于這條中心軌跡，計(jì)算出左輪和右輪的軌跡。這是通過向中心軌跡的左側(cè)和右側(cè)分別偏移一定寬度來實(shí)現(xiàn)的。
  

1.2 目標(biāo)距離

• 對(duì)于中心軌跡以及左輪和右輪的軌跡，我們分別計(jì)算出每個(gè)點(diǎn)到終點(diǎn)的距離。這個(gè)距離是為了確保機(jī)器人在接近終點(diǎn)時(shí)能夠逐漸減速并停下來。
• 同時(shí)，我們還計(jì)算出每個(gè)點(diǎn)到其下一個(gè)點(diǎn)的距離。這個(gè)距離將用于后續(xù)的速度計(jì)算。

1.3 速度限制

• 對(duì)于軌跡上的每個(gè)點(diǎn)，我們都根據(jù)當(dāng)前速度、最大速度限制、最大加速度以及到下一個(gè)點(diǎn)和終點(diǎn)的距離，計(jì)算出三個(gè)速度限制：最大速度限制（max_v）、基于到下一個(gè)點(diǎn)距離的加速限制速度（max_v_acc）和基于到終點(diǎn)距離的減速限制速度（max_v_dec）。
• 注：相鄰兩個(gè)點(diǎn)之間計(jì)算max_v_acc時(shí)，以前一點(diǎn)的速度計(jì)算下一點(diǎn)能達(dá)到的最大速度 (v^2 = u^2 + 2ax)。
• 然后，我們選擇這三個(gè)速度限制中的最小值作為該點(diǎn)的最大允許速度。

1.4 候選速度的計(jì)算與調(diào)整

• 在計(jì)算出中心軌跡上每個(gè)點(diǎn)的最大允許速度后，我們得到一個(gè)候選速度。這個(gè)速度是基于中心軌跡的當(dāng)前速度、最大速度限制、最大加速度以及到下一個(gè)點(diǎn)和終點(diǎn)的距離來計(jì)算的。
• 接著，我們根據(jù)左輪和右輪軌跡上對(duì)應(yīng)點(diǎn)的距離比例，將候選速度分配到左輪和右輪上，得到左輪和右輪的候選速度。
• 如果左輪或右輪的候選速度超過了它們各自的最大允許速度，我們就需要對(duì)候選速度進(jìn)行調(diào)整。調(diào)整的原則是：確保左輪和右輪的速度都不會(huì)超過它們的最大允許速度，并且盡量保持中心軌跡的速度不變。
• 調(diào)整后的候選速度被用作當(dāng)前點(diǎn)的實(shí)際速度，并更新到結(jié)果路徑中。

1.5 路徑生成

• 經(jīng)過上述計(jì)算，我們得到了一個(gè)包含每個(gè)點(diǎn)速度和位置的結(jié)果路徑。這個(gè)路徑考慮了機(jī)器人的最大速度限制、最大加速度以及左輪和右輪的速度限制。
• 最后，我們?cè)诮Y(jié)果路徑的末尾添加了一個(gè)速度為0的點(diǎn)，表示機(jī)器人已經(jīng)完全停止。

二、雙輪軌跡

2.1 計(jì)算梯度

• 首先遍歷中心軌跡上的每個(gè)點(diǎn)（除了最后一個(gè)點(diǎn)），計(jì)算相鄰兩點(diǎn)之間的差值（即位移向量）。
• 使用atan2函數(shù)，根據(jù)位移向量的y分量和x分量計(jì)算出每段軌跡的傾斜角度（即梯度）。這個(gè)角度表示了軌跡在該點(diǎn)的切線方向。
• 為了確保最后一段軌跡也有一個(gè)梯度值，將最后一個(gè)計(jì)算出的梯度值復(fù)制到梯度數(shù)組的末尾。

2.2 計(jì)算偏移軌跡

• 接著創(chuàng)建一個(gè)新的路徑（即偏移軌跡），這個(gè)路徑將包含與中心軌跡相同數(shù)量的點(diǎn)。
• 對(duì)于中心軌跡上的每個(gè)點(diǎn)，根據(jù)該點(diǎn)的梯度值和給定的偏移量（offset），計(jì)算出偏移軌跡上對(duì)應(yīng)點(diǎn)的坐標(biāo)。這個(gè)計(jì)算過程考慮了是計(jì)算左輪軌跡還是右輪軌跡（通過left參數(shù)來控制）。
  • 如果是計(jì)算左輪軌跡，會(huì)給梯度值加上90度（即M_PI / 2.0），這樣計(jì)算出的偏移點(diǎn)就會(huì)位于中心點(diǎn)的左側(cè)。
  • 如果是計(jì)算右輪軌跡，會(huì)給梯度值減去90度（即-M_PI / 2.0），這樣計(jì)算出的偏移點(diǎn)就會(huì)位于中心點(diǎn)的右側(cè)。
• 使用三角函數(shù)（cos和sin）和偏移量，計(jì)算出偏移軌跡上每個(gè)點(diǎn)的x坐標(biāo)和y坐標(biāo)。
• 最后，將計(jì)算出的偏移點(diǎn)添加到偏移軌跡中。

2.3 返回結(jié)果

• 返回計(jì)算出的偏移軌跡，這個(gè)軌跡是一系列坐標(biāo)點(diǎn)構(gòu)成的路徑，表示了左輪或右輪的行駛軌跡。