【路径生成--考虑运动学的采样思想路径探索方法】运动学/动力学Kinodynamic RRT*算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，【防盗标记–盒子君hzj】后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对Kinodynamic RRT*算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、Kinodynamic-RRT*的目的

Kinodynamic-RRT*解决路径连接满足起点和终点的两个状态（位置、朝向、速度、加速度）最优控制的问题【如阿克曼模型不能直接横向平移，要纵向前进的时候才有横向平移】，【防盗标记–盒子君hzj】同时规划出来一条曲线路径可以绕开障碍物

二、Kinodynamic-RRT*基本思路

RRT类算法（启发方向） 【只是和hybird A*的路径探索部分不一样而已】

三、Kinodynamic-RRT*伪代码

核心步骤

---

（1）How to “Sample”
状态空间扩展，系统变量包含位置和速度

需要在整个状态空间里进行采样，因为考虑了动力学，因此不能仅仅采样位置p（x,y,z）,也要进行速度的采样v，进而得到【防盗标记–盒子君hzj】

---

（2）How to define “Near”

如果没有运动约束，可以使用欧几里得距离或曼哈顿距离。
而在有运动约束的状态空间中，引入最优控制。
如果引入最优控制，我们可以得到状态间转移的代价函数。
通常考虑能量与时间最优，如果从一种状态转移到另一种状态的成本很小，则两种状态是接近的。(注意，如果反向转移，成本可能不同)，如果我们知道到达时间(τ \tauτ)和控制策略u ( t ) u\left( t \right)u(t)的转移,我们可以计算成本。这些都是经典的最优控制解。

2）通过启发式公式寻找最近的节点“Near”,【防盗标记–盒子君hzj】再通过BVP和OBVP的方法连接两个节点

---

（3） How to “ChooseParent”

现在如果我们对一个随机状态进行抽样，我们就可以从树中的这些状态节点计算出控制策略和到抽样状态代价,选择一个以最小的成本和检查(x ( t ) x(t)x(t)]和(u ( t ) u(t)u(t)范围。
如果没有找到合格的父节点，则对另一个状态进行取样

---

（4）How to find near nodes efficiently

---

（5）How to “Rewire”

---

参考资料

Kinodynamic-RRT*论文参考

Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints

Kinodynamic-RRT*视频展示

https://www.bilibili.com/video/BV1H64y1q7YS?spm_id_from=333.999.0.0
https://www.youtube.com/watch?v=RB3g_GP0-dU

https://blog.csdn.net/qq_37087723/article/details/113846316