OBVP问题推导

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;
}