libipoptfort4.dll

param NE == 4;
var tf  >= 0.1;
var hi = tf/NE;

param N1{j in {1}};
param N2{j in {1}};
param N3{j in {1}};

################################# 车道尺寸参数
set I :={1..NE};
set I1:={1..NE-1};
set J :={1..3};
set K :={0..3};
param tauj{j in K};
param dljtauk{j in K,k in K};
param omega{j in J};

set ALL :={1..(N1[1] + N2[1] + N3[1])};

param PXPY{j in ALL,k in {1..2}};
param NC == N1[1] + N2[1] + N3[1];
param sequenceddd{j in {1..NC}};

################################# 边界限制参数
param amax == 0.5;
param vmax == 15;
param jerkmax == 0.2;

param n == 0.96;
param l = 2.8;
param m = 0.929;
param b = 0.971;

################################# 声明
var x{i in I,j in K,k in ALL};
var y{i in I,j in K,k in ALL};
var theta{i in I,j in K,k in ALL};
var v{i in I,j in K,k in ALL};
var phy{i in I,j in K,k in ALL};
var w{i in I,j in K,k in ALL};
var a{i in I,j in K,k in ALL};
var jerk{i in I,j in K,k in ALL};
var AX{i in I,j in K,k in ALL};
var BX{i in I,j in K,k in ALL};
var CX{i in I,j in K,k in ALL};
var DX{i in I,j in K,k in ALL};

var AY{i in I,j in K,k in ALL};
var BY{i in I,j in K,k in ALL};
var CY{i in I,j in K,k in ALL};
var DY{i in I,j in K,k in ALL};
################################# 优化时间
minimize criterion:
tf;

s.t. xxx:
tf<= 50;

################################# Vehicle kinematics described via DAEs ####
s.t. DIFF_dxdt {i in I, k in J, xx in ALL}:
sum{j in K}(dljtauk[j,k]*x[i,j,xx]) - hi * v[i,k,xx] = 0;
s.t. DIFF_dydt {i in I, k in J, xx in ALL}:
sum{j in K}(dljtauk[j,k]*y[i,j,xx]) = 0;

s.t. EQ_diffx {i in I1, xx in ALL}:
x[i+1,0,xx] = sum{j in K}((prod{k in K:k<>j}((1-tauj[k])/(tauj[j]-tauj[k])))*x[i,j,xx]);
s.t. EQ_diffy {i in I1, xx in ALL}:
y[i+1,0,xx] = sum{j in K}((prod{k in K:k<>j}((1-tauj[k])/(tauj[j]-tauj[k])))*y[i,j,xx]);

s.t. DIFF_dvdt {i in I, k in J, xx in ALL}:
sum{j in K}(dljtauk[j,k]*v[i,j,xx]) - hi*a[i,k,xx] = 0;
s.t. EQ_diffv {i in I1, xx in ALL}:
v[i+1,0,xx] = sum{j in K}((prod{k in K:k<>j}((1-tauj[k])/(tauj[j]-tauj[k])))*v[i,j,xx]);




s.t. DIFF_dadt {i in I, k in J, xx in ALL}:
sum{j in K}(dljtauk[j,k]*a[i,j,xx]) - hi*jerk[i,k,xx] = 0;

s.t. EQ_diffa {i in I1, xx in ALL}:
a[i+1,0,xx] = sum{j in K}((prod{k in K:k<>j}((1-tauj[k])/(tauj[j]-tauj[k])))*a[i,j,xx]);


################################# Starting Configurations #################################
s.t. EQ_starting_x_all {xx in ALL}:
x[1,0,xx] = PXPY[xx,1];
s.t. EQ_starting_y_all {xx in ALL}:
y[1,0,xx] = PXPY[xx,2];

s.t. EQ_starting_v_all {xx in ALL}:
v[1,0,xx] = 10;
s.t. EQ_ending_v_all {xx in ALL}:
v[NE,3,xx] = 10;

s.t. EQ_starting_a_all {xx in ALL}:
a[1,0,xx] = 0;
s.t. EQ_ending_a_all {xx in ALL}:
a[NE,3,xx] = 0;

s.t. EQ_starting_j_all {xx in ALL}:
jerk[1,0,xx] = 0;
s.t. EQ_ending_j_all {xx in ALL}:
jerk[NE,3,xx] = 0;

################################# 状态及控制变量的上下界限制
s.t. Bonds_v1 {i in I,j in K,xx in ALL}:
v[i,j,xx] <= vmax;
s.t. Bonds_v2 {i in I,j in K,xx in ALL}:
v[i,j,xx] >= 0;

