OBVP问题

最新推荐文章于 2024-07-27 21:51:36 发布

淡定

最新推荐文章于 2024-07-27 21:51:36 发布

阅读量3k

点赞数 5

文章标签：算法

本文链接：https://blog.csdn.net/qq_44686976/article/details/106158890

版权

解决OBVP问题

末速度不确定
推导
执行效果1
执行效果2

末速度不确定

解决OBVP问题时，当末速度没有给定时需要分情况讨论，一种是末速度给定，另一种是末速度没有给定，我们假定末速度为零。

推导

对于解OBVP问题的目标函数J，含有一个隐式的约束，即T>0,我们可以手动推出J的方程和J的导数，当然大多数情况下计算过于复杂，浪费时间不说还容易算错，因此大多借助matlab计算方程得到导数的方程后，可以利用多项式伴随矩阵解出特征值，得到导数方程的根。会调用eigen库来求解伴随矩阵特征值也是可以的。如果不想求解方程的根，直接将目标函数写到程序中，然后添加约束函数，让优化库给出数值最优解，常用的库包括IPOPT，NLOPT。接下来主要对前两种方法进行说明：
首先使用matlab计算出了6个参数关于T的表达式：

syms T P_xf P_yf P_zf V_xo V_yo V_zo P_xo P_yo P_zo V_xf V_yf V_zf
Delta=[P_xf-V_xo*T-P_xo;
    P_yf-V_yo*T-P_yo;
    P_zf-V_zo*T-P_zo;
    V_xf-V_xo;
    V_yf-V_yo;
    V_zf-V_zo];
Variants=[-12/T^3,0,0,6/T^2,0,0;
    0,-12/T^3,0,0,6/T^2,0;
    0,0,-12/T^3,0,0,6/T^2;
    6/T^2,0,0,-2/T,0,0;
    0,6/T^2,0,0,-2/T,0;
    0,0,6/T^2,0,0,-2/T]*Delta

J=T+(Variants(1)^2*T^3/3+Variants(1)*Variants(4)*T^2+Variants(4)^2*T^2)+ ...
(Variants(2)^2*T^3/3+Variants(2)*Variants(5)*T^2+Variants(5)^2*T^2)+ ...
(Variants(3)^2*T^3/3+Variants(3)*Variants(6)*T^2+Variants(6)^2*T^2)
diffJ=diff(J,T)
simpDiff=collect(diffJ,T)
S=solve(simpDiff)

forward simulation没有问题，问题是解J’ = 0的方程比较难。利用了多项式伴随矩阵的特征值是多项式方程的根这一性质求解。

 double P_x0=_start_position(0);
    double P_y0=_start_position(1);
    double P_z0=_start_position(2);
    double V_x0=_start_velocity(0);
    double V_y0=_start_velocity(1);
    double V_z0=_start_velocity(2);
    double P_xf=_target_position(0);
    double P_yf=_target_position(1);
    double P_zf=_target_position(2);
