【自动驾驶】运动规划丨轨迹规划丨米勒投影法

背景：

当自动驾驶车辆进行实时轨迹规划时，需要获取当前时刻的准确坐标位置。因此，需要对GPS/IMU的经纬度进行坐标转换。

常用的地球经纬度与平面坐标的转换方法有米勒投影、墨卡托投影、横轴墨卡托投影（也叫UTM投影，百度地图api就用的是这个）、高斯-克吕格投影、Lambert等角正割圆锥投影等

一、基本介绍

墨卡托投影，是正轴等角圆柱投影，圆柱投影的一种，由荷兰地图学家墨卡托于1569年创拟。为地图投影方法中影响最大的投影。

因为这里需求是地球经纬度坐标转平面笛卡尔坐标，所以选和墨卡托投影方式类似的米勒投影。即假设有一个和赤道垂直的圆柱套在地球上，然后在地心点亮一盏灯，灯光将地球各个点投影在圆柱上，在把圆柱展开，就得到地球的平面投影。

其中，按等角条件将经纬网投影到圆柱面上，将圆柱面展为平面后，得平面经纬线网。

使用这种方式得到的投影地图在两极会拉长

米勒投影和墨卡托投影类似，只是在几点区域面积变形不如后者。

百度地图和Google Maps使用的投影方法都是墨卡托投影。

经过墨卡托投影后的经线是均匀分布，在此主要介绍纬度的变换方法。

