reeds_sheep运动规划算法Python源码分析

本文用于记录Python版本zhm-real / PathPlanning运动规划库中reeds_sheep算法的源码分析

关于reeds sheep算法的原理介绍前文已经介绍过了，链接如下所示：

《Reeds-Shepp曲线学习笔记及相关思考》

《Reeds-Shepp曲线基础运动公式推导过程》

在这里插入图片描述

正文：

1、导入相关库

import math
import numpy as np
import matplotlib.pyplot as plt

import draw   （非Python官方库）

2、参数初始化

STEP_SIZE = 0.2         #步长
MAX_LENGTH = 1000.0     #最大长度
PI = math.pi            #π

（1）STEP_SIZE = 0.2：步长设定为0.2

（2）MAX_LENGTH = 1000.0：最大长度限制设定为1000

（3）PI = math.pi：math.pi是Python中math模块提供的圆周率的近似值，约为3.141592653589793。

3、定义PATH类

# PATH 类
class PATH:def __init__(self, lengths, ctypes, L, x, y, yaw, directions):self.lengths = lengths  # 包含了路径各部分长度的列表 (+: forward, -: backward) [float]self.ctypes = ctypes  # 包含了路径各部分类型的列表 [string]self.L = L  # 整个路径的总长度 [float]self.x = x  # 表示路径最终位置的x坐标 [m]self.y = y  # 表示路径最终位置的y坐标 [m]self.yaw = yaw  # 表示路径最终位置的偏航角（偏转角） [rad]self.directions = directions  #包含了路径各部分方向的列表。列表中的每个元素都是一个整数，表示路径的每个部分的方向。正数表示正向前进，负数表示反向后退  forward: 1, backward:-1

这段代码定义了一个名为PATH的类，这个类表示一个路径对象，其中包含了描述路径的各种信息。类的构造函数__init__用于初始化类的实例对象。

该PATH类有七个属性：

（1）lengths: 这是一个包含了路径各部分长度的列表。列表中的每个元素都是一个浮点数，表示路径的每个部分的长度。列表中的正数表示路径向前延伸，负数表示路径向后延伸。例如，如果路径由一段长为2米的向前部分和一段长为1.5米的向后部分组成，那么lengths列表可能是[2.0, -1.5]。

（2）ctypes: 这是一个包含了路径各部分类型的列表。列表中的每个元素都是一个字符串，表示路径的每个部分的类型。路径可以由不同类型的部分组成，比如直线段、曲线段等。例如，如果路径由一段直线和一段圆弧组成，那么ctypes列表可能是[“直线”, “圆弧”]。

（3）L: 这是一个浮点数，表示整个路径的总长度。

（4）x: 这是一个浮点数，表示路径最终位置的x坐标。

（5）y: 这是一个浮点数，表示路径最终位置的y坐标。

（6）yaw: 这是一个浮点数，表示路径最终位置的偏航角（偏转角）。偏航角通常用弧度来表示，它描述了路径最终位置的朝向角度。

（7）directions: 这是一个包含了路径各部分方向的列表。列表中的每个元素都是一个整数，表示路径的每个部分的方向。正数表示正向前进，负数表示反向后退。例如，如果路径由一段向前的部分和一段向后的部分组成，那么directions列表可能是[1, -1]。

通过这个类，可以方便地创建和管理描述路径的对象，存储路径的各个属性，并在需要时进行相关操作和计算。

4、calc_optimal_path函数

def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)minL = paths[0].Lmini = 0for i in range(len(paths)):if paths[i].L <= minL:minL, mini = paths[i].L, ireturn paths[mini]

calc_optimal_path函数，用于计算最优路径。函数接受一系列参数，包括起始位置、起始偏航角、目标位置、目标偏航角、最大曲率和步长大小。函数的目标是找到一条最优的路径，使得该路径的总长度最小。

函数首先调用calc_all_paths函数来计算所有可能的路径。calc_all_paths函数返回一个路径对象列表，其中包含了所有可能的路径。

