最短路徑算法導航 校內(nèi)所有建筑和道路并具有道路是否可通行的標志

/*
校園導游咨詢
[問題描述]
設(shè)計一個校園導游程序，為來訪的客人提供各種信息查詢服務(wù)。
[基本要求]
(1)設(shè)計你的學校的校園平面圖，所含景點不少于10個。以圖中頂點表示校內(nèi)各景點，存放景點名稱、代號、簡介等信息；以邊表示路徑，存放路徑長度等相關(guān)信息。
(2)為來訪客人提供圖中任意景點相關(guān)信息的查詢。
(3)為來訪客人提供圖中任意景點的問路查詢，即查詢?nèi)我鈨蓚€景點之間的一條最短的簡單路徑。
[實現(xiàn)提示]
一般情況下，校園的道路是雙向通行的，可設(shè)校園平面圖是一個無向網(wǎng)。頂點和邊均含有相關(guān)信息。

一需求分析
1從中北大學平面圖中選取10個大家熟悉的景點，抽象成一個無向帶權(quán)圖（如圖所示）。以圖中頂點表示景點，邊上的權(quán)值表示兩地的距離。
2本程序的目的是為用戶提供路徑咨詢和景點查詢。根據(jù)用戶指定的始點和終點輸出相應(yīng)路徑或者根據(jù)用戶指定的景點輸出景點的信息。

南

北
二、概要設(shè)計
1本文采用的數(shù)據(jù)結(jié)構(gòu)
*/

/*包含頭文件*/
#include
#include

/*定義符號常量*/
#define INT_MAX 10000
#define n 10

/*定義全局變量*/
int cost[n][n];/* 邊的值*/
int shortest[n][n];/* 兩點間的最短距離*/
int path[n][n];/* 經(jīng)過的景點*/

/*自定義函數(shù)原型說明*/
void introduce();
int shortestdistance();
void floyed();
void display(int i,int j);
2個人分工
（1）景點信息查詢
（2）兩景點的最短距離
（3）兩個景點之間的路徑

三、詳細設(shè)計

void main()
{/*主函數(shù)*/
int i,j;
char k;
for(i=0;i<=n;i++)
for(j=0;j<=n;j++)
cost[i][j]=INT_MAX;
cost[1][3]=cost[3][1]=2;
cost[2][3]=cost[3][2]=1;
cost[2][4]=cost[4][2]=2;
cost[3][10]=cost[10][3]=4;
cost[1][10]=cost[10][1]=4；
cost[2][10]=cost[10][2]=4；
cost[4][10]=cost[10][4]=4；
cost[1][4]=cost[4][1]=5;
cost[4][5]=cost[5][4]=3;
cost[4][9]=cost[9][4]=4;
cost[5][9]=cost[9][5]=8;
cost[5][7]=cost[7][5]=4;
cost[5][6]=cost[6][5]=2;
cost[6][7]=cost[7][6]=1;
cost[7][8]=cost[8][7]=3;
cost[8][6]=cost[6][8]=4;
cost[1][1]=cost[2][2]=cost[3][3]=cost[4][4]=cost[5][5]=0;
cost[6][6]=cost[7][7]=cost[8][8]=cost[9][9]=cost[10][10]=0;
while(1)
{
printf("----------------歡迎使用中北大學導游系統(tǒng)!----------------\n");
printf("1.景點信息查詢………請按 i (introduc)鍵\n");
printf("2.景點最短路徑查詢…請按 s (shortestdistance)鍵\n");
printf("3.退出系統(tǒng)……………請按 e (exit)鍵\n");
printf("學校景點列表:\n");
printf("1:學校南門 ");
printf("2:學生公寓 ");
printf("3:柏林園 ");
printf("4:餐廳 ");
printf("5:體育館\n");
printf("6:圖書館 ");
printf("7:重點實驗室 ");
printf("8:主樓 ");
printf("9:科藝苑 ");
printf("10:國防生公寓\n");
printf("請選擇服務(wù):");
scanf("\n%c",k);
switch(k)
{
case 'i':
printf("進入景點信息查詢:");
introduce();
break;
case 's':
printf("進入最短路徑查詢:");
shortestdistance();
break;
case 'e':
exit(0);
default:
printf("輸入信息錯誤！\n請輸入字母i或s或e.\n");
break;
}
}
}/*main*/

