本文章灵感来源于机床数控技术及应用课程，上课老师提过，机床在加工一条直线或者圆弧的时候其实并不是按照理想的轨迹去加工的，其运动轨迹放大之后是锯齿状的（如下图所示），是通过逼近理想轨迹来加工的。那如何通过给定直线两个点，或者圆弧的起点终点，就能找到其运动轨迹呢?因为前阵子刚接触一个四轴电装机器臂，对其运动轨迹也比较感兴趣，可能跟课本的数控加工算法不一样，所以就打算练练手，将算法实现一下。
如何去评价在直线的上方？本文以斜率绝对值来进行比较。
大家可以参考下图第一象限的逆时针以及顺时针讲解。
1.相对于直线插补算法，四象限的圆弧插补确实不好理解，不过也不难，只要大家根据下面的规律总结以及课本，根据规律多尝试就能编出程序。
2.四象限的圆弧插补主要难在顺逆选方向的判断，以及如何确定进给方向？如何确定第几象限？其实只要这三个问题解决了，问题就迎刃而解。
3.顺逆时针的判断：利用法向量进行判断，这个是属于高数的内容，大家可以回去翻一下书，这里我是直接CSDN。旋转判断参考链接
4.象限判断:如何进行象限判断，考虑到圆弧起点以及终点都在坐标轴上，那么如何去判断呢？因此我采用了，((Xs+Xe)/3,(Ys+Ye)/3) 作为判断点。可以判断圆弧起点所在象限。因为无论中终点是否跨象限，都是进行运行计算。
备注：(Xs,Ys)：起点坐标,(Xe,Ye)：终点坐标。
1.先对起点在原点，第一象限直线进行插补
2.起点不在原点的直线，通过求终点相对于起点的相对坐标，这样起点就是原点。
3.根据 (Xi,Yi)与原点（起点）所车成直线斜率跟终点与原点（起点）斜率之间的关系，判断进给是x或者y方向。再根据起点坐标所在象限，得出x,y的正负，再根据正负求其进给方向。
1.同时是先简单后再到难，第一步我们先根据第一象限圆弧，顺逆旋转的特点（看纸质版图解+课本+网上资料会更好理解）。
2.第二步就有些绕，需要根据四象限，顺逆时针旋转，进给方向是往正负x或者正负y。是不是有点乱。不着急，其实大家只要将全部进给方向都以第一象限的逆时针作为参考，通过上图变换规律作为理解。先以第一象限设定好进给x,y，再根据象限判断、旋转方向判断，确定实际进给方向，然后根据进给方向符号，直接求偏差。问题就迎刃而解。（建议大家看代码理解会更好）
# 作者：仲恺农业工程学院 冯耿鑫
# 日期：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()
请输入直线终点坐标: 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
# 日期：2021/10/14
# 功能：逐点比较法实现,能实现四个象限，起点不在原点的直线插补计算
# 参考链接：https://blog.csdn.net/joogle/article/details/7962320
"""
四象限的思路需要解决的是，坐标点不在原点，以及观察四个象限的在直线上方有什么规律，起始只要进行绝对值转换，
都看成第一象限的问题，根据斜率来判断，要是斜率大于0，就是在直线上，要是<0就是在直线下。
关于进给方向：（发现x,y是什么符号，对应就给方向就是什么符号）
在第一象限， Fi >= 0, dx , Fi<0 dy
在第二象限，x取绝对值到第一象限, Fi >= 0, -dx , Fi<0 dy
在第三象限，xy取绝对值到第一象限, Fi >= 0, -dx , Fi<0 -dy
在第一象限，x取绝对值到第四象限, Fi >= 0, -dx , Fi<0 -dy
"""
# 有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）
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
Fi= [0] # 开始的判别偏差
# 为了程序简洁，利用三目运算符，用于判断进给方向，例如第二象限，Fi>0，是-dx,如果在第一象限是+dx
dir_x = "-" if Xe<0 else "+"
dir_y = "-" if Ye<0 else "+"
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):
# 在第一象限，正偏差，位于直线上方，也就是过点（0，0），(xi,yi)的直线斜率大于过原点与终点的斜率
if Fi[i] >=0 :
# 计算
dir = dir_x +"dx" # 都是只要是>=0都是往dx变化，但是到底往什么方向呢？就得看前面三目运算符运算，得出进给方向了
str_Fi = 'F' + str(i ) # 生成Fi，主要是为了打印的整洁性
F_new = Fi[-1] - abs(Ye) # 迭代偏差运算 Fi+1j= F-Ye
x_new = x[i] - 1 if Xe<0 else x[i]+1 # 记录下一个x坐标,x[i+1] = x[i]+/-1，其中+1还是-1得看进给的方向，要是Xe>0,就是+1，<0,就是-1
y_new = y[i] # 记录下一个y坐标,y[i+1] = y[i]
pos_new = (x_new,y_new) # 组成心得坐标
F_new_str = 'F' + str(i + 1) + " = "+str(F_new) # 新偏差计算，主要为的是打印出模拟人工答题的格式而定义新偏差计算的过程
else:
dir = dir_y+ "dy" # 都是只要是<0都是往dy变化，但是到底往什么方向呢？就得看前面三目运算符运算，得出进给方向了
str_Fi = 'F' + str(i) # 生成Fi，主要是为了打印的整洁性
F_new =Fi[-1] + abs(Xe) # 迭代偏差运算 Fi+1j= Fi+Xe
y_new = y[i]-1 if Ye<0 else y[i] +1 # 记录下一个y坐标,y[i+1] = y[i]+/-1，其中+1还是-1得看进给的方向，要是Ye>0,就是+1，<0,就是-1
x_new = x[i] # 记录下一个x坐标，其中x不变x[i+1] = x[i]
pos_new = (x_new,y_new) # 组成心得坐标
F_new_str = 'F' + str(i + 1) + " = "+str(F_new) # 新偏差计算，主要为的是打印出模拟人工答题的格式而定义新偏差计算的过程
# 储存数据进行打印
x.append(x_new) # 对每一个x坐标进行储存
y.append(y_new) # 对每一个y坐标进行储存
str_Fi_list.append(str_Fi) # F1,F2
# 偏差判别储存，对前一个偏差进行符号判断，为什么是-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,-dx
Fi.append(F_new) # 新偏差的结果
new_dev_calc.append(F_new_str) # F1 = -4 ,F4 = 0
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)
# 画图
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()
请输入直线起点坐标: -4,-5
请输入直线终点坐标: -8,-11
序号 偏差判别 进给 新偏差计算 坐标计算 终点判别
F0 = 0 (0, 0)
1 F0=0 -dx F1 = -6 (-1, 0) 9
2 F1<0 -dy F2 = -2 (-1, -1) 8
3 F2<0 -dy F3 = 2 (-1, -2) 7
4 F3>0 -dx F4 = -4 (-2, -2) 6
5 F4<0 -dy F5 = 0 (-2, -3) 5
6 F5=0 -dx F6 = -6 (-3, -3) 4
7 F6<0 -dy F7 = -2 (-3, -4) 3
8 F7<0 -dy F8 = 2 (-3, -5) 2
9 F8>0 -dx F9 = -4 (-4, -5) 1
10 F9<0 -dy F10 = 0 (-4, -6) 0
# -*- 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()
当前进入的是圆心位于原点，圆弧位于第一象限情况
请输入圆弧起点: 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
# -*- 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()
请输入圆弧起点: -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
# 作者：冯耿鑫
# 时间：2021/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()
请输入圆弧起点: 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.觉得文章写得不错的朋友可以点赞关注哦！你的支持是我前进的动力！