自动泊车路径规划
发布时间
阅读量：
阅读量
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)
python
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)
#
# #车位线
# #车位尺寸为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)
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)
img01 = np.ones((1500, 1500, 3), np.uint8)
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)
python
平行车位
垂直车位
全部评论 (0)
还没有任何评论哟~