如何将球面速度坐标转换为笛卡尔坐标

时间:2016-12-15 08:13:32

标签: math vector 3d wgs84

我有高度,经度,海拔高度的速度矢量,我想将其转换为笛卡尔坐标,vx,vy,vz。格式来自WGS84标准。

这是公式

  //------------------------------------------------------------------------------
    template <class T> 
    TVectorXYZ<T> WGS84::ToCartesian(T latitude, T longitude, T elevation)
    //------------------------------------------------------------------------------
    {
        double sinlat, coslat;
        double sinlon, coslon;
        sincos_degree(latitude,  sinlat, coslat);
        sincos_degree(longitude, sinlon, coslon);  

        const double v = a / sqrt(1 - WGS84::ee * sinlat*sinlat);

        TVectorXYZ<T> coord
        (
            static_cast<T>((v + elevation) * coslat * sinlon),
            static_cast<T>(((1 - WGS84::ee) * v + elevation) * sinlat),
            static_cast<T>((v + elevation) * coslat * coslon)                                    
        );

        return coord;
    }

2 个答案:

答案 0 :(得分:1)

根据您之前的question和长注释流,确定您的输入是:

lon [rad], lat [rad], alt [m] // WGS84 position
vlon [m/s], vlat [m/s], alt [m/s] // speed in WGS84  lon,lat,alt directions but in [m/s]

想要输出:

x,y,z // Cartesian position [m/s]
vx,vy,vz // Cartesian velocity [m/s]

并且有有效转换为笛卡尔坐标可供您使用的位置这是我的:

void WGS84toXYZ(double &x,double &y,double &z,double lon,double lat,double alt) // [rad,rad,m] -> [m,m,m]
    {
    const double  _earth_a=6378137.00000;   // [m] WGS84 equator radius
    const double  _earth_b=6356752.31414;   // [m] WGS84 epolar radius
    const double  _earth_e=8.1819190842622e-2; //  WGS84 eccentricity
    const double _aa=_earth_a*_earth_a;
    const double _ee=_earth_e*_earth_e;
    double  a,b,x,y,z,h,l,c,s;
    a=lon;
    b=lat;
    h=alt;
    c=cos(b);
    s=sin(b);
    // WGS84 from eccentricity
    l=_earth_a/sqrt(1.0-(_ee*s*s));
    x=(l+h)*c*cos(a);
    y=(l+h)*c*sin(a);
    z=(((1.0-_ee)*l)+h)*s;
    }

将矢量标准化为单位大小的例程:

void normalize(double &x,double &y,double &z)
    {
    double l=sqrt(x*x+y*y+z*z);
    if (l>1e-6) l=1.0/l;
    x*=l; y*=l; z*=l;
    }

