文章提供了一个基于Java的DWA(DiffusionWaveAlgorithm)路径规划算法实现，主要关注移动机器人的线速度和角速度计算。代码包括一个矩阵类用于矩阵操作，并展示了如何根据给定的MATLAB程序转换为Java代码。算法涉及障碍物避障、速度区间计算和评价函数等关键步骤。
仅仅是根据别人matlab程序改的java版，而且还有问题，有需要的可以拿去改改
我最后只get了线速度和角速度
参考的链接(4条消息) 3 移动机器人路径规划（5- DWA路径规划算法）_爬楼的猪的博客-CSDN博客
//先写了个矩阵类
class Matrix{
private int row;//行
private int col;//列
double [][]Data;
public Matrix() {}
public Matrix(int row, int col,double [][]Data) {
this.row = row;
this.col = col;
this.Data = Data;
}
public void setMatrix(int row , int col, double value) {
this.Data[row - 1][col - 1] = value;
}
public double getMatrix(int row, int col) {
return Data[row - 1][col - 1] ;
}
public int width() {
return col;
}
public int height() {
return row;
}
public Matrix add(Matrix M){//矩阵的加法运算
double[][] temp=new double[M.row][M.col];//两个矩阵是同型矩阵时，对应位置的元素相加
for(int i=0;i<row;i++){
for(int j=0;j<col;j++){
temp[i][j]=this.Data[i][j]+M.Data[i][j];
}
}
Matrix tempM=new Matrix(M.row,M.col,temp);
return tempM;
}
//原始矩阵里增加一行或几行
public Matrix add1(Matrix b) {
if(this.col==0 && this.row==0) {
return b;
}
int r=this.height()+b.height();
double[][] data = new double[r][b.width()];
for(int i = 0;i<this.row;i++) {
for(int j = 0;j<this.col;j++) {
data[i][j] = this.Data[i][j];
}
}
for(int k=0;k<b.height();k++) {
for(int w=0;w<b.width();w++) {
data[this.row+k][w] = b.Data[k][w];
}
}
Matrix res=new Matrix(r,b.width(),data);
return res;
}
public Matrix getpart(int col) {
double[][] part=new double[this.height()][1];
for(int i = 0;i<this.row;i++) {
part[i][0] = this.Data[i][col-1];
}
Matrix partM=new Matrix(this.height(),1,part);
return partM;
}
public double sum(){//计算矩阵中列的和
double Sum=0;
for(int i=0;i<this.height();i++) {
Sum+=this.Data[i][0];
}
return Sum;
}
public Matrix transpose() {
double tran[][] = new double[this.row][this.col];
for(int i = 0;i<this.row;i++) {
for(int j = 0;j<this.col;j++) {
tran[j][i] = this.Data[i][j];
}
}
Matrix another = new Matrix(this.col,this.row,tran);
System.out.println("after transpose:");
return another;
}
public Matrix multiple(Matrix m){//矩阵的乘法运算
double[][] temp=new double[this.row][m.col];
for(int i=0;i<this.row;i++){
double sum=0;
for(int j=0;j<m.col;j++){
for(int k=0;k<m.row;k++) {
sum+=(this.Data[i][k]*m.Data[k][j]);
}
temp[i][j]=sum;//通过上述算法计算每一个元素的值
}
}
Matrix tempM=new Matrix(this.row,m.col,temp);//能进行乘法运算时，执行下列语句
return tempM;
}
public void show()
{
//首先输出行数和列数
System.out.println("This is a matrix with "+col+" columns and "+row+" rows.");
System.out.println("The elements of the matrix:");
for(int i=0;i<row;i++)
{
for(int j=0;j<col;j++)
System.out.printf("%5.2f", this.Data[i][j]);//以格式化方法输出矩阵中每一个元素的值
System.out.println("");
}
}
}
//初始条件和限制条件根据自己的需求设置
public static double[] getSpeed_DWA(robot r,int r_ID,workbench target,robot[] R) {
double[] speed=new double[2];
double robotX=r.x, robotY=r.y, robotW=r.angleSpeed;
double robotV=Math.sqrt(Math.pow(r.lineSpeedX, 2) + Math.pow(r.lineSpeedY, 2));
double robotT=(r.direction>0)? r.direction:2*Math.PI-r.direction;//可能有问题
workbench goal=target;
double[][] dataobstacle=new double[3][2];
robot[] newr = new robot[R.length - 1];
for (int i = r_ID; i < R.length-1; i++) {
R[i]=R[i+1];
}
for (int i = 0; i < newr.length; i++) {
newr[i]=R[i];
}
for(int i=0;i<3;i++) {
dataobstacle[i][0]=newr[i].x;
dataobstacle[i][1]=newr[i].y;
}//除当前机器人外的其他机器人都为障碍
Matrix obstacle=new Matrix(3,2,dataobstacle); //障碍物位置列表 [x(m) y(m)]
double obstacleR=0.53; //冲突判定用的障碍物半径
double dt = 0.001;
double maxVel = 6; //机器人最大速度m/s
double maxRot = Math.PI; // 机器人最大角速度rad/s
double maxVelAcc = 250/(Math.PI*Math.pow(r.radius,2)*r.density); //机器人最大加速度m/ss
double maxRotAcc = 50.0/(Math.PI*Math.pow(r.radius,4)*r.density);
double alpha = 0.05; //方位角评价系数α
double beta = 0.2; //空隙评价系数β
double gama = 0.1; //速度评价系数γ
double periodTime = 0.015; //预测处理时间,也就是绿色搜寻轨迹的长度
double[] area= new double[]{0,50,0,50}; //模拟区域范围 [xmin xmax ymin ymax]
double[] path = new double[] {};
while(true){
//是否到达目的地，到达目的地则跳出循环
double distance=Math.sqrt(Math.pow((robotX- goal.x), 2)+Math.pow((robotY-goal.y), 2));
if(distance<0.45){
break;
}
// 1、求速度区间==============================================
double vel_rangeMin = Math.max(0, robotV-maxVelAcc*dt);
double vel_rangeMax = Math.min(maxVel, robotV+maxVelAcc*dt);
double rot_rangeMin = Math.max(-maxRot, robotW-maxRotAcc*dt);
double rot_rangeMax = Math.min(maxRot, robotW+maxRotAcc*dt);
//存放当前机器人状态的各个线速度角速度下的评价函数的值
//evalDB格式为n*6，其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值
Matrix evalDB=new Matrix();
//2、计算不同线速度角速度下的评价函数，目的取出最合适的线速度和角速度******************
for(double temp_v=vel_rangeMin;temp_v<=vel_rangeMax;temp_v=temp_v+1) {
for(double temp_w=rot_rangeMin;temp_w<=rot_rangeMax;temp_w=temp_w+Math.PI/180) {
//2.1 最关键内容，不同线速度角速度都是建立在机器人最初始状态下的
double[][] data = new double[5][1];//列向量
data[0][0]=robotX;data[1][0]=robotY;data[2][0]=robotT;
data[3][0]=robotV; data[4][0]=robotW;
Matrix rob_perState=new Matrix(5,1,data);
// 2.2 求出在sim_period时间后机器人的状态
for(double time=dt;time<=periodTime;time+=0.5) {
double[][] mate={{1,0,0,0,0},{0,1,0,0,0},{0,0,1,0,0},{0,0,0,0,0},{0,0,0,0,0}};
Matrix matE=new Matrix(5,5,mate);
double[][] matv= {{dt*Math.cos(rob_perState.getMatrix(3, 1)),0},{dt*Math.sin(rob_perState.getMatrix(3, 1)),0},{0,dt},{1,0},{0,1}};
Matrix matV = new Matrix(5,2,matv);
//求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
// //该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.02
double[][] datatemp= {{temp_v},{temp_w}};
Matrix temp=new Matrix(2,1,datatemp);
rob_perState = matE.multiple(rob_perState).add(matV.multiple(temp));
}
//2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态
//①方向角评价函数是用来评价机器人在当前设定的采样速度下，
//②达到模拟轨迹末端时的朝向和目标之间的角度差距
double goalTheta=Math.atan2(goal.y-rob_perState.getMatrix(2, 1),goal.x-rob_perState.getMatrix(1, 1));// 目标点的方位的角度
double evalTheta =Math.abs(rob_perState.getMatrix(3, 1)-goalTheta)/Math.PI*180;
double heading = 180 - evalTheta;
//%% 2.4 计算不同条件下的空隙评价函数
//% ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离
//% ②如果在这条轨迹上没有障碍物，那就将其设定一个常数
// % ③障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重
double dist=1.0/0; //
for(int i=1;i<=obstacle.getpart(1).height();i++) {
double[] b={rob_perState.getMatrix(1,1),rob_perState.getMatrix(2,1)};
double disttmp=Math.abs(Math.sqrt(Math.pow((obstacle.getMatrix(i, 1)- b[0]), 2)+Math.pow((obstacle.getMatrix(i, 2)-b[1]),2))-obstacleR);
if (dist>disttmp){
dist=disttmp;
}
}
if (dist>=2*obstacleR) {
dist=2*obstacleR;
}
// 2.5 速度评价函数
//评价当前轨迹的速度值大小。速度越大，评分越高
double velocity = temp_v;
// 2.6 利用制动距离限定速度是在有效的
// 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客
double stopDist = temp_v*temp_v/(2*maxVelAcc);
if (dist>stopDist) {
double[][] dataad= {{temp_v,temp_w,heading,dist,velocity,0}} ;
Matrix ad=new Matrix(1,6,dataad);
evalDB=evalDB.add1(ad);
}
}
}
//3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态
//如果评价函数为空则使得机器人停止，即evalDB全0
if (evalDB==null) {
double[][] dataeval= {{0,0,0,0,0,0}};
Matrix eval=new Matrix(1,6,dataeval);
evalDB.add1(eval);
}
//得到evalDB第三四五列的和
double sum_h=evalDB.getpart(3).sum();
double sum_d=evalDB.getpart(4).sum();
double sum_v=evalDB.getpart(5).sum();
//将评价函数进行正则化
if(sum_h!=0) {
for(int i=1;i<=evalDB.getpart(3).height();i++) {
double a=evalDB.getpart(3).getMatrix(i,1)/sum_h;
evalDB.setMatrix(i, 3, a);
}
}
if(sum_d!=0) {
for(int i=1;i<=evalDB.getpart(4).height();i++) {
double a=evalDB.getpart(4).getMatrix(i,1)/sum_d;
evalDB.setMatrix(i, 4, a);
}
}
if(sum_v!=0) {
for(int i=1;i<=evalDB.getpart(5).height();i++) {
double a=evalDB.getpart(5).getMatrix(i,1)/sum_v;
evalDB.setMatrix(i, 5, a);
}
}
//最终评价函数的计算
ArrayList<Double> a=new ArrayList<>();
int max=0;
for(int i=0;i<evalDB.height();i++) {
a.add(alpha*(evalDB.getMatrix(i+1, 3))+beta*(evalDB.getMatrix(i+1, 4))+gama*(evalDB.getMatrix(i+1, 5)));
evalDB.setMatrix(i+1, 6, a.get(i));
if(a.get(i)>a.get(max)) {
max=i;
}
}//得到的i就是索引
speed[0]=evalDB.getMatrix(max+1, 1);
speed[1]=evalDB.getMatrix(max+1, 2);
return speed;
}
}
}
602
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
