📄 apfclass.java
字号:
package cn.com.yuzhiqiang.pcimotor;
public class APFClass {
private static final double WHEELBASE = 3;// 前后轮距单位为米
private static final double RWTOFDISTANCE = 3.5;
private static final double HALFWIDTH = 1;
private static final double Ks = 1.2;
private static final double Db = 80;// 边界阻力用 雷达扫描边界距离
private static final double ACCELERATION = -2;// m/s2
private static final double VMAX = 2;// 机器人最高限速m
private static final double Kv = 0.8;
// private static final double PI=3.1415926;
// 对曲率半径和角度的正负约定 右转都为正 左转为负 统一为弧度
private static final double DriveMaxAngle = Math.toRadians(40);// 40度
private static final double DriveMinAngle = Math.toRadians(-40);// -40度
// 驾驶角的正负界限
private static double DriveAngle;
private static double DriveV;
// private static double PrincipalAxisAngle;// 主轴角度
// private static double RearCurRadius; // 后轮曲率半径
// private static double CenterCurRadius; // 中心曲率半径
private static double Ds = Ks * HALFWIDTH;
private static double Length_ICR1, Angle_ICR1;
private static double Length_ICR2, Angle_ICR2;
private static double D1p, D2p;
private static double UppLimAngle_Resistance, LowLimAngle_Resistance;// 阻力角范围
private static double Fp_Resistance;// P点阻力
protected static double Fb_Resistance = 1 / Math.sqrt(Db * Db - Ds * Ds);// 边界点阻力
private static double Length_Object, Angle_Object = 0;// 目标点长度和角度
private static double F_CurrentPass;// 当前视场的通行函数
private static double Angle_CurrentPass;// 当前视场驾驶角度
private static double[] Length_Polar = { 10100, 1010, 1010, 1010, 1020,
1060, 1040, 1050, 1040, 1050, 1000, 1020, 1000, 1020, 1040, 1050,
1040, 1040, 1030, 1010, 1019, 8011, 8070, 8029, 8246, 8054, 10200,
23900, 241, 242, 243, 244, 246, 247, 248, 250, 251, 252, 254, 255,
256, 258, 259, 261, 262, 263, 265, 266, 268, 269, 270, 273, 274,
276, 278, 279, 282, 284, 286, 287, 289, 293, 295, 297, 299, 302,
304, 307, 309, 312, 315, 318, 321, 324, 327, 330, 329, 327, 324,
322, 319, 317, 315, 313, 311, 309, 307, 305, 303, 301, 300, 297,
296, 295, 293, 291, 289, 288, 286, 285, 283, 282, 281, 280, 278,
277, 275, 274, 273, 272, 271, 270, 269, 268, 266, 266, 266, 267,
268, 266, 269, 269, 268, 267, 267, 266, 264, 263, 261, 260, 260,
260, 260, 260, 261, 259, 259, 258, 258, 257, 255, 255, 254, 251,
248, 248, 247, 246, 246, 246, 247, 247, 246, 247, 247, 246, 246,
246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246,
246, 247, 247, 247, 247, 247, 248, 248, 248, 250, 250, 250, 251,
251, 252, 252, 253, 253, 254, 254, 254, 255, 256, 256, 257, 258,
258, 259, 259, 260, 261, 262, 263, 264, 264, 266, 266, 267, 268,
269, 270, 270, 272, 273, 275, 275, 276, 278, 279, 281, 282, 284,
285, 8191, 288, 8191, 289, 8191, 8191, 8191, 8191, 8191, 8191,
8191, 8191, 8191, 8191, 8191, 8191, 1700, 8191, 1400, 8191, 1708,
1978, 1799, 1489, 1888, 1656, 1656, 1688, 1633, 164, 173, 160, 188,
167, 157, 168, 144, 178, 154, 174, 125, 153, 143, 145, 8191, 143,
8191, 125, 8191, 133, 8191, 8191, 8191, 8191, 8191, 8191, 8191,
8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 1709, 1899,
1499, 1389, 1245, 1143, 1242, 1152, 1154, 1052, 9554, 9550, 855,
855, 855, 955, 855, 955, 855, 955, 933, 833, 954, 845, 900, 1006,
966, 1155, 1266, 1366, 1266, 1266, 1366, 1166, 1066, 1066, 1156,
1178, 1189, 1089, 1088, 1099, 1388, 1288, 1288, 1388, 988, 1288,
1288, 1188, 200, 1100, 290, 1200, 320, 1030, 310, 1079, 329, 249,
320, 310, 320, 320, 320, 310, 330, 320, 339, 310, 329, 329 };// 正式程序中应为引用
// 0.5度激光雷达传一个数
// 单位为厘米 正式计算时变为米 调试时用
// double []Length_Polar={101 101 101 101 102 106 104 105 104 105 100 102
// 100
// 102 104 105 104 104 103 101 101 100 102 101 101 102 102 239 241 242 243
// 244
// 246 247 248 250 251 252 254 255 256 258 259 261 262 263 265 266 268 269
// 270
// 273 274 276 278 279 282 284 286 287 289 293 295 297 299 302 304 307 309
// 312
// 315 318 321 324 327 330 329 327 324 322 319 317 315 313 311 309 307 305
// 303
// 301 300 297 296 295 293 291 289 288 286 285 283 282 281 280 278 277 275
// 274
// 273 272 271 270 269 268 266 266 266 267 268 266 269 269 268 267 267 266
// 264
// 263 261 260 260 260 260 260 261 259 259 258 258 257 255 255 254 251 248
// 248
// 247 246 246 246 247 247 246 247 247 246 246 246 246 246 246 246 246 246
// 246
// 246 246 246 246 246 246 247 247 247 247 247 248 248 248 250 250 250 251
// 251
// 252 252 253 253 254 254 254 255 256 256 257 258 258 259 259 260 261 262
// 263
// 264 264 266 266 267 268 269 270 270 272 273 275 275 276 278 279 281 282
// 284
// 285 8191 288 8191 289 8191 8191 8191 8191 8191 8191 8191 8191 8191 8191
// 8191
// 8191 17 8191 14 8191 17 19 17 14 18 16 16 16 16 16 17 16 18 16 15 16 14
// 17 15
// 17 12 15 14 14 8191 14 8191 12 8191 13 8191 8191 8191 8191 8191 8191 8191
// 8191 8191 8191 8191 8191 8191 8191 8191 8191 17 18 14 13 12 11 12 11 11
// 10 9
// 9 8 8 8 9 8 9 8 9 9 8 9 8 9 10 9 11 12 13 12 12 13 11 10 10 11 11 11 10
// 10 10
// 13 12 12 13 9 12 12 11 20 11 29 12 32 13 31 17 32 24 32 31 32 32 32 31 33
// 32
// 33 31 32 32
// };
// private static double []Angle_Polar=new double[]; //数据从-pi/2到pi/2
private static double[] ULAngle = new double[361];// 各阻力角上限和下限
private static double[] LLAngle = new double[361];
protected static double[] F_SingleResistance = new double[361];//正式程序设为私有 下同
protected static double[] Fr_Resistance = new double[361];// 总阻力
protected static double[] F_Attraction = new double[361];// 引力
protected static double[] F_Pass = new double[361];// 通行函数
// 注意所有的角度单位统一为弧度
public APFClass() {
}
protected static void CalICR() {
// 调试用
/*
* for(int i=0;i<361;i++){ Length_Polar[i]=8100; }
* Length_Polar[100]=3500; Length_Polar[105]=4500;//调试结束
*/
// 计算ICR1和ICR2的极坐标
Length_ICR1 = -RWTOFDISTANCE
/ Math.sin(Math.atan(RWTOFDISTANCE * Math.tan(DriveMinAngle)
/ WHEELBASE));// 假设左转角度都小于0
Angle_ICR1 = Math.atan(RWTOFDISTANCE * Math.tan(DriveMinAngle)
/ WHEELBASE)
- Math.PI / 2;
Length_ICR2 = RWTOFDISTANCE
/ Math.sin(Math.atan(RWTOFDISTANCE * Math.tan(DriveMaxAngle)
/ WHEELBASE));
Angle_ICR2 = Math.atan(RWTOFDISTANCE * Math.tan(DriveMaxAngle)
/ WHEELBASE)
+ Math.PI / 2;
}
// 计算单个点阻力 有问题！！
protected static void CalSingleResistance(double pdistance, double pangle) {
// 计算ICR1和ICR2的极坐标
/*
* Length_ICR1 = RWTOFDISTANCE / Math.sin(Math.atan(RWTOFDISTANCE *
* Math.tan(DriveMinAngle) / WHEELBASE)); Angle_ICR1 =
* Math.atan(RWTOFDISTANCE * Math.tan(DriveMinAngle) / WHEELBASE) -
* Math.PI / 2; Length_ICR2 = RWTOFDISTANCE /
* Math.sin(Math.atan(RWTOFDISTANCE * Math.tan(DriveMaxAngle) /
* WHEELBASE)); Angle_ICR2 = Math.atan(RWTOFDISTANCE *
* Math.tan(DriveMaxAngle) / WHEELBASE) - Math.PI / 2;
*/
// CalICR();
// double D1p,D2p,Fp_Resistance,LowLimAngle,UppLimAngle;
D1p = Math.sqrt(Length_ICR1 * Length_ICR1 + pdistance * pdistance - 2
* Length_ICR1 * pdistance * Math.cos(Angle_ICR1 - pangle));
D2p = Math.sqrt(Length_ICR2 * Length_ICR2 + pdistance * pdistance - 2
* Length_ICR2 * pdistance * Math.cos(Angle_ICR2 - pangle));
// 计算P点阻力角范围
if (Math.abs(D1p - Length_ICR1) > Ds)
LowLimAngle_Resistance = pangle - Math.asin(Ds / pdistance);
else
LowLimAngle_Resistance = -Math.PI / 2;
if (Math.abs(D2p - Length_ICR2) > Ds)
UppLimAngle_Resistance = pangle + Math.asin(Ds / pdistance);
else
UppLimAngle_Resistance = Math.PI / 2;
// 计算p点阻力
Fp_Resistance = 1 / Math.sqrt(pdistance * pdistance - Ds * Ds);
/*
* if ((pangle >= LowLimAngle_Resistance) && (pangle <=
* UppLimAngle_Resistance)) Fp_Resistance = 1 / Math.sqrt(pdistance *
* pdistance - Ds * Ds);//防止p点距离过小 平方根里为负 else Fp_Resistance = 0;
*/
}
// 计算每个角度阻力和作用域范围 并赋值
protected static void SetPResistance() {
// double
// D1p,D2p,Fp_Resistance,LowLimAngle_Resistance,UppLimAngle_Resistance;
CalICR();
for (int i = 0; i < 361; i++) {
CalSingleResistance(Length_Polar[i] / 100, -Math.PI / 2
+ Math.toRadians(i * 0.5));//
ULAngle[i] = UppLimAngle_Resistance;
LLAngle[i] = LowLimAngle_Resistance;
F_SingleResistance[i] = Fp_Resistance;
}
}
// 计算每个角度总阻力 程序算法待改进 太耗时 有问题！！！！
protected static void CalTotalResistance() {
double dMaxF_temp;
SetPResistance();
for (int i = 0; i < 361; i++) {
dMaxF_temp = F_SingleResistance[i];
for (int j = 0; j < 361; j++) {
if ((LLAngle[j] <= Math.toRadians(i * 0.5) - Math.PI / 2)
&& (ULAngle[j] >= -Math.PI / 2
+ Math.toRadians(i * 0.5))) {
if (dMaxF_temp < F_SingleResistance[j])
dMaxF_temp = F_SingleResistance[j];
}
}
if (dMaxF_temp < Fb_Resistance)
dMaxF_temp = Fb_Resistance;// 和边界阻力比较
Fr_Resistance[i] = dMaxF_temp;
}
}
// 计算每个角度的引力
protected static void CalAttraction() {
for (int i = 0; i < 361; i++) {
if (Math.abs(-Math.PI / 2 + Math.toRadians(i * 0.5 - Angle_Object)) < Math.PI / 2)
F_Attraction[i] = Math.cos(-Math.PI / 2
+ Math.toRadians(i * 0.5 - Angle_Object));
else
F_Attraction[i] = 0;
}
}
// 计算通行函数
protected static void PassFunction() {
double dMaxFpass, s;
CalTotalResistance();
CalAttraction();
for (int i = 0; i < 361; i++) {
F_Pass[i] = F_Attraction[i] / Fr_Resistance[i];
}
dMaxFpass = F_Pass[0];// 假设第一个通行函数大
s = 0;
for (int i = 1; i < 361; i++) {
if (dMaxFpass < F_Pass[i]) {
dMaxFpass = F_Pass[i];
s = i;
}
}
F_CurrentPass = dMaxFpass;
Angle_CurrentPass = Math.toRadians(s * 0.5) - Math.PI / 2;
}
// 决策输出函数
protected static void OutputFunction() {
double dDriveAngleM, dDriveV;
int s;
dDriveAngleM = Math.atan(WHEELBASE * Math.tan(Angle_CurrentPass)
/ RWTOFDISTANCE);
if (dDriveAngleM < DriveMinAngle)
DriveAngle = DriveMinAngle;
if ((dDriveAngleM >= DriveMinAngle) && (dDriveAngleM <= DriveMaxAngle))
DriveAngle = dDriveAngleM;
if (dDriveAngleM > DriveMaxAngle)
DriveAngle = DriveMaxAngle;
// 驾驶速度
s = (int) ((Angle_CurrentPass + Math.PI / 2) / Math.toRadians(0.5));
dDriveV = Kv * Math.sqrt(-2 * ACCELERATION / Fr_Resistance[s]);
DriveV = Math.min(dDriveV, VMAX);
}
// 读出驾驶角 转化为度
public static double ReadDriveAngle() {
return Math.toDegrees(DriveAngle);
}
// 读驾驶速度
public static double ReadDriveV() {
return DriveV;
}
// 读当前视场通行函数
public static double ReadCurrentPass(){
return F_CurrentPass;
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -