* 构造函数，求解出参考轨迹点上的曲率等信息 */

* 角度对2pi取模，double类型 * @param angle * @return */

* 角度对2pi取模，double类型 * @param angle * @return */

* 构造函数 * @param Kpsi * @param K2 * @param L */

* 计算当前移动机器人距离障碍物最近的几何距离 * @param state 当前机器人状态 * @param obstacle 所有障碍物位置 * @return 移动机器人距离障碍物最近的几何距离 */

* 距离评价函数 * 表示当前速度下对应模拟轨迹与障碍物之间的最近距离； * 如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。 * @param trajectory 轨迹，dim:[n,5] * @param obstacle 障碍物位置，dim:[num_ob

* 方位角评价函数 * 评估在当前采样速度下产生的轨迹终点位置方向与目标点连线的夹角的误差 * @param trajectory 轨迹，dim:[n,5] * @param goal 目标点位置[x,y] * @return 方位角评价数值 */

* 速度评价函数 * 表示当前的速度大小，可以用模拟轨迹末端位置的线速度的大小来表示 * @param trajectory 轨迹，dim:[n,5] * @return 速度评价值 */

Select the backend NOTE:** This must be called before the first plot command to have any effect. Mainly useful to select the non-interactive 'Agg' b

* 计算加速度限制Vd * @return */

* 计算(x,y)离目标点的距离 * @param x * @param y * @return */

* 计算(x,y)离目标点的距离 * @param x * @param y * @return */

* 计算两个节点间的距离和方位角 * @param from_node * @param to_node * @return */

* 计算两个节点间的距离和方位角 * @param from_node * @param to_node * @return */

* 速度采样,得到速度空间窗口 * @param v 当前时刻线速度 * @param w 当前时刻角速度 * @param state 当前机器人状态 * @param obstacle 障碍物位置 * @return [v_low,v_high,w_low,w_high]: 最终采样后

* 计算路径，便于画图 * @param goal_node * @param closed_set * @return */

* 计算路径，便于画图 * @param goal_node * @param closed_set * @return */

* 启发函数计算，与dijkstra不同的地方，astar增加了对目标点的启发函数 * @param n1 节点1 * @param n2 节点2 * @return */

* 计算栅格索引 * @param node * @return */

* 计算栅格索引 * @param node * @return */

* 环境障碍物限制Va * @param state 当前机器人状态 * @param obstacle 障碍物位置 * @return 移动机器人不与周围障碍物发生碰撞的速度空间Va */

* 计算栅格在地图中的位置 * @param index * @param minp * @return */

* 计算栅格在地图中的位置 * @param index * @param minp * @return */

* 解代数里卡提方程 * @param A 状态矩阵A * @param B 状态矩阵B * @param Q Q为半正定的状态加权矩阵, 通常取为对角阵；Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零； * @param R R为正定的控制加权矩阵，R矩阵元素变大意味着希望控制输入能够尽

* 搜索目标邻近路点 * @param robot_state 当前机器人位置 * @param refer_path 参考轨迹（数组） * @return */

* 计算速度边界限制Vm * @return 速度边界限制后的速度空间Vm */

* 计算起点终点的栅格索引 * @param position * @param minp * @return */

* 计算起点终点的栅格索引 * @param position * @param minp * @return */

* 计算代价 * @param from_node * @param to_node * @return */

* 在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$， 作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选 我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上近邻节点 $X_

clears the plot but keep it available

* 计算引力斥力 * @param robot_state 车辆状态：x,y,v * @return 单位合力方向 */

* CCC * @param x * @param y * @param phi * @param paths * @param step_size * @return */

* CSC * @param x * @param y * @param phi * @param paths * @param step_size * @return */

* 画出搜索过程的图 * @param node */

* 画出搜索过程的图 * @param node */

* 计算离目标点的最佳索引 * @return */

* 计算周围一定半径内的所有邻近节点 * @param new_node * @return 所有邻近节点索引 */

* 生成路径 * @param goal_ind * @return */

* 生成路径 * @param goal_ind * @return */

* 计算最近的节点 * @param node_list 节点列表 * @param rnd_node 随机采样的节点 * @return 最近的节点索引 */

* 计算最近的节点 * @param node_list 节点列表 * @param rnd_node 随机采样的节点 * @return 最近的节点索引 */

* 判断是否在可行区域里面 * @param node * @return */

* 判断是否在可行区域里面 * @param node * @return */

* LRL * @param x * @param y * @param phi * @return */

* LSL * @param x * @param y * @param phi * @return */

* LSR * @param x * @param y * @param phi * @return */