0.引言
记录一下。时间紧张,不整理了,直接上草稿纸。一元四次方程求解参考博客。
1.推导
2.code
代码完全按照推导书写,包括变量的命令。
double OBVP(Eigen::Vector3d _start_position,Eigen::Vector3d _start_velocity,Eigen::Vector3d _target_position)
{
double optimal_cost = 100000.0;
double deltaPx = _target_position(0) - _start_position(0);
double deltaPy = _target_position(1) - _start_position(1);
double deltaPz = _target_position(2) - _start_position(2);
double deltaVx = 0 - _start_velocity(0);
double deltaVy = 0 - _start_velocity(1);
double deltaVz = 0 - _start_velocity(2);
double Vx0 = _start_velocity(0);
double Vy0 = _start_velocity(1);
double Vz0 = _start_velocity(2);
double m = deltaPx*deltaPx +deltaPy*deltaPy + deltaPz*deltaPz;
double n = 2*(deltaPx*Vx0 + deltaPy*Vy0 + deltaPz*Vz0) +
deltaPx*deltaVx + deltaPy*deltaVy + deltaPz*deltaVz;
double k = 3*(Vx0*Vx0 + Vy0*Vy0 +Vz0*Vz0 + deltaVx*Vx0 + deltaVy*Vy0 + deltaVz*Vz0) +
deltaVx*deltaVx + deltaVy*deltaVy + deltaVz*deltaVz;
Eigen::Matrix<double, 4, 4> CompanionMatrix44;
Eigen::Matrix<complex<double>, Eigen::Dynamic, Eigen::Dynamic> CompanionMatrix44EigenValues;//复数动态矩阵
double c = - 4*k;
double d = - 24*n;
double e = - 36*m;
vector<double> tmpOptimalT(4 ,0.0);
vector<double> tmpOptimalCost(4, 100000.0);
double optimalT = 0.0;
CompanionMatrix44 << 0, 0, 0, -e,
1, 0, 0, -d,
0, 1, 0, -c,
0, 0, 1, 0 ;
//std::cout<<"CompanionMatrix44: "<<std::endl<<CompanionMatrix44<<std::endl<<std::endl;
CompanionMatrix44EigenValues = CompanionMatrix44.eigenvalues();
//std::cout<<"matrix_eigenvalues: "<<std::endl<<CompanionMatrix44EigenValues<<std::endl;
for(int i = 0; i < CompanionMatrix44EigenValues.size(); i++)
{
//TODO:时间不能为负数
//if(CompanionMatrix44EigenValues(i).imag() == 0)
if(CompanionMatrix44EigenValues(i).imag() == 0 && CompanionMatrix44EigenValues(i).real() > 0)
{
tmpOptimalT[i] = CompanionMatrix44EigenValues(i).real();
double t = tmpOptimalT[i];
double t2 = t*t;
double t3 = t2*t;
tmpOptimalCost[i] = t + (12*m)/(t3) + (12*n)/(t2) + (4*k)/t;
}
else
{
continue;
}
}
for(int i = 0; i < tmpOptimalCost.size(); i++)
{
optimal_cost = std::min(tmpOptimalCost[i],optimal_cost);
}
//可以不用
int flag = 0;
for(int i = 0; i < tmpOptimalCost.size(); i++)
{
if(optimal_cost == tmpOptimalCost[i])
flag = i;
else continue;
}
optimalT = tmpOptimalT[flag];
return optimal_cost;
}