快速扩展随机树算法原理^[14]为递增式构造方法，可以分为两个阶段，第一阶段为树的构建，第二阶段为节点回溯阶段，也为路径查询阶段。给定机器人q一个初始状态q_init，及目标状态q_goal。RRT算法不断在搜索空间中生成随机采样点q_rand，如果其采样点处于C_space空间区域，在扩展随机树中遍历所有节点，找到距qrand随机节点最近节点，将其作为随机点的父节点qnear，从父节点向随机点扩展一个步长h，得到最新qnew采样点，进行碰撞检测，无障碍物是将最新采样点加入随机树中。否则进行重新采样。重复以上步骤，如果随机树中的子节点为目标点qgoal本身或者处于目标点的邻域内，即就认为树扩建完成。接下来为路径生成阶段，从目标点开始回溯，最终得到一条无碰撞路径。依次为随机树扩展示意图和节点回溯示意图，生成的路径节点q_goal-9~1-q_init。