自动泊车路径规划

Coordinate_transformation.py

'''以车位入位点作为原点，得到车身（后轮中心相对于车位点）的新坐标，以及车身角度（相对于车位线）'''

import math
def Coordinate_transformation(xy1,xy2):
    '''

    :param xy1: 入位点1的坐标
    :param xy2: 入位点2的坐标
    :return:    返回值1是车辆后轮轴的坐标，返回值2是车身相对于车位线的角度
    '''
    x1,y1=xy1[0],xy1[1]
    # print(x1,y1)
    x2,y2=xy2[0],xy2[1]
    #两个入位点的中心坐标
    x0=(xy1[0]+xy2[0])//2
    y0=(xy1[1]+xy2[1])//2

    #转换后的车身（后轮中心）坐标
    x_car=375-x0
    y_car=660-y0
    '''以750*1050的环视图为例，后轮轴中心的原始位置是（375,660）'''

    #车身角度（相对于车位线）
    if (x2-x1)==0:
        car_alp=3.14/2
    else:
        car_alp=math.atan((y2-y1)/(x2-x1))

    return (x_car,y_car),car_alp

# if __name__=="__main__":
    # xy1=(800,900)
    # xy2=(1050,900)
    # car_xy,car_alp=Coordinate_transformation(xy1,xy2)
    # print(car_xy)
    # print(car_alp)

path_planning.py

'''垂直车位和平行车位的路径规划，输入为车位的两个入位点像素坐标，输出为路径'''



import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
import time
from Coordinate_transformation import Coordinate_transformation

# img = np.ones((1500, 1500, 3), np.uint8) * 225
#
# #车位线
# #车位尺寸为250*600
# img=cv2.line(img,(0,900),(1500,900),(255,0,0),2)
# img=cv2.rectangle(img,(800,1500),(1050,900),(255,0,0),2)


#车身
car_w=170   #车宽
car_h=470    #车长
L=270        #轴距
# d0=150      #起点处，车身与车位线的平行距离
# R=600       #转弯半径（可由方向盘角度控制）
r=[500,600,700]
# R1=R2=R     #R1,R2分别为第一第二个圆弧的转弯半径
dis=100      #后轮轴与车尾的距离




def vertical_path(xy1,xy2):
    '''
    :param xy1: 入位点1的坐标(在环视图中的坐标)
    :param xy2: 入位点2的坐标
    :param d0:  后轮轴中心距离车位线的距离
    :return:    返回垂直车位的泊车路径
    '''
    d0=abs(xy1[0]-375)
    # img = np.ones((1500, 1500, 3), np.uint8) * 225

    xy0,alp0=Coordinate_transformation(xy1,xy2)
    # print(xy0,alp0)

    #后轮轴中心坐标（以入位点为原点）
    x0=int(xy0[0])
    y0=int(xy0[1])
    print(f'后轮轴初始坐标{
     (x0, y0)}')



    # img = cv2.circle(img, (x0+800, y0+800), 3, (0, 0, 255), 2)

    # 车位入口点坐标
    x_1 = 0
    y_1 = -125
    x_2 = 0
    y_2 = 125

    # 切换点c的坐标计算
    x_c = (x_1 + x_2) // 2 + L // 2
    y_c = (y_1 + y_2) // 2
    print(f"c点坐标{
     (x_c, y_c)}")
    D = L // 2

    # img = cv2.circle(img, (x_c+800, y_c+800), 3, (0, 0, 255), 2)

    list_all=[] #用于存放路径离散点
    for R in r:
        R1=R2=R
        # 圆心O2的坐标计算
        x_o2 = x_c
        y_o2 = y_c-R2

        # 切换点b的坐标计算
        alp = math.asin((d0 + R2 + D) / (R1+R2))      #alp为切换点c到切换点b的转动角度
        x_b = (x_o2 - R2 * (math.sin(alp))) // 1
        y_b = (y_o2 + R2 * (math.cos(alp))) // 1
        x_b = int(x_b)
        y_b = int(y_b)
        print(f'B点坐标{
     (x_b, y_b)}')
        # img = cv2.circle(img, (x_b+800, y_b+800), 3, (0, 0, 255), 2)

        # 圆心O1的坐标计算
        x_o1 = x_o2 - (R1+R2) * (math.sin(alp))
        y_o1 = y_o2 + (R1+R2) * (math.cos(alp))
        x_o1 = int(x_o1