[人工智能]2021-10-17

一、原理介绍

1.1 简介

本文章灵感来源于机床数控技术及应用课程，上课老师提过，机床在加工一条直线或者圆弧的时候其实并不是按照理想的轨迹去加工的，其运动轨迹放大之后是锯齿状的（如下图所示），是通过逼近理想轨迹来加工的。那如何通过给定直线两个点，或者圆弧的起点终点，就能找到其运动轨迹呢?因为前阵子刚接触一个四轴电装机器臂，对其运动轨迹也比较感兴趣，可能跟课本的数控加工算法不一样，所以就打算练练手，将算法实现一下。

1.2 逐点比较法偏差计算(直线)

1.2.1 第一象限逐点比较法思路

如何去评价在直线的上方？本文以斜率绝对值来进行比较。

1.2.2 四象限逐点比较法实现思路

1. 效率较低的方法是，分别写出四个象限情况的程序，根据输入去判断然后进入对应的程序计算就行。这种情况也能够实现，但是不太建议。遇到问题，我们是先进行分析，根据四象限直线的特征，以及上面提到的根据直线斜率来判断点在直线上方或者直线下方。
2. 以这个指标来比较，不难发现在直线上方的所有点进给方向都是x轴方向，直线下方的进给方向都是y轴方向。 还有一个规律就是，终点在哪一象限，x或y的符号是什么，就往正方向或者负方向进给。
3. 可能大家也有一个疑问，就是起点不在原点怎么办？很简单，大家只需要利用终点坐标—起点坐标，这样就是以起点坐标为（0，0），终点坐标对于起点来说就是相对坐标。这跟数控机床编程时，可以用到相对坐标增量来计算类似。

逐点比较法写得不错得一篇博文，大家可以更加仔细了解一下

1.3 逐点比较法偏差计算(圆弧)

1.3.1 第一象限逐点比较法思路（圆弧）

大家可以参考下图第一象限的逆时针以及顺时针讲解。

1.3.2 四象限逐点比较法实现思路（圆弧）

1.相对于直线插补算法，四象限的圆弧插补确实不好理解，不过也不难，只要大家根据下面的规律总结以及课本，根据规律多尝试就能编出程序。
2.四象限的圆弧插补主要难在顺逆选方向的判断，以及如何确定进给方向？如何确定第几象限？其实只要这三个问题解决了，问题就迎刃而解。
3.顺逆时针的判断：利用法向量进行判断，这个是属于高数的内容，大家可以回去翻一下书，这里我是直接CSDN。旋转判断参考链接
4.象限判断:如何进行象限判断，考虑到圆弧起点以及终点都在坐标轴上，那么如何去判断呢？因此我采用了，((Xs+Xe)/3,(Ys+Ye)/3) 作为判断点。可以判断圆弧起点所在象限。因为无论中终点是否跨象限，都是进行运行计算。
备注：(Xs,Ys)：起点坐标,(Xe,Ye)：终点坐标。

二、程序实现思路

2.1 直线插补

1.先对起点在原点，第一象限直线进行插补
2.起点不在原点的直线，通过求终点相对于起点的相对坐标，这样起点就是原点。
3.根据 (Xi,Yi)与原点（起点）所车成直线斜率跟终点与原点（起点）斜率之间的关系，判断进给是x或者y方向。再根据起点坐标所在象限，得出x,y的正负，再根据正负求其进给方向。

2.2 圆弧插补

1.同时是先简单后再到难，第一步我们先根据第一象限圆弧，顺逆旋转的特点（看纸质版图解+课本+网上资料会更好理解）。
2.第二步就有些绕，需要根据四象限，顺逆时针旋转，进给方向是往正负x或者正负y。是不是有点乱。不着急，其实大家只要将全部进给方向都以第一象限的逆时针作为参考，通过上图变换规律作为理解。先以第一象限设定好进给x,y，再根据象限判断、旋转方向判断，确定实际进给方向，然后根据进给方向符号，直接求偏差。问题就迎刃而解。（建议大家看代码理解会更好）

三、代码展示

3.1直线插补程序以及效果

3.1.1 第一象限直线插补程序

# 作者：仲恺农业工程学院 冯耿鑫
# 日期：2021/10/12
# 功能：逐点比较法实现：第一象限过原点
# 参考链接：https://blog.csdn.net/joogle/article/details/7962320
# 输入直线终点坐标
import  matplotlib.pyplot as plt                # 画图库
from matplotlib.pyplot import MultipleLocator   # 坐标设定库

end_pos = input("请输入直线终点坐标:\t")
end_pos = end_pos.split(',')	
Xe = int(end_pos[0])
Ye = int(end_pos[len(end_pos)-1])
n = Xe + Ye

Fi= [0]             # 判别偏差
x = [0]             # 每个x坐标
y = [0]             # 每个y坐标
pos = [(0,0)]       # 每个点的坐标


