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

设输入三点为圆弧上的三个点 $P_1(x_1, y_1)$ ， $P_2(x_2, y_2)$ ) ， $P_3(x_3, y_3)$ ，三点按顺时针或者逆时针在圆弧上排列。

一、三点圆弧

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

$p12=(x2-x1,y2-y1)$

$p23=(x3-x2,y3-y3)$

$p12\times p23 = (x2-x1)*(y3-y2)-(y2-y1)*(x3-x2)$
(1) 结果为正：圆弧是逆时针画
(2) 结果为负：圆弧是顺时针画
(3 )结果为零：三点在同一直线上

2、计算圆弧圆心及半径

设圆心坐标为 $(x_0, y_0)$ ，半径为 $r$ ，则圆的方程可写为：

$(x - x_0)^2 + (y - y_0)^2 = r^2$

将输入的三个点 $P_1(x_1, y_1)$ ， $P_2(x_2, y_2)$ ) ， $P_3(x_3, y_3)$ 代入方程可得：

$\begin{cases} (x_1 - x_0)^2 + (y_1-y_0)^2 = r^2 & (1)\\ (x_2 - x_0)^2 + (y_2-y_0)^2 = r^2 & (2)\\ (x_3 - x_0)^2 + (y_3-y_0)^2 = r^2 & (3) \end{cases}$

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

$(x_1 - x_2) x_0 + (y_1- y_2) y_0 = \frac{(x_1^2-x_2^2)-(y_2^2-y_1^2)}{2}$

$(x_1 - x_3) x_0 + (y_1- y_3) y_0 = \frac{(x_1^2-x_3^2)-(y_3^2-y_1^2)}{2}$

上式中 $x_0,y_0$ 有唯一解的条件是系数行列式不为 $0$ ：

$\begin{vmatrix} (x_1 - x_2) & (y_1- y_2) \\ (x_1 - x_3) & (y_1- y_3) \end{vmatrix} \neq 0$ 简化后为： $\frac{x_1 - x_2}{y_1-y_2} \neq \frac{x_1 - x_3}{y_1-y_3}$

即三点不共线。

设：

$\\a = x_1-x_2\\ b = y_1-y_2\\ c = x_1-x_3\\ d = y_1-y_3\\ \\e = \frac{(x_1^2-x_2^2)-(y_2^2-y_1^2)}{2}\\ f = \frac{(x_1^2-x_3^2)-(y_3^2-y_1^2)}{2}$

则 $x_0,y_0$ 的解为：

$\\x_0 = -\frac{d e-b f}{b c-a d}\\ \\y_0 = -\frac{a f-c e}{b c-a d}$

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

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);
    }
}

二、圆弧插补

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

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

插补流程说明：

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

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

猜你喜欢