运动控制

您所在的位置：网站首页 › 3d画弧线 › 运动控制

运动控制

2023-10-18 13:33| 来源: 网络整理| 查看: 265

设输入三点为圆弧上的三个点 $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); } }

二、圆弧插补

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.x() && InterpolationPoint.y() = 0) { if (InterpolationPoint.x() >= circleCenter.x()&& InterpolationPoint.y() = circleCenter.x() && InterpolationPoint.y() 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

【本文地址】

公司简介

联系我们