接下来，函数通过遍历路径对象列表来找到总长度最小的路径。它首先将第一个路径的总长度设为当前的最小长度，并将对应的索引设为0。然后，对于每个路径，它将比较路径的总长度与当前最小长度的大小。如果路径的总长度小于等于当前最小长度，则将路径的总长度更新为当前最小长度，并将对应的索引更新为当前路径的索引。

最后，函数返回总长度最小的路径对象。

总的来说，calc_optimal_path函数的作用是根据给定的起始点和目标点的位姿信息、最大曲率信息、步长信息，从reeds_sheep曲线中找到所有可能得路径，并从中选出总长度最小的路径，作为最优路径并返回。

5、calc_all_paths函数

def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):q0 = [sx, sy, syaw]q1 = [gx, gy, gyaw]paths = generate_path(q0, q1, maxc)for path in paths:x, y, yaw, directions = \generate_local_course(path.L, path.lengths,path.ctypes, maxc, step_size * maxc)# convert global coordinatepath.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]path.directions = directionspath.lengths = [l / maxc for l in path.lengths]path.L = path.L / maxcreturn paths

calc_all_paths函数，用于计算所有可能的路径。该函数的输入参数包括起始位置、起始偏航角、目标位置、目标偏航角、最大曲率和步长大小（默认为STEP_SIZE）。

在该函数中，首先将起始位置和目标位置以及其对应的偏航角保存在变量q0和q1中。然后，函数调用generate_path函数生成路径。generate_path函数接受起始位置、目标位置和最大曲率作为参数，并返回一个路径对象列表，其中包含了所有可能的路径。

接下来，函数遍历路径对象列表，对于每个路径，它调用generate_local_course函数生成局部路径。generate_local_course函数接受路径的总长度、每个部分路径的长度、每个部分路径的类型、最大曲率和步长作为参数，并返回局部路径的x坐标、y坐标、偏航角和方向信息。

然后，函数将局部路径转换为全局坐标系。它使用起始位置的偏航角将局部路径的x和y坐标进行旋转和平移，从而得到全局路径的x和y坐标。它还使用起始位置的偏航角将局部路径的偏航角进行旋转，得到全局路径的偏航角。

最后，函数将路径对象的属性进行更新，包括x坐标、y坐标、偏航角、方向信息、部分路径长度和总路径长度。其中，每段路径的长度和总长度被归一化为最大曲率，以便进行比较和分析。

最终，函数返回包含所有可能路径的列表。

总的来说，calc_all_paths函数的作用是生成起点到终点之间的所有可能路径，并将这些路径转换为全局坐标系下的路径。

6、set_path函数

def set_path(paths, lengths, ctypes):path = PATH([], [], 0.0, [], [], [], [])path.ctypes = ctypespath.lengths = lengths# check same path existfor path_e in paths:if path_e.ctypes == path.ctypes:if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:return paths  # not insert pathpath.L = sum([abs(i) for i in lengths])if path.L >= MAX_LENGTH:return pathsassert path.L >= 0.01paths.append(path)return paths

set_path函数用于将一个路径添加到路径列表中。该函数接受三个参数：paths表示路径列表，lengths表示路径的长度列表，ctypes表示路径的类型列表。

首先，函数创建一个空的PATH对象，并将路径的类型和长度分配给该对象的ctypes和lengths属性。

然后，函数遍历已有的路径列表paths，检查是否存在相同类型的路径。如果找到一个类型相同且长度相差不大的路径，即两者长度之差小于等于0.01，则直接返回原始路径列表，不插入新路径。

接下来，函数计算路径的总长度，并将其赋值给路径对象的L属性。如果路径的总长度超过了最大长度（MAX_LENGTH），则直接返回原始路径列表，不插入新路径。

最后，函数对路径的总长度进行断言，确保其大于等于0.01。然后，将路径对象添加到路径列表中，并返回更新后的路径列表。

总的来说，set_path函数的作用是将一个新的路径添加到路径列表中，并进行一些简单的验证和检查。

7、48种reeds_sheep曲线计算函数

在这里插入图片描述