print("判别\t\t\t进给\t\t\t运算\t\t\t比较\t\t\t")
for i in range(n):
    # 正偏差，位于直线上方
    if Fi[i] >=0 :
        dir = "+dx"                  # 往x正方向
        print(Fi[-1],end="\t\t\t")
        F_temp = Fi[-1] - Ye        # 迭代发偏差运算
        x_temp = x[i]+1             # 记录下一个x坐标,x[i+1] = x[i]
        y_temp = y[i]               # 记录下一个y坐标,y[i+1] = y[i]
        x.append(x_temp)            # 对每一个x坐标进行储存
        y.append(y_temp)            # 对每一个y坐标进行储存
        pos_temp = (x_temp,y_temp)  # 对每一个坐标点进行储存
        pos.append(pos_temp)
        Fi.append(F_temp)           # 对运算偏差进行存储
        print(dir,end="\t\t\t")
        print(Fi[-1],end="\t\t\t")
        print(n-i)

    else:
        dir = "+dy"                  # 往y正方向
        print(Fi[-1],end="\t\t\t")
        print(dir,end="\t\t\t")
        F_temp =Fi[-1] + Xe         # 迭代发偏差运算
        y_temp = y[i]+1             # 记录下一个y坐标,y[i+1] = y[i]+1
        x_temp = x[i]               # 记录下一个x坐标，其中x不变x[i+1] = x[i]
        x.append(x_temp)            # 对每一个x坐标进行储存
        y.append(y_temp)            # 对每一个y坐标进行储存
        pos_temp =(x_temp,y_temp)   # 对每一个坐标点进行储存
        pos.append(pos_temp)
        Fi.append(F_temp)          # 对运算偏差进行存储
        print(Fi[-1],end="\t\t\t")
        print(n-i)



# 画图
plt.plot([0,Xe],[0,Ye],color = 'r')
plt.plot(x,y,'o',color='b')
plt.plot(x,y)


plt.title('line',fontsize=24)
plt.tick_params(axis='both',which='major',labelsize=14)
plt.xlabel('x',fontsize=14)
plt.ylabel('y',fontsize=14)
x_major_locator=MultipleLocator(1)
#把x轴的刻度间隔设置为1，并存在变量里
y_major_locator=MultipleLocator(1)
#把y轴的刻度间隔设置为10，并存在变量里
ax=plt.gca()
#ax为两条坐标轴的实例
ax.xaxis.set_major_locator(x_major_locator)
#把x轴的主刻度设置为1的倍数
ax.yaxis.set_major_locator(y_major_locator)
#把y轴的主刻度设置为10的倍数
plt.xlim(-0.5,Xe+1)
#把x轴的刻度范围设置为-0.5到11，因为0.5不满一个刻度间隔，所以数字不会显示出来，但是能看到一点空白
plt.ylim(-0.5,Ye+1)
#把y轴的刻度范围设置为-5到110，同理，-5不会标出来，但是能看到一点空白

plt.savefig('.\\1.jpg')
plt.show()

3.1.2 第一象限直线插补程序运行效果

请输入直线终点坐标:	4,6
判别	   进给			运算	   比较			
0			+dx			-6		   10
-6			+dy			-2			9
-2			+dy			2			8
2			+dx			-4			7
-4			+dy			0			6
0			+dx			-6			5
-6			+dy			-2			4
-2			+dy			2			3
2			+dx			-4			2
-4			+dy			0			1

3.1.3 四象限直线插补代码

# 作者：冯耿鑫
# 日期：2021/10/12
# 功能：逐点比较法实现：第一象限过原点
# 参考链接：https://blog.csdn.net/joogle/article/details/7962320
# 输入直线终点坐标

#  有input的时候，要调试就需要用到这个
# -*- coding: UTF-8 -*-
import  matplotlib.pyplot as plt                # 画图库
from matplotlib.pyplot import MultipleLocator   # 用来调整坐标轴




start_pos = input("请输入直线起点坐标:\t")       # 获取起点坐标
end_pos = input("请输入直线终点坐标:\t")         # 获取终点坐标
start_pos = start_pos.split(',')            # 对','进行拆分
end_pos = end_pos.split(',')
Xs = int(start_pos[0])                      # 起点x
Ys = int(start_pos[len(start_pos)-1])       # 起点y
Xe = int(end_pos[0])                        # 终点x
Ye = int(end_pos[len(end_pos)-1])           # 终点y



Xe = Xe - Xs            # 终点相对坐标x
Ye = Ye - Ys            # 终点相对坐标y
n = abs(Xe) + abs(Ye)    # 步数
x = [0]                 # 起点x
y = [0]                 # 起点y
pos = [(0,0)]            # 每个点的坐标，初始化起点坐标为（0，0）


# 用于判断进给方向，例如第二象限，Fi>0，是-dx,如果在第一象限是+dx
dir_x = "-"  if Xe<0 else "+"
dir_y = "-"  if Ye<0 else "+"

"""
if rel_x <0:
    dir_dx ="-dx"
else:
    dir_dx = "+dx"

if rel_y <0:
    dir_dy = "-dy"
else:
    dir_dy = "+dy"
"""



Fi= [0]             # 判别偏差
print("判别\t\t\t进给\t\t\t运算\t\t\t比较\t\t\t")
for i in range(n):
    # 正偏差，位于直线上方
    if Fi[i] >=0 :
        dir = dir_x +"dx"                # 往x正方向
        print(Fi[-1],end="\t\t\t")
        F_temp = Fi[-1] - abs(Ye)        # 迭代发偏差运算
        x_temp = x[i] - 1 if Xe<0 else x[i]+1             # 记录下一个x坐标,x[i+1] = x[i]
        y_temp = y[i]               # 记录下一个y坐标,y[i+1] = y[i]
        x.append(x_temp)            # 对每一个x坐标进行储存
        y.append(y_temp)            # 对每一个y坐标进行储存
        pos_temp = (x_temp,y_temp)  # 对每一个坐标点进行储存
        pos.append(pos_temp)
        Fi.append(F_temp)           # 对运算偏差进行存储
        print(dir,end="\t\t\t")
        print(Fi[-1],end="\t\t\t")
        print(n-i)

    else:
        dir = dir_y+ "dy"                  # 往y正方向
        print(Fi[-1],end="\t\t\t")
        print(dir,end="\t\t\t")
        F_temp =Fi[-1] + abs(Xe)         # 迭代发偏差运算
        y_temp = y[i]-1 if Ye<0 else y[i] +1          # 记录下一个y坐标,y[i+1] = y[i]+1
        x_temp = x[i]               # 记录下一个x坐标，其中x不变x[i+1] = x[i]
        x.append(x_temp)            # 对每一个x坐标进行储存
        y.append(y_temp)            # 对每一个y坐标进行储存
        pos_temp =(x_temp,y_temp)   # 对每一个坐标点进行储存
        pos.append(pos_temp)
        Fi.append(F_temp)          # 对运算偏差进行存储
        print(Fi[-1],end="\t\t\t")
        print(n-i)