是的,你可以尝试推导出@MvG建议的公式,但是从你的菜鸟错误中我强烈怀疑它会导致成功的结果。相反,你可以这样做:

  1. 为您的位置获取lon,lat,alt方向向量(x,y,z)

    很容易在 WGS84 位置使用一些小步长增量转换为笛卡尔减法并归一化为单位矢量。让我们调用这些方向基础向量U,V,W

    double Ux,Uy,Uz;    // [m]
    double Vx,Vy,Vz;    // [m]
    double Wx,Wy,Wz;    // [m]
    double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
    double dl=1.0;      // [m] altitide step 1.0 m
    WGS84toXYZ( x, y, z,lon   ,lat,alt   ); // actual position
    WGS84toXYZ(Ux,Uy,Uz,lon+da,lat,alt   ); // lon direction Nort
    WGS84toXYZ(Vx,Vy,Vz,lon,lat+da,alt   ); // lat direction East
    WGS84toXYZ(Wx,Wy,Wz,lon,lat   ,alt+dl); // alt direction High/Up
    Ux-=x; Uy-=y; Uz-=z;
    Vx-=x; Vy-=y; Vz-=z;
    Wx-=x; Wy-=y; Wz-=z;
    normalize(Ux,Uy,Uz);
    normalize(Vx,Vy,Vz);
    normalize(Wx,Wy,Wz);
    
  2. 将速度从lon,lat,alt转换为vx,vy,vz

    vx = vlon*Ux + vlat*Vx + valt*Wx;
    vy = vlon*Uy + vlat*Vy + valt*Wy;
    vz = vlon*Uz + vlat*Vz + valt*Wz;
    
  3. 希望它足够清楚。像往常一样要小心单位deg/radm/ft/km因为单位很重要。

    Btw U,V,W基础向量形成NEH reference frame,同时也是方向派生词 MvG

    [Edit1]更精确的转化

    //---------------------------------------------------------------------------
    //--- WGS84 transformations ver: 1.00 ---------------------------------------
    //---------------------------------------------------------------------------
    #ifndef _WGS84_h
    #define _WGS84_h
    //---------------------------------------------------------------------------
    // http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
    //---------------------------------------------------------------------------
    // WGS84(a,b,h) = (long,lat,alt) [rad,rad,m]
    // XYZ(x,y,z) [m]
    //---------------------------------------------------------------------------
    const double  _earth_a=6378137.00000;   // [m] WGS84 equator radius
    const double  _earth_b=6356752.31414;   // [m] WGS84 epolar radius
    const double  _earth_e=8.1819190842622e-2; //  WGS84 eccentricity
    //const double  _earth_e=sqrt(1.0-((_earth_b/_earth_a)*(_earth_b/_earth_a)));
    const double  _earth_ee=_earth_e*_earth_e;
    //---------------------------------------------------------------------------
    const double kmh=1.0/3.6;               // [km/h] -> [m/s]
    //---------------------------------------------------------------------------
    void XYZtoWGS84       (double *abh                  ,double *xyz                  ); // [m,m,m] -> [rad,rad,m]
    void WGS84toXYZ       (double *xyz                  ,double *abh                  ); // [rad,rad,m] -> [m,m,m]
    void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel); // [rad,rad,m],[m/s,m/s,m/s] -> [m,m,m],[m/s,m/s,m/s]
    void WGS84toNEH       (reper &neh                   ,double *abh                  ); // [rad,rad,m] -> NEH [m]
    void WGS84_m2rad      (double &da,double &db,double *abh);                           // [rad,rad,m] -> [rad],[rad] representing 1m angle step
    void XYZ_interpolate  (double *pt,double *p0,double *p1,double t);                   // [m,m,m] pt = p0 + (p1-p0)*t in ellipsoid space t = <0,1>
    //---------------------------------------------------------------------------
    void XYZtoWGS84(double *abh,double *xyz)
        {
        int i;
        double  a,b,h,l,n,db,s;
        a=atanxy(xyz[0],xyz[1]);
        l=sqrt((xyz[0]*xyz[0])+(xyz[1]*xyz[1]));
        // estimate lat
        b=atanxy((1.0-_earth_ee)*l,xyz[2]);
        // iterate to improve accuracy
        for (i=0;i<100;i++)
            {
            s=sin(b); db=b;
            n=divide(_earth_a,sqrt(1.0-(_earth_ee*s*s)));
            h=divide(l,cos(b))-n;
            b=atanxy((1.0-divide(_earth_ee*n,n+h))*l,xyz[2]);
            db=fabs(db-b);
            if (db<1e-12) break;
            }
        if (b>0.5*pi) b-=pi2;
        abh[0]=a;
        abh[1]=b;
        abh[2]=h;
        }
    //---------------------------------------------------------------------------
    void WGS84toXYZ(double *xyz,double *abh)
        {
        double  a,b,h,l,c,s;
        a=abh[0];
        b=abh[1];
        h=abh[2];
        c=cos(b);
        s=sin(b);
        // WGS84 from eccentricity
        l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
        xyz[0]=(l+h)*c*cos(a);
        xyz[1]=(l+h)*c*sin(a);
        xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
        }
    //---------------------------------------------------------------------------
    void WGS84toNEH(reper &neh,double *abh)
        {
        double N[3],E[3],H[3];                  // [m]
        double p[3],xyzpos[3];
        const double da=1.567e-7;               // [rad] angular step ~ 1.0 m in lon direction
        const double dl=1.0;                    // [m] altitide step 1.0 m
        vector_copy(p,abh);
        // actual position
        WGS84toXYZ(xyzpos,abh);
        // NEH
        p[0]+=da; WGS84toXYZ(N,p); p[0]-=da;
        p[1]+=da; WGS84toXYZ(E,p); p[1]-=da;
        p[2]+=dl; WGS84toXYZ(H,p); p[2]-=dl;
        vector_sub(N,N,xyzpos);
        vector_sub(E,E,xyzpos);
        vector_sub(H,H,xyzpos);
        vector_one(N,N);
        vector_one(E,E);
        vector_one(H,H);
        neh._rep=1;
        neh._inv=0;
        // axis X
        neh.rep[ 0]=N[0];
        neh.rep[ 1]=N[1];
        neh.rep[ 2]=N[2];
        // axis Y
        neh.rep[ 4]=E[0];
        neh.rep[ 5]=E[1];
        neh.rep[ 6]=E[2];
        // axis Z
        neh.rep[ 8]=H[0];
        neh.rep[ 9]=H[1];
        neh.rep[10]=H[2];
        // gpos
        neh.rep[12]=xyzpos[0];
        neh.rep[13]=xyzpos[1];
        neh.rep[14]=xyzpos[2];
        neh.orto(1);
        }
    //---------------------------------------------------------------------------
    void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel)
        {
        reper neh;
        WGS84toNEH(neh,abhpos);
        neh.gpos_get(xyzpos);
        neh.l2g_dir(xyzvel,abhvel);
        }
    //---------------------------------------------------------------------------
    void WGS84_m2rad(double &da,double &db,double *abh)
        {
        // WGS84 from eccentricity
        double p[3],rr;
        WGS84toXYZ(p,abh);
        rr=(p[0]*p[0])+(p[1]*p[1]);
        da=divide(1.0,sqrt(rr));
        rr+=p[2]*p[2];
        db=divide(1.0,sqrt(rr));
        }
    //---------------------------------------------------------------------------
    void XYZ_interpolate(double *pt,double *p0,double *p1,double t)
        {
        const double  mz=_earth_a/_earth_b;
        const double _mz=_earth_b/_earth_a;
        double p[3],r,r0,r1;
        // compute spherical radiuses of input points
        r0=sqrt((p0[0]*p0[0])+(p0[1]*p0[1])+(p0[2]*p0[2]*mz*mz));
        r1=sqrt((p1[0]*p1[0])+(p1[1]*p1[1])+(p1[2]*p1[2]*mz*mz));
        // linear interpolation
        r   = r0   +(r1   -r0   )*t;
        p[0]= p0[0]+(p1[0]-p0[0])*t;
        p[1]= p0[1]+(p1[1]-p0[1])*t;
        p[2]=(p0[2]+(p1[2]-p0[2])*t)*mz;
        // correct radius and rescale back
        r/=sqrt((p[0]*p[0])+(p[1]*p[1])+(p[2]*p[2]));
        pt[0]=p[0]*r;
        pt[1]=p[1]*r;
        pt[2]=p[2]*r*_mz;
        }
    //---------------------------------------------------------------------------
    #endif
    //---------------------------------------------------------------------------
    

    然而,他们需要基本的3D矢量数学,请参阅此处的方程式:

答案 1 :(得分:0)

使用您使用的公式将位置从地理位置转换为笛卡尔坐标。这是一些向量p(λ,φ,h)∈ℝ³,即你将纬度,经度和高度转换为x,y,z坐标的三元素向量。现在根据三个参数计算该公式的偏导数。你将得到三个向量,它们应该是彼此正交的。相对于经度λ的导数应指向局部东方,相对于纬度φ指向北方的那个,相对于高度h指向的方向。将这些向量与您获得笛卡尔速度向量的速度相乘。

观察单位如何匹配:位置以米为单位,前两个导数是每度数米,速度是每秒度数。或其他东西,也许是英里和弧度。

所有这一切对于球体来说都相当容易。对于WGS84椭圆体,位置公式有点复杂,并且复杂性将通过计算进行。