def LSL(x, y, phi):u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))if t >= 0.0:v = M(phi - t)if v >= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def LSR(x, y, phi):u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))u1 = u1 ** 2if u1 >= 4.0:u = math.sqrt(u1 - 4.0)theta = math.atan2(2.0, u)t = M(t1 + theta)v = M(t - phi)if t >= 0.0 and v >= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def LRL(x, y, phi):u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))if u1 <= 4.0:u = -2.0 * math.asin(0.25 * u1)t = M(t1 + 0.5 * u + PI)v = M(phi - t + u)if t >= 0.0 and u <= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def SCS(x, y, phi, paths):flag, t, u, v = SLS(x, y, phi)if flag:paths = set_path(paths, [t, u, v], ["S", "L", "S"])flag, t, u, v = SLS(x, -y, -phi)if flag:paths = set_path(paths, [t, u, v], ["S", "R", "S"])return pathsdef SLS(x, y, phi):phi = M(phi)if y > 0.0 and 0.0 < phi < PI * 0.99:xd = -y / math.tan(phi) + xt = xd - math.tan(phi / 2.0)u = phiv = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)return True, t, u, velif y < 0.0 and 0.0 < phi < PI * 0.99:xd = -y / math.tan(phi) + xt = xd - math.tan(phi / 2.0)u = phiv = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)return True, t, u, vreturn False, 0.0, 0.0, 0.0def CSC(x, y, phi, paths):flag, t, u, v = LSL(x, y, phi)if flag:paths = set_path(paths, [t, u, v], ["L", "S", "L"])flag, t, u, v = LSL(-x, y, -phi)if flag:paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])flag, t, u, v = LSL(x, -y, -phi)if flag:paths = set_path(paths, [t, u, v], ["R", "S", "R"])flag, t, u, v = LSL(-x, -y, phi)if flag:paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])flag, t, u, v = LSR(x, y, phi)if flag:paths = set_path(paths, [t, u, v], ["L", "S", "R"])flag, t, u, v = LSR(-x, y, -phi)if flag:paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])flag, t, u, v = LSR(x, -y, -phi)if flag:paths = set_path(paths, [t, u, v], ["R", "S", "L"])flag, t, u, v = LSR(-x, -y, phi)if flag:paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])return pathsdef CCC(x, y, phi, paths):flag, t, u, v = LRL(x, y, phi)if flag:paths = set_path(paths, [t, u, v], ["L", "R", "L"])flag, t, u, v = LRL(-x, y, -phi)if flag:paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])flag, t, u, v = LRL(x, -y, -phi)if flag:paths = set_path(paths, [t, u, v], ["R", "L", "R"])flag, t, u, v = LRL(-x, -y, phi)if flag:paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])# backwardsxb = x * math.cos(phi) + y * math.sin(phi)yb = x * math.sin(phi) - y * math.cos(phi)flag, t, u, v = LRL(xb, yb, phi)if flag:paths = set_path(paths, [v, u, t], ["L", "R", "L"])flag, t, u, v = LRL(-xb, yb, -phi)if flag:paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])flag, t, u, v = LRL(xb, -yb, -phi)if flag:paths = set_path(paths, [v, u, t], ["R", "L", "R"])flag, t, u, v = LRL(-xb, -yb, phi)if flag:paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])return pathsdef calc_tauOmega(u, v, xi, eta, phi):delta = M(u - v)A = math.sin(u) - math.sin(delta)B = math.cos(u) - math.cos(delta) - 1.0t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0if t2 < 0:tau = M(t1 + PI)else:tau = M(t1)omega = M(tau - u + v - phi)return tau, omegadef LRLRn(x, y, phi):xi = x + math.sin(phi)eta = y - 1.0 - math.cos(phi)rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))if rho <= 1.0:u = math.acos(rho)t, v = calc_tauOmega(u, -u, xi, eta, phi)if t >= 0.0 and v <= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def LRLRp(x, y, phi):xi = x + math.sin(phi)eta = y - 1.0 - math.cos(phi)rho = (20.0 - xi * xi - eta * eta) / 16.0if 0.0 <= rho <= 1.0:u = -math.acos(rho)if u >= -0.5 * PI:t, v = calc_tauOmega(u, u, xi, eta, phi)if t >= 0.0 and v >= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def CCCC(x, y, phi, paths):flag, t, u, v = LRLRn(x, y, phi)if flag:paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"])flag, t, u, v = LRLRn(-x, y, -phi)if flag:paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"])flag, t, u, v = LRLRn(x, -y, -phi)if flag:paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"])flag, t, u, v = LRLRn(-x, -y, phi)if flag:paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"])flag, t, u, v = LRLRp(x, y, phi)if flag:paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"])flag, t, u, v = LRLRp(-x, y, -phi)if flag:paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"])flag, t, u, v = LRLRp(x, -y, -phi)if flag:paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"])flag, t, u, v = LRLRp(-x, -y, phi)if flag:paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"])return pathsdef LRSR(x, y, phi):xi = x + math.sin(phi)eta = y - 1.0 - math.cos(phi)rho, theta = R(-eta, xi)if rho >= 2.0:t = thetau = 2.0 - rhov = M(t + 0.5 * PI - phi)if t >= 0.0 and u <= 0.0 and v <= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def LRSL(x, y, phi):xi = x - math.sin(phi)eta = y - 1.0 + math.cos(phi)rho, theta = R(xi, eta)if rho >= 2.0:r = math.sqrt(rho * rho - 4.0)u = 2.0 - rt = M(theta + math.atan2(r, -2.0))v = M(phi - 0.5 * PI - t)if t >= 0.0 and u <= 0.0 and v <= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def CCSC(x, y, phi, paths):flag, t, u, v = LRSL(x, y, phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"])flag, t, u, v = LRSL(-x, y, -phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"])flag, t, u, v = LRSL(x, -y, -phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"])flag, t, u, v = LRSL(-x, -y, phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"])flag, t, u, v = LRSR(x, y, phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"])flag, t, u, v = LRSR(-x, y, -phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"])flag, t, u, v = LRSR(x, -y, -phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"])flag, t, u, v = LRSR(-x, -y, phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"])# backwardsxb = x * math.cos(phi) + y * math.sin(phi)yb = x * math.sin(phi) - y * math.cos(phi)flag, t, u, v = LRSL(xb, yb, phi)if flag:paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"])flag, t, u, v = LRSL(-xb, yb, -phi)if flag:paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"])flag, t, u, v = LRSL(xb, -yb, -phi)if flag:paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"])flag, t, u, v = LRSL(-xb, -yb, phi)if flag:paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"])flag, t, u, v = LRSR(xb, yb, phi)if flag:paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"])flag, t, u, v = LRSR(-xb, yb, -phi)if flag:paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"])flag, t, u, v = LRSR(xb, -yb, -phi)if flag:paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"])flag, t, u, v = LRSR(-xb, -yb, phi)if flag:paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"])return pathsdef LRSLR(x, y, phi):# formula 8.11 *** TYPO IN PAPER ***xi = x + math.sin(phi)eta = y - 1.0 - math.cos(phi)rho, theta = R(xi, eta)if rho >= 2.0:u = 4.0 - math.sqrt(rho * rho - 4.0)if u <= 0.0:t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))v = M(t - phi)if t >= 0.0 and v >= 0.0:return True, t, u, vreturn False, 0.0, 0.0, 0.0def CCSCC(x, y, phi, paths):flag, t, u, v = LRSLR(x, y, phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"])flag, t, u, v = LRSLR(-x, y, -phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"])flag, t, u, v = LRSLR(x, -y, -phi)if flag:paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"])flag, t, u, v = LRSLR(-x, -y, phi)if flag:paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"])return paths

