一个简单的路径规划

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

最近在做差速小车pathplanning相关工作，记录一下相关的知识点

---

一、问题描述

任意两条直线的道路交汇于B点，假定小车的转弯半径为R，为小车规划一条路径，从A行驶到C

二、求解步骤

1.求半径为R的圆与两条直线相切时的圆心和切点

代码如下（示例）：

A = [1,1]
B = [10,10]
C = [19,1]
radius = 1.5 #m

def line_2D(pos_start,pos_end):
    k = (pos_end[1]-pos_start[1])/(pos_end[0]-pos_start[0])
    b = pos_end[1] - k*pos_end[0]
    return k,b
    
def find_circle(A,B,C,radius):
    k1,b1 = line_2D(A,B)
    k2,b2 = line_2D(B,C)
    if (A[1]-k2*A[0]-b2)<0:
        bb2 = b2 - radius/math.cos(math.atan(k2))
    else:
        bb2 = b2 + radius/math.cos(math.atan(k2))
    if (C[1]-k1*C[0]-b1)<0:
        bb1 = b1 - radius/math.cos(math.atan(k1))
    else:
        bb1 = b1 + radius/math.cos(math.atan(k1))
    x0 = (bb2 - bb1)/(k1 - k2)
    y0 = k1*x0+bb1
    circle_center = (x0,y0)
    x1 = (x0+k1*(y0-b1))/(1+k1**2)
    y1 = k1*x1+b1
    tangency1 =(x1,y1)
    x2 = (x0+k2*(y0-b2))/(1+k2**2)
    y2 = k2*x2+b2
    tangency2 =(x2,y2)
    return tangency1,tangency2,circle_center

2.判断C点在AB直线的左边还是右边，如果在左边就在到达切点1时选择逆时针方向旋转

def isleft(a,b,c):
    ux,uy = c[0] - a[0],c[1] - a[1]
    vx,vy = b[0] - a[0],b[1] - a[1]
    return ux*vy-uy*vx>0

3.路径path由x，y，yaw组成

class PrePath:
    def __init__(self):
        self.x = []
        self.y = []
        self.yaw = []

def getLinePath(pos_1,pos_2,v,dt):
    path = PrePath()
    k,b = line_2D(pos_1,pos_2)
    x_ = pos_1[0]
    y_ = pos_1[1]
    if pos_2[0] - pos_1[0]>0:
        while(x_ + v*dt*math.cos(math.atan(k)) <= pos_2[0]):
            path.x.append(x_)
            path.y.append(k*x_+b)
            x_ += v*dt*math.cos(math.atan(k))
            path.yaw.append(math.atan(k))
    else:
        while(x_ - v*dt*math.cos(math.atan(k)) >= pos_2[0]):
            path.x.append(x_)
            y_ = k*x_+b
            path.y.append(y_)
            x_ += v*dt*math.cos(math.atan(k))
            path.yaw.append(math.atan(k)+math.pi)
    return path

def zero_to_2pi(theta):
    while(theta<0):
        theta+=2*math.pi
    while(theta>2*math.pi):
        theta-=2*math.pi
    return theta


def getCirclePath(tangcy1,tangcy2,circle_center,radius,trun_dir,V,dt):
    path = PrePath()
    theta1 = math.atan((tangcy1[1]-circle_center[1])/(tangcy1[0]-circle_center[0]))
    if (tangcy1[0]-circle_center[0])<0:
        theta1+=math.pi
    theta1 = zero_to_2pi(theta1)
    theta2 = math.atan((tangcy2[1]-circle_center[1])/(tangcy2[0]-circle_center[0]))
    if (tangcy2[0]-circle_center[0])<0:
        theta2+=math.pi
    theta2 = zero_to_2pi(theta2)
    dtheta = V*dt/radius 
    if trun_dir:
        opera = -1 #顺时针减
        omega = zero_to_2pi(theta1-theta2)
    else:
        opera = 1 #逆时针加
        omega = zero_to_2pi(theta2-theta1)
    num = math.ceil(omega*radius/(V*dt))
    for i in range(num):
        x,y = cacu_circle(circle_center,radius,theta1+i*opera*dtheta)
        path.x.append(x)
        path.y.append(y)
        path.yaw.append(theta1 + opera*(i*dtheta + math.pi/2))
    return path

4.组成完整的path

V = 1.0  #m/s
dt = 0.1 #s

qiedian1,qiedian2,circle_center = find_circle(A, B, C, R)
trun_dir = isleft(A,B,C)
path1 = getLinePath(A,qiedian1,V,dt)
path2 = getCirclePath(qiedian1,qiedian2,circle_center,R,trun_dir,V,dt)
path3 = getLinePath(qiedian2,C,V,dt)
path = PrePath()
path.x = path1.x+path2.x+path3.x
path.y = path1.y+path2.y+path3.y
path.yaw = path1.yaw+path2.yaw+path3.yaw

plt.plot(path.x, path.y, "-r")

---

总结

先求出圆与两条直线的两个切点将路径分为3段，选择外侧圆弧与两条直线拼接得到最终路径