使用GPS信息將設(shè)備匹配到實(shí)際所處道路上蛛株。道路默認(rèn)為兩個(gè)點(diǎn)一條直線，具體的GPS信息通過高德地圖取得掂器，事先要將所需要匹配的城市道路都下載至本地摊欠。GPS的坐標(biāo)是標(biāo)準(zhǔn)的地球坐標(biāo)丢烘，高德和百度地圖都是對(duì)實(shí)際坐標(biāo)進(jìn)行了偏移處理，由于我們道路采集的坐標(biāo)是高德坐標(biāo)系些椒，所以在這個(gè)工程中需要將實(shí)際的GPS坐標(biāo)轉(zhuǎn)換成高德坐標(biāo)系播瞳。

目錄 (Table of Contents)

[TOC]

道路匹配算法流程圖

流程圖

道路匹配算法原理

總體方案：采用基于道路拓?fù)浣Y(jié)構(gòu)的權(quán)重匹配法，主要有三方面：距離D免糕，方向（航向）A赢乓，歷史方向H。最后計(jì)算權(quán)重總和石窑，為每條候選路段計(jì)算權(quán)重總和W牌芋，，則將具有最高權(quán)重和的候選路段作為匹配路段松逊，將該候選路段的虛擬匹配點(diǎn)作為匹配點(diǎn)躺屁。

1.距離匹配度

距離匹配

由于GPS系統(tǒng)內(nèi)在的誤差，在一般情況下接收到的數(shù)據(jù)定位點(diǎn)會(huì)偏離車輛實(shí)際行駛路段经宏。通過對(duì)定位點(diǎn)P作其在各個(gè)候選道路上的投影犀暑，投影點(diǎn)位為P0，所以GPS點(diǎn)到線段最短距離為點(diǎn)到投影距離PP0.
當(dāng)投影點(diǎn)在道路上的投影點(diǎn)不在線段內(nèi)烁兰，則點(diǎn)到線的最短距離母怜，為點(diǎn)到線段兩端的最短距離。

2.方向匹配度

方向匹配

在對(duì)候選路段進(jìn)行判斷時(shí)缚柏，可以通過確定誤差區(qū)域在第一時(shí)間排除不想管的候選路段。由于GPS系統(tǒng)接受到的定位點(diǎn)軌跡與電子地圖中的道路存在著方向上的偏差碟贾，因此需要對(duì)兩者進(jìn)行比較并將此作為一個(gè)重要的判斷因素币喧。
從GPS系統(tǒng)中接受到的數(shù)據(jù)信息可知車輛的運(yùn)行方向，然后分別計(jì)算車輛在每條候選路段上的方向匹配度袱耽。假設(shè)GPS系統(tǒng)顯示的定位點(diǎn)軌跡為路線L1杀餐， L2為實(shí)際道路，則計(jì)算兩條方向向量的角度差cos(α-β)作為方向匹配度的值

3.歷史方向匹配

在進(jìn)行道路判斷時(shí)朱巨，我們會(huì)將每個(gè)GPS點(diǎn)（也就是車輛軌跡）存儲(chǔ)在內(nèi)存空間史翘，加入現(xiàn)在GPS點(diǎn)為P0,前四個(gè)數(shù)據(jù)點(diǎn)分別為P-5,P-4,P-3,P-2,P-1 我們計(jì)算P0 P-，P0 P-4 P0P-3 P0P-2, P0P-1 五條線路與道路方向的角度差并求平均作為歷史方向匹配度的值。

4.權(quán)重分配

1.靜態(tài)：將三個(gè)權(quán)重值平均分別為1/3,則每條道路的總得分為W=1/3*（D+A+H）琼讽，或者在之后的實(shí)際上路測(cè)試不斷調(diào)權(quán)重值得到對(duì)應(yīng)的經(jīng)驗(yàn)值
2.動(dòng)態(tài)： 自動(dòng)分配權(quán)重必峰，先假設(shè)距離、方向钻蹬、歷史方向吼蚁、的權(quán)重值相同。在此從第一時(shí)候開始问欠，分別對(duì)每條候選道路的距離肝匆、方向、實(shí)時(shí)方向匹配度進(jìn)行排序顺献。然后在每時(shí)刻都將做出三個(gè)因素的最高匹配度與次高匹配度的差值旗国。在此，設(shè)定一個(gè)閾值h注整，若在一個(gè)時(shí)刻某個(gè)因素差值最大能曾，并且大于閾值h，則可以認(rèn)為在該時(shí)刻设捐，該因素的權(quán)重值占比重最大借浊，提高其權(quán)重值。

