欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

自治智能体转向力之路径跟随算法

程序员文章站 2022-04-27 13:22:13
...

Craig Reynolds的路径跟随算法：
假设一小车在某处运动，则若小车以恒定速度继续运动，则可以预测过一段时间，小车所处的位置，可以通过这个位置计算小车与道路之间的距离，若距离足够近，则说明小车正在沿路径运动，如果距离太远，则表明小车离路径太远，则需要转向。

单段路径跟随：

1.计算小车未来位置

    // 首先创建速度向量的副本
    PVector predict = velocity.get(); 
    
    // 单位化向量，并将向量向前延申25像素
    predict.normalize(); 
    predict.mult(25);
    
    // 将向量加上当前位置，计算未来的位置
    PVector predictpos = PVector.add(position,predict);
  1. 计算法线交点
  // 点p在线段ab上的法线交点位置计算
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // 向量ap
    PVector ap = PVector.sub(p, a);
    // 向量ab
    PVector ab = PVector.sub(b, a);
    // 向量ab归一化
    ab.normalize(); 
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a, ab);
    return normalPoint;
  }
  1. 计算点p到线段ab的距离
    /*  ---- 3. 计算小车与法线交点的距离 ----  */
    float distance = PVector.dist(predictpos, normalPoint);
  1. 若p到线段ab的距离大于某值，则转向
  /*  ---- 4. 当预测距离 > 能接受的最大距离，则作用转向力 ----  */
    if(distance > path.radius)
    { 
       // 计算道路的方向
      PVector dir = PVector.sub(b, a);
      dir.normalize();
      dir.mult(10);  // This could be based on velocity instead of just an arbitrary 10 pixels
      PVector target = PVector.add(normalPoint, dir);
 
      seek(target);
    }

多路径跟随

为了寻找目标位置，我们必须找到线段上的法线交点。
交点的选择要满足两个条件：

  1. 这个交点必须在路径内
    若不再路径内，则选择终点作为法线交点，并将路径设置为下一路径
  2. 这个点为距小车最近的法线交点
    自治智能体转向力之路径跟随算法 
// 路径跟随算法
  PVector follow(Path path)
  {
    /*  ---- 1. 预计小车的未来位置 ----  */
    // 首先创建速度向量的副本
    PVector predict = velocity.get(); 
    
    // 单位化向量，并将向量向前延申25像素
    predict.normalize(); 
    predict.mult(25);
    
    // 将向量加上当前位置，计算未来的位置
    PVector predictpos = PVector.add(position,predict);
    
    // 根据预测位置找到法线交点，并选择在路径内且离小车最近的法线交点
                 
    PVector normal = null;  // 最终选中的法线交点
    PVector target = null;  // 最终的预测位置
    float worldRecord = 1000000; // 目前最小距离
    
    // 遍历路径
    for(int i = 0; i < path.points.size();i++)
    {
      PVector a = path.points.get(i);  // 起始点
      PVector b = path.points.get((i+1)%path.points.size()); // 结束点，闭环路径
      
      // 获取法线交点
      PVector normalPoint = getNormalPoint(predictpos, a, b);
      
      // 检查法线交点是否在线段内
      if((normalPoint.x < min(a.x,b.x))|| (normalPoint.x > max(a.x,b.x)) ||
         (normalPoint.y < min(a.y,b.y))|| (normalPoint.y > max(a.y,b.y)))
      {
        // 将结束点视为法线交点
        normalPoint = b.get();
        
        // 则走下一条路径
        a = path.points.get((i+1)%path.points.size()); // 起始点
        b = path.points.get((i+2)%path.points.size()); // 结束点    
      }
          
        // 路径方向
        PVector dir = PVector.sub(b,a);
        
        // 小车距法线交点的距离
        float distance = PVector.dist(predictpos,normalPoint);
        // 若距离小于最小距离
        if(distance < worldRecord)
        {
          worldRecord = distance;
          normal = normalPoint;
          
          dir.normalize();
          dir.mult(25);
          target = normal.get();
          target.add(dir);  
        }
     }
     
     // 绘制预测路径
      pushMatrix();
      stroke(0);
      fill(0);
      line(position.x, position.y, predictpos.x, predictpos.y); //当前位置到预测位置
      ellipse(predictpos.x, predictpos.y, 4, 4); // 预测点

      // Draw normal position
      stroke(0);
      fill(0);
      ellipse(normal.x, normal.y, 4, 4);
      // Draw actual target (red if steering towards it)
      line(predictpos.x, predictpos.y, target.x, target.y); //预测位置到目标位置
      if (worldRecord > path.radius) fill(255, 0, 0);
      noStroke();
      ellipse(target.x, target.y, 8, 8); // 目标位置
      popMatrix();
    
   // 若小车到道路中心的距离大于某值，则应用转向力
   if (worldRecord > path.radius)
   {
      return seek(target);
    }
    else 
    {
      return new PVector(0, 0);
    }
 }
 
  // 点p在线段ab上的法线交点位置计算
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // 向量ap
    PVector ap = PVector.sub(p, a);
    // 向量ab
    PVector ab = PVector.sub(b, a);
    // 向量ab归一化
    ab.normalize(); 
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a, ab);
    return normalPoint;
  }

