光栅尺测量数据的修正

光栅尺测量数据的修正

最近有个视觉项目，相机要在一个直线轨道上运动。要求这个直线轨道的运动精度非常的高。300mm 的运动范围内重复定位精度做到3um 以内。还要求绝对定位精度 10um 以内。

重复定位精度相对来说好达到，只要导轨选的好，伺服电机的分辨率足够，一般是能达到 3um 重复定位精度的。
绝对定位精度就难很多了。为了绝对定位精度，平台上安装了 1um 分辨率的光栅尺。但是即使这样，长距离行走时误差还是很大，实测 300mm 行程时误差能达到 50-60um。 为了消除这个误差，就需要分段补偿。下面就讲讲分段补偿的方法。

首先，我们需要一个能测出行走误差的仪器。我们这个项目是用了一把 1um 精度的玻璃线纹尺。玻璃线纹尺上面有刻度，我们认为这个刻度是精确的。用我们的光学系统对这些刻度进行拍照。比如说让图像的中心对准 0mm 处，这时光栅尺的读书为 A0，然后移动平台，让图像中心对准 10mm 处，这时光栅尺读数为 A1。 理论上，A1-A0 = 10mm。如果不是10mm，说明我们的光栅尺的读数与真实的位移是有误差的。这个误差决定了绝对定位精度。

理论上来说，我们可以把玻璃线纹尺的原点与光栅尺的原点对齐，正方向也都同向。这时以此将光学系统移动到玻璃线纹尺的整数刻度处，然后记录当前的光栅尺读数。可以得到一个类似下面的表格。

玻璃线纹尺	光栅尺
0.00	0.00
50.00	49.98
100.00	100.03
150.00	150.05
200.00	199.99
250.00	250.03
300.00	300.06

有了这个表格，我们可以构造两个分段线性插值函数。第一个函数用来将真实位置（玻璃尺坐标系）转换为光栅尺的读数。比如我们要去 100.00 位置处，那么我们就走到光栅尺读数为 100.03 的位置就可以了。第二个函数是将光栅尺读数转换为真实位置的。

这两个插值函数用最简单的分段线性插值就可以了。甚至我们都可以不需要自己来写一个线性插值算法。我下面的代码直接用了 GSL（GNU Scientific Library） 里面的功能。

另外，我们知道光栅尺虽然有误差，但是误差不会很大，我们在标定时只要保证玻璃尺的读数为整数，那么上面的表格的第一列就不需要记录。比如光栅尺读数为150.05 ，那么对应的真实位置一定是150mm。我们只要对光栅尺读数取个整就好了。

另外，我们也不需要将玻璃尺的原点与光栅尺的原点精确对齐。比如下面的数据。

玻璃线纹尺	光栅尺
0.00	4.002
2.00	6.01
3.00	7.003
5.00	9.031
11.00	15.03

这个数据只有光栅尺 4.002 到 15.03 之间的标定数据。那么在这一段做插值。两边做外插值就行了。另外，我们可以把玻璃尺的数据挪一下。

玻璃线纹尺	光栅尺
4.002	4.002
6.002	6.01
7.002	7.003
9.002	9.031
15.002	15.03

这样挪动相当于认为 4.002 以前的数据全都是准确的，相当于我们补了个 0。15.002 之后的数据直接外插。

玻璃线纹尺	光栅尺
0	0
4.002	4.002
6.002	6.01
7.002	7.003
9.002	9.031
15.002	15.03

下面给出代码，主要用到了 GSL 里面的 gsl_interp_eval 函数。大家可以读这个函数的帮助文档。

#ifndef LECALIBRATOR_H
#define LECALIBRATOR_H
#include <gsl/gsl_interp.h>
#include <QVector>
/**
 * @brief The LECalibrator class 光栅尺（Linear Encoder）输出位移修正器。
 *                               由于各种原因，导致光栅尺的测量结果与真实值有一定的差距。
 *                               本 C++类可以对光栅尺的测量结果进行一定程度的修正。
 * 要求校准点之间的真实距离是 1mm 的整数倍，比如有如下校准点： 4.002 6.01  7.003 9.031 15.03
 * 那么程序默认6.01 到 4.002 的距离是 2mm, 7.003 到 4.002 的距离是 3mm, 9.031 到4.002 的距离是 5mm
 * 并以此为依据来修正其他的点。其中最小的点保持不变，也就是 4.002 修正后的结果还是 4.002,
 * 那么 9.031 修正的结果是 9.002.
 */
class LECalibrator
{
public:
    LECalibrator();
    ~LECalibrator();

