【自動駕駛】運動規劃丨軌跡規劃丨米勒投影法
背景：
當自動駕駛車輛進行實時軌跡規劃時，需要獲取當前時刻的準確坐標位置。因此，需要對GPS/IMU的經緯度進行坐標轉換。
常用的地球經緯度與平面坐標的轉換方法有米勒投影、墨卡托投影、橫軸墨卡托投影（也叫UTM投影，百度地圖api就用的是這個）、高斯-克呂格投影、Lambert等角正割圓錐投影等
一、基本介紹
墨卡托投影，是正軸等角圓柱投影，圓柱投影的一種，由荷蘭地圖學傢墨卡托於1569年創擬。為地圖投影方法中影響最大的投影。
因為這裡需求是地球經緯度坐標轉平面笛卡爾坐標，所以選和墨卡托投影方式類似的米勒投影。即假設有一個和赤道垂直的圓柱套在地球上，然後在地心點亮一盞燈，燈光將地球各個點投影在圓柱上，在把圓柱展開，就得到地球的平面投影。
其中，按等角條件將經緯網投影到圓柱面上，將圓柱面展為平面後，得平面經緯線網。
48e243a2068ff7f5e361be511ccefc8b
使用這種方式得到的投影地圖在兩極會拉長
米勒投影和墨卡托投影類似，隻是在幾點區域面積變形不如後者。
百度地圖和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