void introduce()
{/*景點介紹*/
int a;
printf("您想查詢哪個景點的詳細信息?請輸入景點編號:");
scanf("%d",a);
getchar();
printf("\n");
switch(a)
{
case 1:
printf("1:學校南門\n\n 學校的正門，前面豎立著一尊彭德華的石像，氣勢宏偉。\n\n");break;
case 2:
printf("2:學生公寓集中的地方。 \n\n");break;
case 3:
printf("3:柏林園\n\n 晨讀鍛煉得地方。\n\n");break;
case 4:
printf("4:餐廳\n\n 學生老師就餐的地方\n\n");break;
case 5:
printf("5:體育館\n\n 體育館\n\n 學生上體育課及運動的場地，設(shè)有田徑場、足球場、籃球場等。\n\n");break;
case 6:
printf("6:圖書館\n\n 學校信息資源中心，內(nèi)設(shè)大量的自習室。\n\n");break;
case 7:
printf("7:重點實驗室\n\n 我校的研究科研中心\n\n");break;
case 8:
printf("8:主樓\n\n 學校行政辦公的主樓。\n\n");break;
case 9:
printf("9:科藝苑\n\n 有咖啡廳和放映室。\n\n\n");break;
case 10:
printf("10: 國防生公寓\n\n 國防生居住地地方。\n\n");break;
default:
printf("景點編號輸入錯誤！請輸入1->10的數(shù)字編號！\n\n"); break;
}
}/*introduce*/

int shortestdistance()
{/*要查找的兩景點的最短距離*/
int i,j;
printf("請輸入要查詢的兩個景點的編號(1->10的數(shù)字編號并用','間隔):");
scanf("%d,%d",i,j);
if(i>n||i<=0||j>n||j<0)
{
printf("輸入信息錯誤！\n\n");
printf(" 請輸入要查詢的兩個景點的編號(1->10的數(shù)字編號并用','間隔):\n");
scanf("%d,%d",i,j);
}
else
{
floyed();
display(i,j);
}
return 1;
}/*shortestdistance*/

void floyed()
{/*用floyed算法求兩個景點的最短路徑*/
int i,j,k;
for(i=1;i<=n;i++)
for(j=1;j<=n;j++)
{
shortest[i][j]=cost[i][j];
path[i][j]=0;
}
for(k=1;k<=n;k++)
for(i=1;i<=n;i++)
for(j=1;j<=n;j++)
if(shortest[i][j]>(shortest[i][k]+shortest[k][j]))
{/*用path[][]記錄從i到j(luò)的最短路徑上點j的前驅(qū)景點的序號*/
shortest[i][j]=shortest[i][k]+shortest[k][j];
path[i][j]=k;
path[j][i]=k;
}
}/*floyed*/

