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

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 = 0
                        Rover.brake = Rover.brake_set
                        Rover.steer = 0
                        Rover.mode = 'stop'
                        print('set to stop mode')
                else:
                    Rover.time_point = 0

                # 如果为forward模式，可导航地图是好的，速度如果低于最大值，则加速
                if Rover.vel < Rover.max_vel:
                    # 设置油门值
                    Rover.throttle = Rover.throttle_set
                else:
                    Rover.throttle = 0
                Rover.brake = 0
                # 将转向设置为平均夹角，范围为 +/- 15
                Rover.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_set
                    Rover.steer = 0
                    Rover.mode = 'stop'

        # 如果我们已经处于“停止”模式，那么做出不同的决定
        elif Rover.mode == 'stop':
            # 如果我们处于停止模式但仍然继续制动
            if Rover.vel > 0.2:
                Rover.throttle = 0
                Rover.brake = Rover.brake_set
                Rover.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_set
                        Rover.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 -= 1
                        Rover.mode = 'forward'
                # 这个条件用于避障，如果火星车卡在一个位置太久时间，则命令它经行转向尝试
                if Rover.time_point >= 100:
                    Rover.throttle = 0
                    Rover.brake = 0
                    Rover.steer = 15
                    Rover.time_point += 1
                    print(' in stop mode ')
                    if Rover.time_point >= 150:
                        print('then to forward mode')
                        Rover.time_point = 0
                        Rover.mode = 'forward'
                        Rover.throttle = Rover.throttle_set
    else:
        Rover.throttle = Rover.throttle_set
        Rover.steer = 0
        Rover.brake = 0
        
    # 在可拾取岩石的状态下发送拾取命
    if Rover.near_sample and Rover.vel == 0 and not Rover.picking_up:
        Rover.send_pickup = True
    
    return 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] = 255

    return 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 / 180
    xpix_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) + xpos
    ypix_translated = (ypix_rot / scale) + ypos
    return 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, angles


def perception_step(Rover):
    # (1)定义透视变换的原点和目标点
    # 参考图像
    global count_1, count_2
    filename = 'E:/2019/RoboND-Rover-Project-master/calibration_images/example_grid1.jpg'
    image = cv2.imread(filename)

    dst_size = 5
    bottom_offset = 0

    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],
                      ])

    # （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 = True

    else:
        Rover.samples = False

    while Rover.samples:
        print("There find a sample!")
        break

    # (4)更新ROver.vision_image
    # 样本标记
    Rover.vision_image[:, :, 0] = img_thresh_roc
    Rover.vision_image[:, :, 1] = img_thresh_obs
    Rover.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.pi
    view_distances = rover_distances
    for i in range(view_angle.shape[0]):
        if int(view_angle[i]) < -23 & int(view_distances[i]) > 90:
            Rover.count_1 += 1

        if int(view_angle[i]) > -23 & int(view_angle[i]) < 0 :
            Rover.count_2 += 1
    if Rover.count_1 > Rover.count_2 * 2 & Rover.count_1 > 500:
        Rover.angle_offset = 15
        print(Rover.count_1, Rover.count_2,'recht!')
        Rover.count_1 = Rover.count_2 = 0

    return 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  # 计数器1
        self.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 = False
        self.samples_to_find = 0  # 储存样本的初始计数
        self.samples_located = 0  # 储存位于地图上的样本数
        self.samples_collected = 0  # 储存收集的样本数
        self.near_sample = 0  # 设置附近样本数为0
        self.picking_up = 0  # 设置["picking_up"]值
        self.send_pickup = False

（3）优化结果

可以看见，通过这些优化，模型在地图绘制上已经有了较好的完成度。

但是这个项目的目标是收集6个样本，并返回原点。由此可见，还是有很大一段差距的。但是由于时间的原因，这个项目只能先告一段落了。下面的便是ROS操作系统了。