RM 整车预测（半球状模型）

        我作为佛山大学醒狮战队的算法组成员，首先感谢历届醒狮算法组成员的不懈努力，是他们敢想敢干和坚持不懈的精神，为自瞄开发打下坚实的技术基础，才让醒狮拥有整车预测成为现实。

        其次感谢邀请我们进行技术交流活动的华南虎战队和焊匠战队，通过与他们的步兵对抗，我们深知我们算法上的不足以及改进方法和前进方向。

        最后感谢各兄弟学校，醒狮这一路走来，离不开兄弟学校提供的帮助与支持，国赛开始了，希望大家玩的尽兴，取得让自己满意的成绩。

        在此声明，本文章分享的是我个人的思路和部分代码，本人水平不高，望各位大佬多多指教。

思路

        我在原来得到目标装甲板后，不再直接瞄准，而是检测目标装甲板的同id（数字）装甲板组，然后根据装甲板组各装甲位姿解算出车的质点位置，然后通过车的质点位置和装甲板组各装甲位姿，对装甲板组进行平移和旋转预测，最后根据预测的位姿情况选择最优装甲板。本文章着重讲解车的质点位置解算和装甲板旋转预测。

具体实现

1、车的质点解算

       if(same_id_armors.size()==2){
            miss_flag=0;
            double rx1=same_id_armors[0].rvec[0];
            double ry1=same_id_armors[0].rvec[1];
            double rz1=same_id_armors[0].rvec[2];
            double rx2=same_id_armors[1].rvec[0];
            double ry2=same_id_armors[1].rvec[1];
            double rz2=same_id_armors[1].rvec[2];
            double x1=centers[0][0];
            double y1=centers[0][1];
            double z1=centers[0][2];
            double x2=centers[1][0];
            double y2=centers[1][1];
            double z2=centers[1][2];

            double y_average=(y1+y2)/2;
            double num2=(x2-x1)*y1/(ry2*rx1-ry1*rx2);
            car_position(0,0)=num2*rx2+centers[1][0];
            car_position(1,0)=num2*ry2+y_average;
            car_position(2,0)=num2*rz2+z2;
            return true;
        }

对于非平步，当车旋转一定角度时我们可以检测到其两块装甲，装甲的平移矩阵和旋转矩阵，我们可以由此建立垂直装甲板、垂足为装甲板中心的两条直线并求交点既车的质点。

为防止车的装甲上下错落导致无解，我们求出在装甲在y轴上的中点坐标。

        else if(car_position(0,0)==-1&&car_position(1,0)==-1&&car_position(2,0)==-1&&miss_flag<4){

                 double x1=centers[0][0];
                 double y1=centers[0][1];
                 double z1=centers[0][2];

                 car_position(0,0)=x1;
                 car_position(1,0)=y1;
                 car_position(2,0)=z1;
                 miss_flag++;
                 return true;

        }//else2

对于非平步，当车长时间只露出一块装甲板时，则干脆把装甲板的中心点当作车的质点。

注意，此代码的针对对象是非平步。平步代码我还在想······

2、装甲板组的旋转预测