void display(int i,int j)
{/* 打印兩個景點的路徑及最短距離 */
int a,b;
a=i;
b=j;
printf("您要查詢的兩景點間最短路徑是:\n\n");
if(shortest[i][j]!=INT_MAX)
{
if(i {
printf("%d",b);
while(path[i][j]!=0)
{/* 把i到j(luò)的路徑上所有經(jīng)過的景點按逆序打印出來*/
printf("<-%d",path[i][j]);
if(i j=path[i][j];
else
i=path[j][i];
}
printf("<-%d",a);
printf("\n\n");
printf("(%d->%d)最短距離是:%d米\n\n",a,b,shortest[a][b]);
}
else
{
printf("%d",a);
while(path[i][j]!=0)
{/* 把i到j(luò)的路徑上所有經(jīng)過的景點按順序打印出來*/
printf("->%d",path[i][j]);
if(i j=path[i][j];
else
i=path[j][i];
}
printf("->%d",b);
printf("\n\n");
printf("(%d->%d)最短距離是:%5d米\n\n",a,b,shortest[a][b]);
}
}
else
printf("輸入錯誤！不存在此路！\n\n");
printf("\n");
}/*display*/

GNSS+IMU+MM車載組合導航系統(tǒng)

GNSS+IMU+MM車載組合導航系統(tǒng)

前言：近年來，隨著定位業(yè)務(wù)的迅速發(fā)展，用戶對于車載端定位精度提出了越來越高的要求，由原來的導航級逐漸更替到車道級。特別是在城市峽谷環(huán)境下（高樓、高架），用戶無法接收到GNSS信號或GNSS信號受干擾，導致GNSS無定位結(jié)果或定位精度差。這是“有源定位”固有的缺點，無法從算法上來克服。針對這個問題，以GNSS+IMU等多傳感器融合方案越來越受到重視，因為“無源定位”的IMU恰好可以彌補衛(wèi)星定位的短板。

基礎(chǔ)原理

導航衛(wèi)星系統(tǒng)(GNSS)

全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System）是一種依靠衛(wèi)星衛(wèi)星的偽距載波、星歷、時間以及鐘差等信息進行實時定位的空基無線電導航系統(tǒng)，能在地球表面或近地空間的任何地點為用戶提供全天候的三維坐標和速度以及時間信息。GNSS系統(tǒng)的優(yōu)點是精度高、誤差穩(wěn)定不發(fā)散，但容易受到周圍環(huán)境影響，比如樹木樓房遮擋，鏡面等高反射物體引起的多路徑效應(yīng)。

慣性導航系統(tǒng)(IMU)

慣性導航系統(tǒng)（Inertial Navigation System）是一種不依賴于外部信息、也不向外部輻射能量（如無線電導航那樣）的自主式導航系統(tǒng)，主要使用慣性測量單元IMU（Inertial measurement?unit）。其工作環(huán)境不僅包括空中、地面，還可以在水下。慣性導航的基本工作原理是以牛頓力學定律為基礎(chǔ)，通過測量載體在慣性參考系的加速度，將它對時間進行積分，且把它變換到導航坐標系中，就能夠得到在導航坐標系中的速度、偏航角和位置等信息。其優(yōu)點是工作不需要通時，安裝位置隨意，定位范圍全場景，但定位精度不高，且誤差隨時間發(fā)散。與GNSS導航系統(tǒng)互補。

地圖匹配技術(shù)(MM)

地圖匹配技術(shù)MM（Map matching）是結(jié)合用戶位置信息和地圖數(shù)據(jù)，推算用戶在地圖上道路的準確位置，輔助車載導航的精準控制。

航位推算法(DR)

航位推算法DR（Dead Reckoning）是一種跟蹤導航算法，在獲取載體當前時刻坐標位置的前提下，依靠慣性測量單元IMU取得的同周期內(nèi)載體移動的距離和方位，進而推算下一時刻位置。在此文介紹中，主要講建立在已有 GNSS系統(tǒng) 解算下，IMU輔助進行組合導航的算法。

車載定位的痛點

車載導航定位發(fā)展已經(jīng)很久，但隨著精度要求越來越高，車載定位的一些問題也逐漸浮現(xiàn)：

偏航重算：是指在高架或城市峽谷，信號遮擋引起位置點漂移；

無法定位：是指在無信號區(qū)域（停車場、隧道）推算的精度低，導致出口誤差大；

抓路錯誤：是指主輔路、高架上下抓路錯誤。

其中偏航重算和無法定位主要是GNSS定位原理決定，GNSS定位精度受觀測環(huán)境影響，難以改善；對于抓路錯誤，直接原因是正確道路與誤抓道路相隔太近，受定位精度限制無法區(qū)分；根本原因是只使用位置信息進行抓路，沒有發(fā)揮其它數(shù)據(jù)的價值。

技術(shù)方案

以上介紹的關(guān)鍵技術(shù)中，在場景覆蓋以及精度上，各有所長，互相補充。

根據(jù)主流這三種定位技術(shù)進行融合，提出GNSS+IMU+MM方案，依靠算法(DR)+數(shù)據(jù)(POS/HEAD)提高定位的可靠性。

從上述車載定位的幾大問題，可以逐步拆分解決：

數(shù)據(jù)融合：這一部分主要是計算 GNSS模塊 輸出的位置、速度、時間和航向信息，將其數(shù)據(jù)傳遞至數(shù)據(jù)處理終端進行實時數(shù)據(jù)融合計算，判定當前GNSS數(shù)據(jù)質(zhì)量的好壞，根據(jù)其數(shù)據(jù)質(zhì)量組合不同的定位判斷策略。

器件補償：在GNSS信號質(zhì)量不好或無法定位的時候，只能依靠IMU的DR算法進行補償。補償模塊的主要功能是利用GNSS數(shù)據(jù)來補償速度敏感器誤差參數(shù)（比例因子）和IMU的誤差參數(shù)（陀螺儀天向比例因子和陀螺儀三軸零偏）。補償?shù)哪康氖窃跓oGPS信號或弱GPS信號的場景，僅靠DR算法也能得到較為可靠的導航信息（通常短時間也能保證厘米級定位）。

場景識別：依靠內(nèi)置場景化地圖數(shù)據(jù)源以及實時外部傳感器收集的環(huán)境信息進行場景判斷，確定此刻載體地圖位置，輔助系統(tǒng)對于周圍環(huán)境感知進行行為判斷。一般采用高精度街景地圖源、激光雷達和毫米波雷達進行環(huán)境感知。

以 K8模塊 為例，采用自適應(yīng)組合導航設(shè)計，支持RTCM2.X/3.X差分數(shù)據(jù)格式接入，在空曠環(huán)境可實現(xiàn)厘米級的定位精度；內(nèi)置一體化慣導模塊，可以實現(xiàn)在復雜環(huán)境下的高精度導航。

依靠于自主研發(fā)的高精度定位算法，根據(jù)車載載體當前運行環(huán)境，系統(tǒng)自適應(yīng)對當前衛(wèi)星質(zhì)量進行評估，依據(jù)衛(wèi)星質(zhì)量進行組合導航。

當衛(wèi)星條件良好時，以衛(wèi)星導航為主，結(jié)合 高精度RTK 算法，實時定位精度≤±2.5cm，測速精度優(yōu)于0.03m/s；當衛(wèi)導無法正常工作時，以慣性導航為主導，3S內(nèi)精度保持厘米級，10S內(nèi)精度保持米級。

導航軟件是出門必備的工具及時更新導航數(shù)據(jù)的原因是

隨著衛(wèi)星定位技術(shù)的進步以及城市的發(fā)展擴張，導航軟件成為廣大汽車司機的必備工具。
然而實際生活中，由于導航軟件自身設(shè)計的缺陷，給司機駕駛造成一些困擾。導航軟件反應(yīng)不及時、更新滯后、信息內(nèi)容錯誤、規(guī)劃路線不合理等問題飽受詬病。
導航設(shè)計應(yīng)以人為本，唯有完善數(shù)據(jù)庫優(yōu)化算法，才能讓導航更及時、有效、科學。
算法設(shè)計應(yīng)當以人為本。雖說算法應(yīng)該給用戶提供更多方案和選擇，但方案本身應(yīng)經(jīng)過篩選且符合實際。對絕大部分人而言，時間短、距離近、路費低的路線應(yīng)該是首選。導航軟件規(guī)劃路線時理應(yīng)遵從“高效率低成本”原則，自動剔除那些又遠又堵又貴的路線。然而實際使用中，司機們發(fā)現(xiàn)一些導航軟件竟然提供了費用相當?shù)嚯x更遠耗時更長的選項，一旦用戶操作失誤或者因理解錯誤而誤選這種選項，無疑將增加成本。軟件算法設(shè)計要跳出純理論計算的誤區(qū)，深入分析使用者的習慣和喜好，從提升用戶體驗的角度加以改進。
導航的數(shù)據(jù)庫應(yīng)不斷更新更正。有人說導航只是一個輔助工具，司機在駕駛中要綜合路面實際情況作出判斷。但有過駕駛經(jīng)歷的朋友都知道，當司機專注于導航引導時，大腦往往難以及時作出變更，甚至會產(chǎn)生完全依賴放棄思考。此外，當司機相信甚至依賴導航時，導航的信息將具備重要的參考價值，發(fā)揮著重要作用，如果出現(xiàn)錯誤將容易引發(fā)糾紛、違章乃至事故。因此導航軟件要及時更新數(shù)據(jù)庫，尤其對于新開通、封堵、變更的道路信息要及時更新，并將交通規(guī)則充分納入算法范圍。唯有讓數(shù)據(jù)更加系統(tǒng)更加真實，才能保證駕駛安全保證行駛效率。
導航提醒應(yīng)當更加及時。現(xiàn)實中，受到外部環(huán)境以及駕駛習慣的影響，導航的數(shù)據(jù)傳輸和抵達可能存在不暢順現(xiàn)象，導致司機得到信息時可能已錯過最佳操作時間，只能選擇緊急變道或重新規(guī)劃路線，這無疑會導致安全隱患，降低駕駛效率。要讓司機有充足的操作時間，更安全的操作距離，達到更高效的路線指引，軟件的設(shè)計者就必須根據(jù)用戶的實際需求和道路的具體情況對導航軟件加以完善，利用技術(shù)手段保障數(shù)據(jù)傳輸、降低數(shù)據(jù)容量、提前給予指引。