5.1激光雷达跟随(冰达机器人)

news/2024/9/25 10:30:51/

5.1激光雷达跟随功能开发

5.1.1激光雷达跟随功能需求分析

在完成激光雷达跟踪之前,我们先来拆解一下功能。要实现跟随,首先需要确定跟随的目标,在这个例程中,我们使机器人根据离它最近的物体。周围物体的距离可以通过激光雷达测量结果得到,但是激光雷达是360°的,在跟随的过程中很容易被周围的物体带“跑偏”.例如我们想要让机器人跟随人走过一段走廊,在跟随的过程中有可能走廊的墙体会比人距离更近,机器人会试图以墙体作为跟随目标。针对这种情况我们可以从激光雷达的数据中找出一定角度范围内的最近物体,如图10-2-1所示。

在检测到需要跟随的目标后,根据目标距离机器人的距离和角度信息,计算机器人的运动速度,使机器人朝目标运动。

机器人跟随也不能无限制的接近跟随的物体,否则就撞上了,可以设置一个安全的跟随距离,当小于跟随距离后,机器人不再继续运动靠近跟随目标。

图5-1-1 跟随物体范围

图5-1-2 激光雷达数据类型

通过查看激光雷达消息类型的内容,如图5-1-2所示,ranges数组中存放的是距离信息,所以距离信息可以通过查找数组中最小值获取。数组元素和角度的对应关系可以通过angle_min和angle_increment计算得到。例如ranges中第113个元素为最小值,那么最小值所对应的角度就是angle_min+angle_increment*113。但是这个角度是相对于雷达的坐标系而言,机器人的坐标系和雷达坐标系可能还有旋转关系,我们需要对这个旋转关系做变换。NanoRobot上的激光雷达和机器人之间的坐标有着绕Z轴旋转180°的变换关系,我们需要对这个变换关系做处理。

5.1.2编写代码实现激光雷达跟随功

理解清楚设计目标后就可以设计代码了,笔者编写的代码是lidar_follow.py,代码如下

#!/usr/bin/python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import numpy as npclass LidarTracker:def __init__(self):rospy.init_node('lidar_follow', anonymous=False)self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.scancallback)self.cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)self.followDistance = rospy.get_param('~followDistance',0.5)self.minAngle = rospy.get_param('~minAngle',-0.5)self.maxAngle = rospy.get_param('~maxAngle',0.5)self.deltaDist = rospy.get_param('~deltaDist',0.2)self.winSize = rospy.get_param('~winSize',2)self.lidarInstallAngle = rospy.get_param('~lidarInstallAngle',0)self.max_velocity = 0.3self.min_velocity = 0.05rospy.spin()def scancallback(self,scan_data):if __name__ == '__main__':try:LidarTracker()except rospy.ROSInterruptException:pass

这段代码中,定的LidarTracker类中,定义了一个cmd_vel话题发布器cmd_pub和scan话题订阅器scanSubscriber,话题订阅器的回调函数为scancallback。并设置一系列参数用于指定跟随角度范围,跟随最近距离,滤波窗口大小,雷达安装角度等。跟随动作的实现是在scancallback函数中,代码如下

def scancallback(self,scan_data):# make scan data as a arrayranges = np.array(scan_data.ranges)# arrange data index ascending orderrangesIndex = np.argsort(ranges)tempMinDistance = float("inf")for i in rangesIndex:tempMinDistance = ranges[i]tempMinDistanceAngle = scan_data.angle_min + i*scan_data.angle_incrementtempMinDistanceAngle += self.lidarInstallAngleif tempMinDistanceAngle > 3.14159:tempMinDistanceAngle -= 3.14159*2elif tempMinDistanceAngle < -3.14159:tempMinDistanceAngle += 3.14159*2else:passwindowIndex = np.clip([i-self.winSize, i+self.winSize+1],0,len(ranges))window = ranges[windowIndex[0]:windowIndex[1]]# filt senser noise pointwith np.errstate(invalid='ignore'):if(np.any(abs(window - tempMinDistance) < self.deltaDist)):if tempMinDistanceAngle > self.minAngle and tempMinDistanceAngle < self.maxAngle:print (tempMinDistance,tempMinDistanceAngle)breakelse:passelse:pass#catches no scan, no minimum found, minimum is actually infif tempMinDistance > scan_data.range_max:rospy.logwarn('laser no object found')else:twist = Twist()if tempMinDistance > self.followDistance:twist.linear.x = tempMinDistance - self.followDistance#set a velocity threshold controlif twist.linear.x < self.min_velocity:twist.linear.x = self.min_velocityelif twist.linear.x > self.max_velocity:twist.linear.x = self.max_velocityelse:passelse:twist.linear.x = 0.0if abs(tempMinDistanceAngle) > 0.05:twist.angular.z = tempMinDistanceAngle*3self.cmd_pub.publish(twist)

在这段代码中,先将雷达消息中的ranges元素转换为一个numpy的数组ranges方便后续处理,然后获取数组的长度rangesIndex,将最小值设置为无穷大“inf”。

接下来就从ranges数组中搜索最小距离值tempMinDistance,计算出最小距离值相对雷达的角度tempMinDistanceAngle,再根据雷达-底盘之间的安装角度关系对角度值做变换,转换为相对机器人底盘的角度。

