10.優化和樣本找回

(1)優化概述

在之前的一篇筆記中，我們已經實現了基本的漫游者號的自主駕駛功能。但是因為我們的感知子函數和決策子函數都過于簡單，使漫游者號不能很好的自動駕駛和樣本找回。

這篇筆記的主要內容就是涉及如何對我們的自主駕駛模型進行優化，如何使漫游者號遍歷整個地圖，并將巖石樣本進行找回并使漫游者號返回起始位置。所以我們要對兩個子函數進行優化：perception.py 和decision.py。更改漫游者對不同情況的處理，增加一些功能。

主要度量標準（如上圖所示）是時間，映射百分比，保真度和找到的巖石數量。在最佳情況下，使漫游者號將以非常高的保真度映射整個環境，并在最短的總時間內定位并收集所有六個巖石樣本。要做到這一點，不僅需要優化映射分析的準確性，還需要優化遍歷環境的效率。

下面給出可能的優化的要求：

1. 優化地圖保真度：
   透視變換上僅在運動和俯仰角接近零時才有效。如果正在剎車或轉彎，它們可能會顯著偏離零，并且您的變換后的圖像將不再是有效的地圖。考慮在滾動和俯仰中設置接近零的閾值以確定哪些變換圖像對于映射有效。

    # (7)更新漫游者號的世界地圖# 優化地圖保真度if int(Rover.pitch) <= 1 | int(Rover.pitch) >= 359:if int(Rover.roll) <= 1 | int(Rover.roll) >= 359:Rover.worldmap[x_pix_world, y_pix_world] += 10

2. 優化時間：
   更快，更有效地移動將最大限度地縮短總時間。考慮允許更高的最大速度，并使漫游者號不再重新訪問先前已經映射過的區域。
3. 優化映射：
   考慮如何“關閉”地圖中的邊界。
4. 優化尋找所有巖石：
   巖石總是出現在墻壁附近。考慮讓火星車成為一個“墻壁爬行器”，通過始終在左側或右側保持墻壁來探索環境。

（2）部分程序

因為時間安排只有一天時間，所以我沒有太多的機會去想如何去解決如何解決樣本拾取和返回原位等算法。只能簡單的優化了感知和決策子程序。讓整個地圖的保真度提高，還稍微提高了漫游者號的運行速度。利用一個簡單的處理方式來解決貼墻走的問題，但是仍在使用過程中有不少的問題。

奈何還是技術有限，現在還不能想到更好的解決辦法。以后若有機會一定會把這個火星車的項目好好完善一下。尤其是返回原點的算法，我感覺還是很有意思的。現在先挖一個坑，但就就不知道何時來填上這個坑了。

下面是決策程序，主要優化了面對障礙物的和貼右側行進的算法。

import numpy as np
import time
# 命令基于perception_step（）函數的輸出def decision_step(Rover):if Rover.nav_angles is not None:# 檢查 Rover.mode 狀態if Rover.mode == 'forward':# 檢查可導航地形的范圍if len(Rover.nav_angles) >= Rover.stop_forward:# 統計速度小于閾值的時長if int(Rover.vel*100) in range(-50, 50):Rover.time_point += 1# 一旦速度大于閾值，則計時器清零if Rover.time_point > 100:print('Error!', Rover.time_point)Rover.throttle = 0Rover.brake = Rover.brake_setRover.steer = 0Rover.mode = 'stop'print('set to stop mode')else:Rover.time_point = 0# 如果為forward模式，可導航地圖是好的，速度如果低于最大值，則加速if Rover.vel < Rover.max_vel:# 設置油門值Rover.throttle = Rover.throttle_setelse:Rover.throttle = 0Rover.brake = 0# 將轉向設置為平均夾角，范圍為 +/- 15Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi) - 5 - Rover.angle_offset, -15, 15)if Rover.angle_offset >= 0:Rover.angle_offset -= 0.1# 如果缺少地形圖，則設置為停止模式elif len(Rover.nav_angles) < Rover.stop_forward:# 設置到停止模式并剎車Rover.throttle = 0# 設置制動值Rover.brake = Rover.brake_setRover.steer = 0Rover.mode = 'stop'# 如果我們已經處于“停止”模式，那么做出不同的決定elif Rover.mode == 'stop':# 如果我們處于停止模式但仍然繼續制動if Rover.vel > 0.2:Rover.throttle = 0Rover.brake = Rover.brake_setRover.steer = 0# 如果我們沒有移動（vel <0.2），那就做點別的elif Rover.vel <= 0.2:if Rover.time_point < 1000:# 現在為停止狀態，依靠視覺數據，判斷是否有前進的道路if len(Rover.nav_angles) < Rover.go_forward:Rover.throttle = 0# 松開制動器以便轉動Rover.brake = 0# 轉彎范圍為+/- 15度，當停止時，下一條線將引起4輪轉向Rover.steer = 15#如果停下來但看到前面有足夠的可導航地形，那就啟動if len(Rover.nav_angles) >= Rover.go_forward :# 將油門設置回存儲值Rover.throttle = Rover.throttle_setRover.brake = 0# 將轉向設置為平均角度Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi) + Rover.angle_offset , -15, 15)if Rover.angle_offset >= 0:Rover.angle_offset -= 1Rover.mode = 'forward'# 這個條件用于避障，如果火星車卡在一個位置太久時間，則命令它經行轉向嘗試if Rover.time_point >= 100:Rover.throttle = 0Rover.brake = 0Rover.steer = 15Rover.time_point += 1print(' in stop mode ')if Rover.time_point >= 150:print('then to forward mode')Rover.time_point = 0Rover.mode = 'forward'Rover.throttle = Rover.throttle_setelse:Rover.throttle = Rover.throttle_setRover.steer = 0Rover.brake = 0# 在可拾取巖石的狀態下發送拾取命if Rover.near_sample and Rover.vel == 0 and not Rover.picking_up:Rover.send_pickup = Truereturn Rover

