DWA根据速度生成路径

最新推荐文章于 2024-07-10 16:47:58 发布

CAMEL__

最新推荐文章于 2024-07-10 16:47:58 发布

阅读量361

点赞数 6

文章标签：算法

本文链接：https://blog.csdn.net/Carzy_/article/details/135337986

版权

generateTrajectory 生成路径

输入变量: 当前坐标pos 当前速度 vel 采样出来的目标速度sample_target_vel

判断速度是否在 sample_target_vel在最大最小值区间内 [min_vel_trans, max_vel_trans]
计算仿真的步数，num_steps
1. 根据sim_time_和sample_target_vel计算出 sim_time_distance
2. num_steps = sim_time_distance / sim_granularity_(每一小段的距离值----颗粒)
得到每一个细分点的仿真时间 dt = sim_time_ / num_steps
获取得到轨迹
1. 将当前坐标点push到路径中
2. 根据当前坐标 pos, 速度loop_vel及两个点的时间 dt 计算新的点
3. 通过computeNewPositions计算得到新的点插入路径中

代码实现

computeNewPositions
  Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f &pos,
                                                                 const Eigen::Vector3f &vel, double dt)
  {
    Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
    new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
    new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
    new_pos[2] = pos[2] + vel[2] * dt;
    return new_pos;
  }

实现示意图

在这里插入图片描述

公式推导

将上图的vx vy映射到全局坐标系下，得到如下公式
$(v_x.cos(\theta)) + v_y.cos(\pi/2+ \theta)).dt \\ y' = y + (v_x.sin(\theta)) + v_y.sin(\pi/2+ \theta)).dt\\ \theta' = \theta + \omega.dt$