# 画图
plt.plot([0,Xe],[0,Ye],color = 'r') # 起点、终点连线
plt.plot(x,y,'o',color='b')     # 画点
plt.plot(x,y)                   # 画线




plt.savefig('.\\2.jpg')
plt.show()

3.1.4 四象限直线插补代码运行效果

请输入直线起点坐标:	-4,-5
请输入直线终点坐标:	-8,-11
判别		进给		运算		比较			
0			-dx			-6			10
-6			-dy			-2			9
-2			-dy			2			8
2			-dx			-4			7
-4			-dy			0			6
0			-dx			-6			5
-6			-dy			-2			4
-2			-dy			2			3
2			-dx			-4			2
-4			-dy			0			1

3.2 圆弧插补程序以及效果

3.2.1 第一象限圆弧插补程序

# -*- coding: cp936 -*-
import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator
import  math
# 通过键盘输入获取圆弧信息
print("当前进入的是圆心位于原点，圆弧位于第一象限情况")
pos_s = input("请输入圆弧起点:\t")
pos_e = input("请输入圆弧的终点:\t")
pos_s = pos_s.split(',')    # 对开始坐标进行获取，3，4拆分下来就是x=3,y=4
pos_e = pos_e.split(',')    # 对结束坐标进行获取


# 获取起点终点x,y值,以及总步数
Xs = int(pos_s[0])
Ys = int(pos_s[1])
Xe = int(pos_e[0])
Ye = int(pos_e[1])
n = abs(Xs-Xe)+abs(Ys-Ye)



# 定义列表用于储存需要计算以及打印的数据
Fi= [0]             # 包括原点的偏差数据
x = [Xs]            # 每个x坐标
y = [Ys]            # 每个y坐标
pos = [(Xs,Ys)]     # 每个点的坐标
dir  = []           # 储存运动方向
div_cri = []        # Deviation criterion 误差判别>0/<0
str_Fi_list = []    # 将F+编号i生成一个字符串
new_dev_calc = []    # 用于储存新偏差数据eg：F1>0 New deviation calculation



# 对第一行信息进行打印
print("序号\t\t\t偏差判别\t\t\t进给\t\t\t新偏差计算\t\t\t坐标计算\t\t终点判别\t\t\t")
print("\t\t\t\t\t\t", end="\t\t\t\t")
print("F0 =",Fi[-1],"\t\t\t\t",pos[-1],"\t\t")



# 插补算法的计算
for i in range(n):

    # 逆时针加工
    if Xs > Xe:
        # 点位于圆外
        if Fi[i]>=0:
            str_Fi= 'F'+str(i+1)      # 生成Fi

            # 偏差计算、坐标计算
            F_new = Fi[-1] - 2 * x[i] + 1       # 迭代发偏差运算
            x_new= x[-1] - 1                    # 记录下一个x坐标,x[i+1] = x[i]
            y_new = y[-1]                       # 记录下一个y坐标,y[i+1] = y[i]
            pos_new = (x_new, y_new)            # 对每一个坐标点进行储存
            F_new_str = str_Fi+" = "+str(F_new) #  新偏差公式


            x.append(x_new)                # 对每一个x坐标进行储存
            y.append(y_new)                # 对每一个y坐标进行储存
            str_Fi_list.append(str_Fi)      # 对F0,F1 等字符串进行储存
            div_cri.append(str_Fi+">0")        # 对偏差判别进行储存 Fi>0
            dir.append("-dx")               # 对进给方向进行储存 -dx
            Fi.append(F_new)                # 对新偏差进行存储 Fi
            new_dev_calc.append(F_new_str)   # 对新偏差计算结果公式进行储存  F0 = -5
            pos.append(pos_new)             # 对每一个心得坐标位置及逆行储存


        else:

            str_Fi = 'F' + str(i+1)                 # 生成Fi
            F_new = Fi[-1] + 2 * y[i] + 1          # 迭代发偏差运算
            x_new = x[-1]                          # 记录下一个x坐标,x[i+1] = x[i]
            y_new = y[-1] + 1                      # 记录下一个y坐标,y[i+1] = y[i]
            pos_temp = (x_new, y_new)              # 对每一个坐标点进行储存
            F_temp_str = str_Fi+" = "+str(F_new)    #  新偏差公式




            x.append(x_new)              # 对每一个x坐标进行储存
            y.append(y_new)             # 对每一个y坐标进行储存
            str_Fi_list.append(str_Fi)   # 对F0,F1 等字符串进行储存
            div_cri.append(str_Fi+"<0") # 对偏差判别进行储存 Fi>0
            dir.append("+dy")           # 对进给方向进行储存 -dx
            Fi.append(F_new)            # 对新偏差进行存储 Fi
            new_dev_calc.append(F_temp_str) # 对新偏差计算结果公式进行储存  F0 = -5
            pos.append(pos_temp)        # 对每一个心得坐标位置及逆行储存




    # 顺时针运算
    else:
        # 点位于圆外
        if Fi[i] >= 0:
            str_Fi = 'F' + str(i + 1)  # 生成Fi

            # 偏差计算、坐标计算
            F_new = Fi[-1] - 2 * y[i] + 1  # 迭代发偏差运算
            x_new = x[-1]   # 记录下一个x坐标,x[i+1] = x[i]
            y_new = y[-1] -1 # 记录下一个y坐标,y[i+1] = y[i]
            pos_new = (x_new, y_new)  # 对每一个坐标点进行储存
            F_new_str = str_Fi + " = " + str(F_new)  # 新偏差公式

            x.append(x_new)  # 对每一个x坐标进行储存
            y.append(y_new)  # 对每一个y坐标进行储存
            str_Fi_list.append(str_Fi)  # 对F0,F1 等字符串进行储存
            div_cri.append(str_Fi + ">0")  # 对偏差判别进行储存 Fi>0
            dir.append("-dx")  # 对进给方向进行储存 -dx
            Fi.append(F_new)  # 对新偏差进行存储 Fi
            new_dev_calc.append(F_new_str)  # 对新偏差计算结果公式进行储存  F0 = -5
            pos.append(pos_new)  # 对每一个心得坐标位置及逆行储存


        else:

            str_Fi = 'F' + str(i + 1)  # 生成Fi
            F_new = Fi[-1] + 2 * x[i] + 1  # 迭代发偏差运算
            x_new = x[-1] +1 # 记录下一个x坐标,x[i+1] = x[i]
            y_new = y[-1]   # 记录下一个y坐标,y[i+1] = y[i]
            pos_temp = (x_new, y_new)  # 对每一个坐标点进行储存
            F_temp_str = str_Fi + " = " + str(F_new)  # 新偏差公式

            x.append(x_new)  # 对每一个x坐标进行储存
            y.append(y_new)  # 对每一个y坐标进行储存
            str_Fi_list.append(str_Fi)  # 对F0,F1 等字符串进行储存
            div_cri.append(str_Fi + "<0")  # 对偏差判别进行储存 Fi>0
            dir.append("+dy")  # 对进给方向进行储存 -dx
            Fi.append(F_new)  # 对新偏差进行存储 Fi
            new_dev_calc.append(F_temp_str)  # 对新偏差计算结果公式进行储存  F0 = -5
            pos.append(pos_temp)  # 对每一个心得坐标位置及逆行储存