下面是感知程序，主要優化了對樣本的閾值處理、保真度等算法。

import numpy as np
import cv2
import matplotlib.pyplot as plt##圖像處理
# 定義二值化圖像函數
def color_thresh(img, rgb_thresh=(160, 160, 160),rgb_thresh_top=(255, 255, 255)):img_thresh = np.zeros_like(img[:, :, 0])above_thresh = (rgb_thresh_top[0] > img[:, :, 0]) & (img[:, :, 0]> rgb_thresh[0]) \& (rgb_thresh_top[1] > img[:, :, 0]) & (img[:, :, 1] > rgb_thresh[1]) \& (rgb_thresh_top[2] > img[:, :, 0]) & (img[:, :, 2] > rgb_thresh[2])img_thresh[above_thresh] = 255return img_thresh##透視變換
# 定義圖像映射函數，將攝像頭的圖像映射到平面坐標中去
def perspect_transform(img, src, dst):M = cv2.getPerspectiveTransform(src, dst)  # 定義變換矩陣img_perspect = cv2.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect##坐標變換
# 定義從圖像坐標轉換成漫游者號坐標函數
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# 定義旋轉操作函數
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# 定義平移操作函數
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# 定義綜合函數，將旋轉和平移函數進行結合，并限制了圖像范圍
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# 定義轉換為極坐標函數
def to_polar_coords(xpix, ypix):dist = np.sqrt(xpix**2 + ypix ** 2)angles = np.arctan2(ypix, xpix)return dist, anglesdef perception_step(Rover):# (1)定義透視變換的原點和目標點# 參考圖像global count_1, count_2filename = 'E:/2019/RoboND-Rover-Project-master/calibration_images/example_grid1.jpg'image = cv2.imread(filename)dst_size = 5bottom_offset = 0src = 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],])# （2）應用透視變換img_perspect = perspect_transform(Rover.img, src, dst)# (3)應用顏色閾值來識別可導航的地形/障礙物/巖石樣本img_thresh_ter = color_thresh(img_perspect, rgb_thresh=(160, 160, 160))img_thresh_obs = color_thresh(img_perspect, rgb_thresh=(160, 160, 160))img_thresh_roc = color_thresh(Rover.img, rgb_thresh=(0, 130, 170), rgb_thresh_top=(50, 170, 255))img_thresh_roc_nonzero = np.nonzero(img_thresh_roc)print(img_thresh_roc_nonzero)if img_thresh_roc_nonzero[0].any() > 0:Rover.samples = Trueelse:Rover.samples = Falsewhile Rover.samples:print("There find a sample!")break# (4)更新ROver.vision_image# 樣本標記Rover.vision_image[:, :, 0] = img_thresh_rocRover.vision_image[:, :, 1] = img_thresh_obsRover.vision_image[:, :, 2] = img_thresh_ter# (5)將地圖像素值轉換為以漫游者號為中心的坐標xpix, ypix = rover_coords(img_thresh_ter)# (6)將以漫游者號為中心的像素值坐標轉化為世界地圖坐標x_pix_world, y_pix_world = pix_to_world(xpix, ypix, xpos=Rover.pos[0], ypos=Rover.pos[1], yaw=Rover.yaw, world_size=200, scale=10)# (7)更新漫游者號的世界地圖# 優化地圖保真度if int(Rover.pitch) <= 1 | int(Rover.pitch) >= 359:if int(Rover.roll) <= 1 | int(Rover.roll) >= 359:Rover.worldmap[x_pix_world, y_pix_world] += 10# (8)將以漫游者號為中心的像素位置轉換為極坐標rover_distances, rover_angles = to_polar_coords(xpix, ypix)# 可導航地形像素的角度的數組Rover.nav_angles = rover_angles# 獲取轉向角，使用極坐標系角度的平均值，這樣可以運行，但是會有意外情況avg_angle_degrees = np.mean((rover_angles ) * 180 / np.pi)   # 使漫游者號靠右側行走print('rover_distances:',np.mean((rover_distances)), 'rover_angles:',avg_angle_degrees)Rover.steer= np.clip(avg_angle_degrees, -15, 15) - 5# 獲取油門速度，使用平均極坐標系的平均值Rover.throttle = np.mean(rover_distances)# 向右偏轉功能，如果檢測到右側道路，則使小車偏向右側形式一段時間view_angle = (rover_angles) * 180 / np.piview_distances = rover_distancesfor i in range(view_angle.shape[0]):if int(view_angle[i]) < -23 & int(view_distances[i]) > 90:Rover.count_1 += 1if int(view_angle[i]) > -23 & int(view_angle[i]) < 0 :Rover.count_2 += 1if Rover.count_1 > Rover.count_2 * 2 & Rover.count_1 > 500:Rover.angle_offset = 15print(Rover.count_1, Rover.count_2,'recht!')Rover.count_1 = Rover.count_2 = 0return Rover

