首页 > Udacity机器人软件工程师课程笔记（六）-样本搜索和找回-基于漫游者号模拟器-优化和样本找回

Udacity机器人软件工程师课程笔记（六）-样本搜索和找回-基于漫游者号模拟器-优化和样本找回

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
  1. 优化时间：

    更快，更有效地移动将最大限度地缩短总时间。考虑允许更高的最大速度，并使漫游者号不再重新访问先前已经映射过的区域。
  2. 优化映射：

    考虑如何“关闭”地图中的边界。
  3. 优化寻找所有岩石：

    岩石总是出现在墙壁附近。考虑让火星车成为一个“墙壁爬行器”，通过始终在左侧或右侧保持墙壁来探索环境。

（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
                

                    
更多相关：

©2024 11GX.COM