for i in range(n):

    print(i+1,"\t\t\t",div_cri[i],"\t\t",dir[i],"\t\t\t",new_dev_calc[i],"\t\t\t",pos[i+1],"\t\t",n-1-i)

figure, axes = plt.subplots()
r = math.sqrt(Xs**2+Ys**2)
axes.add_artist(plt.Circle((0, 0), max(Xs,Ys),fill=False))

# 画图
# plt.plot([0,Xe],[0,Ye],color = 'r')
# plt.plot(x,y,'o',color='b')
plt.plot(x,y)


plt.title('X',fontsize=24)
plt.tick_params(axis='both',which='major',labelsize=14)
plt.xlabel('X',fontsize=14)
plt.ylabel('Y',fontsize=14)
x_major_locator=MultipleLocator(1)
#把x轴的刻度间隔设置为1，并存在变量里
y_major_locator=MultipleLocator(1)
#把y轴的刻度间隔设置为10，并存在变量里
ax=plt.gca()
#ax为两条坐标轴的实例
ax.xaxis.set_major_locator(x_major_locator)
#把x轴的主刻度设置为1的倍数
ax.yaxis.set_major_locator(y_major_locator)
#把y轴的主刻度设置为10的倍数
plt.xlim(-0.5,max(Xs,Xe)+1)
#把x轴的刻度范围设置为-0.5到11，因为0.5不满一个刻度间隔，所以数字不会显示出来，但是能看到一点空白
plt.ylim(-0.5,max(Ys,Ye)+1)
#把y轴的刻度范围设置为-5到110，同理，-5不会标出来，但是能看到一点空

plt.savefig('.\\3.jpg')
plt.show()

3.2.2 第一象限圆弧插补程序效果

当前进入的是圆心位于原点，圆弧位于第一象限情况
请输入圆弧起点:	6,0
请输入圆弧的终点: 0,6
序号		偏差判别			进给		新偏差计算			坐标计算		终点判别			
										F0 = 0 				 (6, 0) 		
1 			 F1>0 		 -dx 			 F1 = -11 			 (5, 0) 		 11
2 			 F2<0 		 +dy 			 F2 = -10 			 (5, 1) 		 10
3 			 F3<0 		 +dy 			 F3 = -7 			 (5, 2) 		 9
4 			 F4<0 		 +dy 			 F4 = -2 			 (5, 3) 		 8
5 			 F5<0 		 +dy 			 F5 = 5 			 (5, 4) 		 7
6 			 F6>0 		 -dx 			 F6 = -4 			 (4, 4) 		 6
7 			 F7<0 		 +dy 			 F7 = 5 			 (4, 5) 		 5
8 			 F8>0 		 -dx 			 F8 = -2 			 (3, 5) 		 4
9 			 F9<0 		 +dy 			 F9 = 9 			 (3, 6) 		 3
10 			 F10>0 		 -dx 			 F10 = 4 			 (2, 6) 		 2
11 			 F11>0 		 -dx 			 F11 = 1 			 (1, 6) 		 1
12 			 F12>0 		 -dx 			 F12 = 0 			 (0, 6) 		 0

3.2.3 四象限圆弧插补程序（自己思路可能会有些繁琐建议参考就行）

# -*- coding: UTF-8 -*-
# [开头注释需要有此解码语句]
# funcion：四象限圆弧插补
# author：仲恺农业工程学院 机械设计制造及其自动化 冯耿鑫
# date:2021/10/15
# contact:194462052@qq.com
# 三角形顺逆时针旋转判断链接：https://blog.csdn.net/qq_30638831/article/details/49638393


