轨迹生成

WPILib contains classes that help generating trajectories. A trajectory is a smooth curve, with velocities and accelerations at each point along the curve, connecting two endpoints on the field. Generation and following of trajectories is incredibly useful for performing autonomous tasks. Instead of a simple autonomous routine – which involves moving forward, stopping, turning 90 degrees to the right, then moving forward – using trajectories allows for motion along a smooth curve. This has the advantage of speeding up autonomous routines, creating more time for other tasks; and when implemented well, makes autonomous navigation more accurate and precise.

本文介绍了如何生成轨迹。本系列的下几篇文章将介绍如何实际遵循生成的轨迹。在深入研究轨迹世界之前,您的机器人必须具备一些条件:

  • 一种测量机器人每一边的位置和速度的方法。编码器是实现这一点的最佳方式;然而,其他方法可能包括光流量传感器等。

  • 一种测量机器人底盘的角度或角速度的方法。陀螺仪是做到这一点的最好方法。虽然角速率可以计算使用编码器速度,但这种方法不推荐,因为车轮会刮磨。

If you are looking for a simpler way to perform autonomous navigation, see the section on driving to a distance.

样条函数

样条曲线是指在点之间插入的一组曲线。可以将其视为连接点,但曲线除外。 WPILib支持两种类型的样条:艾尔米特三次方和艾尔米特五次方。

  • 艾尔米特三次方:对于大多数用户,建议使用此选项。使用这些样条线生成轨迹涉及指定所有点的(x,y)坐标以及起点和终点的航向。内部航路点的航向会自动确定,以确保始终保持连续的曲率(航向的变化率)。

  • 艾尔米特五次方:这是一个更高级的选项,要求用户为所有航路点指定(x,y)坐标和航向。如果对艾尔米特三次方样条曲线生成的轨迹不满意,或者希望对内部点的航向进行更好的控制,则应使用此方法。

样条线用作生成轨迹的工具;但是,样条本身没有有关速度和加速度的任何信息。因此,不建议您直接使用样条线类。为了生成具有速度和加速度的平滑路径,必须生成轨迹。

创建轨迹配置

必须创建配置才能生成轨迹。该配置除了开始速度和结束速度外,还包含有关特殊约束,最大速度,最大加速度的信息。该配置还包含有关是否应该反转轨迹(机器人沿航路点向后移动)的信息。 应该使用“TrajectoryConfig”类来构造配置。此类的构造函数采用两个参数,即最大速度和最大加速度。创建对象时,其他字段(startVelocity, endVelocity, reversed, constraints) 默认为合理值(0, 0, false, {}) 。如果你想修改任何这些字段的值,则可以调用以下方法:

  • setStartVelocity(double startVelocityMetersPerSecond) (Java) / SetStartVelocity(units::meters_per_second_t startVelocity) (C++)

  • setEndVelocity(double endVelocityMetersPerSecond) (Java) / SetEndVelocity(units::meters_per_second_t endVelocity) (C++)

  • setReversed(boolean reversed) (Java) / SetReversed(bool reversed) (C++)

  • addConstraint(TrajectoryConstraint constraint) (Java) / AddConstraint(TrajectoryConstraint constraint) (C++)

备注

``reversed’’属性仅表示机器人是否向后移动。如果您指定了四个航路点a,b,c和d,则当“ reversed”标记设置为“ true”时,机器人仍将以相同的顺序通过航路点。这也意味着在提供航路点时必须考虑机器人的方向。例如,如果您的机器人正对着联盟站墙并向后移动到某个场地元素,则起始航路点应旋转180度。

生成轨迹

用于生成轨迹的方法是``generateTrajectory(…)``。此方法有四个重载。其中两个使用夹紧的三次样条曲线,另外两个使用五次样条曲线。对于每种类型的样条,都有两种构造轨迹的方法。最简单的方法是接受“ Pose2d”对象的重载。

