Python 绘制两点之间的圆弧路径

我正试图绘制一条弯曲的路径，以便机器人遵循，并以以下内容为指导：

我的代码没有创建以目标为终点的路径。我希望路径向左或向右弯曲，这取决于目标所在的象限（+x+y，+x-y，-x+y，-x-y）

导入数学
开始=[400500]
目的地=[200300]
速度=10
startangle=0
rc=0
rotv=0
拉德=0
def getPos（t）：
ang=（rotv*t）+rads
x=开始[0]-rc*数学sin（rads）+rc*数学sin（rotv*（t）+rads）
y=开始[1]+rc*数学cos（rads）-rc*数学cos（rotv*（t）+rads）
返回（整数（x）、整数（y）、ang）
dx=dest[0]-开始[0]
dy=dest[1]-开始[1]
rads=数学常数2（-dy，dx）
rads%=2*math.pi
距离=（dx**2+dy**2）**.5#rg
手镯=2*rads
rc=距离/（2*数学sin（rads））
如果rads>（数学pi/2）：
手镯=2*（rads math.pi）
rc=-rc
如果rads<-（数学pi/2）：
手镯=2*（rads+math.pi）
rc=-rc
路径长度=rc*手镯
xc=开始[0]-rc*数学sin（rads）
yc=开始[1]+rc*数学cos（rads）
旋转中心=[xc，yc]
旅行时间=路径长度/速度
rotv=手镯/旅行时间
对于范围内的p（int（旅行时间））：
pos=getPos（p）

起点：蓝色，终点：红色，旋转点：紫色

更新：

---

我添加了代码来允许正x/y值和负x/y值。我已经更新了图像。

要回答您的问题，我首先阅读了您链接的文章。我认为它非常有趣，并且很好地解释了公式背后的思想，尽管它缺少起始位置不在原点和起始角度不为0的公式

我花了一点时间才想出这些公式，但现在它适用于我能想到的每一种情况。为了能够使用链接文章中给出的公式，我使用了这里给出的变量名称。注意，我还使用了带有

t_0

的符号作为开始时间，您刚才忽略了它。您可以轻松删除

t_0

的任何实例或设置

t_0=0

下面代码的最后一部分用于测试并创建一个小红龟，它沿着指定方向跟踪计算出的圆弧的路径。黑海龟表示目标位置。在动画结束时，两个海龟彼此靠近，但它们并不直接在彼此上方，因为我只对整数进行迭代，并且t_1可能不是整数

from math import pi, hypot, sin, cos, atan2, degrees

def norm_angle(a):
    # Normalize the angle to be between -pi and pi
    return (a+pi)%(2*pi) - pi

# Given values
# named just like in http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
x_0, y_0 = [400,500] # initial position of robot
theta_0 = -pi/2      # initial orientation of robot
s = 10               # speed of robot
x_1, y_1 = [200,300] # goal position of robot
t_0 = 0              # starting time

# To be computed:
r_G = hypot(x_1 - x_0, y_1 - y_0)        # relative polar coordinates of the goal
phi_G = atan2(y_1 - y_0, x_1 - x_0)
phi = 2*norm_angle(phi_G - theta_0)      # angle and 
r_C = r_G/(2*sin(phi_G - theta_0))       # radius (sometimes negative) of the arc
L = r_C*phi                              # length of the arc
if phi > pi:
    phi -= 2*pi
    L = -r_C*phi
elif phi < -pi:
    phi += 2*pi
    L = -r_C*phi
t_1 = L/s + t_0                        # time at which the robot finishes the arc
omega = phi/(t_1 - t_0)                # angular velocity           
x_C = x_0 - r_C*sin(theta_0)           # center of rotation
y_C = y_0 + r_C*cos(theta_0)

def position(t):
    x = x_C + r_C*sin(omega*(t - t_0) + theta_0)
    y = y_C - r_C*cos(omega*(t - t_0) + theta_0)
    return x, y

def orientation(t):
    return omega*(t - t_0) + theta_0

#--------------------------------------------
# Just used for debugging
#--------------------------------------------
import turtle

screen = turtle.Screen()
screen.setup(600, 600)
screen.setworldcoordinates(0, 0, 600, 600)

turtle.hideturtle()
turtle.shape("turtle")
turtle.penup()
turtle.goto(x_1, y_1)
turtle.setheading(degrees(orientation(t_1)))
turtle.stamp()
turtle.goto(x_0, y_0)
turtle.color("red")
turtle.showturtle()
turtle.pendown()
for t in range(t_0, int(t_1)+1):
    turtle.goto(*position(t))
    turtle.setheading(degrees(orientation(t)))