8、generate_local_cours函数

def generate_local_course(L, lengths, mode, maxc, step_size):point_num = int(L / step_size) + len(lengths) + 3px = [0.0 for _ in range(point_num)]py = [0.0 for _ in range(point_num)]pyaw = [0.0 for _ in range(point_num)]directions = [0 for _ in range(point_num)]ind = 1if lengths[0] > 0.0:directions[0] = 1else:directions[0] = -1if lengths[0] > 0.0:d = step_sizeelse:d = -step_sizepd = dll = 0.0for m, l, i in zip(mode, lengths, range(len(mode))):if l > 0.0:d = step_sizeelse:d = -step_sizeox, oy, oyaw = px[ind], py[ind], pyaw[ind]ind -= 1if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:pd = -d - llelse:pd = d - llwhile abs(pd) <= abs(l):ind += 1px, py, pyaw, directions = \interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)pd += dll = l - pd - d  # calc remain lengthind += 1px, py, pyaw, directions = \interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)# remove unused datawhile px[-1] == 0.0:px.pop()py.pop()pyaw.pop()directions.pop()return px, py, pyaw, directions

generate_local_cours函数是用于生成局部路径的。输入参数包括：:总路径的长度L、包含每个局部路径的长度的列表lengths、包含每个局部路径模式的列表mode、控制曲线的最大曲率maxc、生成路径时进行插值时的步长大小step_size。