"""
1.先观察四象限中顺逆旋转，Fi>0以及 Fi<0的情况。个人总结如下。建议配图一起理解

解题思路：
第一象限   x,y   逆： Fi>=0 -dx === Fi<0 +dy  顺 ：Fi>=0,-dy === Fi<0 +dx
第二象限   -x,y  逆： Fi>=0 -dy === Fi<0 -dx  顺：Fi>=0 ,+dx ===Fi<0  +dy
第三象限   -x,-y 逆： Fi>=0 +dx === Fi<0 -dy  顺：Fi>=0 ,+dy ===Fi<0 -dx
第四象限   x,-y  逆： Fi>=0 +dy === Fi<0 +dx  顺：Fi>=0, -dx ===Fi<0 -dy


进给方向总结需要结合图片理解：
逆： x>0 +dy    Fi+1=Fi+2y+1      顺： x>0 -dy    Fi+1=Fi-2y+1
    x<0 -dy    Fi+1=Fi-2y+1          x<0 +dy    Fi+1=Fi+2y+1
    y>0 -dx    Fi+1=Fi-2x+1          y>0 +dx    Fi+1=Fi+2x+1
    y<0 +dx    Fi+1=Fi+2x+1          y<0 -dx    Fi+1=Fi-2x+1
"""




import matplotlib.pyplot as plt
import math

def counterclockwise_dir(Xs, Ys,Xd,Yd,quad):
    """
    功能：对逆时针情况下，四象限下，运动的解算
    为了程序的简介性，本程序将多个语句表达式用;放在同一行上。
    """


    # 先对四个象限中Fi>=0 进行处理，
    if Fi[i]>=0:
        if Yd>0:
            if Xd>0:    # 第一象限
                x_new = x[i]-1; dir = "-dx"; y_new = y[i]
            else:       # 第二象限
                y_new = y[i]-1; dir = "-dy"; x_new = x[i]
        else:
            if Xd<0:    # 第三象限
                x_new = x[i]+1; dir = "+dx"; y_new = y[i]
            else:       # 第四象限
                y_new = y[i]+1; dir = "+dy"; x_new = x[i]


    # 先对四个象限中Fi<0 进行处理，
    else:
        if Yd>0:
            if Xd>0:    # 第一象限
                y_new = y[i]+1; dir = "+dy"; x_new = x[i]
            else:       # 第二象限
                x_new = x[i]-1; dir = "-dx"; y_new = y[i]
        else:
            if Xd<0:    # 第三象限
                y_new = y[i]-1; dir = "-dy"; x_new = x[i]
            else:       # 第四象限
                x_new = x[i]+1; dir = "+dx"; y_new = y[i]

    # 根据符号确定是正进给还是负进给
    if dir[0] == '-':
        mul = -2                # 新偏差计算  Fi+1 = Fi+2x+1        Fi+1 = Fi-2x+1   根据进给方决定次数为1部分
        add = -1                 # 进给       x_new = x[-1]+1      x_new = x[-1]-1
    else:
        mul = 2
        add = 1

    # 进行运算
    if dir[-1] == 'x':
        diff = x[i]             # 定义一个变量diff，根据是往x或者y进给的方向，如果是x，就是x[i]
        x_new = x[-1] + add     # 根据进给的方向是'-'或者'+'，对坐标进行运算
        y_new = y[-1]
    else:
        diff = y[i]; y_new = y[-1] + add;x_new = x[-1]

    F_new = Fi[-1] + mul * diff + 1     # 根据新偏差计算工时：Fi+1 = Fi-/+x/y+1
    return dir, F_new, x_new, y_new
def clockwise_dir(Xs, Ys,Xd,Yd,quad):
    """功能：对顺时针情况下，四象限下，运动的解算"""
    if Fi[i]>=0:
        if Yd>0:
            if Xd>0:
               y_new = y[i]-1; dir = "-dy"; x_new = x[i]
            else:
                x_new = x[i]+1; dir = "+dx"; y_new = y[i]
        else:
            if Xd<0:
                y_new = y[i]+1; dir = "+dy"; x_new = x[i]
            else:
                x_new = x[i]-1; dir = "-dx"; y_new = y[i]
    else:
        if Yd>0:
            if Xd>0:
                x_new = x[i]+1; dir = "+dx"; y_new = y[i]
            else:
                y_new = y[i]+1; dir = "+dy"; x_new = x[i]
        else:
            if Xd<0:
                x_new = x[i]-1; dir = "-dx"; y_new = y[i]
            else:
                y_new = y[i]-1; dir = "-dy"; x_new = x[i]


    # 符号判断
    if dir[0] == '-':
        mul = -2; add = -1
    else:
        mul = 2; add = 1

    # 进给方向判断
    if dir[-1] == 'x':
        diff = x[i]; x_new = x[-1] + add ;y_new = y[-1]
    else:
        diff = y[i]; y_new = y[-1] + add; x_new = x[-1]

    F_new = Fi[-1] + mul * diff + 1

    return dir, F_new, x_new, y_new
def dir_rot(Xs,Ys,Xe,Ye):
    a = Xs * Ye - Xe * Ys
    if a>0:
        rot = 'N'
    else:
        rot = 'S'
    return  rot





# 通过键盘输入获取圆弧信息
# pos_o = input("请输入圆弧的圆心:\t")
pos_s = input("请输入圆弧起点:\t")
pos_e = input("请输入圆弧的终点:\t")
# pos_o = pos_o.split(',')  # 对结束坐标进行获取
pos_s = pos_s.split(',')  # 对开始坐标进行获取，3，4拆分下来就是x=3,y=4
pos_e = pos_e.split(',')  # 对结束坐标进行获取

# 获取起点终点x,y值,以及总步数
# Xo = int(pos_o[0])
# Yo = int(pos_o[1])
Xs = int(pos_s[0])
Ys = int(pos_s[1])
Xe = int(pos_e[0])
Ye = int(pos_e[1])