从数学导入pi、hypot、sin、cos、atan2、degrees
def标准_角（a）：
#将角度规格化为-pi和pi之间
收益率（a+pi）%（2*pi）-pi
#给定值
#名字和我的名字一模一样http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
x_0，y_0=[400500]#机器人的初始位置
θu 0=-pi/2#机器人的初始方向
s=10#机器人速度
x_1，y_1=[200300]#机器人的目标位置
t_0=0#开始时间
#待计算：
r_G=hypop（x_1-x_0，y_1-y_0）#目标的相对极坐标
phi_G=atan2（y_1-y_0，x_1-x_0）
φ=2*标准角（φG-θ0）#角和
r_C=r_G/（2*sin（φG-θ0））#弧的半径（有时为负）
L=r_C*phi#弧长
如果φ>π：
φ-=2*pi
L=-r_C*phi
elif phi<-pi：
φ+=2*pi
L=-r_C*phi
t_1=L/s+t_0#机器人完成电弧的时间
ω=φ/（t_1-t_0）#角速度
x_C=x_0-r_C*sin（θ_0）#旋转中心
y_C=y_0+r_C*cos（θ_0）
def位置（t）：
x=x_C+r_C*sin（ω*（t-t_0）+θ_0）
y=y_C-r_C*cos（ω*（t-t_0）+θ_0）
返回x，y
def方向（t）：
返回ω*（t-t_0）+θ0
#--------------------------------------------
#只是用来调试的
#--------------------------------------------
进口海龟
screen=turtle.screen（）
屏幕设置（600600）
屏幕设置世界坐标（0,060600）
乌龟
海龟形状（“海龟”）
乌龟
乌龟。后藤（x_1，y_1）
海龟.设置航向（度（方位（t_1）））
乌龟邮票（）
乌龟。后藤（x_0，y_0）
海龟。颜色（“红色”）
海龟
乌龟
对于范围内的t（t_0，int（t_1）+1）：
乌龟。转到（*位置（t））
海龟.设置航向（度（方向（t）））

我不确定您的代码在哪一点失败，但我希望这对您有用。如果您想在代码中多次使用此代码段，请考虑将其封装在以给定值为参数的函数中，并返回<代码>位置< /C>函数（如果您喜欢<代码>旋转< /代码>函数），

from math import pi, hypot, sin, cos, atan2, degrees

def norm_angle(a):
    # Normalize the angle to be between -pi and pi
    return (a+pi)%(2*pi) - pi

# Given values
# named just like in http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
x_0, y_0 = [400,500] # initial position of robot
theta_0 = -pi/2      # initial orientation of robot
s = 10               # speed of robot
x_1, y_1 = [200,300] # goal position of robot
t_0 = 0              # starting time

# To be computed:
r_G = hypot(x_1 - x_0, y_1 - y_0)        # relative polar coordinates of the goal
phi_G = atan2(y_1 - y_0, x_1 - x_0)
phi = 2*norm_angle(phi_G - theta_0)      # angle and 
r_C = r_G/(2*sin(phi_G - theta_0))       # radius (sometimes negative) of the arc
L = r_C*phi                              # length of the arc
if phi > pi:
    phi -= 2*pi
    L = -r_C*phi
elif phi < -pi:
    phi += 2*pi
    L = -r_C*phi
t_1 = L/s + t_0                        # time at which the robot finishes the arc
omega = phi/(t_1 - t_0)                # angular velocity           
x_C = x_0 - r_C*sin(theta_0)           # center of rotation
y_C = y_0 + r_C*cos(theta_0)

def position(t):
    x = x_C + r_C*sin(omega*(t - t_0) + theta_0)
    y = y_C - r_C*cos(omega*(t - t_0) + theta_0)
    return x, y

def orientation(t):
    return omega*(t - t_0) + theta_0

#--------------------------------------------
# Just used for debugging
#--------------------------------------------
import turtle

screen = turtle.Screen()
screen.setup(600, 600)
screen.setworldcoordinates(0, 0, 600, 600)

turtle.hideturtle()
turtle.shape("turtle")
turtle.penup()
turtle.goto(x_1, y_1)
turtle.setheading(degrees(orientation(t_1)))
turtle.stamp()
turtle.goto(x_0, y_0)
turtle.color("red")
turtle.showturtle()
turtle.pendown()
for t in range(t_0, int(t_1)+1):
    turtle.goto(*position(t))
    turtle.setheading(degrees(orientation(t)))