该函数的主要步骤如下：

（1）根据总路径长度和步长大小计算局部路径的点数（point_num）。

（2）创建空列表 px、py、pyaw 和 directions，用于存储路径点的 x 坐标、y 坐标、偏航角和方向信息。

（3）初始化变量 ind 为 1，directions[0] 为起始路径的方向（正向或反向）。

（4）根据起始路径的长度，确定路径方向（正向为正步长，反向为负步长）。

（5）进入循环，依次处理每个局部路径：

（5.1）计算局部路径的步长方向 d。

（5.2）获取前一个路径点的坐标和偏航角作为起点。

（5.3）根据前一个路径点的索引，以及前一个路径长度和当前路径长度的符号关系，计算当前路径段的步长方向 pd。

（5.4）在当前路径段的长度范围内，以步长 d 进

（5.5）行插值，生成路径点，并更新路径信息。

（5.6）计算剩余长度 ll。

（5.7）在当前路径段的末尾，以当前路径长度进行插值，生成路径点，并更新路径信息。

（6）移除未使用的数据，即末尾多余的零值点。

（7）返回生成的路径点列表 px、py、pyaw 和 directions。

总的来说，generate_local_cours函数的作用是根据给定的局部路径长度和模式，生成一系列插值点，用于描述机器人在整个路径上的运动轨迹。路径点包括 x 和 y 坐标、偏航角和方向信息，可以在后续的路径规划和控制中使用。

9、interpolate函数

def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):if m == "S":px[ind] = ox + l / maxc * math.cos(oyaw)py[ind] = oy + l / maxc * math.sin(oyaw)pyaw[ind] = oyawelse:ldx = math.sin(l) / maxcif m == "L":ldy = (1.0 - math.cos(l)) / maxcelif m == "R":ldy = (1.0 - math.cos(l)) / (-maxc)gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldygdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldypx[ind] = ox + gdxpy[ind] = oy + gdyif m == "L":pyaw[ind] = oyaw + lelif m == "R":pyaw[ind] = oyaw - lif l > 0.0:directions[ind] = 1else:directions[ind] = -1return px, py, pyaw, directions

这个程序定义了一个插值函数 interpolate，它根据给定的参数生成路径点。函数接受以下参数：:要生成的路径点的索引ind、路径段的长度l、路径段的模式m（可以是 “S”、“L” 或 “R”）控制曲线的最大曲率maxc、前一个路径点的坐标和偏航角ox, oy, oyaw:、存储路径点的列表px, py, pyaw、存储路径方向的列表directions。

函数的主要逻辑如下：

首先，根据路径段的模式，分为直线段（“S”）和曲线段（“L”、“R”）两种情况进行处理。如果路径段是直线段，则计算直线段上每个点的 x 和 y 坐标，基于前一个路径点的坐标、偏航角和路径长度。更新路径点列表 px、py 和 pyaw。

如果路径段是曲线段，则根据曲线段的模式和路径长度，计算曲线段在 x 和 y 方向上的增量，将增量根据前一个路径点的偏航角进行旋转和平移，计算得到当前路径点的坐标。更新路径点列表 px 和 py。

然后，根据路径段的模式，计算当前路径点的偏航角，并更新路径点列表 pyaw。之后，根据路径段的长度，确定路径方向（正向或反向），并更新路径方向列表 directions。

最后，返回更新后的路径点列表 px、py、pyaw 和 directions。