Xd = (Xs+Xe)/3
Yd = (Ys+Ye)/3
quad = Xd*Yd
n = abs(Xs - Xe) + abs(Ys - Ye)

# 定义列表用于储存需要计算以及打印的数据
Fi = [0]  # 包括原点的偏差数据
x = [Xs]  # 每个x坐标
y = [Ys]  # 每个y坐标
pos = [(Xs, Ys)]  # 每个点的坐标
dir_list = []  # 储存运动方向
div_cri = ['F0=0']  # Deviation criterion 误差判别>0/<0
str_Fi_list = ["F0"]  # 将F+编号i生成一个字符串
new_dev_calc = []  # 用于储存新偏差数据eg：F1>0 New deviation calculation

# 对第一行信息进行打印
print("序号\t\t\t偏差判别\t\t\t进给\t\t\t新偏差计算\t\t\t坐标计算\t\t终点判别\t\t\t")
print("\t\t\t\t\t\t", end="\t\t\t\t")
print("F0 =", Fi[-1], "\t\t\t\t", pos[-1], "\t\t",n)


rot = dir_rot(Xs,Ys,Xe,Ye)



# 插补算法的计算
for i in range(n):
    # 逆时针加工
    if rot == 'N':
        # 传递起始坐标进去，就会自动判断进给方向
        dir, F_new, x_new, y_new = counterclockwise_dir(Xs, Ys,Xd,Yd,quad)
        str_Fi = 'F' + str(i + 1)  # 生成Fi
        pos_new = (x_new, y_new)  # 对每一个坐标点进行储存
        F_new_str = str_Fi + " = " + str(F_new)  # 新偏差公式

    # 顺时针运算
    else:
        # 传递起始坐标进去，就会自动判断进给方向
        dir, F_new, x_new, y_new = clockwise_dir(Xs,Ys,Xd,Yd,quad)
        str_Fi = 'F' + str(i + 1)  # 生成Fi
        pos_new = (x_new, y_new)  # 对每一个坐标点进行储存
        F_new_str = str_Fi + " = " + str(F_new)  # 新偏差公式


    x.append(x_new)  # 对每一个x坐标进行储存
    y.append(y_new)  # 对每一个y坐标进行储存
    str_Fi_list.append(str_Fi)  # 对F0,F1 等字符串进行储存
    # 偏差判别储存，对前一个偏差进行符号判断，为什么是-1，因为在没有储存新偏差的时候就，列表的最后一个偏差就是上一个偏差
    if Fi[i] == 0:
        div_cri.append(str_Fi_list[-1] + "=0")  # 偏差判别 F1>0 Fi=1
    elif Fi[i] > 0:
        div_cri.append(str_Fi_list[-1] + ">0")  # 偏差判别 F1>0 Fi=1
    else:
        div_cri.append(str_Fi_list[-1] + "<0")  # 偏差判别 F1>0 Fi=1
    dir_list.append(dir)  # 对进给方向进行储存 -dx
    Fi.append(F_new)  # 对新偏差进行存储 Fi
    new_dev_calc.append(F_new_str)  # 对新偏差计算结果公式进行储存  F0 = -5
    pos.append(pos_new)  # 对每一个心得坐标位置及逆行储存

#  结果打印
for i in range(n):
    print(i + 1, "\t\t\t", div_cri[i + 1], "\t\t", dir_list[i], "\t\t\t", new_dev_calc[i], "\t\t\t", pos[i + 1], "\t\t",
          n - 1 - i)



figure, axes = plt.subplots()
r = math.sqrt(Xs**2+Ys**2)
axes.add_artist(plt.Circle((0, 0), r,fill=False))
plt.plot(x,y,'o',color='r')
plt.plot(x, y,color = 'b')
plt.savefig('.\\4.jpg')

plt.show()

3.2.4 四象限圆弧插补程序效果（自己思路可能会有些繁琐建议参考就行）

请输入圆弧起点:	-6,0
请输入圆弧的终点:	0,-6
序号		偏差判别			进给		新偏差计算			坐标计算		终点判别			
										F0 = 0 				 (-6, 0) 		 12
1 			 F1=0 		 +dx 			 F1 = -11 			 (-5, 0) 		 11
2 			 F2<0 		 -dy 			 F2 = -10 			 (-5, -1) 		 10
3 			 F3<0 		 -dy 			 F3 = -7 			 (-5, -2) 		 9
4 			 F4<0 		 -dy 			 F4 = -2 			 (-5, -3) 		 8
5 			 F5<0 		 -dy 			 F5 = 5 			 (-5, -4) 		 7
6 			 F6>0 		 +dx 			 F6 = -4 			 (-4, -4) 		 6
7 			 F7<0 		 -dy 			 F7 = 5 			 (-4, -5) 		 5
8 			 F8>0 		 +dx 			 F8 = -2 			 (-3, -5) 		 4
9 			 F9<0 		 -dy 			 F9 = 9 			 (-3, -6) 		 3
10 			 F10>0 		 +dx 			 F10 = 4 			 (-2, -6) 		 2
11 			 F11>0 		 +dx 			 F11 = 1 			 (-1, -6) 		 1
12 			 F12>0 		 +dx 			 F12 = 0 			 (0, -6) 		 0

3.2.5四象限圆弧插补程序（根据课本思路进行优化）

# 作者：冯耿鑫
# 时间：2011/10/20
# 功能：通过判断象限以及旋转方向，得出进给方向求出四象限圆弧
# -*- coding: UTF-8 -*-

import matplotlib.pyplot as plt
import  math


