最短路径算法导航 校内所有建筑和道路并具有道路是否可通行的标志
/*
校园导游咨询
[问题描述]
设计一个校园导游程序，为来访的客人提供各种信息查询服务。
[基本要求]
(1)设计你的学校的校园平面图，所含景点不少于10个。以图中顶点表示校内各景点，存放景点名称、代号、简介等信息；以边表示路径，存放路径长度等相关信息。
(2)为来访客人提供图中任意景点相关信息的查询。
(3)为来访客人提供图中任意景点的问路查询，即查询任意两个景点之间的一条最短的简单路径。
[实现提示]
一般情况下，校园的道路是双向通行的，可设校园平面图是一个无向网。顶点和边均含有相关信息。
一需求分析
1从中北大学平面图中选取10个大家熟悉的景点，抽象成一个无向带权图（如图所示）。以图中顶点表示景点，边上的权值表示两地的距离。
2本程序的目的是为用户提供路径咨询和景点查询。根据用户指定的始点和终点输出相应路径或者根据用户指定的景点输出景点的信息。
南
北
二、概要设计
1本文采用的数据结构
*/
/*包含头文件*/
#include
#include
/*定义符号常量*/
#define INT_MAX 10000
#define n 10
/*定义全局变量*/
int cost[n][n];/* 边的值*/
int shortest[n][n];/* 两点间的最短距离*/
int path[n][n];/* 经过的景点*/
/*自定义函数原型说明*/
void introduce();
int shortestdistance();
void floyed();
void display(int i,int j);
2个人分工
（1）景点信息查询
（2）两景点的最短距离
（3）两个景点之间的路径
三、详细设计
void main()
{/*主函数*/
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("----------------欢迎使用中北大学导游系统!----------------\n");
printf("1.景点信息查询………请按 i (introduc)键\n");
printf("2.景点最短路径查询…请按 s (shortestdistance)键\n");
printf("3.退出系统……………请按 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("请选择服务:");
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 学生上体育课及运动的场地，设有田径场、足球场、篮球场等。\n\n");break;
case 6:
printf("6:图书馆\n\n 学校信息资源中心，内设大量的自习室。\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的数字编号！\n\n"); break;
}
}/*introduce*/
int shortestdistance()
{/*要查找的两景点的最短距离*/
int i,j;
printf("请输入要查询的两个景点的编号(1->10的数字编号并用','间隔):");
scanf("%d,%d",i,j);
if(i>n||i<=0||j>n||j<0)
{
printf("输入信息错误！\n\n");
printf(" 请输入要查询的两个景点的编号(1->10的数字编号并用','间隔):\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的最短路径上点j的前驱景点的序号*/
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的路径上所有经过的景点按逆序打印出来*/
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的路径上所有经过的景点按顺序打印出来*/
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车载组合导航系统
GNSS+IMU+MM车载组合导航系统
前言：近年来，随着定位业务的迅速发展，用户对于车载端定位精度提出了越来越高的要求，由原来的导航级逐渐更替到车道级。特别是在城市峡谷环境下（高楼、高架），用户无法接收到GNSS信号或GNSS信号受干扰，导致GNSS无定位结果或定位精度差。这是“有源定位”固有的缺点，无法从算法上来克服。针对这个问题，以GNSS+IMU等多传感器融合方案越来越受到重视，因为“无源定位”的IMU恰好可以弥补卫星定位的短板。
基础原理
导航卫星系统(GNSS)
全球导航卫星系统（Global Navigation Satellite System）是一种依靠卫星卫星的伪距载波、星历、时间以及钟差等信息进行实时定位的空基无线电导航系统，能在地球表面或近地空间的任何地点为用户提供全天候的三维坐标和速度以及时间信息。GNSS系统的优点是精度高、误差稳定不发散，但容易受到周围环境影响，比如树木楼房遮挡，镜面等高反射物体引起的多路径效应。
惯性导航系统(IMU)
惯性导航系统（Inertial Navigation System）是一种不依赖于外部信息、也不向外部辐射能量（如无线电导航那样）的自主式导航系统，主要使用惯性测量单元IMU（Inertial measurement unit）。其工作环境不仅包括空中、地面，还可以在水下。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。其优点是工作不需要通时，安装位置随意，定位范围全场景，但定位精度不高，且误差随时间发散。与GNSS导航系统互补。
地图匹配技术(MM)
地图匹配技术MM（Map matching）是结合用户位置信息和地图数据，推算用户在地图上道路的准确位置，辅助车载导航的精准控制。
航位推算法(DR)
航位推算法DR（Dead Reckoning）是一种跟踪导航算法，在获取载体当前时刻坐标位置的前提下，依靠惯性测量单元IMU取得的同周期内载体移动的距离和方位，进而推算下一时刻位置。在此文介绍中，主要讲建立在已有 GNSS系统 解算下，IMU辅助进行组合导航的算法。
车载定位的痛点
车载导航定位发展已经很久，但随着精度要求越来越高，车载定位的一些问题也逐渐浮现：
偏航重算：是指在高架或城市峡谷，信号遮挡引起位置点漂移；
无法定位：是指在无信号区域（停车场、隧道）推算的精度低，导致出口误差大；
抓路错误：是指主辅路、高架上下抓路错误。
其中偏航重算和无法定位主要是GNSS定位原理决定，GNSS定位精度受观测环境影响，难以改善；对于抓路错误，直接原因是正确道路与误抓道路相隔太近，受定位精度限制无法区分；根本原因是只使用位置信息进行抓路，没有发挥其它数据的价值。
技术方案
以上介绍的关键技术中，在场景覆盖以及精度上，各有所长，互相补充。
根据主流这三种定位技术进行融合，提出GNSS+IMU+MM方案，依靠算法(DR)+数据(POS/HEAD)提高定位的可靠性。
从上述车载定位的几大问题，可以逐步拆分解决：
数据融合：这一部分主要是计算 GNSS模块 输出的位置、速度、时间和航向信息，将其数据传递至数据处理终端进行实时数据融合计算，判定当前GNSS数据质量的好坏，根据其数据质量组合不同的定位判断策略。
器件补偿：在GNSS信号质量不好或无法定位的时候，只能依靠IMU的DR算法进行补偿。补偿模块的主要功能是利用GNSS数据来补偿速度敏感器误差参数（比例因子）和IMU的误差参数（陀螺仪天向比例因子和陀螺仪三轴零偏）。补偿的目的是在无GPS信号或弱GPS信号的场景，仅靠DR算法也能得到较为可靠的导航信息（通常短时间也能保证厘米级定位）。
场景识别：依靠内置场景化地图数据源以及实时外部传感器收集的环境信息进行场景判断，确定此刻载体地图位置，辅助系统对于周围环境感知进行行为判断。一般采用高精度街景地图源、激光雷达和毫米波雷达进行环境感知。
以 K8模块 为例，采用自适应组合导航设计，支持RTCM2.X/3.X差分数据格式接入，在空旷环境可实现厘米级的定位精度；内置一体化惯导模块，可以实现在复杂环境下的高精度导航。
依靠于自主研发的高精度定位算法，根据车载载体当前运行环境，系统自适应对当前卫星质量进行评估，依据卫星质量进行组合导航。
当卫星条件良好时，以卫星导航为主，结合 高精度RTK 算法，实时定位精度≤±2.5cm，测速精度优于0.03m/s；当卫导无法正常工作时，以惯性导航为主导，3S内精度保持厘米级，10S内精度保持米级。
导航软件是出门必备的工具及时更新导航数据的原因是
随着卫星定位技术的进步以及城市的发展扩张，导航软件成为广大汽车司机的必备工具。
然而实际生活中，由于导航软件自身设计的缺陷，给司机驾驶造成一些困扰。导航软件反应不及时、更新滞后、信息内容错误、规划路线不合理等问题饱受诟病。
导航设计应以人为本，唯有完善数据库优化算法，才能让导航更及时、有效、科学。
算法设计应当以人为本。虽说算法应该给用户提供更多方案和选择，但方案本身应经过筛选且符合实际。对绝大部分人而言，时间短、距离近、路费低的路线应该是首选。导航软件规划路线时理应遵从“高效率低成本”原则，自动剔除那些又远又堵又贵的路线。然而实际使用中，司机们发现一些导航软件竟然提供了费用相当但距离更远耗时更长的选项，一旦用户操作失误或者因理解错误而误选这种选项，无疑将增加成本。软件算法设计要跳出纯理论计算的误区，深入分析使用者的习惯和喜好，从提升用户体验的角度加以改进。
导航的数据库应不断更新更正。有人说导航只是一个辅助工具，司机在驾驶中要综合路面实际情况作出判断。但有过驾驶经历的朋友都知道，当司机专注于导航引导时，大脑往往难以及时作出变更，甚至会产生完全依赖放弃思考。此外，当司机相信甚至依赖导航时，导航的信息将具备重要的参考价值，发挥着重要作用，如果出现错误将容易引发纠纷、违章乃至事故。因此导航软件要及时更新数据库，尤其对于新开通、封堵、变更的道路信息要及时更新，并将交通规则充分纳入算法范围。唯有让数据更加系统更加真实，才能保证驾驶安全保证行驶效率。
导航提醒应当更加及时。现实中，受到外部环境以及驾驶习惯的影响，导航的数据传输和抵达可能存在不畅顺现象，导致司机得到信息时可能已错过最佳操作时间，只能选择紧急变道或重新规划路线，这无疑会导致安全隐患，降低驾驶效率。要让司机有充足的操作时间，更安全的操作距离，达到更高效的路线指引，软件的设计者就必须根据用户的实际需求和道路的具体情况对导航软件加以完善，利用技术手段保障数据传输、降低数据容量、提前给予指引。