总的来说，interpolate函数的作用是根据给定的路径段信息，在前一个路径点的基础上计算出当前路径点的坐标、偏航角和方向，并更新路径信息。这些路径点可以用于描述机器人在路径上的运动轨迹。

10、generate_path函数

def generate_path(q0, q1, maxc):dx = q1[0] - q0[0]dy = q1[1] - q0[1]dth = q1[2] - q0[2]c = math.cos(q0[2])s = math.sin(q0[2])x = (c * dx + s * dy) * maxcy = (-s * dx + c * dy) * maxcpaths = []paths = SCS(x, y, dth, paths)paths = CSC(x, y, dth, paths)paths = CCC(x, y, dth, paths)paths = CCCC(x, y, dth, paths)paths = CCSC(x, y, dth, paths)paths = CCSCC(x, y, dth, paths)return paths

这个程序定义了一个函数 generate_path，用于生成从起始状态到目标状态的路径。函数接受以下参数：

q0: 起始状态，包含 x、y 和偏航角（以弧度表示）的列表。

q1: 目标状态，与 q0 结构相同。

maxc: 控制曲线的最大曲率。

函数的主要逻辑如下：

首先，计算 x、y 和偏航角的差值 dx、dy 和 dth。根据起始状态的偏航角，计算出一个旋转矩阵的元素 c 和 s。根据旋转矩阵将差值 dx 和 dy 进行旋转和缩放，得到 x 和 y。

然后，创建一个空的路径列表 paths。将 x、y 和 dth 分别传递给一系列路径生成函数（SCS、CSC、CCC、CCCC、CCSC 和 CCSCC），并在每次调用后更新 paths。

最后，返回生成的路径列表 paths。

总的来说，generate_path函数的作用是根据起始状态和目标状态之间的差异，利用不同的路径生成函数生成一系列可能的路径。这些路径描述了从起始状态到目标状态的运动轨迹，可以用于路径规划和机器人运动控制。

11、pi_2_pi函数

def pi_2_pi(theta):while theta > PI:theta -= 2.0 * PIwhile theta < -PI:theta += 2.0 * PIreturn theta

这个程序定义了一个函数 pi_2_pi，用于将给定的角度值 theta 转换到区间 [-π, π] 内。函数的实现逻辑如下：

进入一个循环，只要 theta 大于 π，就执行下面的操作。循环的目的是将角度值转换为 [-π, π] 范围内的值。在每次迭代中，从 theta 中减去 2π，直到 theta 小于或等于 π。

当 theta 小于 -π 时，执行另一个循环。循环的目的是将角度值转换为 [-π, π] 范围内的值。在每次迭代中，将 theta 加上 2π，直到 theta 大于或等于 -π。

最后，返回转换后的角度值 theta。

总的来说，pi_2_pi函数的作用是将任意角度值映射到一个等价的角度值，使其处于 [-π, π] 范围内。这在许多应用中很有用，例如在机器人控制中，确保角度的连续性和一致性。

12、R函数

def R(x, y):"""Return the polar coordinates (r, theta) of the point (s, y)"""r = math.hypot(x, y)theta = math.atan2(y, x)return r, theta

这个程序定义了一个函数 R，用于计算点 (x, y) 的极坐标 (r, theta)。函数的实现逻辑如下：

首先，使用 math.hypot(x, y) 函数计算点 (x, y) 到原点的距离 r。math.hypot 函数返回两个参数的欧几里得范数，即 (x, y) 的平方和的平方根。然后，使用 math.atan2(y, x) 函数计算点 (x, y) 的极角 theta。math.atan2 函数返回 (y, x) 的反正切值，以弧度表示。最后，返回计算得到的极坐标 (r, theta)。

总的来说，这个R函数可以用于将直角坐标系下的点 (x, y) 转换为极坐标系下的 (r, theta) 表示。极坐标可以更方便地描述点的位置和方向，特别适用于需要考虑距离和角度的问题，如机器人路径规划、图像处理等领域。

13、M函数

def M(theta):"""Regulate theta to -pi <= theta < pi"""phi = theta % (2.0 * PI)if phi < -PI:phi += 2.0 * PIif phi > PI:phi -= 2.0 * PIreturn phi

