Kinodynamic RRT-connect（Rapidly-exploring Random Tree-Connect）算法
是一种结合了快速扩展随机树（RRT）和动力学约束的路径规划算法。它特别适用于需要考虑动力学和运动学约束的复杂机器人系统，如人形机器人、无人驾驶汽车等。
主要原理
RRT基本原理：
- RRT算法通过在空间中随机采样点，并从现有树的节点向这些随机点扩展，逐步构建一棵覆盖工作空间的树。
- RRT-connect算法在两个方向上扩展树：一个从起点开始，一个从终点开始，并尝试连接这两棵树。
动力学约束（Kinodynamic Constraints）：
- 在考虑动力学约束时，机器人不仅需要在空间中找到一条可行路径，还需要满足速度、加速度等动力学约束。
- 这种情况下，路径的每一步必须符合机器人运动学和动力学模型。
算法步骤
1、初始化：
- 初始化两棵树：一棵从起点开始，另一棵从终点开始。
- 将起点和终点分别作为两棵树的根节点。
2、随机采样：
- 在工作空间中随机采样一个状态。
- 在考虑动力学约束时，这个状态不仅包含位置，还包含速度