Pixhawk/PX4 GPS位置互补算法的理解...持续更新...

joyrus

我以Firmware-1.0.0-rc4发布的第一个版本为基础。

我尝试以观察一个系统的视角出发来逐步理解PX4的位置算法。

一个系统有一个输入量，一个输出量，系统对输入做处理然后输出，所以我先要找到并搞清楚输入量和输出量。

我在看代码之前会先思考我会如何设计这个系统，首先从姿态模块获取到机体的当前姿态和旋转矩阵（在这里需要至少3轴加速度，3轴陀螺仪，3轴地磁数据才能计算得到完整的旋转矩阵，在地磁缺失的情况下是无法通过旋转矩阵实现坐标转换的），再从3轴加速度计获取机体的3维加速度，然后通过旋转矩阵将机体坐标系下的3维加速度转换到惯性坐标系下（NED，北东地）也是GPS所对应的坐标。然后就可以把惯性坐标系下的加速度进行一次积分得到速度，再进行二次积分得到位置。这样机体的水平速度和水平位置就得到了，其实很简单，有了物理和微积分的知识就可以很快写出代码。当然之前提到的旋转矩阵和坐标变换看似有些复杂，这些知识实际上需要线性代数作为基础的。但是如果没有学过线性代数甚至是微积分都不没关系，我们只要看一下固定的公式记下来就可以了，毕竟我们也并不需要去写自己的代码。

真正的难度是这样的——积分结果是发散的，也就是说即使飞机一点都没有移动位置，积分得到的速度与位置也会变得越来越大，最后超出计算机的表示范围而溢出，这显然是违背事实的，所以根本无法实现控制！Pix4提供了 一种互补算法来解决这个问题，这里我就是想说说我对互补算法的理解。当然更复杂更好的算法是EKF（扩展卡尔曼滤波）估算，但是确实太过复杂，因为太多的维数，恕我能力有限。

那么这个互补算法的原理是什么呢，把他作为一个系统来理解我想应该会更容易一些，所以还是回到开始，先看一下系统输出量，也就是速度与位置的数据。

我们来看一下源代码在modules/position_estimator_inav/position_estimator_inav_main.c

系统输出量在代码的末尾处：

/* publish local position */
                        local_pos.xy_valid = can_estimate_xy;
                        local_pos.v_xy_valid = can_estimate_xy;
                        local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
                        local_pos.z_global = local_pos.z_valid && use_gps_z;
                        local_pos.x = x_est[0];
                        local_pos.vx = x_est[1];
                        local_pos.y = y_est[0];
                        local_pos.vy = y_est[1];
                        local_pos.z = z_est[0];
                        local_pos.vz = z_est[1];
                        local_pos.landed = landed;
                        local_pos.yaw = att.yaw;
                        local_pos.dist_bottom_valid = dist_bottom_valid;
                        local_pos.eph = eph;
                        local_pos.epv = epv;

                        if (local_pos.dist_bottom_valid) {
                                local_pos.dist_bottom = -z_est[0] - surface_offset;
                                local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
                        }

在位置互补算法中我们最关心的就是3维速度与位置，上面粗体的部分就是x，y，z三个维度的速度与位置数据。在后续的位置控制算法中将使用他们来稳定控制无人机的位置。

欢迎和我一起讨论，QQ15953321

joyrus

续...

上一回找到了输出量，现在找输入量，这里的输入量比较多，如上回说的最重要的输入量是姿态模块计算后得到的旋转矩阵，以及加速度的数据。

看一下源代码：

                                if (sensor.accelerometer_timestamp != accel_timestamp) {
                                        if (att.R_valid) {
                                                /* correct accel bias */
                                                sensor.accelerometer_m_s2[0] -= acc_bias[0];     
                                                sensor.accelerometer_m_s2[1] -= acc_bias[1];
                                                sensor.accelerometer_m_s2[2] -= acc_bias[2];

                                                /* transform acceleration vector from body frame to NED frame */
                                                for (int i = 0; i < 3; i++) {
                                                        acc[i] = 0.0f;

                                                        for (int j = 0; j < 3; j++) {
                                                                acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
                                                        }
                                                }

                                                acc[2] += CONSTANTS_ONE_G;

                                        } else {
                                                memset(acc, 0, sizeof(acc));
                                        }

                                        accel_timestamp = sensor.accelerometer_timestamp;
                                        accel_updates++;
                                }

att.R_valid 就是确定旋转矩阵是否有效；

sensor.accelerometer_m_s2 就是加速度的数据这里单位是米每秒平方；

acc_bias 是经过位置互补算法估算得到的加速度计噪声；

acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; 这里att.R就是旋转矩阵，通过俩个for循环完成了3x3的矩阵计算，将机体坐标系的加速度旋转到导航坐标系下的加速度，这里的旋转目的就是为了得到无人机的水平加速度，也是和GPS相对应的坐标系；

acc[2] += CONSTANTS_ONE_G; 这里是将计算得到的垂直加速度去掉重力加速度，因为任何静止不动的物体在地球引力的作用下都会有重力加速度存在，所以加速度计就会测得这个重力加速度，在地球不同的地方这个重力加速度会有变化，但是绝大多数情况下我们人为他是固定的9.8米每秒平方。

欢迎和我一起讨论，QQ15953321

joyrus

续...

上一回提到了最重要的几个输入量，而acc_bias就是通过位置互补算法得到加速度的误差补偿量，输入的3轴加速度数据经过补偿之后就能够比较真实的还原无人机的速度与位置信息。

后面我们一起讨论如何获得acc_bias，为了实现acc_bias的计算，输入量中必须另外要有提供速度或/和位置的传感器，GPS就是其中最常用的一种获得地球位置坐标（经纬度），以及水平速度的一个传感器。

这里或许有不了解的朋友会问既然有了GPS为什么还要通过加速度进行积分，甚至二次积分来获取无人机的速度与位置？其实排除美国对民用GPS定位精确度的限制之外，最主要的原因是GPS的数据更新速度是非常缓慢的，一般仅有1-10Hz，也就是每秒最多只能给出10次位置的更新。这样的数据更新速度是不可能实现有效的实时控制的。所以借助加速度的高带宽，高采样率（一般为1KHz）来获取有效的实时控制能力。当然Pixhawk还提供了许多接口用于其他位置传感器的接入，比如光流，视觉等。实际上整个位置互补算法将可能用到的位置传感器都考虑到了并实现了代码，但是作为学习和讨论单独探索其中某个传感器的数据处理会更容易掌握核心的算法流程，有利于后续再学习其他的传感器数据处理方法。


欢迎和我一起讨论，QQ15953321

joyrus

续...

接下去讨论GPS数据的处理，GPS是位置互补算法系统的输入量，来自外接的GPS模块，GPS模块的数据将会用于校正加速度计积分得到的位置。

直接看代码：

                        /* vehicle GPS position */
                        orb_check(vehicle_gps_position_sub, &updated);

                        if (updated) {
                                orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);

                                bool reset_est = false;

                                /* hysteresis for GPS quality */
                                if (gps_valid) {
                                        if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
                                                gps_valid = false;
                                                mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
                                        }

                                } else {
                                        if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
                                                gps_valid = true;
                                                reset_est = true;
                                                mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
                                        }
                                }

首先要判断GPS的数据质量，因为GPS往往容易因为被建筑物遮挡而导致无法收取卫星数据，或者数据不准确。

