APF航迹规划

最新推荐文章于 2024-07-11 08:58:54 发布

小白程序员成长史

最新推荐文章于 2024-07-11 08:58:54 发布

阅读量461

点赞数

文章标签： c++ 自动驾驶

本文链接：https://blog.csdn.net/weixin_42394594/article/details/105071611

版权

本文主要探讨了APF（Artificial Potential Field）在自动驾驶中的应用，通过Routplanning.h、Routplanning.cpp及main.cpp三个关键文件阐述了航迹规划的实现过程，重点介绍了APF算法如何为车辆提供安全、高效的行驶路径。

摘要由CSDN通过智能技术生成

APF航迹规划

Routplanning.h

#include<iostream>
#include<fstream>
#include<math.h>
#include<numeric>
#include<string>

using namespace std;

#define pi 3.1415926

class Routplanning
{
public:
	Routplanning(void);
	~Routplanning(void);
	double Rt;
	double r;
	double delta_X[9];
	double delta_Y[9];
	double R[9];
	double Rat;
	double rat;
	double Rrei[9];
	double rre[9];
	double Yre_rx[9];
	double Yre_ry[9];
	double Yrer[9];
	double Yata[9];
	double l;//步长
	double X_0[2];//起点
	int k;//引力场所需要的增益系数
	int K;//初始化
	int m;//斥力场所需要的增益指数
	int a;//目标方向的角度
	int J;//步数
	double x_0[9];//存放障碍点的x坐标
	double y_0[9];//存放障碍点的y坐标
	double X_j[2];//存放路径的中间变量
	double  Angle;//角度
	double angle_repulsion[9];//存放斥力的角度
	double Theta[9];//各个障碍的角度
	double F_sum_y_j;//X轴方向的合力
	double F_sum_x_j;//Y轴方向的合力
	double X_next[2];//下一个点
	double Yat_x;//引力在x轴上分量
	double Yat_y;//引力在y轴上的分量
	double Yre_rxx;//斥力叠加在x轴上的分量
	double Yre_ryy;//斥力叠加在y轴上的分量
	double goal[2000][2];//路径


    void compute_attack(double X[2],double sum[9][2],int k,double angle,double& Yat_x,double& Yat_y );//引力计算
    void computer_angle(double X[2],double sum[9][2],double n,double theta[9]);//角度计算
    void compute_repulsion(double X[2],double sum[9][2],int m,double angle_repulsion[9],int n,double P_0[9],int a,double& Yre_rxx,double& Yre_ryy);//斥力计算
    int sgn(double x);//符号函数
	void plan(double X_sum[9][2],int n,int r[9]);//路径规划
};

Routplanning.cpp

#include "Routplanning.h"


Routplanning::Routplanning(void)
{
}


Routplanning::~Routplanning(void)
{
}

void Routplanning:: compute_attack(double X[2],double sum[9][2],int k,double angle,double& Yat_x,double& Yat_y )
    {
	    Rt = pow((X[0]-sum[0][0]),2)+pow((X[1]-sum[0][1]),2); //路径点和目标的距离的平方
    	r = sqrt(Rt);                                  //路径点和目标的距离
	    Yat_x = k*r*cos(angle);                   //x轴上的分量    
	    Yat_y = k*r*sin(angle);//y轴上的分量
    }
//角度计算
void Routplanning::computer_angle(double X[2],double sum[9][2],double n,double theta[9])
    {
	    for(int i=0;i<n;i++)
	    {
		    delta_X[i]=sum[i][0]-X[0];
		    delta_Y[i]=sum[i][1]-X[1];//各个障碍距离点X向量在X轴和Y轴的投影
		    R[i]=sqrt(pow(delta_X[i],2)+pow(delta_Y[i],2));  //障碍点距离X点的距离
		    theta[i] = sgn(delta_Y[i])*acos(delta_X[i]/R[i]);//角度的求解
	    }
    }
//斥力计算
void Routplanning::compute_repulsion(double