绘制两点之间的圆弧路径

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)))

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

绘制两点之间的圆弧路径

Plot arc path between two points

python

math

trigonometry

robot