墨卡托投影把纬度为Φ (-90°<Φ<90°)的点投影到：y = sign(Φ)*ln(tan(45° + abs(Φ/2))

其中：

当Φ<0时，sign(Φ)=-1；

当Φ=0时，sign(Φ)=0；

当Φ>0时，sign(Φ)=1；

abs（Φ）是Φ的绝对值。

这种投影算法使得赤道附近的纬线较密，极地附近的纬线较稀。极点被投影到无穷远，所以这种投影不适合在高纬度地区使用。Google Maps的选取的范围为 -π<y<π ，这样近似的有 -85°<Φ<85°。

二、Miller投影转换代码

#include <math.h>
#include <iostream>
using namespace std;
#define M_PI       3.14159265358979323846
 
double* MillierConvertion(double lat, double lon)
{
	double L = 6381372 * M_PI * 2;//地球周长  
	double W = L;// 平面展开后，x轴等于周长  
	double H = L / 2;// y轴约等于周长一半  
	double mill = 2.3;// 米勒投影中的一个常数，范围大约在正负2.3之间  
	double x = lon * M_PI / 180;// 将经度从度数转换为弧度  
	double y = lat * M_PI / 180;// 将纬度从度数转换为弧度  
	y = 1.25 * log(tan(0.25 * M_PI + 0.4 * y));// 米勒投影的转换  
	// 弧度转为实际距离  
	x = (W / 2) + (W / (2 * M_PI)) * x;
	y = (H / 2) - (H / (2 * mill)) * y;
	double* result = new double[2];
	result[0] = (int)x;
	result[1] = (int)y;
	return result;
}
 
double* MillierConvertion1(double x, double y)
{
        double L = 6381372 * M_PI * 2;//地球周长  
	double W = L;// 平面展开后，x轴等于周长  
	double H = L / 2;// y轴约等于周长一半  
	double mill = 2.3;// 米勒投影中的一个常数，范围大约在正负2.3之间  
	double lat;
	lat = ((H / 2 - y) * 2 * mill) / (1.25 * H);
	lat = ((atan(exp(lat)) - 0.25 * M_PI) * 180) / (0.4 * M_PI);
	double lon;
	lon = (x - W / 2) * 360 / W;
	double* result = new double[2];
	result[0] = lon;
	result[1] = lat;
	return result;
}
 
 
void main()
{
	double a, b;
	double *test;
	double *tet;
	a = 0;
	b = 0;
	tet = MillierConvertion(a, b);
	cout << tet[0] << endl << tet[1];
	cout << endl;
	test = MillierConvertion1(tet[0], tet[1]);
	cout << test[0] << endl << test[1];
}


%————————————————————————————————————————
%%MATLAB代码
close all;
clc;clear;

%经纬度

latitude=[31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628334	31.44628143	31.44628143	31.44628143	31.44628143	31.44627953	31.44627953	31.44627762	31.44627571	31.44627571	31.44627571	31.4462738	31.44626999	31.44626999	31.44626808	31.44626808	31.44626617	31.44626427	31.44626045	31.44626045	31.44625854	31.44625473	31.44625282	31.44625282	31.4462471	31.44624519	31.44624519	31.44624138	31.44624138	31.44623947	31.44623566	31.44623375	31.44623184	31.44623184	31.44622803	31.44622803	31.44622421	31.44622231	31.4462204	31.44621849	31.44621849	31.44621468	31.44621086	31.44621086	31.44620895	31.44620705	31.44620132	31.44619942	31.44619942	31.4461937	31.4461937	31.44618988	31.44618416	31.44618225	31.44618225	31.44617844	31.44617653	31.4461689	31.4461689	31.44616318	31.44616318	31.44615746	31.44615555	31.44615173	31.44615173	31.44614983	31.44614601	31.44614029	31.44614029	31.44613457	31.44613075	31.44613075	31.44612503	31.44612503	31.44612122	31.44611549	31.44611168	31.44610977	31.44610977	31.44610596	31.44609833	31.44609451	31.4460907	31.44608688	31.44608307	31.44607925	31.44607544	31.44607162	31.44606781	31.446064	31.44606018	31.44605637	31.44605255	31.44604874	31.44604874	31.44604492	31.44604111	31.44603729	31.44603348	31.44602585	31.44602013	31.44602013	31.4460125	31.44600868	31.44600868	31.44600487	31.44599724	31.44599724	31.44598961	31.44598579	31.44598198	31.44598198	31.44597435	31.44597054	31.44596672	31.44596291	31.44595909	31.44595909	31.44595146	31.44594765	31.44594765	31.44594002	31.4459362	31.44593239	31.44592857	31.44592476	31.44592094	31.44592094	31.44591713	31.4459095	31.44590569	31.44590569	31.44589806	31.44589806	31.44589424	31.44588661	31.4458828	31.44588089	31.44587708	31.44587326	31.44587326	31.44586945	31.44586372	31.44586372	31.44585991	31.44585419	31.44585419	31.44584656	31.44584465	31.44584084	31.44584084	31.44583893	31.44583321	31.44583321	31.44582939	31.44582367	31.44582367	31.44581795	31.44581604	31.44581413	31.44581032	31.44580841	31.4458046	31.4458046	31.44580269	31.44580078	31.44579697	31.44579506	31.44578934	31.44578934	31.44578552	31.4457798	31.44577789	31.44577789	31.44577217	31.44576836	31.44576836	31.44576263	31.44576073	31.44576073	31.44575691	31.44575119	31.44574928	31.44574928	31.44574547	31.44574165	31.44573975	31.44573402	31.44573021	31.44573021	31.44572639	31.44572067	31.44571877	31.44571877	31.44571495	31.44570923	31.44570923	31.4457016	31.44569969	31.44569588	31.44569588	31.44569016	31.44568634	31.44568253	31.44568253	31.4456768	31.4456768	31.44566917	31.44566727	31.44566727	31.44566345	31.44565964	31.44565392	31.44565392	31.44564629	31.44564629	31.44564056	31.44563675	31.44563293	31.44562912	31.44562721	31.4456234	31.44561958	31.44561958	31.44561386	31.44561386	31.44560814	31.44560432	31.44560051	31.44560051	31.44559479	31.44559097	31.44558907	31.44558907	31.44558144	31.44557953	31.44557571	31.44557381	31.44557381	31.44556808	31.44556427	31.44556046	31.44555855	31.44555855	31.44555283	31.44555283	31.4455471	31.44554329	31.44554138	31.44554138	31.44553566	31.44553375	31.44552994	31.44552803	31.44552422	31.44552231	31.44552231	31.44551659	31.44551468	31.44551468	31.44550896	31.44550896	31.44550323	31.44550133	31.44549942	31.44549942	31.44549751	31.44549179	31.44548988	31.44548988	31.44548607	31.44548225	31.44548035	31.44547844	31.44547653	31.44547653	31.44547462	31.44547272	31.4454689	31.4454689	31.44546509	31.44546509	31.44546127	31.44546127	31.44545746	31.44545746	31.44545364	31.44545174	31.44545174	31.44544792	31.44544601	31.44544601	31.44544411	31.4454422	31.44544029	31.44544029	31.44543839	31.44543648	31.44543648	31.44543648	31.44543266	31.44543266	31.44543076	31.44543076	31.44542885	31.44542885	31.44542694	31.44542694	31.44542503	31.44542503	31.44542313	31.44542313	31.44542122	31.44542122	31.44542122	31.44542122	31.44541931	31.44541931	31.4454174	31.4454174	31.4454174	31.4454174	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.44541359	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454155	31.4454174	31.4454174	31.4454174	31.4454174	31.4454174	31.4454174	31.4454174	31.4454174	31.4454174	31.44541931	31.44541931	31.44541931	31.44541931	31.44541931	31.44541931	31.44542122	31.44542122	31.44542122	31.44542122	31.44542122	31.44542122	31.44542313	31.44542313	31.44542313	31.44542313	31.44542313	31.44542313	31.44542313	31.44542313	31.44542503	31.44542503	31.44542503	31.44542503	31.44542503	31.44542503	31.44542694	31.44542694	31.44542694	31.44542694	31.44542694	31.44542694	31.44542885	31.44542885	31.44542885	31.44542885	31.44542885	31.44543076	31.44543076	31.44543076	31.44543076	31.44543266	31.44543266	31.44543266	31.44543266	31.44543266	31.44543266	31.44543457	31.44543457	31.44543457	31.44543457	31.44543648	31.44543648	31.44543648	31.44543648	31.44543648	31.44543648	31.44543839	31.44543839	31.44543839	31.44543839	31.44544029	31.44544029	31.44544029	31.44544029	31.44544029	31.44544029	31.4454422	31.4454422	31.4454422	31.4454422	31.44544411	31.44544411	31.44544411	31.44544601	31.44544601	31.44544601	31.44544601	31.44544601	31.44544601	31.44544792	31.44544792	31.44544792	31.44544792	31.44544792	31.44544983	31.44544983	31.44544983	31.44544983	31.44544983	31.44544983	31.44545174	31.44545174	31.44545174	31.44545174	31.44545174	31.44545364	31.44545364	31.44545364	31.44545364	31.44545364	31.44545555	31.44545555	31.44545555	31.44545555	31.44545555	31.44545746	31.44545746	31.44545746	31.44545746	31.44545746	31.44545746	31.44545746	31.44545937	31.44545937	31.44545937	31.44545937	31.44545937	31.44546127	31.44546127	31.44546127	31.44546127	31.44546127	31.44546127	31.44546127	31.44546318	31.44546318	31.44546318	31.44546318	31.44546318	31.44546318	31.44546318	31.44546509	31.44546509	31.44546509	31.44546509	31.44546509	31.44546509	31.44546509	31.445467	31.445467	31.445467	31.445467	31.445467	31.445467	31.445467	31.4454689	31.4454689	31.4454689	31.4454689	31.4454689	31.4454689	31.4454689	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081	31.44547081]
longitude=[121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795441	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795517	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.0795593	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.079567	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795746	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795822	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795898	121.0795975	121.0795975	121.0795975	121.0795975	121.0795975	121.0795975	121.0795975	121.0795975	121.0795975	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796051	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796127	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.0796204	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.079628	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796356	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796433	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796509	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796585	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796661	121.0796738	121.0796738	121.0796738	121.0796738	121.0796738	121.0796738	121.0796738	121.0796738	121.0796738	121.0796814	121.0796814	121.0796814	121.0796814	121.0796814	121.0796814	121.0796814	121.0796814	121.0796814	121.079689	121.079689	121.079689	121.079689	121.079689	121.079689	121.079689	121.079689	121.079689	121.0796967	121.0796967	121.0796967	121.0796967	121.0796967	121.0797043	121.0797043	121.0797043	121.0797043	121.0797043	121.0797043	121.0797119	121.0797119	121.0797119	121.0797119	121.0797119	121.0797195	121.0797195	121.0797195	121.0797195	121.0797272	121.0797272	121.0797272	121.0797272	121.0797272	121.0797348	121.0797348	121.0797348	121.0797348	121.0797424	121.0797424	121.0797424	121.0797501	121.0797501	121.0797501	121.0797501	121.0797577	121.0797577	121.0797577	121.0797653	121.0797653	121.0797653	121.0797653	121.0797653	121.0797729	121.0797729	121.0797806	121.0797806	121.0797806	121.0797882	121.0797882	121.0797882	121.0797882	121.0797958	121.0797958	121.0798035	121.0798035	121.0798035	121.0798111	121.0798111	121.0798111	121.0798111	121.0798187	121.0798187	121.0798187	121.0798264	121.0798264	121.0798264	121.0798264	121.079834	121.079834	121.0798416	121.0798416	121.0798416	121.0798492	121.0798492	121.0798492	121.0798569	121.0798569	121.0798569	121.0798645	121.0798645	121.0798645	121.0798721	121.0798721	121.0798721	121.0798721	121.0798798	121.0798798	121.0798798	121.0798874	121.0798874	121.0798874	121.0798874	121.079895	121.079895	121.0799026	121.0799026	121.0799026	121.0799026	121.0799103	121.0799103	121.0799103	121.0799179	121.0799179	121.0799179	121.0799255	121.0799255	121.0799255	121.0799255	121.0799332	121.0799332	121.0799332	121.0799408	121.0799408	121.0799408	121.0799484	121.0799484	121.0799484	121.0799561	121.0799561	121.0799561	121.0799637	121.0799637	121.0799713	121.0799713	121.0799789	121.0799789	121.0799789	121.0799866	121.0799866	121.0799866	121.0799942	121.0799942	121.0799942	121.0800018	121.0800018	121.0800018	121.0800095	121.0800095	121.0800171	121.0800171	121.0800171	121.0800247	121.0800247	121.0800247	121.0800247	121.0800323	121.0800323	121.08004	121.08004	121.08004	121.0800476	121.0800476	121.0800476	121.0800552	121.0800629	121.0800629	121.0800629	121.0800705	121.0800705	121.0800705	121.0800781	121.0800781	121.0800781	121.0800858	121.0800858	121.0800934	121.0800934	121.0800934	121.080101	121.080101	121.080101	121.0801086	121.0801086	121.0801086	121.0801163	121.0801163	121.0801163	121.0801239	121.0801239	121.0801239	121.0801315	121.0801315	121.0801315	121.0801392	121.0801392	121.0801468	121.0801468	121.0801544	121.0801544	121.0801544	121.0801544	121.080162	121.080162	121.0801697	121.0801697	121.0801697	121.0801773	121.0801773	121.0801773	121.0801849	121.0801849	121.0801849	121.0801926	121.0801926	121.0801926	121.0801926	121.0802002	121.0802002	121.0802078	121.0802078	121.0802078	121.0802155	121.0802155	121.0802155	121.0802155	121.0802231	121.0802231	121.0802231	121.0802307	121.0802307	121.0802383	121.0802383	121.0802383	121.080246	121.080246	121.080246	121.080246	121.080246	121.0802536	121.0802536	121.0802612	121.0802612	121.0802612	121.0802612	121.0802612	121.0802689	121.0802689	121.0802689	121.0802765	121.0802765	121.0802765	121.0802765	121.0802765	121.0802841	121.0802841	121.0802841	121.0802917	121.0802917	121.0802917	121.0802917	121.0802994	121.0802994	121.0802994	121.0802994	121.0802994	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307	121.080307]

Lat_degree=latitude
Lon_degree=longitude
Lat_rad=Lat_degree*pi/180
Lon_rad=Lon_degree*pi/180
Earth_Radius_m=6381372;
W=Earth_Radius_m*pi*2;
H=W/2;
X=W/2+W/(2*pi)*Lon_rad
mill=2.3;%米勒投影的一个常数[-2.3,2.3]
Y=H/2-H/(2*mill)*(1.25*log(tan(Lat_rad*0.4+pi/4)))
X=X-X(1);Y=Y-Y(1);
plot(X,Y,'o')
hold on

参考链接：

利用米勒投影法实现经纬度和平面坐标的相互转换_Daniel_mc的博客-CSDN博客​blog.csdn.net/qq_32693445/article/details/79597213
https://blog.csdn.net/qq_35732147/article/details/83856513​blog.csdn.net/qq_35732147/article/details/83856513