源代碼（C++）

GPS數(shù)據(jù)解析

//"$GPRMC,075507.00,A,3027.40466,N,11425.31127,E,0.792,,120517,,,A*7F\r\n" /*數(shù)據(jù)詳解：$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh
 <1> UTC 時(shí)間萝招，hhmmss(時(shí)分秒)格式
 <2> 定位狀態(tài)蚂斤，A=有效定位，V=無效定位
 <3>緯度ddmm.mmmm(度分)格式(前面的0也將被傳輸)
 <4> 緯度半球N(北半球)或S(南半球)
 <5>經(jīng)度dddmm.mmmm(度分)格式(前面的0也將被傳輸)
  <6> 經(jīng)度半球E(東經(jīng))或W(西經(jīng))
  <7>地面速率(000.0~999.9節(jié)槐沼，前面的0也將被傳輸)
  <8>地面航向(000.0~359.9度曙蒸，以正北為參考基準(zhǔn)，前面的0也將被傳輸)
   <9> UTC 日期岗钩，ddmmyy(日月年)格式
   <10>磁偏角(000.0~180.0度纽窟，前面的0也將被傳輸)
   <11> 磁偏角方向，E(東)或W(西)
   <12>模式指示(僅NMEA01833.00版本輸出兼吓，A=自主定位臂港，D=差分，E=估算视搏，N=數(shù)據(jù)無效)
解析內(nèi)容：
1.時(shí)間审孽，這個(gè)是格林威治時(shí)間，是世界時(shí)間（UTC）浑娜，我們需要把它轉(zhuǎn)換成北京時(shí)間（BTC）佑力，BTC和UTC差了8個(gè)小時(shí)，要在這個(gè)時(shí)間基礎(chǔ)上加8個(gè)小時(shí)筋遭。
2. 定位狀態(tài)打颤，在接收到有效數(shù)據(jù)前暴拄，這個(gè)位是‘V’，后面的數(shù)據(jù)都為空编饺，接到有效數(shù)據(jù)后乖篷，這個(gè)位是‘A’，后面才開始有數(shù)據(jù)反肋。
 3. 緯度那伐，我們需要把它轉(zhuǎn)換成度分秒的格式，計(jì)算方法：如接收到的緯度是：4546.40891      4546.40891/100=45.4640891可以直接讀出45度, 4546.40891–45*100=46.40891, 可以直接讀出46分46.40891–46 =0.40891*60=24.5346讀出24秒, 所以緯度是：45度46分24秒石蔗。
4. 南北緯罕邀，這個(gè)位有兩種值‘N’（北緯）和‘S’（南緯）
5. 經(jīng)度的計(jì)算方法和緯度的計(jì)算方法一樣
6. 東西經(jīng)，這個(gè)位有兩種值‘E’（東經(jīng)）和‘W’（西經(jīng)）
7. 速率养距，這個(gè)速率值是海里/時(shí)诉探，單位是節(jié)，要把它轉(zhuǎn)換成千米/時(shí)棍厌，根據(jù)：1海里=1.85公里肾胯，把得到的速率乘以1.85。
8. 航向耘纱，指的是偏離正北的角度
9. 日期敬肚，這個(gè)日期是準(zhǔn)確的，不需要轉(zhuǎn)換*/

GPS坐標(biāo)轉(zhuǎn)高德坐標(biāo)系（火星坐標(biāo)系）/百度坐標(biāo)系

/*******************************GPS坐標(biāo)系轉(zhuǎn)換***********************************/

const double pi = 3.14159265358979324;
const double a = 6378245.0;
const double ee = 0.00669342162296594323;
const  double x_pi = 3.14159265358979324 * 3000.0 / 180.0;