s.t. Bonds_jerk {i in I,j in K,xx in ALL}:
(jerk[i,j,xx])^2 <= (jerkmax)^2;

s.t. Bonds_a {i in I,j in K,xx in ALL}:
(a[i,j,xx])^2 <= (amax)^2;


s.t. constant_w {i in I,j in K,xx in ALL}:
w[i,j,xx] == 0;
s.t. constant_phy {i in I,j in K,xx in ALL}:
phy[i,j,xx] == 0;
s.t. constant_theta {i in I, k in K, xx in ALL}:
theta[i,k,xx] = 0;

############################### 碰撞相关的议题 ##################################
s.t. CQ_IQ1 {i in I,j in K,xx in {1..(N1[1]-1)}}:
x[i,j,xx] - x[i,j,(xx+1)] >= 4.6890;
s.t. CQ_IQ2 {i in I,j in K,xx in {(N1[1]+1)..(N1[1]+N2[1]-1)}}:
x[i,j,xx] - x[i,j,(xx+1)] >= 4.6890;
s.t. CQ_IQ3 {i in I,j in K,xx in {(N1[1]+N2[1]+1)..(N1[1]+N2[1]+N3[1]-1)}}:
x[i,j,xx] - x[i,j,(xx+1)] >= 4.6890;

############################### 最终结果 ##################################
s.t. Final {xx in ALL}:
x[NE,3,1] - x[NE,3,xx] = sequenceddd[xx];




s.t. RELATIONSHIP_AX {i in I,j in K,xx in ALL}:
AX[i,j,xx] = x[i,j,xx] + (n+l) * cos(theta[i,j,xx]) - b * sin(theta[i,j,xx]);

s.t. RELATIONSHIP_BX {i in I,j in K,xx in ALL}:
BX[i,j,xx] = x[i,j,xx] + (n+l) * cos(theta[i,j,xx]) + b * sin(theta[i,j,xx]);

s.t. RELATIONSHIP_CX {i in I,j in K,xx in ALL}:
CX[i,j,xx] = x[i,j,xx] - (m) * cos(theta[i,j,xx]) + b * sin(theta[i,j,xx]);

s.t. RELATIONSHIP_DX {i in I,j in K,xx in ALL}:
DX[i,j,xx] = x[i,j,xx] - (m) * cos(theta[i,j,xx]) - b * sin(theta[i,j,xx]);

s.t. RELATIONSHIP_AY {i in I,j in K,xx in ALL}:
AY[i,j,xx] = y[i,j,xx] + (n+l) * sin(theta[i,j,xx]) + b * cos(theta[i,j,xx]);

s.t. RELATIONSHIP_BY {i in I,j in K,xx in ALL}:
BY[i,j,xx] = y[i,j,xx] + (n+l) * sin(theta[i,j,xx]) - b * cos(theta[i,j,xx]);

s.t. RELATIONSHIP_CY {i in I,j in K,xx in ALL}:
CY[i,j,xx] = y[i,j,xx] - (m) * sin(theta[i,j,xx]) - b * cos(theta[i,j,xx]);

s.t. RELATIONSHIP_DY {i in I,j in K,xx in ALL}:
DY[i,j,xx] = y[i,j,xx] - (m) * sin(theta[i,j,xx]) + b * cos(theta[i,j,xx]);


data;
param: PXPY := include PXPY;
param: N1 := include OLD1;
param: N2 := include OLD2;
param: N3 := include OLD3;
param: sequenceddd := include sequenceddd;

param: dljtauk :=
         0         0   -9.0000
         0    1.0000   -4.1394
         0    2.0000    1.7394
         0    3.0000   -3.0000
    1.0000         0   10.0488
    1.0000    1.0000    3.2247
    1.0000    2.0000   -3.5678
    1.0000    3.0000    5.5320
    2.0000         0   -1.3821
    2.0000    1.0000    1.1678
    2.0000    2.0000    0.7753
    2.0000    3.0000   -7.5320
    3.0000         0    0.3333
    3.0000    1.0000   -0.2532
    3.0000    2.0000    1.0532
    3.0000    3.0000    5.0000;

param: tauj :=
	0		0
	1		0.1550510257216822
	2		0.6449489742783178
	3		1.0;

param: omega:=
	1		 3.76403062700467e-1 
	2		 5.12485826188421e-1
	3		 1.11111111111111e-1;