运动控制——三点圆弧和平面圆弧插补

设输入三点为圆弧上的三个点 ，) ，，三点按顺时针或者逆时针在圆弧上排列。

一、三点圆弧

1、判断三点是否共线及圆弧走向
求与的向量积 

 (1) 结果为正：圆弧是逆时针画  
 (2) 结果为负：圆弧是顺时针画 
 (3 )结果为零：三点在同一直线上

2、计算圆弧圆心及半径

设圆心坐标为，半径为 ，则圆的方程可写为： 

 

将输入的三个点 ，) ，代入方程可得：

其中公式(1)和(2)相减，(1)和(3)相减化简后可得：

上式中有唯一解的条件是系数行列式不为 ： 

                            简化后为：                  

即三点不共线。

设：

则的解为：

将结果带入公式(1)(2)(3)其中之一即可求出半径。

3、代码实现

void drawArcImage(QPointF dataA,QPointF dataB,QPointF dataC)
{
    int judge= (int)((dataB.x()-dataA.x())*(dataC.y()-dataB.y())- (dataB.y()-dataA.y())*(dataC.x()-dataB.x()));//判断是否共线
    int arcDire = 1;//判断逆圆和顺圆
    if(judge != 0 )
    {
        if(judge > 0) arcDire =0;
        else if(judge < 0) arcDire =1;

        //计算圆心和半径
        double a = dataA.x() - dataB.x();
        double b = dataA.y() - dataB.y();
        double c = dataA.x() - dataC.x();
        double d = dataA.y() - dataC.y();
        double e = ((dataA.x()*dataA.x()-dataB.x()*dataB.x())-(dataB.y()*dataB.y()-dataA.y()*dataA.y()))/2;
        double f = ((dataA.x()*dataA.x()-dataC.x()*dataC.x())-(dataC.y()*dataC.y()-dataA.y()*dataA.y()))/2;
        double x0 = -(d*e-b*f)/(b*c-a*d);
        double y0 = -(a*f-c*e)/(b*c-a*d);
        double rSqure =  sqrt((dataA.x()-x0)*(dataA.x()-x0) +(dataA.y()-y0)*(dataA.y()-y0));
        QPointF centre(x0,y0);

        //圆弧插补
        circleInterpolation(centre,rSqure,dataA,dataC,1.0,arcDire);
    }
}

 

二、圆弧插补

1、插补原理说明

所谓插补即是沿着规定的轮廓、在轮廓的起点和终点之间确定若干个中间点的方法。即“插入”“补上”运动中间点的坐标，实质上是完成数据点密化的工作。

此处采用逐点比较法进行圆弧插补，这种方法每次仅向一个坐标轴输出一个进给脉冲，同时每走一步都要通过偏差函数计算，判断插补点的瞬时坐标与规定加工轨迹之间的偏差，然后决定下一步的进给方向。每个插补循环由偏差判别、进给、偏差函数计算和终点判别四个步骤组成。

 插补流程说明：

                                                           

a)顺时针行进的圆弧插补

如上图顺圆走向所示，以圆心为坐标系原点，将圆分为四部分；

(1) 第一象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让xi不变，yi向y轴负方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让yi不变，xi向x轴正方向移动一个步长；

(2) 第二象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让yi不变，xi向x轴正方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让xi不变，yi向y轴正方向移动一个步长；

(3) 第三象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让xi不变，yi向y轴正方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让yi不变，xi向x轴负方向移动一个步长；

(4) 第四象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让yi不变，xi向x轴负方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让xi不变，yi向y轴负方向移动一个步长。

b)逆时针行进的圆弧插补

如上图逆圆走向所示，以圆心为坐标系原点，将圆分为四部分；

(1) 第一象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让xi不变，yi向y轴正方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让yi不变，xi向x轴负方向移动一个步长；

(2) 第二象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让yi不变，xi向x轴负方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让xi不变，yi向y轴负方向移动一个步长。

(3) 第三象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让xi不变，yi向y轴负方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让yi不变，xi向x轴正方向移动一个步长；

(4) 第四象限部分，当上一个插入点(xi,yi)在圆的外部时，我们让yi不变，xi向x轴正方向移动一个步长（步长越小，精度越高）；当插入点(xi,yi)在圆的内部时，我们让xi不变，yi向y轴正方向移动一个步长；

 

2、代码实现

void circleInterpolation(QPointF circleCenter, //圆心坐标
                        double Radius,         //半径
                        QPointF StartPoint,    //轨迹起点
                        QPointF EndPoint,      //轨迹终点
                        float m_StepSize,      //插补步距
                        int direction)         // 圆的方向

void circleInterpolation(QPointF circleCenter,double Radius,QPointF StartPoint,QPointF EndPoint,float m_StepSize,int direction)
{
    QPointF InterpolationPoint;
    QPointF startArcPoints;
    InterpolationPoint.setX(StartPoint.x());
    InterpolationPoint.setY(StartPoint.y());
    startArcPoints.setX(InterpolationPoint.x());
    startArcPoints.setY(InterpolationPoint.y());
    double InsideOrOutside;

    while ((abs(InterpolationPoint.x() - EndPoint.x())>m_StepSize) ||(abs(InterpolationPoint.y() - EndPoint.y())>m_StepSize))
    {
        //计算点在圆的内部还是外部
        InsideOrOutside = sqrt((InterpolationPoint.x() - circleCenter.x())* (InterpolationPoint.x() - circleCenter.x()) + (InterpolationPoint.y() - circleCenter.y())* (InterpolationPoint.y() - circleCenter.y())) - Radius;

        //顺圆
        if (direction == 1)
        {
            //圆的外部
            if (InsideOrOutside >= 0)
            {
                //第一象限
                if (InterpolationPoint.x() >= circleCenter.x()&& InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() - m_StepSize);
                //第二象限
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() + m_StepSize);
                //第三象限
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() + m_StepSize);
                //第四象限
                else if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() - m_StepSize);
            }
            //圆的内部
            else
            {
                if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() - m_StepSize);
                else if (InterpolationPoint.x() <= circleCenter.x()&&InterpolationPoint.y() <=circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() - m_StepSize);
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() + m_StepSize);
                else if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() + m_StepSize);
            }
        }

        //逆圆
        if (direction == 0)
        {
            if (InsideOrOutside >= 0)
            {
                if (InterpolationPoint.x() >= circleCenter.x()&& InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() + m_StepSize);
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() + m_StepSize);
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() - m_StepSize);
                else if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() - m_StepSize);
            }
            else
            {
                if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() + m_StepSize);
                else if (InterpolationPoint.x() <=circleCenter.x()&&InterpolationPoint.y() <= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() - m_StepSize);
                else if (InterpolationPoint.x() <= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setX(InterpolationPoint.x() - m_StepSize);
                else if (InterpolationPoint.x() >= circleCenter.x() && InterpolationPoint.y() >= circleCenter.y())
                    InterpolationPoint.setY(InterpolationPoint.y() + m_StepSize);
            }
        }
        //画线
        gview->addLine(startArcPoints.x(),startArcPoints.y(),InterpolationPoint.x(),InterpolationPoint.y(),apen);
        //插补点设置为起点
        startArcPoints = InterpolationPoint;
    }
}

 

参考：

https://blog.csdn.net/liyuanbhu/article/details/52891868

https://blog.csdn.net/qq_36552550/article/details/79356577

http://www.busnc.com/ly/zhudian/yuanhuchabu.htm