Robotics Software engineer編程筆記（二）

5.確定漫游者號(hào)的行進(jìn)方向

（1）漫游者號(hào)如何確定自己的行進(jìn)方向?

我們已經(jīng)有了一個(gè)由前置攝像頭得到的圖像，然后可以通過(guò)對(duì)圖像進(jìn)行處理，來(lái)確定漫游者號(hào)應(yīng)該轉(zhuǎn)動(dòng)的方向。

通過(guò)將漫游者坐標(biāo)轉(zhuǎn)換成極坐標(biāo)，我們就能確定小車(chē)應(yīng)該前進(jìn)的方向，其中每個(gè)像素位置由距離原點(diǎn)的距離和從正x方向逆時(shí)針的角度表示。

如圖所示， 漫游者號(hào)能行駛的范圍只有在右邊，所以漫游者號(hào)應(yīng)該向右側(cè)旋轉(zhuǎn)，來(lái)找到正確的道路。我們通過(guò)平局角度來(lái)計(jì)算小車(chē)應(yīng)該轉(zhuǎn)動(dòng)的方向。而角度應(yīng)該可以使用極坐標(biāo)來(lái)轉(zhuǎn)換。

轉(zhuǎn)換為極坐標(biāo)是一個(gè)簡(jiǎn)單的兩部過(guò)程：

def to_polar_coords(xpix, ypix):# 第一步： 計(jì)算距離 dist = np.sqrt(xpix**2 + ypix**2)# 第二步： 計(jì)算角度 angles = np.arctan2(ypix, xpix)return dist, angles

通過(guò)這個(gè)函數(shù)我們可以輕易的將直角坐標(biāo)系轉(zhuǎn)換成極坐標(biāo)系，從而方便我們計(jì)算平均角度。

（二）相關(guān)程序

程序在原來(lái)項(xiàng)目上進(jìn)行了更改，為了在matplotlib中輸出一個(gè)4格圖像，我使用了matplotlib.image.imread（）函數(shù)來(lái)取代cv2.imread（）函數(shù)。但是我在實(shí)際使用中，使用cv2.imread()讀取出來(lái)的圖像在matplotlib中會(huì)有顏色的改變。具體原因我也不是很清楚。

相比之前的程序，我還更改了輸出部分的程序，將輸出世界地圖的程序刪除了，添加了輸出轉(zhuǎn)向角度的代碼。在定義函數(shù)上，為了方便以后使用，沒(méi)有進(jìn)行更改。

下面是示例程序

import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv
import matplotlib.image as mpimg# 定義二值化圖像函數(shù)
def color_thresh(img, thresh=160):img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)ret, img_thresh = cv.threshold(img_gray, thresh, 255, cv.THRESH_BINARY)return img_thresh# 定義圖像映射函數(shù)，將攝像頭的圖像映射到平面坐標(biāo)中去
def perspect_transform(img, src, dst):M = cv.getPerspectiveTransform(src, dst)  # 定義變換矩陣img_perspect = cv.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect# 定義從圖像坐標(biāo)轉(zhuǎn)換函數(shù)
def rover_coords(binary_img):ypos, xpos = binary_img.nonzero()x_pixel = -(ypos - binary_img.shape[0]).astype(np.float)y_pixel = -(xpos - binary_img.shape[1]/2 ).astype(np.float)return x_pixel, y_pixel# 定義旋轉(zhuǎn)操作函數(shù)
def rotate_pix(xpix, ypix, yaw):yaw_rad = yaw * np.pi / 180xpix_rotated = (xpix * np.cos(yaw_rad)) - (ypix * np.sin(yaw_rad))ypix_rotated = (xpix * np.sin(yaw_rad)) + (ypix * np.cos(yaw_rad))return xpix_rotated, ypix_rotated# 定義平移操作函數(shù)
def translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale):xpix_translated = (xpix_rot / scale) + xposypix_translated = (ypix_rot / scale) + yposreturn xpix_translated, ypix_translated# 定義綜合函數(shù)，將旋轉(zhuǎn)和平移函數(shù)進(jìn)行結(jié)合，并限制了圖像范圍
def pix_to_world(xpix, ypix, xpos, ypos, yaw, world_size, scale):xpix_rot, ypix_rot = rotate_pix(xpix, ypix, yaw)xpix_tran, ypix_tran = translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale)x_pix_world = np.clip(np.int_(xpix_tran), 0, world_size - 1)y_pix_world = np.clip(np.int_(ypix_tran), 0, world_size - 1)return x_pix_world, y_pix_world# 定義轉(zhuǎn)換為極坐標(biāo)函數(shù)
def to_polar_coords(xpix, ypix):dist = np.sqrt(xpix**2 + ypix ** 2)angles = np.arctan2(ypix, xpix)return dist, angles# Define the filename, read and plot the image
filename = '…/sample2.jpg'
image = mpimg.imread(filename)# 隨機(jī)生成漫游者坐標(biāo)和角度
rover_yaw = np.random.random(1)*360
rover_xpos = np.random.random(1)*160 + 20
rover_ypos = np.random.random(1)*160 + 20# 函數(shù)參數(shù)定義部分
# 映射的圖片的一半邊長(zhǎng)
dst_size = 5
# 映射的點(diǎn)距離x軸的距離
bottom_offset = 0
# 將圖像二值化
image_thresh = color_thresh(image)
# 定義原圖像空間坐標(biāo)和映射圖像空間坐標(biāo)
src = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]])
dst = np.float32([[image.shape[1]/2 - dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - bottom_offset],[image.shape[1]/2 + dst_size, image.shape[0] - 2*dst_size - bottom_offset],[image.shape[1]/2 - dst_size, image.shape[0] - 2*dst_size - bottom_offset],])
# 映射圖像
image_prespect = perspect_transform(image_thresh, src, dst)
# 在二值化圖像尋找非零點(diǎn)
xpix, ypix = rover_coords(image_prespect)# 計(jì)算平局角度
dist, angles = to_polar_coords(xpix, ypix)
avg_angle = np.mean(angles)
avg_angle_degrees = avg_angle*180/np.pi
steering = np.clip(avg_angle_degrees, -15, 15)
print(steering)warped = perspect_transform(image, src, dst)
colorsel = color_thresh(image)
# 輸出部分
fig = plt.figure(figsize=(12,9))
plt.subplot(221)
plt.imshow(image)
plt.subplot(222)
plt.imshow(warped)
plt.subplot(223)
plt.imshow(colorsel, cmap='gray')
plt.subplot(224)
plt.plot(xpix, ypix, '.')
plt.ylim(-160, 160)
plt.xlim(0, 160)
arrow_length = 100
x_arrow = arrow_length * np.cos(avg_angle)
y_arrow = arrow_length * np.sin(avg_angle)
plt.arrow(0, 0, x_arrow, y_arrow, color='red', zorder=2, head_width=10, width=2)
plt.show()
cv.imshow('image_prespect', image_prespect)
cv.imshow('thresh', image_thresh)
cv.waitKey()

下面是matplotlib的輸出。可以看到程序正確輸出了相應(yīng)的方向。

總結(jié)

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（二）-样本搜索和找回-基于漫游者号模拟器的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。