if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3)这句就是从3个条件来判断数据质量如何，gps.eph，gps.epv，gps.fix_type的来自于GPS模块的提供的数据。

                                if (gps_valid) {
                                        double lat = gps.lat * 1e-7;
                                        double lon = gps.lon * 1e-7;
                                        float alt = gps.alt * 1e-3;

                                        /* initialize reference position if needed */
                                        if (!ref_inited) {
                                                if (ref_init_start == 0) {
                                                        ref_init_start = t;

                                                } else if (t > ref_init_start + ref_init_delay) {
                                                        ref_inited = true;

                                                        /* set position estimate to (0, 0, 0), use GPS velocity for XY */
                                                        x_est[0] = 0.0f;
                                                        x_est[1] = gps.vel_n_m_s;
                                                        y_est[0] = 0.0f;
                                                        y_est[1] = gps.vel_e_m_s;

                                                        local_pos.ref_lat = lat;
                                                        local_pos.ref_lon = lon;
                                                        local_pos.ref_alt = alt + z_est[0];
                                                        local_pos.ref_timestamp = t;

                                                        /* initialize projection */
                                                        map_projection_init(&ref, lat, lon);
                                                        // XXX replace this print
                                                        warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
                                                        mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
                                                }
                                        }

如果数据可用则开始计算GPS数据：

                                        double lat = gps.lat * 1e-7;     //纬度
                                        double lon = gps.lon * 1e-7;   //经度
                                        float alt = gps.alt * 1e-3;        //高度

这里就是从GPS模块获取了经度，纬度和高度。

                                                        x_est[0] = 0.0f;                     //北位置
                                                        x_est[1] = gps.vel_n_m_s;     //北速度，单位米/秒
                                                        y_est[0] = 0.0f;                     //东位置
                                                        y_est[1] = gps.vel_e_m_s;     //东速度，单位米/秒

这里是初始化了水平方向估算的位置和速度，这实际上就是最终的输出量，在系统上电后需要初始化一下无人机当前的位置和速度。

                                                        local_pos.ref_lat = lat;
                                                        local_pos.ref_lon = lon;
                                                        local_pos.ref_alt = alt + z_est[0];    //GPS高度加上了垂直方向估算的高度
                                                        local_pos.ref_timestamp = t;

这里将初始化后的数据日后作为控制的参考量。


map_projection_init(&ref, lat, lon);

这一步是对经纬度做投影准备，实际上调用了下面这个函数。

__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{

        ref->lat_rad = lat_0 * M_DEG_TO_RAD;               //角度转换到弧度
        ref->lon_rad = lon_0 * M_DEG_TO_RAD;
        ref->sin_lat = sin(ref->lat_rad);                          //计算sin值
        ref->cos_lat = cos(ref->lat_rad);                        //计算cos值

        ref->timestamp = timestamp;
        ref->init_done = true;

        return 0;
}

返回的GPS投影ref（参照）结构体日后用作GPS投影计算准备。

欢迎和我一起讨论，QQ15953321

joyrus

续...

接着上一回，继续看代码：

                                        if (ref_inited) {
                                                /* project GPS lat lon to plane */
                                                float gps_proj[2];
                                                map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));

                                                /* reset position estimate when GPS becomes good */
                                                if (reset_est) {
                                                        x_est[0] = gps_proj[0];
                                                        x_est[1] = gps.vel_n_m_s;
                                                        y_est[0] = gps_proj[1];
                                                        y_est[1] = gps.vel_e_m_s;
                                                }

                                                /* calculate index of estimated values in buffer */
                                                int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
                                                if (est_i < 0) {
                                                        est_i += EST_BUF_SIZE;
                                                }

                                                /* calculate correction for position */
                                                corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
                                                corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
                                                corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];

                                                /* calculate correction for velocity */
                                                if (gps.vel_ned_valid) {
                                                        corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
                                                        corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
                                                        corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];

                                                } else {
                                                        corr_gps[0][1] = 0.0f;
                                                        corr_gps[1][1] = 0.0f;
                                                        corr_gps[2][1] = 0.0f;
                                                }

                                                /* save rotation matrix at this moment */
                                                memcpy(R_gps, R_buf[est_i], sizeof(R_gps));

                                                w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
                                                w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
                                        }


map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); 记得上一回提到的为GPS投影准备的ref（参照）结构体吗？这里就用到了。

看一下这个函数的原型：

__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
        if (!map_projection_initialized(ref)) {
                return -1;
        }

        double lat_rad = lat * M_DEG_TO_RAD;
        double lon_rad = lon * M_DEG_TO_RAD;

        double sin_lat = sin(lat_rad);
        double cos_lat = cos(lat_rad);
        double cos_d_lon = cos(lon_rad - ref->lon_rad);

        double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
        double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));

        *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
        *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;

        return 0;
}

函数内容较为复杂，稍后我查阅资料后分析，计算后的x，y为投影后的GPS位置数据。

                                                /* calculate correction for position */
                                                corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];                   //水平方向
                                                corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];                   //水平方向
                                                corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];     //高度

corr_gps 就是计算后的GPS修正量，就是通过投影后的GPS数据得到的。

memcpy(R_gps, R_buf[est_i], sizeof(R_gps));  这句是把R_buf[est_i]应该是估算后的旋转矩阵，复制到R_gps。


                                                w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
                                                w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);

最后根据GPS数据的质量来计算GPS的权重。

欢迎和我一起讨论，QQ15953321

joyrus

续...

接上一回，继续看代码：


                /* accelerometer bias correction for GPS (use buffered rotation matrix) */
                float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };

                if (use_gps_xy) {
                        accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
                        accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
                        accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
                        accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
                }

                if (use_gps_z) {
                        accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
                }

                /* transform error vector from NED frame to body frame */
                for (int i = 0; i < 3; i++) {
                        float c = 0.0f;

                        for (int j = 0; j < 3; j++) {
                                c += R_gps[j][i] * accel_bias_corr[j];
                        }

                        if (isfinite(c)) {
                                acc_bias[i] += c * params.w_acc_bias * dt;
                        }
                }

记得上一回提到的corr_gps，这里就派到关键用处了。

                        accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;   //位置
                        accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;                        //速度
                        accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;  //位置
                        accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;                       //速度

accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; //高度