练习完整代码：

Path path;
ArrayList<Vehicle> vehicles;

void newPath() {
  path = new Path();
  float offset = 30;
  path.addPoint(offset,offset);
  path.addPoint(width-offset,offset);
  path.addPoint(width-offset,height-offset);
  path.addPoint(width/2,height-offset*3);
  path.addPoint(offset,height-offset);
}

void newVeh()
{
  vehicles = new ArrayList<Vehicle>();
  for(int i = 0; i < 12; i++)
  {
    vehicles.add(new Vehicle(random(width),random(height)));
  }
}


void setup()
{
  size(800,400);
  newPath();
  newVeh();
}

void draw()
{
  background(255);
  path.display();
  
  for(Vehicle v:vehicles)
  {
    v.run();
    v.applyForce(v.follow(path));
  }

}

class Path
{
  // 路径由点的连线组成
  ArrayList<PVector> points; 
  // 道路半径
  float radius;
  
  Path()
  {
    radius = 20;
    points = new ArrayList<PVector>();
  }
  
  void addPoint(float x, float y)
  {
    PVector point = new PVector(x,y);
    points.add(point);  
  }
  
  void display()
  {
     pushMatrix();
    // 拐角模式 ROUND、MITER、BEVEL
    strokeJoin(ROUND);
   
    // 绘制粗路径
    stroke(175);
    strokeWeight(radius * 2);
    noFill();
    beginShape();
    for(PVector v:points)
    {
      vertex(v.x,v.y);
    }
    endShape(CLOSE);
    
    // 绘制中心路径
    stroke(0);
    strokeWeight(1);
    noFill();
    beginShape();
    for (PVector v : points) {
      vertex(v.x, v.y);
    }
    endShape(CLOSE);
    popMatrix();
  }  
}

class Vehicle
{
  // 历史运动记录
  ArrayList<PVector> history = new ArrayList<PVector>();
  
  PVector position;
  PVector velocity;
  PVector acceleration;
  float r;
  float maxforce;
  float maxspeed;
  
  Vehicle(float x,float y)
  {
    acceleration = new PVector();
    velocity = new PVector(2,0);
    position = new PVector(x,y);
    r = 6;
    maxspeed = 4;
    maxforce = 1;
  }
  
  void run()
  {
    update();
    display();
    
  }
  