为了防止获取到的最小值是由雷达传感器噪声产生的,可以对最小值和数组中临近的值做比较,如果最小值没有通过滤波器,则丢弃数据,继续寻找新的最小值。如果通过了滤波器,则判断最近距离点是否在设定的跟随角度范围内,如果不在,则丢弃数据,继续寻找新的最小值。如果在则跳出搜索最小值的循环。

在结束搜索最小值的循环后,对最小值做判断,如果是有效的最小值,则开始计算机器人运动速度。如果最小值大于跟随距离,则根据当前到目标的距离计算机器人沿X轴运动的线速度。如果最小值小于跟随距离,则将X轴线速度设为0。Z轴的角速度是根据跟随目标点和机器人的角度tempMinDistanceAngle值计算。计算完成后通过cmd_pub话题发布器发布话题。

现在可以测试一下效果,启动机器人底盘、雷达节点,或者启动仿真器环境,然后运行雷达跟随节点。

rosrun bingda_application lidar_follow.py lidarInstallAngle:=3.14159

需要注意的是,NanoRobot硬件上雷达和底盘直接由180°的旋转关系,仿真器中的机器人模型是不存在的,所以使用机器人硬件运行例程时,需要将lidarInstallAngle参数设置为3.14159,使用仿真器则不需要设置,使用默认值0即可。

使用机器人硬件可以使用人站在机器人前方,通过运动来检验跟随效果。在仿真环境中可以通过添加或者移动仿真场景中的物体位置来检验跟随效果。例如笔者通过移动仿真场景中桌子的位置,使机器人使用跟随桌腿运动,如图所示。

移动仿真场景中物体位置验证跟随效果


http://www.ppmy.cn/news/1429087.html

相关文章

.NET StackExchange.Redis 操作redis

下面是一个简单的示例&#xff0c;展示了如何使用 C# 中的 StackExchange.Redis 库与 Redis 进行交互&#xff0c;包括字符串&#xff08;String&#xff09;、哈希&#xff08;Hash&#xff09;、列表&#xff08;List&#xff09;、集合&#xff08;Set&#xff09;和有序集合…

overflow(溢出)4个属性值,水平/垂直溢出,文字超出显示省略号的详解

你好&#xff0c;我是云桃桃。 一个希望帮助更多朋友快速入门 WEB 前端的程序媛。 云桃桃-大专生&#xff0c;一枚程序媛&#xff0c;感谢关注。回复 “前端基础题”&#xff0c;可免费获得前端基础 100 题汇总&#xff0c;回复 “前端工具”&#xff0c;可获取 Web 开发工具合…

【八股】Spring Boot

SpringBoot是如何实现自动装配的&#xff1f; 首先&#xff0c;SpringBoot的核心注解SpringBootApplication里面包含了三个注解&#xff0c;SpringBootConfigurationEnableAutoConfigurationComponentScan&#xff0c;其中EnableAutoConfiguration是实现自动装配的注解&#x…

深入理解计算机网络:从基本原理到实践应用

前言&#xff1a; 计算机网络是现代信息技术的基石&#xff0c;它连接了全球数以亿计的设备&#xff0c;使得信息传输和资源共享成为可能。本文将从计算机网络的基本原理出发&#xff0c;深入探讨其关键技术&#xff0c;并分享一些实践应用的经验。 一、计算机网络的基本原理 1…

【埋点探针】微信小程序SDK安装

一、下载微信小程序SDK埋点代码 选择Wechat&#xff0c;复制sdk代码 在项目根目录下&#xff0c;创建sdk文件&#xff0c;webfunny.event.js 二、在app.js文件中&#xff0c;引入埋点SDK代码 首先引入sdk代码 require("./webfunny.event.js")引入兼容代码&#x…

C# Solidworks二次开发:访问平面、曲面相关API详解

大家好&#xff0c;今天要介绍的是关于平面、曲面相关的API。 下面是相关的API: &#xff08;1&#xff09;第一个为ISurfacePlanarFeatureData&#xff0c;这个API的含义为允许访问平面表面特征&#xff0c;下面是官方的具体解释&#xff1a; 下面是官方使用的例子&#xff…

【Linux】NFS网络文件系统搭建

一、服务端配置 #软件包安装 [roothadoop01 ~]# yum install rpcbind nfs-utils.x86_64 -y [roothadoop01 ~]# mkdir /share#配置文件修改 #格式为 共享资源路径 [主机地址] [选项] # [roothadoop01 ~]# vi /etc/exports /share 192.168.10.0/24(rw,sync,no_root_squash) #…

测试大语言模型在嵌入式设备部署的可能性——模型TinyLlama-1.1B-Chat-v1.0

测试模型TinyLlama-1.1B-Chat-v1.0修改推理参数&#xff0c;观察参数变化与推理时间变化之间的关系。 本地环境&#xff1a; 处理器 Intel Core™ i5-8400 CPU 2.80GHz 2.80 GHz 机带 RAM 16.0 GB (15.9 GB 可用) 集显 Intel UHD Graphics 630 独显 NVIDIA GeForce GTX 1050 主…