利用corr_gps中的GPS速度与位置修正量来计算accel_bias_corr，呵呵这个和我们要得到最终加速度计误差补偿量的acc_bias很相似了，事实紧跟其后再经过最后一步的坐标旋转就得到我们要的acc_bias了，这就是对应到机体坐标系下的3轴加速度误差修正量了。

到这里实际上最关键的误差补偿量得到了，后面就是使用互补算法计算实际的机体速度和位置了。


欢迎和我一起讨论，QQ15953321

joyrus

续...

这回再回头提一下GPS投影的ref（参照）结构体，其实这个结构体还用于保存home position的位置数据，当home点被更新的时候，看一下代码：

                        /* home position */
                        orb_check(home_position_sub, &updated);

                        if (updated) {
                                orb_copy(ORB_ID(home_position), home_position_sub, &home);

                                if (home.timestamp != home_timestamp) {
                                        home_timestamp = home.timestamp;

                                        double est_lat, est_lon;
                                        float est_alt;

                                        if (ref_inited) {
                                                /* calculate current estimated position in global frame */
                                                est_alt = local_pos.ref_alt - local_pos.z;
                                                map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
                                        }

                                        /* update reference */
                                        map_projection_init(&ref, home.lat, home.lon);

                                        /* update baro offset */
                                        baro_offset += home.alt - local_pos.ref_alt;

                                        local_pos.ref_lat = home.lat;
                                        local_pos.ref_lon = home.lon;
                                        local_pos.ref_alt = home.alt;
                                        local_pos.ref_timestamp = home.timestamp;

                                        if (ref_inited) {
                                                /* reproject position estimate with new reference */
                                                map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
                                                z_est[0] = -(est_alt - local_pos.ref_alt);
                                        }

                                        ref_inited = true;
                                }
                        }

map_projection_init(&ref, home.lat, home.lon); 这句就是用home点的经纬度来设定ref（参照）的位置数据。

欢迎和我一起讨论，QQ15953321

joyrus

续...

这回就要说位置互补算法了，还是先上代码，慢慢看。

                if (can_estimate_xy) {
                        /* inertial filter prediction for position */
                        inertial_filter_predict(dt, x_est, acc[0]);    //x_est[1]=vel, [0]=pos, pitch
                        inertial_filter_predict(dt, y_est, acc[1]);           //y_est[1]=vel, [0]=pos, roll

                        if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
                                write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
                                memcpy(x_est, x_est_prev, sizeof(x_est));
                                memcpy(y_est, y_est_prev, sizeof(y_est));
                        }

                        if (use_gps_xy) {
                                eph = fminf(eph, gps.eph);

                                inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
                                inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);

                                if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
                                        inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
                                        inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
                                }
                        }

                        if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
                                write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
                                memcpy(x_est, x_est_prev, sizeof(x_est));
                                memcpy(y_est, y_est_prev, sizeof(y_est));
                                memset(corr_gps, 0, sizeof(corr_gps));
                                memset(corr_vision, 0, sizeof(corr_vision));
                                memset(corr_flow, 0, sizeof(corr_flow));

                        } else {
                                memcpy(x_est_prev, x_est, sizeof(x_est));
                                memcpy(y_est_prev, y_est, sizeof(y_est));
                        }
                } else {
                        /* gradually reset xy velocity estimates */
                        inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
                        inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
                }

就这么简单！

                        inertial_filter_predict(dt, x_est, acc[0]);    //x_est[1]=vel, [0]=pos, pitch
                        inertial_filter_predict(dt, y_est, acc[1]);           //y_est[1]=vel, [0]=pos, roll

void inertial_filter_predict(float dt, float x[2], float acc)
{
        if (isfinite(dt)) {
                if (!isfinite(acc)) {
                        acc = 0.0f;
                }
                x[0] += x[1] * dt + acc * dt * dt / 2.0f;//位置
                x[1] += acc * dt;                                //速度
        }
}

首先用加速度计的数据预测无人机的速度与位置，这里的加速度数据是已经经过误差补偿的结果。计算过程简单明了，就是物理知识。



                        inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
                        inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);

                        inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
                        inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);

上面是位置互补，下面是速度互补，看一下函数原型：

void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
        if (isfinite(e) && isfinite(w) && isfinite(dt)) {
                float ewdt = e * w * dt;
                x += ewdt;

                if (i == 0) {
                        x[1] += w * ewdt;
                }
        }
}

e就是corr_gps的补偿量，这里实际上就是位置的误差，ewdt 就是依据误差与时间和权重的计算结果，用于对估算结果的补偿。

好了完成了！第一阶段的初步流程学习笔记已经整理完成，接下来准备第二阶段的部分细节的展开，需要些时间来消耗，大家见谅！

欢迎和我一起讨论，QQ15953321