    /**
     * @brief loadPoints 将校准点传入
     * @param points 校准点，要求这些校准点之间的真实距离是 1mm 的整数倍。校正点不需要排序，程序内部会自动排序。
     *                校准点也不需要是等间隔的，但是从修正效果考虑，建议为实际距离等间隔点
     */
    void loadPoints(QVector<double> points);

    void loadPoints(double p[], int N);
    /**
     * @brief encoderPosToTruePos 对光栅尺的结果进行修正
     * @param value 光栅尺的原始数值
     * @return 修正后的数值
     */
    double encoderPosToTruePos(double value);
    double truePosToEncoderPos(double value);
    void print();/// 测试用
private:
    void reset();
    void build();
    gsl_interp * m_interp;
    gsl_interp * m_interp_rev;
    gsl_interp_accel * m_accel;
    gsl_interp_accel * m_accel_rev;
    double * m_xa;
    double * m_ya;
    int m_size;
};

#endif // LECALIBRATOR_H

#include "LECalibrator.h"
#include <QtAlgorithms>
#include <QDebug>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_nan.h>

LECalibrator::LECalibrator()
    : m_xa(nullptr),
      m_ya(nullptr),
      m_size(0),
      m_interp(nullptr),
      m_interp_rev(nullptr),
      m_accel(nullptr),
      m_accel_rev(nullptr)
{

}

LECalibrator::~LECalibrator()
{
    reset();
}

void LECalibrator::print()
{
    for(int i = 0; i < m_size; i ++)
    {
        qDebug() << "(" << m_xa[i] << ", " << m_ya[i] << ")";
    }
}

double LECalibrator::encoderPosToTruePos(double value)
{
    if(m_size == 0 || value <= m_xa[0])
    {
        return value;
    }
    if(value >= m_xa[m_size - 1]) //如果超出最后一个点，则外插
    {
        return m_ya[m_size - 1] + (value - m_xa[m_size - 1]);
    }

    double ret = 0;
    if(m_interp)
    {
        ret = gsl_interp_eval (m_interp, m_xa, m_ya, value, m_accel);

        if(ret == GSL_NAN)
        {
            return value;
        }
        return ret;
    }
    return value;
}

double LECalibrator::truePosToEncoderPos(double value)
{
    if(m_size == 0 || value <= m_ya[0])
    {
        return value;
    }
    if(value >= m_ya[m_size - 1]) //如果超出最后一个点，则外插
    {
        return m_xa[m_size - 1] + (value - m_ya[m_size - 1]);
    }

    double ret = 0;
    if(m_interp_rev)
    {
        ret = gsl_interp_eval (m_interp_rev, m_ya, m_xa, value, m_accel_rev);

        if(ret == GSL_NAN)
        {
            return value;
        }
        return ret;
    }
    return value;
}


void LECalibrator::reset()
{
    if(m_xa)
    {
        delete[] m_xa;
        delete[] m_ya;
        m_xa = nullptr;
        m_ya = nullptr;
    }
    m_size = 0;
    if(m_interp)
    {
        gsl_interp_free(m_interp);
        gsl_interp_free(m_interp_rev);
        gsl_interp_accel_free(m_accel);
        gsl_interp_accel_free(m_accel_rev);
        m_interp = nullptr;
        m_accel = nullptr;
        m_accel_rev = nullptr;
    }
}

inline double round( double x)
{
    int ret = static_cast<int>(x + 0.5);
    return ret;
}

void LECalibrator::build()
{
    gsl_sort(m_xa, 1, m_size);

    m_ya[0] = m_xa[0];
    for(int i = 1; i < m_size; i++)
    {
        m_ya[i] = round( m_xa[i] - m_xa[0] ) + m_xa[0];
    }
    m_interp = gsl_interp_alloc(gsl_interp_linear, m_size);
    m_interp_rev = gsl_interp_alloc(gsl_interp_linear, m_size);
    gsl_interp_init(m_interp, m_xa, m_ya, m_size);
    gsl_interp_init(m_interp_rev, m_ya, m_xa, m_size);
    m_accel = gsl_interp_accel_alloc();
    m_accel_rev = gsl_interp_accel_alloc();
}

void LECalibrator::loadPoints(double p[], int N)
{
    if(N < 2)
    {
        return;
    }

    reset();
    m_size = N;
    m_xa = new double[m_size];
    m_ya = new double[m_size];

    for(int i = 0; i < m_size; i++)
    {
        m_xa[i] = p[i];
    }

    build();
}

void LECalibrator::loadPoints(QVector<double> points)
{
    if(points.size() < 2)
    {
        return;
    }

    reset();
    m_size = points.size();
    m_xa = new double[m_size];
    m_ya = new double[m_size];

    for(int i = 0; i < m_size; i++)
    {
        m_xa[i] = points.at(i);
    }
    gsl_sort(m_xa, 1, m_size);

    build();
}