  // 路径跟随算法
  PVector follow(Path path)
  {
    /*  ---- 1. 预计小车的未来位置 ----  */
    // 首先创建速度向量的副本
    PVector predict = velocity.get(); 
    
    // 单位化向量，并将向量向前延申25像素
    predict.normalize(); 
    predict.mult(25);
    
    // 将向量加上当前位置，计算未来的位置
    PVector predictpos = PVector.add(position,predict);
    
    // 根据预测位置找到法线交点，并选择在路径内且离小车最近的法线交点
                 
    PVector normal = null;  // 最终选中的法线交点
    PVector target = null;  // 最终的预测位置
    float worldRecord = 1000000; // 目前最小距离
    
    // 遍历路径
    for(int i = 0; i < path.points.size();i++)
    {
      PVector a = path.points.get(i);  // 起始点
      PVector b = path.points.get((i+1)%path.points.size()); // 结束点，闭环路径
      
      // 获取法线交点
      PVector normalPoint = getNormalPoint(predictpos, a, b);
      
      // 检查法线交点是否在线段内
      if((normalPoint.x < min(a.x,b.x))|| (normalPoint.x > max(a.x,b.x)) ||
         (normalPoint.y < min(a.y,b.y))|| (normalPoint.y > max(a.y,b.y)))
      {
        // 将结束点视为法线交点
        normalPoint = b.get();
        
        // 则走下一条路径
        a = path.points.get((i+1)%path.points.size()); // 起始点
        b = path.points.get((i+2)%path.points.size()); // 结束点    
      }
          
        // 路径方向
        PVector dir = PVector.sub(b,a);
        
        // 小车距法线交点的距离
        float distance = PVector.dist(predictpos,normalPoint);
        // 若距离小于最小距离
        if(distance < worldRecord)
        {
          worldRecord = distance;
          normal = normalPoint;
          
          dir.normalize();
          dir.mult(25);
          target = normal.get();
          target.add(dir);  
        }
     }
     
     // 绘制预测路径
      pushMatrix();
      stroke(0);
      fill(0);
      line(position.x, position.y, predictpos.x, predictpos.y); //当前位置到预测位置
      ellipse(predictpos.x, predictpos.y, 4, 4); // 预测点

      // Draw normal position
      stroke(0);
      fill(0);
      ellipse(normal.x, normal.y, 4, 4);
      // Draw actual target (red if steering towards it)
      line(predictpos.x, predictpos.y, target.x, target.y); //预测位置到目标位置
      if (worldRecord > path.radius) fill(255, 0, 0);
      noStroke();
      ellipse(target.x, target.y, 8, 8); // 目标位置
      popMatrix();
    
   // 若小车到道路中心的距离大于某值，则应用转向力
   if (worldRecord > path.radius)
   {
      return seek(target);
    }
    else 
    {
      return new PVector(0, 0);
    }
 }
 
  // 点p在线段ab上的法线交点位置计算
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // 向量ap
    PVector ap = PVector.sub(p, a);
    // 向量ab
    PVector ab = PVector.sub(b, a);
    // 向量ab归一化
    ab.normalize(); 
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a, ab);
    return normalPoint;
  }
  
  // 转向力计算
  PVector seek(PVector target) {
    // 小车的当前位置指向目标位置
    PVector desired = PVector.sub(target,position); 
    // 设置最大期望速度
    desired.setMag(maxspeed);
    // 转向力 = 期望速度 - 当前速度
    PVector steer = PVector.sub(desired,velocity);
    // 限制在最大转向力内
    steer.limit(maxforce);    
    return steer;
  }
    
  void applyForce(PVector force)
  {
    acceleration.add(force);
  }
  
  void update()
  {
    velocity.add(acceleration);
    velocity.limit(maxspeed);
    position.add(velocity);

    acceleration.mult(0);
  
    history.add(position.get());
    if (history.size() > 100) {
      history.remove(0);
    }
  }
  
  void display() {
      
    beginShape();
    stroke(0);
    strokeWeight(1);
    noFill();
    for(PVector v: history) {
      vertex(v.x,v.y);
    }
    endShape();
    
      
    // Draw a triangle rotated in the direction of velocity
    float theta = velocity.heading2D() + PI/2;
    
    fill(127);
    stroke(0);
    strokeWeight(1);
    pushMatrix();
    translate(position.x,position.y);
    rotate(theta);
    beginShape();
    vertex(0, -r*2);
    vertex(-r, r*2);
    vertex(r, r*2);
    endShape(CLOSE);
    popMatrix();
  }
  
}
相关标签： processing

上一篇： Sculptor 1.9.0 版发布 ，支持MongDB和Event-Driven架构

下一篇： 【python】Draw a straight line

推荐阅读