Python--基于离散点的等距线（equidistant line）计算

 

此为毕业设计需要而写

本人毕业设计需要利用差分原理，控制两个电机，达到完成上位机给定的轨迹。

 

基本思路：

一.通过离散点分别计算出中垂线上与远线等距的两个点，计算可得两个值，分别是（+-d）值

二.选取两个值中的一个

准则1：

（1）计算出第一个双值时，随便选取一个值A。

（2）计算出第二个双值时，分别用A与两值做连线，即计算两个直线方程。

（3）两个直线方程将与原直线分别有一个交点，计算出交点。

（4）计算该两个交点与原直线上的recent_dot的距离。

（5）选择距离大的点的值。

准则2：

（1）计算A点与双值之间的距离

（2）选取距离小的

准则1，准则2相与，可以完成筛选。

Python代码如下：

'''
    拟合出第二条线
'''
import math
import sys
import sympy
i = 0
j = 0
cc=[0,0]
output_1=0.0
output_2=0.0
line1_x = 0
line1_y = 0
line2_x = 0
line2_y = 0
x=0
b10 = 0
b11 = 0
b20 = 0
b21 = 0
d1_x = 0
d1_y = 0
d2_x = 0
d2