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)
        y_o1 = int(y_o1)

        # 切换点A的坐标计算
        x_a = int(x_o1+R1)
        y_a = int(y_o1)
        print(f'A点坐标{(x_a, y_a)}')
        # img = cv2.circle(img, (x_a+800, y_a+800), 3, (0, 0, 255), 2)

        # 结束点后轮轴中心坐标
        x_end = int((x_1 + x_2) // 2 + car_h)
        y_end = int((y_1 + y_2) // 2)
        print(f'后轮轴结束坐标{(x_end, y_end)}')
        # img = cv2.circle(img, (x_end+800, y_end+800), 3, (0, 0, 255), 2)

        #从初始点到A点
        list0=[]
        for y in range(y_a,y0):
            list0.append((x0,y))
        list0.reverse()
        # img = cv2.line(img, (x0+800, y0+800), (x_a+800, y_a+800), (0, 0, 0), 2)



        list1 = []

        #从a点到b点
        for x in range( x_b,x_a):
            y = int(-math.sqrt(R ** 2 - (x - x_o1) ** 2) + y_o1)
            list1.append((x,y))
            # img = cv2.circle(img, (x+800, y+800), 2, (0, 0, 255), 2)
        list1.reverse()
        print(list1[1])

        list2 = []
        #从b点到c点
        for x in range(x_b, x_c):
            y = int(math.sqrt(R ** 2 - (x - x_o2) ** 2) + y_o2)
            # img = cv2.circle(img, (x+800, y+800), 2, (0, 0, 255), 2)

            list2.append((x,y))
            # list2.append(x)
            # list2.append(y)
        # arr2 = np.array(list2, np.int).reshape(1, x_b - x_c, 2)
        # img = cv2.polylines(img, arr2, False, (255, 0, 0))

        #从c点到结束点
        list3=[]
        for x in range(x_c,x_end):
            list3.append((x,y_end))
        # img = cv2.line(img, (x_c+800, y_c+800), (x_end+800, y_end+800), (0, 0, 0), 2)

        list_all.append(list0+list1+list2+list3)    #不同半径下产生的路径
    # print(type(list_all[1][1][0]))


    return list_all



def parallel_path(xy1,xy2):
    '''

    :param xy1: 入位点1的坐标
    :param xy2: 入位点2的坐标
    :return:
    '''
    d0 = abs(xy1[0] - 375)   #初始位置后轮轴到车位线的距离

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

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


    #入位点坐标
    x_1 = 0
    y_1 = -300
    x_2 = 0
    y_2 = 300

    #点c的坐标计算
    x_c=int(x_1+250/2)
    y_c=int((y_1+y_2)/2+2*L/3)

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

        #第二段圆弧的角度计算
        alp=math.acos(1-(d0+250/2)/(R1+R2))

        #切换点b的坐标计算
        x_b=int(x_o2+R2*math.cos(alp))
        y_b=int(y_o2-R2*math.sin(alp))

        #圆心o1的坐标计算
        x_o1=int(x_b+R1*math.cos(alp))
        y_o1=int(y_b-R1*math.sin(alp))

        #切换点A的计算
        x_a=int(x_o1-R1)
        y_a=int(y_o1)

        # 从初始点到A点的路径
        list0 = []
        for y in range(y_a, y0):
            list0.append((x0, y))
        list0.reverse()
        # img = cv2.line(img, (x0+800, y0+800), (x_a+800, y_a+800), (0, 0, 0), 2)


        # 从a点到b点
        list1 = []
        for x in range(x_a, x_b):
            y = int(math.sqrt(R1 ** 2 - (x - x_o1) ** 2) + y_o1)
            list1.append((x, y))
            # img = cv2.circle(img, (x+800, y+800), 2, (0, 0, 255), 2)
        # print(list1[1])

        list2 = []
        # 从b点到c点
        for x in range(x_b, x_c):
            y = int(-math.sqrt(R2 ** 2 - (x - x_o2) ** 2) + y_o2)
            # img = cv2.circle(img, (x+800, y+800), 2, (0, 0, 255), 2)

            list2.append((x, y))
            # list2.append(x)
            # list2.append(y)
        # arr2 = np.array(list2, np.int).reshape(1, x_b - x_c, 2)
        # img = cv2.polylines(img, arr2, False, (255, 0, 0))



        list_all.append(list0+list1+list2)    #不同半径下产生的路径以列表的形式存放在list_all中

    return list_all



# 画图函数
def show(xy1,xy2):
    img = np.ones((1500, 1500, 3), np.uint8) * 225
    img01 = np.ones((1500, 1500, 3), np.uint8) * 225


    if 150<abs(xy1[1]-xy2[1])<350:   #垂直车位
        if xy1[1]> xy2[1]:
            xy1,xy2=xy2,xy1
        cv2.rectangle(img,(0+xy1[0],-125+xy1[1]),(0+xy1[0]+600,-125+xy1[1]+250),(255,0,0),2)
        cv2.rectangle(img01,(0+xy1[0],-125+xy1[1]),(0+xy1[0]+600,-125+xy1[1]+250),(255,0,0),2)

        path=vertical_path(xy1,xy2)

        #画出所有情况下的路径
        for i in range(len(path)):
            for j in path[i]:
                # print(i)
                img = cv2.circle(img, (j[0]+xy1[0], j[1]+xy1[1]), 2, (0, 0, 255), 2)
        cv2.imwrite('vertical_demo.png',img)
        plt.imshow(img[:,:,::-1])
        plt.title("frame")
        plt.show()

        # 画出最优路径（依据就是路径在入位时距离中心点最近）
        y_list = []
        for i in range(len(path)):
            for j in range(len(path[i])):
                if path[i][j][0] == 0:
                    y = path[i][j][1]
                    y_list.append(y)
        y_center_list = []
        for i in range(len(y_list)):
            y_center = abs(y_list[i])
            y_center_list.append(y_center)
        y_center = min(y_center_list)
        i = y_center_list.index(y_center)
        final_path=path[i]
        for i in final_path:
            # print(i[0])
            img = cv2.circle(img01, (i[0] + xy1[0], i[1] + xy1[1]), 2, (0, 0, 255), 2)
        cv2.imwrite('vertical_demo01.png',img01)
        plt.imshow(img01[:, :, ::-1])
        plt.title("frame01")
        plt.show()




    if 500<abs(xy1[1]-xy2[1])<700:  #平行车位
        if xy1[1]> xy2[1]:
            xy1,xy2=xy2,xy1
        cv2.rectangle(img, (0 + xy1[0], -300 + xy1[1]), (0 + xy1[0] + 250, -125 + xy1[1] + 600), (255, 0, 0), 2)
        cv2.rectangle(img01, (0 + xy1[0], -300 + xy1[1]), (0 + xy1[0] + 250, -125 + xy1[1] + 600), (255, 0, 0), 2)

        path = parallel_path(xy1, xy2)

        #画出所有情况下的路径
        for i in range(len(path)):
            for j in path[i]:
                # print(i)
                img = cv2.circle(img, (j[0]+xy1[0], j[1]+xy1[1]), 2, (0, 0, 255), 2)
        cv2.imwrite('parallel_demo.png',img)
        plt.imshow(img[:,:,::-1])
        plt.title("frame")
        plt.show()

        # 画出最优路径（依据就是路径在入位时距离中心点最近）
        y_list = []
        for i in range(len(path)):
            for j in range(len(path[i])):
                if path[i][j][0] == 0:
                    y = path[i][j][1]
                    y_list.append(y)
        y_center_list = []
        for i in range(len(y_list)):
            y_center = abs(y_list[i])
            y_center_list.append(y_center)
        y_center = min(y_center_list)
        i = y_center_list.index(y_center)
        final_path=path[i]
        for i in final_path:
            # print(i[0])
            img = cv2.circle(img01, (i[0] + xy1[0], i[1] + xy1[1]), 2, (0, 0, 255), 2)
        cv2.imwrite('parallel_demo01.png',img01)
        plt.imshow(img01[:, :, ::-1])
        plt.title("frame01")
        plt.show()
        # for i in path:
        #     img = cv2.circle(img, (i[0] + 800, i[1] + 800), 2, (0, 0, 255), 2)
        # cv2.imwrite('parallel_demo.png',img)
        # plt.imshow(img[:, :, ::-1])
        # plt.title("frame")
        # plt.show()


#将两种情况的路径规划函数综合到一个主函数
def path(xy1,xy2):
    if 150 < abs(xy1[1] - xy2[1]) < 350:  # 垂直车位
        path=vertical_path(xy1,xy2)
        y_list = []
        for i in range(len(path)):
            for j in range(len(path[i])):
                if path[i][j][0] == 0:
                    y = path[i][j][1]
                    y_list.append(y)
        y_center_list = []
        for i in range(len(y_list)):
            y_center = abs(y_list[i])
            y_center_list.append(y_center)
        y_center = min(y_center_list)
        i = y_center_list.index(y_center)
        final_path=path[i]
        return final_path

    if 500 < abs(xy1[1] - xy2[1]) < 700:  # 平行车位
        path=parallel_path(xy1,xy2)
        y_list=[]
        for i in range(len(path)):
            for j in range(len(path[i])):
                if path[i][j][0] == 0:
                    y = path[i][j][1]
                    y_list.append(y)
        y_center_list = []
        for i in range(len(y_list)):
            y_center = abs(y_list[i])
            y_center_list.append(y_center)
        y_center = min(y_center_list)
        i = y_center_list.index(y_center)
        final_path=path[i]
        return final_path




if __name__=='__main__':
    #垂直车位的例子
    # xy1=(600,300)
    # xy2=(600,550)

    #平行车位的例子
    xy1=(600,800)
    xy2=(600,1400)

    # show(xy1,xy2)

    final_path=path(xy1,xy2)
    print(final_path)

平行车位

垂直车位

最后

以上就是仁爱发带为你收集整理的自动泊车路径规划平行车位垂直车位的全部内容，希望文章能够帮你解决自动泊车路径规划平行车位垂直车位所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。