#include "dps/inc/dps.h" #include "qmath.h" #include "bus/inc/bus_manager.h" #include void Dps::DistanceCalculate(double currentLongitude, double currentLatitude, double targetLongitude, double targetLatitude,DistanceInfo &returnDis) { DistanceInfo distance; distance.disx=qAbs(currentLongitude-targetLongitude)*qCos(targetLatitude/180*M_PI)*111700; distance.disy=qAbs(currentLatitude-targetLatitude)*111700; distance.dis=qSqrt(distance.disx*distance.disx+distance.disy*distance.disy);//当前点与临时点坐标差 returnDis = distance; } double Dps::GetHead(double aLongitude,double aLatitude,double bLongitude,double bLatitude) { double dx; double dy; double angle; dx = (bLongitude - aLongitude) * 111700 * cos(aLatitude / 180 * M_PI);//当前点与最近点经度距离 dy = (bLatitude - aLatitude) * 111700;//当前点与最近点纬度距离 if(dx>-0.0001&&dx<0.0001) { if(dy>0) angle=0; else angle=180; } else { angle = qAtan((double)dy / dx) / M_PI * 180;//当前点与最近点两点连线与正北的夹角 if (dx > 0) angle = 90 - angle; else angle = 270 - angle; } return angle; } void Dps::GetUsualPointKey(const QList &gpsList,double currentLongitude,double currentLatitude,double currentHead,double vehicleSpeed,const KeyPointInfo &keyPointLast,KeyPointInfo &retKeyPoint) { KeyPointInfo keyPoint;//关键点信息 int gpsListsize;//传入路径list大小 GPSInfo memoryGps;//临时GPS信息 double memoryLongitude;//临时经度 double memoryLatitude;//临时纬度 double memoryHead;//临时航向角 double disHead;//航向差 int preNum;//检索路径点个数 DistanceInfo dis;//两点间距 double currentGear;//当前档位 int desiredGear;//期望档位 double minDistance = MINDISTANCE;//寻找keyPoint的最小值 double rtkSpd; int begine; double preDis; keyPoint = keyPointLast; begine = keyPointLast.num; gpsListsize=gpsList.size(); DistanceInfo pointdistance; DistanceCalculate(gpsList.at(9).iLongitude,gpsList.at(9).iLatitude,gpsList.at(10).iLongitude,gpsList.at(10).iLatitude,pointdistance); pointdistance.dis=qAbs(pointdistance.dis); //qDebug()<<"间距:"< 14) { preNum = qRound(14/pointdistance.dis); } m_datapool->GetValue(desiredGear,DataPool::DESIRESHIFT); m_datapool->GetValue(currentGear,DataPool::VEHSFTPSNG); QVariant ina; ina = m_datapool->GetValue(DataPool::RTKDATA,0); rtkSpd = ina.value().gSpd; if((desiredGear==2&¤tGear==4)||(desiredGear==4&¤tGear==2)) { preDis = 20; preNum = qRound(preDis/pointdistance.dis); begine =keyPointLast.num+qRound(6/pointdistance.dis); } int jLimit; //qDebug()<<"preNum:"<M_PI) disHead=disHead-2*M_PI; if(disHead<-M_PI) disHead=2*M_PI+disHead; if((desiredGear==2&¤tGear==4)||(desiredGear==4&¤tGear==2)) { if((minDistance > dis.dis)&&((disHead>90)&&(disHead<270))) { minDistance = dis.dis; keyPoint.num=memoryGps.num; keyPoint.id=memoryGps.id; keyPoint.type=memoryGps.type; pointFlag = true; timeCount = 0; historygear=currentGear; } } else { if((minDistance > dis.dis)&&((disHead<90)||(disHead>270))) { minDistance = dis.dis; keyPoint.num=memoryGps.num; keyPoint.id=memoryGps.id; keyPoint.type=memoryGps.type; pointFlag = true; timeCount = 0; } } } if(!pointFlag) { if(timeCount == 0) { newPointtime.restart(); } timeCount++; if((vehicleSpeed > 1||rtkSpd >1) && newPointtime.elapsed() > 1000) { GetGlobalPointKey(gpsList,currentLongitude,currentLatitude,currentHead,keyPointLast.num,keyPoint); timeCount = 0; } else { keyPoint = keyPointLast; } } retKeyPoint = keyPoint; } void Dps::LateralPlan()//横向规划:输出关键点、横向偏差、航向偏差 { int ij; double tLng;//实时经度 double tLat;//实时纬度 double tHead;//实时航向角 double currentHead;//实时航向角 SpecialCurve specialCurve; double mCur;//临时曲率 double mLngs; double mLats; PrePoint prePoint; int prepointcount; int prepointcount2; double prepointdis; double prepointdis2; double m_angle = 0;//航向偏差 double m_anglex[5]; double pointangle;//当前点与预瞄点连线夹角 double pointangle2;//当前点与关键点连线夹角 double near_angle; double vehicleSpeed;//当前车速 double strAng;//当前方向盘转角-0811 KeyPointInfo keyPointLast;//上次计算的关键点 KeyPointInfo keyPointLastRef;//上次计算的关键点 QList pGpsList; double curveatKeypoint; //关键点附近的曲率 double curveatPreview; //预瞄点1附近的曲率 double curveatPreview2; //预瞄点2附近的曲率 double pointDisx = 0; //横向偏差 QVariant ina; ina = m_datapool->GetValue(DataPool::RTKDATA,0); tLng = ina.value().gLng; tLat = ina.value().gLat; tHead = ina.value().gHead; currentHead = ina.value().gHead; QVariant inb; inb = m_datapool->GetValue(DataPool::VEHSPD,0); vehicleSpeed = inb.value(); QVariant inc; inc = m_datapool->GetValue(DataPool::KEYPOINTINFO,0); keyPointLast = inc.value(); auto ind = m_datapool->GetValue(DataPool::TMS_GPS_PATH, 0); auto *gpsinfo = ind.value(); QList *normPath = (QList*)gpsinfo; if(normPath) { pGpsList=*normPath; } QVariant ine;//取方向盘转角值 ine = m_datapool->GetValue(DataPool::VEHSTRANG,0); strAng = ine.value(); GetUsualPointKey( pGpsList,tLng,tLat,tHead,vehicleSpeed,keyPointLast, dpsKeyPoint);//计算车辆位置的关键点 QVariant outa; outa = QVariant::fromValue(dpsKeyPoint); m_datapool->SetValue(DataPool::KEYPOINTINFO,outa); pointangle2=GetHead(tLng,tLat, pGpsList.at(dpsKeyPoint.num).iLongitude, pGpsList.at(dpsKeyPoint.num).iLatitude); /*计算预瞄轨迹*/ QList globalUTMList = m_datapool->GetValue(DataPool::UTMROUT, 0).value>(); double cur_X,cur_Y,cur_heading; m_gpsCoorTrans.LatLonToUTMXY(tLat, tLng, 50, cur_X, cur_Y); cur_heading=currentHead/180*M_PI; float path_length = 0.0,dx=0.0,dy=0.0; std::vector ref_traj; for (int i = dpsKeyPoint.num; i < globalUTMList.size()-1; i++) { //车辆坐标系下参考轨迹 UTMInfo traj; double x_t,y_t,heading_t; x_t = globalUTMList.at(i).x - cur_X; y_t = globalUTMList.at(i).y - cur_Y; traj.y= x_t * sin(M_PI*3.0/2.0+cur_heading) + y_t * cos(M_PI*3.0/2.0+cur_heading); traj.x = x_t * cos(M_PI*3.0/2.0+cur_heading) - y_t * sin(M_PI*3.0/2.0+cur_heading);//rtk to rear para广德的车要减0.9 traj.yaw = -(dpsGlobalPathList.at(i).iHead/180*M_PI - cur_heading); if(traj.yaw < -M_PI){ traj.yaw = traj.yaw + 2*M_PI; } if(traj.yaw > M_PI){ traj.yaw = traj.yaw - 2*M_PI; } traj.k=dpsGlobalPathList.at(i).iCurvature; ref_traj.push_back(traj); if(i > 0){ dx = globalUTMList.at(i).x - globalUTMList.at(i-1).x ; dy = globalUTMList.at(i).y - globalUTMList.at(i-1).y ; path_length += sqrt(dx*dx+dy*dy); } if((path_length > vehicleSpeed/3.6*2)&&(path_length >5)){ break; } } double dist = 0; std::vector mpc_ref_traj; if(mpc_ref_traj.size()>0) { mpc_ref_traj.clear(); } mpc_ref_traj.push_back(ref_traj.at(0)); for(int i = 1;i < ref_traj.size();i++){ dx = ref_traj.at(i).x - ref_traj.at(i-1).x; dy = ref_traj.at(i).y - ref_traj.at(i-1).y; dist += sqrt(dx*dx + dy*dy); if(dist > (vehicleSpeed/3.6*0.02)){ mpc_ref_traj.push_back(ref_traj.at(i-1)); dist = dist - vehicleSpeed/3.6*0.02; } if(mpc_ref_traj.size() > 30){ break; } } /*将横向偏差、航向偏差、关键点、预瞄点、关注点曲率信息放入datapool*/ angleExpect.biaDistance=pointDisx; angleExpect.biaHead=m_angle; QVariant outb; outb = QVariant::fromValue(angleExpect); m_datapool->SetValue(DataPool::HORIZONTALDISIRED,outb); prePoint.prePointCount=prepointcount; prePoint.prePointDistance=prepointdis; QVariant outc; outc = QVariant::fromValue(prePoint); m_datapool->SetValue(DataPool::PREPOINT,outc); QVariant outg; outg = QVariant::fromValue(mpc_ref_traj); m_datapool->SetValue(DataPool::REFTRAIL,outg); }