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

server/2024/9/23 14:23:33/

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/server/6182.html

相关文章

python基础语法+爬虫+图像处理+NumpyPandas数据处理(12天速成,第7天上-爬虫Scrapy)

爬虫&#xff08;Scrapy&#xff09;:写一段程序代码&#xff08;网络访问&#xff09;&#xff0c;自动获取网页&#xff08;网络&#xff09;上的数据服务端语言&#xff1a;网络编程&#xff0c;都可以作为爬虫java c c python 等均可写爬虫程序js不是一个典型的服务端程序&…

线上研讨会 | 新一代数字化技术赋能机器人及智能产线行业高质量发展

随着智能制造的快速推进&#xff0c;制造业转型升级到了关键阶段。在此背景下&#xff0c;全球各主要经济体积极围绕以工业机器人为主的智能制造业展开激烈竞争。作为打造从“制造”到“智造”的关键基础设施&#xff0c;工业机器人扮演者愈加重要的角色。我国作为工业机器人的…

【2024】深度学习配置环境常见报错,持续更新中....

No such file or directory: ‘:/usr/local/cuda-10.1:/usr/local/cuda-10.1/bin/nvcc‘: 确保CUDA正常后&#xff0c;直接运行以下代码&#xff1a; export CUDA_HOME/usr/local/cuda详细原因参照&#xff0c;安装mmcv-full时报错&#xff1a;Could not build wheels for mm…

TensorFlow 的基本概念和使用场景

TensorFlow 的基本概念和使用场景 TensorFlow 是一个开源的机器学习框架&#xff0c;由 Google 的 Google Brain 团队开发。它广泛用于数据科学、机器学习、深度学习和其他相关领域。以下是一篇关于 TensorFlow 的基本概念和使用场景的概述文章。 1. TensorFlow 简介 Tensor…

在PostgreSQL中如何实现分区表以提高查询效率和管理大型表?

文章目录 解决方案1. 确定分区键2. 创建分区表3. 数据插入与查询4. 维护与管理 示例代码1. 创建父表和子表2. 插入数据3. 查询数据 总结 随着数据量的增长&#xff0c;单一的大型表可能会遇到性能瓶颈和管理难题。PostgreSQL的分区表功能允许我们将一个大型表分割成多个较小的、…

XiaodiSec day034 Learn Note 小迪渗透学习笔记

XiaodiSec day034 Learn Note 小迪渗透学习笔记 记录得比较凌乱&#xff0c;不尽详细 day34 黑盒审计和白盒审计 与 cms 相关 .net java php 代码审计 开始 黑盒&#xff1a;找文件上传的功能 个人用户中心是否存在文件上传功能后台管理系统是否存在文件上传功能字典目录…

JavaScript-4.正则表达式、BOM

正则表达式 正则表达式包含在"/"&#xff0c;"/"中 开始与结束 ^ 字符串的开始 $ 字符串的结束 例&#xff1a; "^The"&#xff1a;表示所有以"The"开始的字符串&#xff08;"There"、"The cat"等&#x…

儿童悬吊训练系统都有哪些锻炼形式

儿童悬吊训练作为一种创新的儿童训练方法&#xff0c;旨在结合儿童的身心发展特点&#xff0c;通过儿童悬吊系统的巧妙运用&#xff0c;达到提升儿童各项功能能力的目的。治疗师在运用悬吊系统对儿童进行训练时&#xff0c;必须紧密结合儿童的发育规律与自身功能水平&#xff0…