这个程序定义了一个函数 M，用于将给定角度 theta 调整到 -pi <= theta < pi 的范围内。函数的实现逻辑如下：

首先，使用取模运算 % 将输入角度 theta 转换为在 0 到 2*pi 之间的角度 phi。取模运算是将一个数除以另一个数得到的余数。

然后进行判断，如果 phi 小于 -pi，则将其调整增加 2pi，使其落在 [-pi, pi) 的范围内。如果 phi 大于等于 pi，则将其调整减去 2pi，使其落在 [-pi, pi) 的范围内。

最后，返回调整后的角度 phi。

总的来说，这个M函数的作用是规范化角度，确保其在 -pi 到 pi 之间。这在很多计算和编程场景中非常有用，因为在这个范围内，角度的表示是唯一的，且方便进行数值计算和比较。

14、get_label函数

def get_label(path):label = ""for m, l in zip(path.ctypes, path.lengths):label = label + mif l > 0.0:label = label + "+"else:label = label + "-"return label

这个程序定义了一个函数 get_label，用于根据给定路径 path 生成一个标签。路径是由一系列运动模式（m）和对应的长度（l）组成的。函数的实现逻辑如下：

首先，定义一个空字符串 label，用于存储生成的标签。然后，使用 zip 函数将运动模式和长度两个列表进行逐个配对，依次遍历路径中的每个运动模式和长度。对于每个配对的运动模式和长度，将运动模式添加到 label 字符串中。

然后进行判断，如果长度 l 大于 0.0，则在运动模式后面添加一个正号 +，否则添加一个负号 -。最后，遍历完所有配对后，返回生成的标签 label。

总的来说，get_label函数的作用是根据路径中的运动模式和长度生成一个可读性较高的标签，用于描述路径的运动方式和方向。

15、calc_curvature函数

def calc_curvature(x, y, yaw, directions):c, ds = [], []for i in range(1, len(x) - 1):dxn = x[i] - x[i - 1]dxp = x[i + 1] - x[i]dyn = y[i] - y[i - 1]dyp = y[i + 1] - y[i]dn = math.hypot(dxn, dyn)dp = math.hypot(dxp, dyp)dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)d = (dn + dp) / 2.0if np.isnan(curvature):curvature = 0.0if directions[i] <= 0.0:curvature = -curvatureif len(c) == 0:ds.append(d)c.append(curvature)ds.append(d)c.append(curvature)ds.append(ds[-1])c.append(c[-1])return c, ds

这个程序定义了一个函数 calc_curvature，用于计算路径的曲率。函数接受四个参数：x、y、yaw和directions，分别表示路径的横坐标、纵坐标、偏航角和方向。

函数的实现逻辑如下：

首先，定义两个空列表 c 和 ds，用于存储计算得到的曲率和路径段长度。然后，使用 range 函数从索引1开始，遍历路径中的每个点，到倒数第2个点为止。

对于每个点，计算该点与前后相邻点的坐标差值，并计算出前后两个向量的模长。根据差值和模长计算出局部斜率和曲率，并考虑曲率的正负。如果曲率为非数值（NaN），将其设置为0.0。

如果路径段的方向 directions[i] 小于等于0.0，则将曲率取反。如果 c 列表为空（即第一个点），则将当前路径段长度 d 添加到 ds 列表中，并将当前曲率 curvature 添加到 c 列表中。

对于其他点，将路径段长度 d 添加到 ds 列表中，并将曲率 curvature 添加到 c 列表中。将最后一个路径段的长度和曲率添加到 ds 和 c 列表的末尾，使其长度与输入的 x 和 y 列表相同。

最后，返回计算得到的曲率列表 c 和路径段长度列表 ds。

总的来说，calc_curvature函数的目的是根据路径的坐标和方向信息计算路径上各点的曲率，并返回曲率和路径段长度的列表。曲率描述了路径弯曲的程度，可以用于路径规划和控制。

16、check_path函数