此外，在嘗試算法的過程中，我還對RoverState（）類進行了一些添加：

# 定義RoverState（）類
class RoverState():def __init__(self):self.start_time = None  # 記錄導航開始的時間self.total_time = None  # 記錄導航的總持續時間self.img = None  # 當前的相機圖像self.pos = None  # 當前的位置 (x, y)self.yaw = None  # 當前的偏航角self.pitch = None  # 當前的俯仰角self.roll = None  # 當前的旋轉角self.vel = None  # 當前的速度self.steer = 0  # 當前的轉向角self.throttle = 0  # 當前的油門值self.brake = 0  # 當前的制動值self.nav_angles = None  # 可導航地形像素的角度self.nav_dists = None  # 可導航地形像素的距離self.ground_truth = ground_truth_3d  # 真實的世界地圖self.mode = 'forward'  # 當前模式 (可以是前進或者停止)self.throttle_set = 0.4  # 加速時的節流設定self.brake_set = 10  # 剎車時的剎車設定# 下面的stop_forward和go_forward字段表示可導航地形像素的總數。# 這是一種非常粗糙的形式來表示漫游者號當何時可以繼續前進或者何時應該停止。# 可以隨意添加新字段或修改這些字段。self.stop_forward = 50  # T啟動停止時的閾值self.go_forward = 500  # 再次前進的閾值self.max_vel = 3  # 最大速度 (m/s)self.time_point = 0  # 障礙計時器self.angle_offset = 0  # 角度修正self.count_1 = 0  # 計數器1self.count_2 = 0  # 計數器2# 感知步驟的圖像輸入# 更新此圖像以顯示中間分析步驟# 在自主模式下的屏幕上self.vision_image = np.zeros((160, 320, 3), dtype=np.float)# 世界地圖# 使用可導航的位置更新此圖像# 障礙物和巖石樣本self.worldmap = np.zeros((200, 200, 3), dtype=np.float)self.samples_pos = None  # 存儲實際樣本的位置self.samples_cv = Falseself.samples_to_find = 0  # 儲存樣本的初始計數self.samples_located = 0  # 儲存位于地圖上的樣本數self.samples_collected = 0  # 儲存收集的樣本數self.near_sample = 0  # 設置附近樣本數為0self.picking_up = 0  # 設置["picking_up"]值self.send_pickup = False

（3）優化結果

可以看見，通過這些優化，模型在地圖繪制上已經有了較好的完成度。

但是這個項目的目標是收集6個樣本，并返回原點。由此可見，還是有很大一段差距的。但是由于時間的原因，這個項目只能先告一段落了。下面的便是ROS操作系統了。

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。