远程控制slam小车
1.
把小车树莓派及pc端ubuntu通过无线路由器连接到统一局域网中
2.登入路由器查看是设备是否连接成功
http://192.168.1.1/
路由器密码
树莓派: zxcar 密码:123
ssh zxcar@树莓派的IP 然后输入密码 就可以了 确保自己的电脑和树莓派在同一个网络
远程拷贝
scp -r src zxcar@192.168.1.12:~/itheima4_ws远程登入
ssh zxcar@192.168.1.12
密码 123
上位机单独启动驱动命令
rosrun car_driver car_driver.pyubuntu链接串口
ls -al /dev/ttyU*
ros 控制键盘调试
rosrun teleop_twist_keyboard teleop_twist_keyboard.py键盘控制各个键介绍：
---------------------------
U I O
J K L
M < >
K—停止
I、J、<、L—前、左、后、右
q/z : 最大速度增加/减少10%
w/x : 仅线性速度增加10%
e/c : 仅角速度增加10%
查找设备
lsusb -vvv设置设备串口udev,定义rules文件
激活rules
１．拷贝到以下目录
sudo cp 58-robot.rules /etc/udev/rules.d/
或
sudo cp 58-robot.rules /lib/udev/rules.d/
２．加载文件
sudo service udev reload
３．启动
sudo service udev restart
启动报错提示用下面的命令：
sudo systemctl daemon-reload
４．插上设备 查看是否安装完成
ls -al /dev/skser*
ls -al /dev/sulid*命令
roslaunch zxcar bringup.launch树莓派启动命令建图
roslaunch zxcar lidar_slam.launch
这个要在pc本机启动
roslaunch zxcar lidar_slam_rviz.launch 本机启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
问题:
sk@sk-PC:~/workspas/slam$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Traceback (most recent call last):
File "/home/sk/workspas/slam/src/teleop_twist_keyboard/teleop_twist_keyboard.py", line 7, in <module>
import roslib; roslib.load_manifest('teleop_twist_keyboard')
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 64, in load_manifest
sys.path = _generate_python_path(package_name, _rospack) + sys.path
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 97, in _generate_python_path
m = rospack.get_manifest(pkg)
File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 171, in get_manifest
return self._load_manifest(name)
File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 215, in _load_manifest
retval = self._manifests[name] = parse_manifest_file(self.get_path(name), self._manifest_name, rospack=self)
File "/usr/lib/python2.7/dist-packages/rospkg/manifest.py", line 414, in parse_manifest_file
_static_rosdep_view = init_rospack_interface()
File "/usr/lib/python2.7/dist-packages/rosdep2/rospack.py", line 59, in init_rospack_interface
lookup = _get_default_RosdepLookup(Options())
File "/usr/lib/python2.7/dist-packages/rosdep2/main.py", line 134, in _get_default_RosdepLookup
verbose=options.verbose)
File "/usr/lib/python2.7/dist-packages/rosdep2/sources_list.py", line 594, in create_default
sources = load_cached_sources_list(sources_cache_dir=sources_cache_dir, verbose=verbose)
File "/usr/lib/python2.7/dist-packages/rosdep2/sources_list.py", line 552, in load_cached_sources_list
with open(cache_index, 'r') as f:
IOError: [Errno 13] Permission denied: '/home/sk/.ros/rosdep/sources.cache/index'
IOError: [Errno 13] 权限问题
解决:
cd /home/sk/.ros/rosdep/sources.cache/
sudo chmod 777 index
问题2:链接不上或找不到节点 可能ip问题
输入
ifconfig与
echo $ROS_HOSTNAME查看ip是否一致
强制更改 本机ip; 把动态命令写死
export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`
改
export ROS_HOSTNAME=192.168.1.10动态命令在多网卡或网络中断会导致把本机ip发送给mast节点的ip不准确
第一种:动态多机部署ros 方法:
主机:树莓派端配置
export ROS_IP=`hostname -I | awk '{print $1}'`
export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`
export ROS_MASTER_URI=http://`hostname -I | awk '{print $1}'`:11311从机:
export ROS_IP=`hostname -I | awk '{print $1}'`
export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`
export ROS_MASTER_URI=http://树莓派的IP地址:11311第二种:多机部署ros 方法:
> 主机和从机要在同一个局域网内
1. 主机添加在hosts中添加所有节点的ip和主机名(主机配置)
```bash
sudo gedit /etc/hosts
```> ip 主机名
2. 从机配置master节点服务(主机也要修改)
```bash
sudo gedit ~/.bashrc
export ROS_HOSTNAME=本机ip
export ROS_MASTER_URI=http://主机ip:11311设置树莓派连接wifi
HUAWEI-sukai1 01524922
2.
把树莓派tf卡插入电脑
1.新建一个文件名字ssh的空文件
4 ） 使 用 记 事 本 工 具 ， 按 照 下 面 的 参 考 格 式 填 入 记 事 本 内 容 ， 然 后 命 名 为
“wpa_supplicant.conf ”（温馨提示：可先输入下方内容，保存并退出该文件。然后将文件名
进行重命名，需删除包含 txt 在内的所有文件名，替换为上方名称。如有弹窗提示，选择“是”
34即可），再保存至“boot”盘符内。
country=CN
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
network={
ssid="Hiwonder"
psk="123456789"
key_mgmt=WPA-PSK
priority=1
}
弹出tf卡插入树莓派,让树莓派与电脑都连接到热点
PC与树莓派ssh链接时出现间歇性联通段开网络故障
sendmsg: No buffer space acailable
sendmsg: Network is Unreachable
From 192.168.**.*** icmp_se=183 Destination Host Unreachable
connection closed by 192.168.**.*** port 22
以下是ping的图片
故障排除
如果当你尝试连接到小派时遇到connection reset by peer错误，可能是由SSH key引起的。你可以通过下面的命令来重置它。
首先，删除旧的key文件：
sudo rm /etc/ssh/ssh_host_*然后生成一个新的：
sudo dpkg-reconfigure openssh-server然后再试一次。
重启
reboot在windows 中使用putty登入试试
NameError: global name 'ser' is not defined
serial 这个报错：
查出来必须要卸载serial 不要安装：
pip uninstall serial
要安装：
pip install pyserialslam小车pid调试
安装 rqt_plot
rosdep install rqt_plot
启动命令:
rqt_plot
在上位机car_driver驱动中需要把速度信息发布出来:
rostopic list 查看节点
控制台输出句柄消息
rostopic echo /zxcar/get_vel
rqt_plot 中监听速度
命令
rqt_plot
调整y方向的范围:
调试:
Rosrun teleop_twist_keyboard teleop_twist_keyboard.py //启动键盘控制
$rviz //参考上节开启navigatin.rviz观察机器人仿真情况
采用键盘控制模拟机器人，模拟机器人开始移动。注意鼠标指针必须位于teleop_twist_keyboard终端页面，否则控制键盘模拟机器人无法移动。
键盘控制各个键介绍：
---------------------------
U I O
J K L
M < >
K—停止
I、J、<、L—前、左、后、右
q/z : 最大速度增加/减少10%
w/x : 仅线性速度增加10%
e/c : 仅角速度增加10%
速度设置到0.215
启动rosrun rqt_topic rqt_topic
小车反馈回来,小车收到的数据
看角速度的数据
py_slam_exploring_slam.py自动导航建模
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib;
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt
class NavTest():
def __init__(self):
rospy.init_node('exploring_slam', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间 (单位：s)
self.rest_time = rospy.get_param("~rest_time", 2)
# 是否仿真？
self.fake_test = rospy.get_param("~fake_test", True)
# 到达目标的状态
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# 设置目标点的位置
# 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点
# 在终端中就会看到该点的坐标信息
locations = dict()
locations['1'] = Pose(Point(4.589, -0.376, 0.000), Quaternion(0.000, 0.000, -0.447, 0.894))
locations['2'] = Pose(Point(4.231, -6.050, 0.000), Quaternion(0.000, 0.000, -0.847, 0.532))
locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))
locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))
locations['6'] = Pose(Point(2.924, 0.018, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存机器人的在rviz中的初始位置
initial_pose = PoseWithCovarianceStamped()
# 保存成功率、运行时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 如果已经走完了所有点，再重新开始排序
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# 如果最后一个点和第一个点相同，则跳过
if sequence[0] == last_location:
i = 1
# 在当前的排序中获取下一个目标点
location = sequence[i]
# 跟踪行驶距离
# 使用更新的初始位置
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# 存储上一次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下一个目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# 让用户知道下一个位置
rospy.loginfo("Going to: " + str(location))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
#取消机器人运动,不需要运动到指定点了
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def update_initial_pose(self, initial_pose):
self.initial_pose = initial_pose
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def trunc(f, n):
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])
if __name__ == '__main__':
try:
NavTest()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Exploring SLAM finished.")py_opencv_motion_detector.py物体跟随
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, RegionOfInterest
from cv_bridge import CvBridge, CvBridgeError
class motionDetector:
def __init__(self):
rospy.on_shutdown(self.cleanup);
# 创建cv_bridge
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
# 设置参数：最小区域、阈值
self.minArea = rospy.get_param("~minArea", 500)
self.threshold = rospy.get_param("~threshold", 25)
self.firstFrame = None
self.text = "Unoccupied"
# 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
def image_callback(self, data):
# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
frame = np.array(cv_image, dtype=np.uint8)
except CvBridgeError, e:
print e
# 创建灰度图像
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (21, 21), 0)
# 使用两帧图像做比较，检测移动物体的区域
if self.firstFrame is None:
self.firstFrame = gray
return
#获取差分图 就是将两幅图像作差 两个图片相减，这里用的是灰度图，类型是uint8
frameDelta = cv2.absdiff(self.firstFrame, gray)
thresh = cv2.threshold(frameDelta, self.threshold, 255, cv2.THRESH_BINARY)[1]
thresh = cv2.dilate(thresh, None, iterations=2)
binary, cnts, hierarchy= cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
for c in cnts:
# 如果检测到的区域小于设置值，则忽略
if cv2.contourArea(c) < self.minArea:
continue
# 在输出画面上框出识别到的物体
(x, y, w, h) = cv2.boundingRect(c)
cv2.rectangle(frame, (x, y), (x + w, y + h), (50, 255, 50), 2)
self.text = "Occupied"
# 在输出画面上打当前状态和时间戳信息
cv2.putText(frame, "Status: {}".format(self.text), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
# 将识别后的图像转换成ROS消息并发布
self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
def cleanup(self):
print "Shutting down vision node."
cv2.destroyAllWindows()
if __name__ == '__main__':
try:
# 初始化ros节点
rospy.init_node("motion_detector")
rospy.loginfo("motion_detector node is started...")
rospy.loginfo("Please subscribe the ROS image.")
motionDetector()
rospy.spin()
except KeyboardInterrupt:
print "Shutting down motion detector node."
cv2.destroyAllWindows()
py_slam_startros_在代码中启动ros节点roslaunch和rosrun
import subprocess
import rospy
import rosnode
class launch_demo:
def __init__(self, cmd=None):
self.cmd = cmd
def launch(self):
self.child = subprocess.Popen(self.cmd)
return True
def shutdown(self):
self.child.terminate()
self.child.wait()
return True
if __name__ == "__main__":
rospy.init_node('launch_demo',anonymous=True)
launch_nav = launch_demo(["roslaunch", "pibot_simulator", "nav.launch"])
launch_nav.launch()
r = rospy.Rate(0.2)
r.sleep()
rospy.loginfo("switch map...")
r = rospy.Rate(1)
r.sleep()
rosnode.kill_nodes(['map_server'])
map_name = "/home/pibot/ros_ws/src/pibot_simulator/maps/blank_map_with_obstacle.yaml"
map_node = subprocess.Popen(["rosrun", "map_server", "map_server", map_name, "__name:=map_server"])
while not rospy.is_shutdown():
r.sleep()上面使用python代码启动了一个PIBOT模拟器的导航，然后5s后切换了一个地图
- 使用
subprocess.Popen可以启动一个进程(roslaunch或者rosrun) - 使用
rosnode.kill_nodes可以杀死一个rosnode
py_slam_定点导航
from launch_demo import launch_demo
import rospy
import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from math import pi
class navigation_demo:
def __init__(self):
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(60))
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def _done_cb(self, status, result):
rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
def _active_cb(self):
rospy.loginfo("[Navi] navigation has be actived")
def _feedback_cb(self, feedback):
rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
return True
def cancel(self):
self.move_base.cancel_all_goals()
return True
if __name__ == "__main__":
rospy.init_node('navigation_demo',anonymous=True)
launch_nav = launch_demo(["roslaunch", "pibot_simulator", "nav.launch"])
launch_nav.launch()
r = rospy.Rate(0.2)
r.sleep()
rospy.loginfo("set pose...")
r = rospy.Rate(1)
r.sleep()
navi = navigation_demo()
navi.set_pose([-0.7,-0.4,0])
rospy.loginfo("goto goal...")
r = rospy.Rate(1)
r.sleep()
navi.goto([0.25,4, 90])
while not rospy.is_shutdown():
r.sleep()上面完成设置机器人位置和导航到某一位置的功能
navi.set_pose([-0.7,-0.4,0]) 设置机器人位于地图中位置（-0.7，-0.4） 姿态yaw=0°
navi.goto([0.25,4, 90]) 该接口调用一个服务完成机器人运动至位置（0.25，4）姿态yaw=90°
有了该接口就不难完成多点导航巡逻等应用
Twiddle 调校最优解PID
为了避免人为的猜测、试错来获取比较好的PID值，我们可以设计一种自动化的调参逻辑循环，根据使指定目标函数的值达到最小时，得到更加优秀的参数列表。
一种更智能、自动的方法是使用梯度下降算法。 前提是您从三个增益的初始猜测向量开始。 通常对P使用小的非零值，对I和D使用0。 然后，分别对每个收益进行小幅更改，然后测试目标函数是否降低。 如果降低了，将沿相同方向不断更改参数，否则尝试沿相反方向调整参数。 如果增益值的增加或减少均不会降低成本函数，则减小增益增量的大小并重复。 整个循环应继续进行，直到增量大小降至某个阈值以下。
我们可以把以下过程称之为 Twiddle（旋弄、捻弄），可以帮助我们确定合适的P、I、D三个值，其原理是构建一个初始化为 [0,0,0][0,0,0] 的参数列表，然后循环多次，根据每次循环返回的误差均值，调大或调小参数（调整的步伐参数由另一个3个值的列表控制）。直到步伐参数之和小于一定阈值时，停止循环，把此时参数列表作为最终的PID参数。
代码实现：
- 拷贝依赖文件 robot.py 到项目目录
- 新建文件
pid_twiddle.py并编写如下内容：
from robot import Robot, show
import numpy as np
def make_robot():
"""
创建并初始化机器人小车, 设置初始位置为(0, -1), 初始旋转角度为0
"""
robot = Robot()
robot.set(0, -1, 0)
robot.set_steering_drift(10 / 180 * np.pi)
return robot
# NOTE: We use params instead of k_p, k_d, k_i
def run(robot, params, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
err = 0
prev_cte = 0 - robot.y
int_cte = 0
for i in range(2 * n):
cte = 0 - robot.y
diff_cte = cte - prev_cte
int_cte += cte
prev_cte = cte
steer = params[0] * cte + params[1] * diff_cte + params[2] * int_cte
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
if i >= n:
err += cte ** 2
return x_trajectory, y_trajectory, err / n
# Make this tolerance bigger if you are timing out!
def twiddle(tol=0.2):
p = [0, 0, 0]
dp = [1, 1, 1]
robot = make_robot()
x_trajectory, y_trajectory, best_err = run(robot, p)
it = 0
while sum(dp) > tol:
# 循环，直到系数之和小于等于阈值（默认阈值为0.2，起始值为3.0）
print("Iteration {}, best error = {}".format(it, best_err))
for i in range(len(p)):
p[i] += dp[i]
robot.reset()
x_trajectory, y_trajectory, err = run(robot, p)
if err < best_err:
best_err = err
dp[i] *= 1.1 # 此值有助于减少总误差，下次可以多加点
else:
p[i] -= 2 * dp[i]# 此值不利于于减少总误差，直接把刚加的去掉，并且向反方向减一倍
robot.reset()
x_trajectory, y_trajectory, err = run(robot, p)
if err < best_err:
best_err = err
dp[i] *= 1.1 # 如果反方向有利于减少总误差，扩大此值
else:
p[i] += dp[i] # 恢复为原值
dp[i] *= 0.9 # 把缩小变化系数
it += 1
return p, best_err
params, err = twiddle()
print("Final twiddle error = {} params = {}".format(err, params))
robot = make_robot()
x_trajectory, y_trajectory, err = run(robot, params)
show(x_trajectory, y_trajectory, label="Twiddle PID")
运行结果：
可以与之前使用的PID进行对比，明显可以看出当前PID在x=50附近就开始在目标轨迹附近稳定下来。而之前的要到x=100的位置才可以稳定下来。
输出日志：
根据最后的输出日志可知：
最小的误差均值为 7.940560962605189e-07
此时对应的P、D、I分别为： 10.716018504541431, 18.68325573584582, 0.020275559590445292
Iteration 0, best error = 7972.071547906822
Iteration 1, best error = 0.048853806107299856
Iteration 2, best error = 0.03026214567061226
Iteration 3, best error = 0.0077046028132098255
Iteration 4, best error = 0.003222969736312333
Iteration 5, best error = 0.0016693580238629137
Iteration 6, best error = 0.0009763548793623677
Iteration 7, best error = 0.0006143945322198215
Iteration 8, best error = 0.0006143945322198215
Iteration 9, best error = 0.0006143945322198215
Iteration 10, best error = 0.0006143945322198215
Iteration 11, best error = 0.0006143945322198215
Iteration 12, best error = 0.0006143945322198215
Iteration 13, best error = 0.0006143945322198215
Iteration 14, best error = 0.0006143945322198215
Iteration 15, best error = 0.0006143945322198215
Iteration 16, best error = 0.0006143945322198215
Iteration 17, best error = 0.0006143945322198215
Iteration 18, best error = 0.000612580641120018
Iteration 19, best error = 0.0005393890590252054
Iteration 20, best error = 0.0004707073557690764
Iteration 21, best error = 0.00040854195467355256
Iteration 22, best error = 0.00036173191894990056
Iteration 23, best error = 0.0003496794660242756
Iteration 24, best error = 0.00030372592629584626
Iteration 25, best error = 0.00026649497706284647
Iteration 26, best error = 0.00026502850791677363
Iteration 27, best error = 0.00023267758106029443
Iteration 28, best error = 0.00023267758106029443
Iteration 29, best error = 0.00023267758106029443
Iteration 30, best error = 0.00023267758106029443
Iteration 31, best error = 0.00023267758106029443
Iteration 32, best error = 0.00023222333181571606
Iteration 33, best error = 0.00023222333181571606
Iteration 34, best error = 0.00023222333181571606
Iteration 35, best error = 0.00023222333181571606
Iteration 36, best error = 0.00023222333181571606
Iteration 37, best error = 0.00022978905387007294
Iteration 38, best error = 0.00019428921416175855
Iteration 39, best error = 0.00014919774411248834
Iteration 40, best error = 2.2810673236127803e-05
Iteration 41, best error = 6.65198948619874e-06
Iteration 42, best error = 8.957452817602662e-07
Iteration 43, best error = 8.957452817602662e-07
Iteration 44, best error = 8.957452817602662e-07
Iteration 45, best error = 8.957452817602662e-07
Iteration 46, best error = 8.957452817602662e-07
Iteration 47, best error = 8.957452817602662e-07
Iteration 48, best error = 8.957452817602662e-07
Iteration 49, best error = 8.957452817602662e-07
Iteration 50, best error = 8.957452817602662e-07
Iteration 51, best error = 7.940560962605189e-07
Iteration 52, best error = 7.940560962605189e-07
Iteration 53, best error = 7.940560962605189e-07
Iteration 54, best error = 7.940560962605189e-07
Iteration 55, best error = 7.940560962605189e-07
Iteration 56, best error = 7.940560962605189e-07
Iteration 57, best error = 7.940560962605189e-07
Final twiddle error = 7.940560962605189e-07 params = [10.716018504541431, 18.68325573robot.py
import random
import numpy as np
import matplotlib.pyplot as plt
class Robot(object):
def __init__(self, length=20.0):
"""
鍒涘缓鏈哄櫒浜哄苟鍒濆鍖栦綅缃拰鏂瑰悜涓�0, 0, 0.
"""
self.x = 0.0
self.y = 0.0
self.orientation = 0.0 # 涓嶺杞存鏂瑰悜鐨勫す瑙掞紙鍗曚綅涓哄姬搴︼級
self.length = length # 鍓嶅悗杞瓙鐨勮酱璺�
self.steering_noise = 0.0 # 鏂瑰悜鍣０
self.distance_noise = 0.0 # 璺濈鍣０
self.steering_drift = 0.0 # 鏂瑰悜婕傜Щ
self.default_state = {"x": self.x,"y": self.y,"o": self.orientation}
def reset(self):
self.x = self.default_state["x"]
self.y = self.default_state["y"]
self.orientation = self.default_state["o"]
def set(self, x, y, orientation):
"""
璁剧疆鏈哄櫒浜虹殑鍧愭爣鍙婃柟鍚�
"""
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)
self.default_state = {"x": self.x,"y": self.y,"o": self.orientation}
def set_noise(self, steering_noise, distance_noise):
"""
璁剧疆鍣０鍙傛暟
:param steering_noise: 杞悜鍣０
:param distance_noise: 璺濈鍣０
"""
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = steering_noise
self.distance_noise = distance_noise
def set_steering_drift(self, drift):
"""
璁剧疆绯荤粺鐨勮浆鍚戞紓绉诲弬鏁�
"""
self.steering_drift = drift
def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
"""
灏忚溅鐨勭Щ鍔ㄥ嚱鏁�
:param steering: 鍓嶈疆鐨勮浆鍚戣锛屾渶澶у€间负max_steering_angle
:param distance: 鎬昏椹惰窛绂伙紝涓€鑸负闈炶礋
:param tolerance: 杞悜鐨勬渶灏忓樊鍊硷紙闃堝€硷級锛屽皬浜庢闃堝€兼椂锛岃灏忚溅璧扮洿绾匡紝鍗曚綅涓哄姬搴�
:param max_steering_angle: 鏈€澶ц浆鍚戣锛岄粯璁や负 180 / 4.0 = 45掳
"""
# if steering > max_steering_angle:
# steering = max_steering_angle
# if steering < -max_steering_angle:
# steering = -max_steering_angle
steering = np.clip(steering, -max_steering_angle, max_steering_angle)
if distance < 0.0:
distance = 0.0
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# 瑙掗€熷害 = 绾块€熷害 / 杞集鍗婂緞
# Execute motion
turn = np.tan(steering2) * distance2 / self.length
if abs(turn) < tolerance:
# approximate by straight line motion 杩戜技鐩寸嚎妯″瀷
self.x += distance2 * np.cos(self.orientation)
self.y += distance2 * np.sin(self.orientation)
self.orientation = (self.orientation + turn) % (2.0 * np.pi)
else:
# approximate bicycle model for motion 杩戜技鑷杞︽ā鍨�
radius = distance2 / turn
cx = self.x - (np.sin(self.orientation) * radius)
cy = self.y + (np.cos(self.orientation) * radius)
self.orientation = (self.orientation + turn) % (2.0 * np.pi)
self.x = cx + (np.sin(self.orientation) * radius)
self.y = cy - (np.cos(self.orientation) * radius)
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
def show(x_trajectory, y_trajectory, p_array=[], i_array=[], d_array=[], label = 'PID'):
n = len(x_trajectory)
# fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
# fig, ax1 = plt.subplots(figsize=(8, 4))
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(x_trajectory, np.zeros(n), 'pink', label='reference')
ax1.plot(x_trajectory, y_trajectory, 'black', label= label + ' controller')
ax1.set_xlabel('x') # Add an x-label to the axes.
ax1.set_ylabel('y') # Add a y-label to the axes.
ax1.set_title('Car-Position')
h, l = ax1.get_legend_handles_labels()
ax1.legend(h, l) # h 涓虹嚎鏉″璞″垪琛紝 l涓烘枃瀛楁弿杩板垪琛�
ax2 = fig.add_subplot(212)
if len(p_array) > 0:
ax2.plot(x_trajectory, p_array, color='r', label='p')
if len(i_array) > 0:
ax2.plot(x_trajectory, i_array, color='g', label='i')
if len(d_array) > 0:
ax2.plot(x_trajectory, d_array, color='b', label='d')
ax2.set_title('PID-Value')
h, l = ax2.get_legend_handles_labels()
ax2.legend(h, l) # h 涓虹嚎鏉″璞″垪琛紝 l涓烘枃瀛楁弿杩板垪琛�
plt.ylim((-1.5, 1.5))
plt.tight_layout()
plt.show()小车跟随颜色块
小车跟随黄色块进行运动,当然如果大家在地面上贴一根黄色的线,小车也可以跟随黄色线进行运动.
#! /usr/bin/env python
# encoding: utf-8
import rospy
from geometry_msgs.msg import Twist
import cv2 as cv
def shutdown():
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0
cmd_vel_Publisher.publish(twist)
print "stop car..."
if __name__ == '__main__':
rospy.init_node("yellow_follow")
# 当程序退出
rospy.on_shutdown(shutdown);
# ros控制的频率
rate = rospy.Rate(100)
# 定义publisher : cmd_vel
cmd_vel_Publisher = rospy.Publisher("/cmd_vel",Twist,queue_size=1)
capture = cv.VideoCapture(0)
print capture.isOpened()
ok,frame = capture.read()
lowerb = (23,43,46)
upperb = (34,255,255)
height,width = frame.shape[0:2]
screen_center = width / 2
offset = 50
while not rospy.is_shutdown():
# 将图像转成HSV颜色空间
hsv_frame = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
# 基于颜色的物品提取
mask = cv.inRange(hsv_frame,lowerb,upperb)
# 找出面积最大的区域
_,contours,_ = cv.findContours(mask,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)
maxArea = 0
maxIndex = 0
for i,c in enumerate(contours):
area = cv.contourArea(c)
if area > maxArea:
maxArea = area
maxIndex = i
# 绘制
cv.drawContours(frame,contours,maxIndex,(0,0,255),2)
# 获取外切矩形
x,y,w,h = cv.boundingRect(contours[maxIndex])
cv.rectangle(frame,(x,y),(x+w,y+h),(255,255,0),2)
# 获取外切矩形的中心像素点
center_x = int(x + w/2)
center_y = int(y + h/2)
cv.circle(frame,(center_x,center_y),5,(0,0,255),-1)
# 判断当前小车应该是左转还是右转还是直行
twist = Twist()
if center_x < screen_center - offset:
twist.linear.x = 0.05
twist.angular.z = 0.2
print "turn left"
elif center_x >= screen_center - offset and center_x <= screen_center + offset:
twist.linear.x = 0.1
twist.angular.z = 0.0
print "go"
elif center_x > screen_center + offset:
twist.linear.x = 0.05
twist.angular.z = -0.2
print "turn right"
else:
twist.linear.x = 0
twist.angular.z = 0
print "stop"
# 将速度信息发送出去
cmd_vel_Publisher.publish(twist)
cv.imshow("mask",mask)
cv.imshow("frame",frame)
cv.waitKey(1)
rate.sleep()
ok, frame = capture.read()pid
intergral = 0;
derivative = 0;
prev_error = 0;
pid = 0
def pid_control(curr,target):
global intergral,derivative,prev_error,pid
error = target - curr
intergral += error;
derivative = prev_error;
# pid 计算公式
kp = 0.2
ki = 0.0
kd = 0.02
pid = kp*error + ki * intergral + kd* derivative
prev_error = error;
print pid;
return pid视觉巡线
##基于颜色提取
#车道线表现出来是黄色的,所以我们可以利用我们前面基于HSV颜色范围的方式进行提取,最终提取出来的结果#
def fetch_yellow(img):
hsv_img = cv.cvtColor(img,cv.COLOR_BGR2HSV)
low = (25,46,46);
upper = (35,255,255)
binary = cv.inRange(hsv_img,low,upper)
if DEBUG:
cv.imshow("binary color",binary);
return binary
##截取ROI
def fetch_roi(img):
# 提取感兴趣的而区域
height,width = img.shape[0:2]
roi = img[int(height/2):height,0:width]
# 对提取的图像进行闭操作
kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))
roi = cv.morphologyEx(roi,cv.MORPH_CLOSE,kernel,iterations=3)
_, contours, _ = cv.findContours(roi,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)
dst = np.zeros_like(roi)
for i,c in enumerate(contours):
area = cv.contourArea(c)
if area>200:
cv.drawContours(dst,contours,i,255,-1)
if DEBUG:
cv.imshow("roi",dst)
return dst;
##提取边缘
def fetch_edge(img):
canny = cv.Canny(img,200,400)
if DEBUG:
cv.imshow("canny",canny)
return canny
##找出直线
#先提取了黄色,然后又提取感兴趣的区域,在感兴趣的区域中,我们提取了图像的边缘信息,在这一步我们就需要#来找出视线中可能的车道线了
def make_points(line_kb,height):
# 求右边的平均斜率和截距
lines_avg_kb = np.average(line_kb,axis=0);
# y = kx + b 绘制右边的直线
y1 = height*0.5;
x1 = (y1 - lines_avg_kb[1]) / lines_avg_kb[0];
y2 = 0
x2 = (y2 - lines_avg_kb[1]) / lines_avg_kb[0];
return [x1,y1+0.5*height,x2,y2+0.5*height]
def fetch_lines(img,height,width):
lines = cv.HoughLinesP(img,1,np.pi/180,10,minLineLength=40,maxLineGap=5);
left_kb = [];
right_kb = [];
if lines is None:
return [];
boundary = 1/3
left_region_boundary = width * (1 - boundary) # left lane line segment should be on left 2/3 of the screen
right_region_boundary = width * boundary
# 计算每一条线段的斜率和截距
for line in lines:
x1,y1,x2,y2 = line[0];
# 计算当前线段的斜率和截距 y = kx + b
if x1 == x2:
continue
params = np.polyfit((x1,x2),(y1,y2),1);
# 斜率
k = params[0];
# 截距
b = params[1];
if k < 0:
if x1 < left_region_boundary and x2 < left_region_boundary:
left_kb.append((k,b));
else:
if x1 > right_region_boundary and x2 > right_region_boundary:
right_kb.append((k,b));
lines = []
if len(left_kb)>0:
left_line = make_points(left_kb,480)
lines.append(left_line)
if len(right_kb)>0:
right_line = make_points(right_kb,height)
lines.append(right_line)
return lines;
#计算引导线
def calc_head_line(lines,width,height):
if len(lines) == 0:
return None;
x = width/2
y = height
if len(lines) == 1:
x1,y1,x2,y2 = lines[0]
# params = np.polyfit((x1,x2),(y1,y2),1);
# # 斜率
# k = params[0];
# # 截距
# b = params[1];
x_head = x2 + (x - x1)
y_head = 0.5*height;
# x_head = (y_head - b)/k
return [x,y,x_head,y_head]
else:
_,_,line1_x2,_ = lines[0]
_,_,line2_x2,_ = lines[1]
x_head = (line1_x2 + line2_x2)/2
y_head = 0.5*height
return [x,y,x_head,y_head]
#为了方便观察,显示所有线的函数
def show_lines(lines,frame,head_line=None):
if DEBUG:
try:
line_img = np.zeros_like(frame)
for x1,y1,x2,y2 in lines:
cv.line(line_img,(int(x1),int(y1)),(int(x2),int(y2)),(0,255,0),10)
if head_line:
x1,y1,x2,y2 = head_line
cv.line(line_img,(int(x1),int(y1)),(int(x2),int(y2)),(0,0,255),10)
dst = cv.addWeighted(line_img,0.8,frame,1,0)
cv.imshow("dst",dst)
except Exception as e:
print e
##计算偏差角度
#为了便于控制小车转向的角度大小,我们计算引导线偏离屏幕中央的度数. 若引导线偏离中线较大,则角速度要#增大,反之角速度要减小.这里其实也是为我们接下来视觉PID引导做铺垫
def calc_angle(x_offset,y_offset):
angle_to_mid_radian = math.atan(x_offset / y_offset)
return angle_to_mid_radian
##pid
#并且在这个过程中,偏离的角度越大,小车的角速度也越大,反之越小.这个正好可以用PID来进行控制,基于这样#的设想,我们用上位机视觉PID来控制小车,我们的目标就是要把偏离角度控制在0附近
intergral = 0;
derivative = 0;
prev_error = 0;
pid = 0
def pid_control(curr,target):
global intergral,derivative,prev_error,pid
error = target - curr
intergral += error;
derivative = prev_error;
# pid 计算公式
kp = 0.2
ki = 0.0
kd = 0.02
pid = kp*error + ki * intergral + kd* derivative
prev_error = error;
print pid;
return pid
if __name__ == '__main__':
# Give the node a name
rospy.init_node('heima_linefollow', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(shutdown)
# How fast will we check the odometry values?
rate = rospy.Rate(100)
# Publisher to control the robot's speed
cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
capture = cv.VideoCapture(0);
ok,frame = capture.read()
height,width = frame.shape[0:2]
# while ok:
while not rospy.is_shutdown():
ok,frame = capture.read()
# 提取黄色
binary = fetch_yellow(frame)
# 提取看兴趣的内容
roi = fetch_roi(binary)
# 提取边缘
canny = fetch_edge(roi)
# 提取线
lines = fetch_lines(canny,height,width)
head_line = calc_head_line(lines,width,height)
if head_line:
mid = width/2 ;
_,_,curr,_ = head_line
x_offset = curr - mid;
y_offset = height*0.5
angle = calc_angle(x_offset,y_offset)
if angle > 60 or angle < -60:
continue
show_lines(lines,frame,head_line)
value = pid_control(angle,0)
print "x_offset:{},{},{},angle={}".format(x_offset,mid,curr,angle)
# 假设恒定线速度为0.25
R = 0.1445/math.tan(value)
V = 0.10
W = V/R;
twist = Twist()
twist.linear.x = V
twist.angular.z = W
cmd_vel.publish(twist)
cv.imshow("frame",frame)
cv.waitKey(1)
rate.sleep()=========================================================================================================================
扫一扫
13万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