def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)assert len(paths) >= 1for path in paths:assert abs(path.x[0] - sx) <= 0.01assert abs(path.y[0] - sy) <= 0.01assert abs(path.yaw[0] - syaw) <= 0.01assert abs(path.x[-1] - gx) <= 0.01assert abs(path.y[-1] - gy) <= 0.01assert abs(path.yaw[-1] - gyaw) <= 0.01# course distance checkd = [math.hypot(dx, dy)for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),np.diff(path.y[0:len(path.y) - 1]))]for i in range(len(d)):assert abs(d[i] - STEP_SIZE) <= 0.001

这个程序定义了一个函数 check_path，用于检查计算得到的路径是否满足一些约束条件。

函数接受七个参数：起点坐标 sx 和 sy、起点偏航角 syaw、终点坐标 gx 和 gy、终点偏航角 gyaw，以及最大曲率 maxc。

函数的实现逻辑如下：

首先，通过调用 calc_all_paths 函数计算从起点到终点的所有可能路径，并将这些路径存储在列表 paths 中。并使用 assert 语句确保计算得到的路径列表 paths 不为空（长度至少为1），如果为空，则会触发异常。

然后，对于每个路径 path，使用 assert 语句确保起点和终点的坐标、偏航角与输入的起点 sx、sy、syaw 以及终点 gx、gy、gyaw 的坐标、偏航角相差不超过0.01。如果超过了这个阈值，也会触发异常。

接下来，检查路径上相邻两点之间的距离是否接近预设的步长 STEP_SIZE（这个步长的值未在提供的代码片段中给出，可能在其他地方定义）。对于每个路径，计算路径上相邻两点的距离 d 并与 STEP_SIZE 比较，如果距离与 STEP_SIZE 的差值超过0.001，则会触发异常。

总的来说，check_path函数的作用是在路径规划过程中验证计算得到的路径是否满足一些基本的约束条件，包括起点、终点和路径长度的检查。如果有任何条件不满足，该函数将会触发异常，提示路径计算存在问题。

17、主函数

def main():states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),(35, 10, 180), (32, -10, 180), (5, -12, 90)]max_c = 0.1  # max curvaturepath_x, path_y, yaw = [], [], []for i in range(len(states) - 1):s_x = states[i][0]s_y = states[i][1]s_yaw = np.deg2rad(states[i][2])g_x = states[i + 1][0]g_y = states[i + 1][1]g_yaw = np.deg2rad(states[i + 1][2])path_i = calc_optimal_path(s_x, s_y, s_yaw,g_x, g_y, g_yaw, max_c)path_x += path_i.xpath_y += path_i.yyaw += path_i.yaw# animationplt.ion()plt.figure(1)for i in range(len(path_x)):plt.clf()plt.plot(path_x, path_y, linewidth=1, color='gray')for x, y, theta in states:draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)plt.axis("equal")plt.title("Simulation of Reeds-Shepp Curves")plt.axis([-5, 15,-25, -10])plt.draw()plt.pause(0.001)plt.pause(1)if __name__ == '__main__':main()

终于到主函数了，主函数中执行路径规划的示例并进行动画展示。

首先。定义了一个状态列表 states，其中包含了一系列起点和终点的坐标以及偏航角。每个状态由三个元素组成，分别表示 x 坐标、y 坐标和偏航角（以度为单位）。

接下来定义了最大曲率 max_c，用于路径规划过程中限制曲率的最大值。

然后，通过循环遍历状态列表，对相邻的两个状态进行路径规划。对于每对相邻状态，提取起点的 x、y 坐标和偏航角，以及终点的 x、y 坐标和偏航角，并调用 calc_optimal_path 函数计算最优路径。并将每个计算得到的路径的 x、y 坐标和偏航角分别添加到 path_x、path_y 和 yaw 列表中。

接下来，使用 plt.ion() 开启交互模式，并创建一个图形窗口。通过循环遍历 path_x 列表中的每个元素，在每个时间步中，清除图形窗口的内容，绘制路径、起点和终点的箭头以及当前时间步的车辆位置。然后设置图形窗口的坐标轴范围、标题等，并调用 plt.draw() 和 plt.pause() 函数进行动画展示。

最后，通过调用 plt.pause(1) 在动画结束后暂停一秒钟。

总的来说，主函数中主要执行路径规划的示例，并通过动画展示路径规划的结果。

在这里插入图片描述