#ifdef PARTIAL_FREE
    /// set other final state to 0
    ///
    double poly[5]={0}; // 0 1 2 3 4
    poly[0]= -36*((P_xf-P_x0)*(P_xf-P_x0)+(P_yf-P_y0)*(P_yf-P_y0)+(P_zf-P_z0)*(P_zf-P_z0));
    poly[1]=24*((P_xf-P_x0)*V_x0+(P_yf-P_y0)*V_y0+(P_zf-P_z0)*V_z0);
    poly[2]=-4*(V_x0*V_x0+V_y0*V_y0+V_z0*V_z0);
    poly[4]=1;
    Matrix4d Compan;
    Compan<<0,-poly[2],-poly[1],-poly[0],
        1,0,0,0,
        0,1,0,0,
        0,0,1,0;

    EigenSolver<MatrixXd> solver(Compan);
    auto T_vals=solver.eigenvalues();

    for(int i=0;i<4;i++)
    {
      auto T_val=T_vals(i);
      if(T_val.real()<=0||std::abs(T_val.imag())>0.00001)
        continue;

      //compute J
      double T=T_val.real();
      double T_squre=T*T,T_cube=T_squre*T;

      double a1=(12*(P_x0-P_xf+T*V_x0))/T_cube-(6*V_x0)/T_squre;
      double a2=(12*(P_y0-P_yf+T*V_y0))/T_cube-(6*V_y0)/T_squre;
      double a3=(12*(P_z0-P_zf+T*V_z0))/T_cube-(6*V_z0)/T_squre;
      double b1=(2*V_x0)/T-(6*(P_x0-P_xf+T*V_x0))/T_squre;
      double b2=(2*V_y0)/T-(6*(P_y0-P_yf+T*V_y0))/T_squre;
      double b3=(2*V_z0)/T-(6*(P_z0-P_zf+T*V_z0))/T_squre;

      double J=T+(a1*a1*T_cube/3+a1*b1*T_squre+b1*b1*T)+
          (a2*a2*T_cube/3+a2*b2*T_squre+b2*b2*T)+(a3*a3*T_cube/3+a3*b3*T_squre+b3*b3*T);
      if(J<optimal_cost)
      {
        optimal_cost=J;

        traj_coeff[0]=a1;
        traj_coeff[1]=a2;
        traj_coeff[2]=a3;
        traj_coeff[3]=b1;
        traj_coeff[4]=b2;
        traj_coeff[5]=b3;
        traj_coeff[6]=T;
      }


    }
#endif
        /// partial free
    double poly[5]={0}; // 0 1 2 3 4
    poly[0]= -3*((P_xf-P_x0)*(P_xf-P_x0)+(P_yf-P_y0)*(P_yf-P_y0)+(P_zf-P_z0)*(P_zf-P_z0));
    poly[1]=4*((P_xf-P_x0)*V_x0+(P_yf-P_y0)*V_y0+(P_zf-P_z0)*V_z0);
    poly[2]=-(V_x0*V_x0+V_y0*V_y0+V_z0*V_z0);
    poly[4]=1;
    Matrix4d Compan;
    Compan<<0,-poly[2],-poly[1],-poly[0],
        1,0,0,0,
        0,1,0,0,
        0,0,1,0;

    EigenSolver<MatrixXd> solver(Compan);
    auto T_vals=solver.eigenvalues();

    for(int i=0;i<4;i++)
    {
      auto T_val=T_vals(i);
      if(T_val.real()<=0||std::abs(T_val.imag())>0.00001)
        continue;

      //compute J
      double T=T_val.real();
      double T_squre=T*T,T_cube=T_squre*T;

      double a1=(3*(P_x0-P_xf+T*V_x0))/T_cube;
      double a2=(3*(P_y0-P_yf+T*V_y0))/T_cube;
      double a3=(3*(P_z0-P_zf+T*V_z0))/T_cube;
      double b1=-a1*T;
      double b2=-a2*T;
      double b3=-a3*T;

      double J=T+(a1*a1*T_cube/3+a1*b1*T_squre+b1*b1*T)+
          (a2*a2*T_cube/3+a2*b2*T_squre+b2*b2*T)+(a3*a3*T_cube/3+a3*b3*T_squre+b3*b3*T);
      if(J<optimal_cost)
      {
        optimal_cost=J;

        traj_coeff[0]=a1;
        traj_coeff[1]=a2;
        traj_coeff[2]=a3;
        traj_coeff[3]=b1;
        traj_coeff[4]=b2;
        traj_coeff[5]=b3;
        traj_coeff[6]=T;
      }


    }
    return optimal_cost;

执行效果1

为了让显示更加直观，添加了终点（绿色方块）和OBVP计算出的最优路径（淡蓝色）的显示功能。这里是末速度为零的情况
在这里插入图片描述 lattice graph中，红色表示与障碍物碰撞的路径，蓝色表示可行但不是最优的路径，绿色表示最优的路径。可以看出，OBVP并没有考虑障碍物的信息。

在这里插入图片描述我们用初速度表示lattice graph前向积分的终末速度。当目标比较近的时候，因为要考虑到速度约束（OBVP末速度为0），所以不能选择初速度与目标点方向一致的路径，而是选择了初速度最小的路径，因此产生了一个sharp turn。这种情况在下面的partial free条件下就很少出现。