2.5.c Dubins曲线代码解析

2.5.c.1 Dubins曲线代码实现

plan_dubins_path 函数, 是Dubins曲线生成接口. 输入始末点状态和曲率:s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature , 输出一组曲线点的状态,组合方式和总长度:x_list, y_list, yaw_list, modes, lengths

_PATH_TYPE_MAP 中存储了Dubins曲线的6种组合方式, 之后先把全局坐标系下的始末点做了平移旋转, 转换到了以起始点为原点的坐标系下. 调用了_dubins_path_planning_from_origin 接口.

在_dubins_path_planning_from_origin 中, 计算了2.5.4中的几个参数:

之后遍历所有组合, 生成曲线. d1, d2, d3 是3个运动基元曲线的长度, mode是3个运动基元类型.

在所有曲线中, 挑选总长度最小的作为最佳曲线.

具体的运动基元组合planner , 以上一节推导的LSL为例.

d1, d2, d3对应, 对应是2.5.4提到的正则化, 因为R是1, 角度即是弧长.

挑出最佳曲线之后, 需要将坐标转回到全局坐标系. 也要将曲率半径重新乘回去, 收放到正则化之前坐标.

2.5.c.2 Dubins曲线代码测试

在main() 函数中, 生成了一组始末状态的随机数, 调用接口, 并可视化.

动手学控制理论

端到端-理论与实战视频课程