绘制两点之间的圆弧路径

Question

我正在尝试使用以下内容作为指导，为机器人绘制一条曲线路径：http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm

我的代码没有创建在目的地结束的路径。我期望根据目的地所在的象限向左或向右弯曲的路径 (+x+y,+x-y,-x+y,-x-y)

import math
start = [400,500]
dest = [200,300]
speed = 10
startangle = 0
rc =0
rotv =0
rads =0

def getPos(t):
    ang = (rotv*t)+rads
    x = start[0] - rc * math.sin(rads) + rc * math.sin(rotv*(t)+rads)
    y = start[1] + rc * math.cos(rads) - rc * math.cos(rotv*(t)+rads)
    return (int(x),int(y), ang)

dx = dest[0] - start[0]
dy = dest[1] - start[1]
rads = math.atan2(-dy,dx)
rads %= 2*math.pi
distance = (dx**2 + dy**2)**.5  #rg
bangle = 2*rads
rc = distance /(2 * math.sin(rads))
if rads > (math.pi/2):
    bangle = 2*(rads-math.pi)
    rc= -rc
if rads < -(math.pi/2):
    bangle = 2*(rads+math.pi)
    rc= -rc
pathlength = rc * bangle
xc = start[0] - rc * math.sin(rads)
yc = start[1] + rc * math.cos(rads)
rotcenter = [xc,yc]
traveltime = pathlength/speed
rotv = bangle/traveltime
for p in range(int(traveltime)):
    pos = getPos(p)

开始：蓝色，结束：红色，旋转点：紫色

更新： 我添加了代码以允许正负 x/y 值。我已经更新了图片。

Answer 1

为了回答您的问题，我首先阅读了您链接的文章。我认为这很有趣，并且很好地解释了公式背后的想法，尽管它缺少起始位置不在原点和起始角度不为 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)))

我不确定您的代码在哪一点失败，但我希望这对您有用。如果您打算在代码中多次使用此 sn-p，请考虑将其封装在一个函数中，该函数将给定值作为参数并返回 position 函数（如果您也喜欢 rotation 函数）。