* 状态获取 * @return */

* 控制量为转向角delta_f和加速度a * @param accel 加速度 * @param delta_f 转向角控制量 */

* 阶乘实现 * @param n * @return */

* 角度归一化 * @param angle * @return */

* 基函数定义 * @param i * @param k B样条阶数k * @param u 自变量 * @param node_vector 节点向量 array([u0,u1,u2,...,u_n+k],shape=[1,n+k+1]. */

* 得到距离参考轨迹最近点的下标 * @param robot_state 机器人状态（x,y） * @param refer_path 参考路径 * @return 距离参考轨迹最近点的下标 */

* 计算跟踪误差 * @param robot_state 机器人状态 * @return */

* 计算参考轨迹上离当前状态最近的路点 * @param state * @param cx * @param cy * @param cyaw * @param pind * @return */

* compute segment p1p2's closed point to point p. * @param p1 * @param p2 * @param p * @return */

* 将模型离散化后的状态空间表达 * @param ref_delta 名义控制输入 * @param ref_yaw 名义偏航角 * @return */

* 贝塞尔公式 * @param Ps * @param t * @return */

* 得到障碍物信息图，有障碍物的地方标记为true，没有标记为false * @param ox 障碍物x坐标集合 * @param oy 障碍物y坐标集合 */

* 得到障碍物信息图，有障碍物的地方标记为true，没有标记为false * @param ox 障碍物x坐标集合 * @param oy 障碍物y坐标集合 */

* 计算控制输出 * @param state 当前状态量 * @return */

* 计算邻近路点 * @param robot_state 当前机器人位置 * @param refer_path 参考轨迹（数组） * @param l_d 前向距离 * @return */

* 参考轨迹 * @param state * @param cx * @param cy * @param cyaw * @param ck * @param sp * @param dl * @param target_ind * @param xref */

* 计算参考轨迹点，统一化变量数组，只针对MPC优化使用 * @param robot_state 车辆的状态(x,y,yaw,v) * @param param 超參數 * @param dl Defaults to 1.0. * @return {xref, dref, ind}结构体

* 滚动窗口算法控制器 * @param state 机器人当前状态--[x,y,yaw,v,w] * @param goal 目标点位置，[x,y] * @param obstacle 障碍物位置，dim:[num_ob,2] * @return 最优控制量[v,w]、最优轨迹 */

* 标记移动代价 * @return */

* 标记移动代价 * @return */

* 机器人运动学模型 * @param state 状态量---x,y,yaw,v,w * @param control 控制量---v,w,线速度和角速度 * @param dt 采样时间 * @return 下一步的状态 */

* LQR控制器 * @param robot_state * @param refer_path * @param s0 * @param A * @param B * @param Q * @param R * @return */

* 规划 * @param start 起点 * @param goal 终点 * @return 规划后的路径 */

* 规划 * @param start 起点 * @param goal 终点 * @return 规划后的路径 */

* rrt path planning,两边同时进行搜索 * @return 轨迹数据 */

* rrt path planning * @return 轨迹数据 */

* 画箭头 * @param x * @param y * @param yaw * @param len * @param width */

* purePursuitControl * @param robot_state 当前机器人位置 * @param current_ref_point 参考轨迹点 * @param l_d 前向距离 * @param psi 机器人航向角 * @param L 轴距 * @return

* 后轮位置反馈控制 * @param robot_state 机器人位姿，包括x,y,yaw,v * @param e 误差 * @param k 曲率 * @param ref_psi 参考轨迹上点的切线方向的角度 * @return */

* 人工势场法控制器 * @param robot_state * @return 车辆下一步位置 */

* stanley控制 * @param robot_state 机器人位姿，包括x,y,yaw,v * @param refer_path 参考轨迹的位置和参考轨迹上点的切线方向的角度 x,y,theta * @return 控制量+目标点索引 */

* 分段B样条 * 首末值定义为 0 和 1 * 分段Bezier曲线的节点向量计算，共n+1个控制顶点，k阶B样条，k-1次曲线 * 分段Bezier端节点重复度为k，内间节点重复度为k-1,且满足n/(k-1)为正整数 * @param n 控制点个数-1，控制点共n+1个 * @pa

* 准均匀B样条的节点向量计算 * 首末值定义为 0 和 1 * @param n 控制点个数-1，控制点共n+1个 * @param k B样条阶数k， k阶B样条，k-1次曲线. * @return */

* 车辆状态更新 * @param state * @param a 加速度 * @param delta 转角控制量 */

* 机器人运动学模型构造 * @param x 位置x * @param y 位置y * @param psi 偏航角 * @param v 速度 * @param l 轴距 * @param dt 采样时间 */