class InterAlgorithm:

    def __init__(self,pos_s,pos_e):

        # 获取起点终点x,y值,以及总步数
        self.Xs = int(pos_s[0])     # 获取起点坐标x
        self.Ys = int(pos_s[1])     # 获取起点坐标y
        self.Xe = int(pos_e[0])     # 获取终点坐标x
        self.Ye = int(pos_e[1])     # 获取终点坐标y
        self.Xd = (self.Xs + self.Xe) / 3     # 用来判断象限
        self.Yd = (self.Ys + self.Ye) / 3

        # 定义列表用于储存需要计算以及打印的数据
        self.Fi = [0]  # 包括原点的偏差数据
        self.x = [self.Xs]  # 每个x坐标
        self.y = [self.Ys]  # 每个y坐标
        self.pos = [(self.Xs, self.Ys)]  # 每个点的坐标
        self.dir_list = []  # 储存运动方向
        self.div_cri = []  # Deviation criterion 误差判别>0/<0
        self.str_Fi_list = []  # 将F+编号i生成一个字符串
        self.new_dev_calc = []  # 用于储存新偏差数据eg：F1>0 New deviation calculation

        self.n = abs(self.Xs - self.Xe) + abs(self.Ys - self.Ye)  # 计算需要走的步长
        self.all_dir = ["+dx","+dy","-dx","-dy"]
        self.dir_x = self.all_dir[2]   # -dx
        self.dir_y = self.all_dir[1]   # +dy
        self.rot = 'N'
        self.quad = 1
        self.dir_temp = "+dx"
    def print_first_line(self):
        # 对第一行信息进行打印
        print("序号\t\t\t偏差判别\t\t\t进给\t\t\t新偏差计算\t\t\t坐标计算\t\t终点判别\t\t\t")
        print("\t\t\t\t\t\t", end="\t\t\t\t")
        print("F0 =", self.Fi[-1], "\t\t\t\t", self.pos[-1], "\t\t")
    # 判断旋转方向
    def dir_rot(self):
        a = self.Xs * self.Ye - self.Xe * self.Ys   # 利用法向量求旋转方向
        if a>0:
            self.rot = 'N'
        else:
            self.rot = 'S'
    # 判断第几象限
    def quad_judge(self):
        if self.Yd>=0:
            if self.Xd>0:        # 第一象限
                self.quad =1
            else:           # 第二象限
                self.quad = 2
        else:
            if self.Xd<0:        # 第三象限
                self.quad =3
            else:           # 第四象限
                self.quad =4
    # 判断进给方向
    def dir_juede(self):

    # ------------------进给方向的确定--------------------------
        # 逆时针
        if self.rot == 'N':
            if self.quad ==1:
                if self.Fi[-1] >= 0:
                    self.dir_temp = self.dir_x
                else:
                    self.dir_temp = self.dir_y
            elif self.quad == 2:
                self.dir_y = self.all_dir[3]    #-dy
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_y
                else:
                    self.dir_temp = self.dir_x

            elif self.quad == 3:
                self.dir_x = self.all_dir[0]    # +dx
                self.dir_y = self.all_dir[3]    # -dy
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_x
                else:
                    self.dir_temp = self.dir_y
            elif self.quad ==4:
                self.dir_x = self.all_dir[0]    # +dx
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_y
                else:
                    self.dir_temp = self.dir_x
        # 顺时针
        else:
            if self.quad == 1:
                self.dir_x = self.all_dir[0]   # +dx
                self.dir_y = self.all_dir[3]   # -dy
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_y
                else:
                    self.dir_temp = self.dir_x
            elif self.quad == 2:
                self.dir_x = self.all_dir[0]   # +dx
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_x
                else:
                    self.dir_temp = self.dir_y
            elif self.quad == 3:

                if self.Fi[-1] >= 0:
                    self.dir_temp = self.dir_y
                else:
                    self.dir_temp = self.dir_x
            else:
                self.dir_y = self.all_dir[3]   # -dy
                if self.Fi[-1]>=0:
                    self.dir_temp = self.dir_x
                else:
                    self.dir_temp = self.dir_y
    # 偏差计算
    def calc(self):
        """用来根据第几象限来变号计算"""
        # ================ 关键算法计算 ==================
        # 为了更好地理解变换，同时减少代码量，用一个列表，根据哪象限，顺逆时针旋转根据第一象限逆时针进行变换

    #-------------- 新偏差计算-----------------
        for i in range(self.n):
            self.dir_juede()
            if self.Fi[-1] >= 0:
                if self.dir_temp[0] == '-':
                    sum_sign = -1
                    add_sign = -1
                else:
                    sum_sign = 1
                    add_sign = 1

                if self.dir_temp[-1] == 'x':
                    F_new = self.Fi[-1] + sum_sign * 2 * self.x[i] + 1
                    x_new= self.x[-1]+add_sign                   # 记录下一个x坐标,x[i+1] = x[i]
                    y_new = self.y[-1]                       # 记录下一个y坐标,y[i+1] = y[i]
                    pos_new = (x_new, y_new)            # 对每一个坐标点进行储存
                    str_Fi = 'F' + str(i+1)                 # 生成Fi
                    self.div_cri.append(str_Fi+">0") # 对偏差判别进行储存 Fi>0
                    self.dir_list.append(self.dir_temp)
                else:
                    F_new = self.Fi[-1] + sum_sign * 2 * self.y[i] + 1
                    x_new = self.x[-1]
                    y_new = self.y[-1] + add_sign
                    pos_new = (x_new, y_new)
                    str_Fi = 'F' + str(i + 1)  # 生成Fi
                    self.div_cri.append(str_Fi + ">0")  # 对偏差判别进行储存 Fi<0
                    self.dir_list.append(self.dir_temp)



            else:
                if self.dir_y[0] == '-':
                    sum_sign = -1
                    add_sign = -1
                else:
                    sum_sign = 1
                    add_sign = 1

                if self.dir_temp[-1] == 'x':
                    F_new = self.Fi[-1] + sum_sign * 2 * self.x[i] + 1
                    x_new= self.x[-1]+add_sign                   # 记录下一个x坐标,x[i+1] = x[i]
                    y_new = self.y[-1]                       # 记录下一个y坐标,y[i+1] = y[i]
                    pos_new = (x_new, y_new)            # 对每一个坐标点进行储存
                    str_Fi = 'F' + str(i+1)                 # 生成Fi
                    self.div_cri.append(str_Fi+"<0") # 对偏差判别进行储存 Fi>0
                    self.dir_list.append(self.dir_temp)

                else:
                    F_new = self.Fi[-1] + sum_sign * 2 * self.y[i] + 1
                    x_new = self.x[-1]
                    y_new = self.y[-1] + add_sign
                    pos_new = (x_new, y_new)
                    str_Fi = 'F' + str(i + 1)  # 生成Fi
                    self.div_cri.append(str_Fi + "<0")  # 对偏差判别进行储存 Fi<0
                    self.dir_list.append(self.dir_temp)

            self.x.append(x_new)              # 对每一个x坐标进行储存
            self.y.append(y_new)             # 对每一个y坐标进行储存
            self.str_Fi_list.append(str_Fi)   # 对F0,F1 等字符串进行储存
            self.Fi.append(F_new)            # 对新偏差进行存储 Fi
            F_new_str = str_Fi + " = " + str(F_new)  # 新偏差公式
            self.new_dev_calc.append(F_new_str) # 对新偏差计算结果公式进行储存  F0 = -5
            self.pos.append(pos_new)        # 对每一个心得坐标位置及逆行储存
    # 显示图片
    def show(self):
        # 画出圆弧
        figure, axes = plt.subplots()
        r = math.sqrt(self.Xs ** 2 + self.Ys ** 2)  # 利用欧氏距离计算r
        axes.add_artist(plt.Circle((0, 0), r, fill=False))  # 画圆，true时，圆弧会填充
        plt.plot(self.x, self.y, 'o', color='b')  # 坐标点
        plt.plot(self.x, self.y)  # 将所有点连线
        plt.savefig('./img/2_圆弧.jpg')  # 保存图片
        plt.show()  # 显示图片
    # 程序结束自动打印结果
    def __del__(self):
        # 程序结束自动调用此函数
        for i in range(self.n):
            #         序号			偏差判别			        进给			        新偏差计算			    坐标计算		终点判别
            print(i + 1, "\t\t\t", self.div_cri[i], "\t\t", self.dir_list[i], "\t\t\t", self.new_dev_calc[i], "\t\t\t", self.pos[i + 1],"\t\t", self.n - i - 1)