对于钳制三次样条曲线,此方法接受两个“ Pose2d”对象,一个对象为起点,另一个为终点。该方法接受代表内部航路点的``Translation2d’’对象的向量。这些内部航路点的航向是自动确定的,以确保连续的曲率。对于五次样条曲线,该方法仅获取“ Pose2d”对象的列表,每个“ Pose2d”代表一个点并指向该字段。

更复杂的重载接受样条曲线的“控制向量”。在使用Pathweaver生成轨迹时可以使用此方法,在该轨迹中,您可以控制每个点处切线向量的大小。 ControlVector类由两个double数组组成。每个数组表示一个维度(x或y),其元素表示该点的导数。例如,``x’’数组的元素0处的值表示x坐标(第0导数),元素1处的值表示x维度上的一阶导数,依此类推。

使用钳制三次样条曲线时,数组的长度必须为2(0和1的导数),而使用五次样条曲线时,数组的长度应为3(0、1和2的导数)。除非您确切地知道自己在做什么,否则强烈建议您使用第一种更简单的方法来手动生成轨迹。 (即不使用Pathweaver JSON文件时)。

这是为2018年游戏FIRST Power Up使用艾尔米特的三次方生成轨迹的示例:

class ExampleTrajectory {
  public void generateTrajectory() {

    // 2018 cross scale auto waypoints.
    var sideStart = new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23),
        Rotation2d.fromDegrees(-180));
    var crossScale = new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8),
        Rotation2d.fromDegrees(-160));

    var interiorWaypoints = new ArrayList<Translation2d>();
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(14.54), Units.feetToMeters(23.23)));
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(21.04), Units.feetToMeters(18.23)));

    TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(12), Units.feetToMeters(12));
    config.setReversed(true);

    var trajectory = TrajectoryGenerator.generateTrajectory(
        sideStart,
        interiorWaypoints,
        crossScale,
        config);
  }
}
void GenerateTrajectory() {
  // 2018 cross scale auto waypoints
  const frc::Pose2d sideStart{1.54_ft, 23.23_ft, frc::Rotation2d(180_deg)};
  const frc::Pose2d crossScale{23.7_ft, 6.8_ft, frc::Rotation2d(-160_deg)};

  std::vector<frc::Translation2d> interiorWaypoints{
      frc::Translation2d{14.54_ft, 23.23_ft},
      frc::Translation2d{21.04_ft, 18.23_ft}};

  frc::TrajectoryConfig config{12_fps, 12_fps_sq};
  config.SetReversed(true);

  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
      sideStart, interiorWaypoints, crossScale, config);
}

备注

The Java code utilizes the Units utility, for easy unit conversions.

备注

Generating a typical trajectory takes about 10 ms to 25 ms. This isn’t long, but it’s still highly recommended to generate all trajectories on startup (robotInit).

Concatenating Trajectories

Trajectories in Java can be combined into a single trajectory using the concatenate(trajectory) function. C++ users can simply add (+) the two trajectories together.

警告

It is up to the user to ensure that the end of the initial and start of the appended trajectory match. It is also the user’s responsibility to ensure that the start and end velocities of their trajectories match.

var trajectoryOne =
TrajectoryGenerator.generateTrajectory(
   new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
   List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
   new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));

var trajectoryTwo =
TrajectoryGenerator.generateTrajectory(
   new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   List.of(new Translation2d(4, 4), new Translation2d(6, 3)),
   new Pose2d(6, 0, Rotation2d.fromDegrees(0)),
   new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));

var concatTraj = trajectoryOne.concatenate(trajectoryTwo);
auto trajectoryOne = frc::TrajectoryGenerator::GenerateTrajectory(
   frc::Pose2d(0_m, 0_m, 0_rad),
   {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
   frc::Pose2d(3_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));

auto trajectoryTwo = frc::TrajectoryGenerator::GenerateTrajectory(
   frc::Pose2d(3_m, 0_m, 0_rad),
   {frc::Translation2d(4_m, 4_m), frc::Translation2d(5_m, 3_m)},
   frc::Pose2d(6_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));

auto concatTraj = m_trajectoryOne + m_trajectoryTwo;