bool CarDetector::carRotatedSolver(){
    static vector<double>delta_angles;
    for(int i=0;i<same_id_armors.size();i++){
        if(same_id_armors.size()==2){
            double temp=-1.0;
            double minus_num=0.0;//右正左负
            static double sum=same_id_armors[0].rvec(1,0)+same_id_armors[1].rvec(1,0);
            delta_angles.push_back(sum/2);
            if(delta_angles.size()>=3) delta_angles.erase(delta_angles.begin());
            if(temp==-1.0) temp=sum;
            else{
                minus_num=sum-temp;
                if(minus_num>=1.071) car_clock_wise=1;
                else if(minus_num<=-1.071) car_clock_wise=-1;
                else car_clock_wise=0;
                temp=sum;
            }
        }
        else{
            double temp=-1.0;
            double minus_num=0.0;//右正左负
            static double sum2=same_id_armors[0].rvec(1,0);
            delta_angles.push_back(sum2);
            if(delta_angles.size()>=3) delta_angles.erase(delta_angles.begin());
            if(temp==-1.0) temp=sum2;
            else{
                minus_num=sum2-temp;
                if(minus_num>=0.5236) car_clock_wise=1;
                else if(minus_num<=-0.5236) car_clock_wise=-1;
                else car_clock_wise=0;
                temp=sum2;
            }
        }
    }

    if(delta_angles.size()==2){
        double v_angle=delta_angles[1]/time;
        double a_angle=(delta_angles[1]/time-delta_angles[0]/time)/time;
        Eigen::Vector3d see_angle;
        see_angle<<delta_angles[1],v_angle,a_angle;
        car_rotated_condition = kalman_rotated.update(see_angle,time)(0,0);
        return true;
    }

    else return false;
}

bool CarDetector::pridectArmorWithCarState(cv::Mat &src,vector<ArmorInfo> armor_all_list,ArmorInfo armor_all_last,AngleSolver &angle_solver,GetNum &getnum){
    if(sameIdArmorSearch(src,armor_all_list,armor_all_last,getnum)){
        if(carCenterSolver(angle_solver)){
            if(carRotatedSolver()&&pridectCarMove()){
                bool get_best=0;
                std::cout<<"we get all we want about car!!"<<endl;
                vector<double> armors_next_angles;
                for(int i=0;i<same_id_armors.size();i++){
                    armors_next_angles.push_back(same_id_armors[i].rvec(1,0));
                    armors_next_angles[i]+=car_rotated_condition;
                }
                for(int i=0;i<armors_next_angles.size();i++){
                    if(abs(armors_next_angles[i]-1.5707)>0.7853)
                        best_armors.push_back(same_id_armors[i]);
                }

                if(best_armors.size()>1){
                    for(int i=0;i<best_armors.size();i++){
                        if(car_clock_wise==1){
                            if(best_armors[i].rvec(1,0)>1.5707){
                                best_armor=best_armors[i];
                                get_best=1;
                                break;
                            }
                        }
                        else if(car_clock_wise==-1){
                            if(best_armors[i].rvec(1,0)<1.5707){
                                best_armor=best_armors[i];
                                get_best=1;
                                break;
                            }
                        }
                    }
                }
                else if(best_armors.size()==1){
                    best_armor=best_armors[0];
                    get_best=1;
                }
                if(get_best==1){
                    double r=sqrt(pow(best_armor.tvec(0,0)-car_position(0,0),2)+pow(best_armor.tvec(1,0)-car_position(1,0),2)+pow(best_armor.tvec(1,0)-car_position(1,0),2));
                    if(r>0){

                        Eigen::Vector3d cos_rotated_angles;
                        cos_rotated_angles<<cos(best_armor.rvec(0,0)),cos(best_armor.rvec(1,0)),cos(best_armor.rvec(2,0));
                        armor_predict_pos=car_position+cos_rotated_angles*r+car_next_postion;
                    }
                    else{//说明same_id_armors只有一个装甲板
                        armor_predict_pos=car_position+car_next_postion;
                    }
                }
                else{
                    std::cout<<"no best_armorsssssss!!!!!!!!"<<std::endl;
                    return false;
                }
            }
        }
    }

}

装甲板的数量、旋转向量变化反应车的旋转情况（carRotatedSolver），根据旋转情况、旋转半径、旋转方向得到装甲板组预测后的位置并选择最优装甲板（pridectArmorWithCarState）。

总结

        理论上该代码只能得到对方非平步的前半圆部分的装甲运动情况，对比君佬的模型，本人的代码必然是逊色很多。由于裁判系统被官方收回去了，我又不会仿真，所以代码精准度待测试······并且由于代码冗余，我怀疑该代码的解算速度跟不上万向轮的旋转速度，权当分享出来让大家看个乐。