if __name__ == '__main__':
    # 通过键盘输入获取圆弧信息
    pos_s = input("请输入圆弧起点:\t")
    pos_e = input("请输入圆弧的终点:\t")
    pos_s = pos_s.split(',')  # 对开始坐标进行获取，3，4拆分下来就是x=3,y=4
    pos_e = pos_e.split(',')
    # 定义arc对象
    arc = InterAlgorithm(pos_s, pos_e)
    # 调用打印标题方法
    arc.print_first_line()
    # 调用判断旋转方向方法
    arc.dir_rot()
    # 调用判断象限方法
    arc.quad_judge()
    # 调用进给方向判断方法
    arc.dir_juede()
    # 调用数据计算方法
    arc.calc()
    # 调用显示图像方法
    arc.show()

3.2.6 四象限圆弧插补程序效果（根据课本思路进行优化)

请输入圆弧起点:	6,0
请输入圆弧的终点:	0,6
序号		偏差判别		进给			新偏差计算			坐标计算		终点判别			
										F0 = 0 				 (6, 0) 		
1 			 F1>0 		 -dx 			 F1 = -11 			 (5, 0) 		 11
2 			 F2<0 		 +dy 			 F2 = -10 			 (5, 1) 		 10
3 			 F3<0 		 +dy 			 F3 = -7 			 (5, 2) 		 9
4 			 F4<0 		 +dy 			 F4 = -2 			 (5, 3) 		 8
5 			 F5<0 		 +dy 			 F5 = 5 			 (5, 4) 		 7
6 			 F6>0 		 -dx 			 F6 = -4 			 (4, 4) 		 6
7 			 F7<0 		 +dy 			 F7 = 5 			 (4, 5) 		 5
8 			 F8>0 		 -dx 			 F8 = -2 			 (3, 5) 		 4
9 			 F9<0 		 +dy 			 F9 = 9 			 (3, 6) 		 3
10 			 F10>0 		 -dx 			 F10 = 4 			 (2, 6) 		 2
11 			 F11>0 		 -dx 			 F11 = 1 			 (1, 6) 		 1
12 			 F12>0 		 -dx 			 F12 = 0 			 (0, 6) 		 0

3.3 其它数据截图

五、总结

1.很高兴又实现了自己的想法，在这个过程中遇到不少困难，不过通过查找资料以及思考，还是解决了。还记得大信老师说的一句话：“遇到困难，客服它，你就变得更加强大了！”
2.个人觉得思考、总结、记录时非常重要的，一个人的大脑装不下太多东西，通过记录就可以不用刻意去记住这些内容。

六 、期望

1.本次文章写篇幅较长，排版也不是很美观。接下来将通过个人的不断记录提升文章质量。
2.通过py进行算法验证后，有时间将利用C++再次复现，为大家提供更多的参考。
3.觉得文章写得不错的朋友可以点赞关注哦！你的支持是我前进的动力！