bool outOfChina(double lat, double lon)
{
    if (lon < 72.004 || lon > 137.8347)
    {
        return true;
    }

    if (lat < 0.8293 || lat > 55.8271)
    {
        return true;
    }

    return false;
}

double transformLat(double x, double y)
{
    double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
    ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
    ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;

    return ret;
}

double transformLon(double x, double y)
{
    double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
    ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
    ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;

    return ret;
}

/**
 * 地球坐標(biāo)轉(zhuǎn)換為火星坐標(biāo)（WGS-84到GCJ-02）
 * World Geodetic System ==> Mars Geodetic System
 *
 * @param wgLat  地球坐標(biāo)
 * @param wgLon
 *
 * mglat,mglon 火星坐標(biāo)
 */
void Transform2Mars(double wgLat, double wgLon,double &mgLat,double &mgLon)
{
    if (outOfChina(wgLat, wgLon))
    {
        qDebug() << "outOfChina";
        mgLat = wgLat;
        mgLon = wgLon;
        return;
    }

    double dLat = transformLat(wgLon - 105.0, wgLat - 35.0);
    double dLon = transformLon(wgLon - 105.0, wgLat - 35.0);
    double radLat = wgLat / 180.0 * pi;
    double magic = sin(radLat);

    magic = 1 - ee * magic * magic;
    double sqrtMagic = sqrt(magic);
    dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
    dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
    mgLat = wgLat + dLat;
    mgLon = wgLon + dLon;
    qDebug() << "mgLat111=" << mgLat << "mgLon111=" << mgLon;
}

/**
 * 火星坐標(biāo)轉(zhuǎn)換為百度坐標(biāo)（GCJ-02到BD-09）
 * @param gg_lat
 * @param gg_lon
 */
void BD_encrypt(double gg_lat, double gg_lon,double &bd_lat,double &bd_lon)
{
    double x = gg_lon, y = gg_lat;
    double z = sqrt(x * x + y * y) + 0.00002 * sin(y * x_pi);
    double theta = atan2(y, x) + 0.000003 * cos(x * x_pi);

    bd_lon = z * cos(theta) + 0.0065;
    bd_lat = z * sin(theta) + 0.006;
}

/**
 * 百度轉(zhuǎn)火星
 * @param bd_lat
 * @param bd_lon
 */
void BD_decrypt(double bd_lat, double bd_lon,double &gg_lat,double &gg_lon)
{
    double x = bd_lon - 0.0065, y = bd_lat - 0.006;
    double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);
    double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);

    gg_lon = z * cos(theta);
    gg_lat = z * sin(theta);
}

球面坐標(biāo)轉(zhuǎn)換成平面坐標(biāo)

struct geo{
    int projectType;   //分帶選擇：1--->3°帶投影    2--->6°帶投影
    int  parameter;    //參考橢球:1---->克拉索夫斯基橢球    2---->1975國際協(xié)議橢球    3---->WGS-84橢球
    double geo_B_d;    //經(jīng)度： geo_B_d--->度
    double geo_B_m;    //經(jīng)度： geo_B_m--->分
    double geo_B_s;    //經(jīng)度： geo_B_s--->秒
    double geo_L_d;    //維度： geo_L_d--->度
    double geo_L_m;    //緯度： geo_L_m--->分
    double geo_L_s;    //維度： geo_L_s--->秒
    double centerLong;  //中央子午線經(jīng)度
    double plane_X;     //平面坐標(biāo)x
    double plane_Y;     //平面坐標(biāo)y
    double a;           //橢球參數(shù)a
    double b;           //橢球參數(shù)b
};

