Robotics Software engineer编程笔记（二）

5.确定漫游者号的行进方向

（1）漫游者号如何确定自己的行进方向?

我们已经有了一个由前置摄像头得到的图像，然后可以通过对图像进行处理，来确定漫游者号应该转动的方向。

通过将漫游者坐标转换成极坐标，我们就能确定小车应该前进的方向，其中每个像素位置由距离原点的距离和从正x方向逆时针的角度表示。

如图所示，漫游者号能行驶的范围只有在右边，所以漫游者号应该向右侧旋转，来找到正确的道路。我们通过平局角度来计算小车应该转动的方向。而角度应该可以使用极坐标来转换。

转换为极坐标是一个简单的两部过程：

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

通过这个函数我们可以轻易的将直角坐标系转换成极坐标系，从而方便我们计算平均角度。

（二）相关程序

程序在原来项目上进行了更改，为了在matplotlib中输出一个4格图像，我使用了matplotlib.image.imread（）函数来取代cv2.imread（）函数。但是我在实际使用中，使用cv2.imread()读取出来的图像在matplotlib中会有颜色的改变。具体原因我也不是很清楚。

相比之前的程序，我还更改了输出部分的程序，将输出世界地图的程序删除了，添加了输出转向角度的代码。在定义函数上，为了方便以后使用，没有进行更改。

下面是示例程序

import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv
import matplotlib.image as mpimg# 定义二值化图像函数
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# 定义图像映射函数，将摄像头的图像映射到平面坐标中去
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# 定义从图像坐标转换函数
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, angles# Define the filename, read and plot the image
filename = '…/sample2.jpg'
image = mpimg.imread(filename)# 随机生成漫游者坐标和角度
rover_yaw = np.random.random(1)*360
rover_xpos = np.random.random(1)*160 + 20
rover_ypos = np.random.random(1)*160 + 20# 函数参数定义部分
# 映射的图片的一半边长
dst_size = 5
# 映射的点距离x轴的距离
bottom_offset = 0
# 将图像二值化
image_thresh = color_thresh(image)
# 定义原图像空间坐标和映射图像空间坐标
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)
# 在二值化图像寻找非零点
xpix, ypix = rover_coords(image_prespect)# 计算平局角度
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的输出。可以看到程序正确输出了相应的方向。

Udacity机器人软件工程师课程笔记（二）-样本搜索和找回-基于漫游者号模拟器相关推荐

Udacity机器人软件工程师课程笔记（五）-样本搜索和找回-基于漫游者号模拟器-自主驾驶
9.自主驾驶在接下来的环节中,我们要实现漫游者号的自动驾驶功能. 完成这个功能我们需要四个程序,第一个为感知程序,其对摄像头输入的图片进行变换处理和坐标变换使用.第二个程序为决策程序,功能是帮助漫游 ...
Udacity机器人软件工程师课程笔记（一）-样本搜索和找回-基于漫游者号模拟器
Robotics Software engineer编程笔记(一) 使用Udacity提供的漫游者号模拟器创建环境地图,寻找样本. 该项目是根据美国国家航空航天局(NASA)的样本返回挑战进行建模的. ...
Udacity机器人软件工程师课程笔记（六）-样本搜索和找回-基于漫游者号模拟器-优化和样本找回
10.优化和样本找回 (1)优化概述在之前的一篇笔记中,我们已经实现了基本的漫游者号的自主驾驶功能.但是因为我们的感知子函数和决策子函数都过于简单,使漫游者号不能很好的自动驾驶和样本找回. 这篇笔记 ...
Udacity机器人软件工程师课程笔记（三）-样本搜索和找回-基于漫游者号模拟器-使用moviepy输出测试视频
6.方法测试在这个部分我们要整体的测试我们的程序,对前面的知识和内容有一个整体的应用和概括. 这是Udacity提供的相应资料,在code文件夹中有一个Rover_Project_Test_Note ...
Udacity机器人软件工程师课程笔记（四）-样本搜索和找回-基于漫游者号模拟器-决策树
7.做出决策我们已经建立了感知步骤Perception.py,接下来要构建决策函数decision.py . 我们利用一个基础的人工智能模型--决策树模型.其结构如下: 由于我们采用一个非常简单的模 ...
Udacity机器人软件工程师课程笔记（三十五) - SLAM - 基于网格的FastSLAM
一.SLAM介绍即使定位和建图问题(simultaneous localization and mapping),一般简称为SLAM, 也称作(Concurrent Mapping and Loca ...
Udacity机器人软件工程师课程笔记（七）-ROS介绍和Turtlesim包的使用
Robotics Software engineer笔记 1.ROS简介与虚拟机配置 (1)ROS简介 ROS是一款机器人软件框架,即机器人操作系统(Robot Operating System). ...
Udacity机器人软件工程师课程笔记（二十七) - 卷积神经网络（CNN）
1.卷积神经网络介绍 **卷积神经网络(Convolutional Neural Network,CNN)**是一种前馈神经网络,它的人工神经元可以响应一部分覆盖范围内的周围单元,对于大型图像处理有出 ...
Udacity机器人软件工程师课程笔记（二十四) - 控制（其二） - PID优化，梯度下降算法，带噪声的PID控制
7.非理想情况 (1)积分饱和到目前为止,我们一直使用的"理想"形式的PID控制器很少用于工业中."时间常数"形式更为常见. 当前说明了理想形式的一些重大缺陷 ...

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

Robotics Software engineer编程笔记（二）

5.确定漫游者号的行进方向

（1）漫游者号如何确定自己的行进方向?

（二）相关程序

Udacity机器人软件工程师课程笔记（二）-样本搜索和找回-基于漫游者号模拟器相关推荐

最新文章

热门文章