void gaussConvert(geo* m)     //坐標(biāo)轉(zhuǎn)換束析，將大地坐標(biāo)轉(zhuǎn)換成直角坐標(biāo)
{

        double N;
        double t;
        double Eta;
        double X;
        double A0,A2,A4,A6,A8;
        double RadB;

        double Rou;
        Rou=180*3600/PI;

        double a,b,e1,e2;  //橢球參數(shù)

        LawJudge::getParameter(m);
        a=m->a;
        b=m->b;
        e1=sqrt(a*a-b*b)/a;
        e2=sqrt(a*a-b*b)/b;

        double l;
        double L0;
        LawJudge::getProjectType(m);
        L0=m->centerLong;
        double L;
        L=m->geo_L_d+m->geo_L_m/60+m->geo_L_s/3600;
        l=(L-L0)*3600;

        RadB=(m->geo_B_d+m->geo_B_m/60+m->geo_B_s/3600)*PI/180;

        N=a/sqrt(1-e1*e1*sin(RadB)*sin(RadB));
        t=tan(RadB);
        Eta=e2*cos(RadB);

        A0=1+3.0/4*e1*e1+45.0/64*pow(e1,4)+350.0/512*pow(e1,6)+11025.0/16384*pow(e1,8);
        A2=-1.0/2*(3.0/4*e1*e1+60.0/64*pow(e1,4)+525.0/512*pow(e1,6)+17640.0/16384*pow(e1,8));
        A4=1.0/4*(15.0/64*pow(e1,4)+210.0/512*pow(e1,6)+8820.0/16384*pow(e1,8));
        A6=-1.0/6*(35.0/512*pow(e1,6)+2520.0/16384*pow(e1,8));
        A8=1.0/8*(315.0/16384*pow(e1,8));
        X=a*(1-e1*e1)*(A0*RadB+A2*sin(2*RadB)+A4*sin(4*RadB)+A6*sin(6*RadB)+A8*sin(8*RadB));

        //計(jì)算平面橫軸
        m->plane_X=X+N/(2*Rou*Rou)*sin(RadB)*cos(RadB)*l*l+
            N/(24*pow(Rou,4))*sin(RadB)*pow(cos(RadB),3)*(5-t*t+9*Eta*Eta+4*pow(Eta,4))*pow(l,4)+
            N/(720*pow(Rou,6))*sin(RadB)*pow(cos(RadB),5)*(61-58*t*t+pow(t,4))*pow(l,6);

        //計(jì)算平面縱軸
        m->plane_Y=N/Rou*cos(RadB)*l+
            N/(6*pow(Rou,3))*pow(cos(RadB),3)*(1-t*t+Eta*Eta)*pow(l,3)+
            N/(120*pow(Rou,5))*pow(cos(RadB),5)*(5-18*t*t+pow(t,4)+14*Eta*Eta-58*Eta*Eta*t*t)*pow(l,5);

       //計(jì)算中央經(jīng)線經(jīng)度
      //  centerLong=L0;

}


geo typegeo={0,0,0,0,0,0,0,0,0,0,0,0,0};
typegeo.parameter=1;
typegeo.projectType=1;
typegeo.geo_B_d=******;  //測(cè)試經(jīng)度點(diǎn)
typegeo.geo_L_d=******;   //測(cè)試緯度點(diǎn)
gaussConvert(&typegeo);
double x=typegeo.plane_X;
double y=typegeo.plane_Y;

點(diǎn)到線段的最短距離

double distancePointLine(double x,double y,double x1,double y1,double x2,double y2)  //p(x,y)實(shí)際點(diǎn) A(x1,y1) B(x2,y2) 點(diǎn)到線段的最短距離
{
    double R=(x2-x1)*(x-x1)+(y2-y1)*(y-y1);
    double D=(x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
    if(R<=0)                                            //最短距離等于AP
    {
        return sqrt(pow((x1-x),2)+pow((y1-y),2));
    }
    else if(R>=D)                                       //最短距離等于BP
    {
        return sqrt(pow((x2-x),2)+pow((y2-y),2));
    }
    else                                                //最短距離等于垂線
    {
        double r=R/D;
        double px=x1+(x2-x1)*r;
        double py=y1+(y2-y1)*r;
        return sqrt(pow((px-x),2)+pow((py-y),2));
    }

}

兩條線段的夾角

double LawJudge::straightAngle(double x,double y,double x1,double y1)   //求兩條方向向量A=(x,y)和 B=(x1,y1)的夾角
{
    double A=x*x1+y*y1;
    double B;
    if(A==0)
    {
        B=1;
    }
    else
    {
        B=(sqrt(pow(x,2)+pow(y,2)))*(sqrt(pow(x1,2)+pow(y1,2))) ;
    }
    double angel=abs(A/B);
    double hudu=acos(angel);         //弧度
    double jiaodu=(180*hudu)/PI;     //角度
    return jiaodu;
}