• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

智能机器人基于激光雷达和摄像头实现360°跟踪移动人物

人工智能 否极 1173次浏览 0个评论

目前,网上大部分源代码有的是基于摄像头检测人物,有的是基于激光雷达信号检测人物,两种方式各有利弊。摄像头检测准确度高,但有死角;激光雷达检测准确度偏低,但能做到360°无死角,基于这个考虑,笔者编写一个算法,集成两者优长,做到智能机器人360°高精度跟踪移动人物。

准备工作:

1.启动小车驱动,这个因不同小车而异,主要是能控制小车前进、后退、旋转,再次不做阐述;

2.启动opencv_app,这个程序及用法在ROS官网上有源代码,主要实现脸部检测,再次不做阐述.http://wiki.ros.org/opencv_apps;

3.启动find_moving_objects,这个程序及用法也同样在ROS官网上有源代码,主要实现基于激光雷达检测移动物体,可下载研究,再次不做阐述。http://wiki.ros.org/find_moving_objects;

4.启动主程序源代码,请结合注释阅读代码。

#!/usr/bin/env python
#-*- coding: utf-8 -*-
import rospy
import math
import time
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Imu,Image,LaserScan
from geometry_msgs.msg import Twist, Vector3
# from xtark_tutorials.msg import Num
from find_moving_objects.msg import MovingObject as MovingObjectMsg
from find_moving_objects.msg import MovingObjectArray as MovingObjectArrayMsg
from opencv_apps.msg import FaceArrayStamped as FaceArrayStampedMsg


class findPerson:
    def __init__(self):
        self.active_move = False
        self.active_face = False
        self.active_go = False
        # self.active_scan = True
        self.PI = 3.1415926
        # 设置雷达检测移动对象接收器
        self.objectSubscriber=rospy.Subscriber("moving_objects", MovingObjectArrayMsg, self.objectCallback)
        # 设置陀螺仪信号接收器
        self.imuSubscriber=rospy.Subscriber("imu", Imu, self.imuCallback)
        # 设置脸部检测接收器
        self.facesSubscriber = rospy.Subscriber("face_detection/faces", FaceArrayStampedMsg, self.facesCallback)
        # self.imageSubscriber = rospy.Subscriber("face_detection/image", Image, self.imageCallback)
        # 设置移动信号发射器
        self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size =3)
        # 设置雷达检测信号接收器
        self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.scanCallback)
        # 角度控制PID
        self.PID_controller_angle = simplePID(320, 0.003,0.003, 0)
        # 位移控制PID
        self.PID_controller_distance = simplePID(1.6, 0.5,0.04, 0)
        # 程序退出时,小车停止
        rospy.on_shutdown(self.controllerLoss)
    
    # 用于检测周边最小距离避免碰撞;用于检测前方最小距离向脸部位置前进(保持在1.6米),当active_go信号为真时,小车开始移动
    def scanCallback(self,scan_data):
        ranges = np.array(scan_data.ranges)
        rospy.loginfo("safe distance:{}".format(np.amin(np.hstack((ranges[0:90],ranges[-90:]))) ))
        if np.amin(np.hstack((ranges[0:90],ranges[-90:]))) < 0.3:
            self.controllerLoss()
        # rospy.loginfo("scan_data:{}".format(ranges))
        distance_array = np.hstack((ranges[0:5],ranges[-5:]))
            # rospy.loginfo("distance_array:{}".format(distance_array))
            # distance=np.mean(distance_array)
        distance=np.amin(distance_array)
        rospy.loginfo("min_distance:{}".format(distance))
        if self.active_face == True and self.active_go == True:
            go_distance = self.PID_controller_distance.update(distance)
            rospy.loginfo("pid_distance:{}".format(go_distance))
            velocity = Twist()
            velocity.linear = Vector3(-go_distance,0,0)
            velocity.angular= Vector3(0., 0.,0.)
            self.cmdVelPublisher.publish(velocity)

    def imageCallback(self,data):
        rospy.loginfo(data.width)
        rospy.sleep(1)

# 检测脸部信息,使小车移动让脸部在屏幕中心位置,当脸部在屏幕中心位置时,给出active_go信号,让车可以向前移动
    def facesCallback(self,data):
        # rospy.loginfo(data.faces)
        if data.faces != []:
            self.active_face = True
            face_location = data.faces[0].face.x
            rospy.loginfo("face_location:{}".format(face_location))
            if face_location >= 290 and face_location <= 350:
                self.active_go = True
            else:
                self.active_go = False
            angularSpeed = self.PID_controller_angle.update(face_location)
            angularSpeed = np.clip(angularSpeed, -0.5,0.5)
            velocity = Twist()
            velocity.linear = Vector3(0,0,0)
            velocity.angular= Vector3(0., 0.,angularSpeed)
            if self.active_move == False:
                self.cmdVelPublisher.publish(velocity)
        else:
            self.active_face = False

# 标记小车是否在移动
    def imuCallback(self,data):
        move_angular_z=data.angular_velocity.z
        # rospy.loginfo("move_angular_z:%s",self.move_angular_z)
        if abs(move_angular_z) > 0.01:
            self.active_move = True
            # rospy.sleep(1)
        else:
            self.active_move = False

# 雷达检测反馈周边疑似移动物体,经过seen_width和value.speed两轮筛选,使小车转向筛选后的可疑角度,结合摄像头脸部检测,最终确定目标位置
    def objectCallback(self,data):
        # rospy.loginfo(data)
        # rospy.loginfo(rospy.get_caller_id()+"I heard %d",data.num)
        # data_objects=np.array(data.objects)
        data_objects = np.array(data.objects)
        a = np.array([])
        if self.active_move == False:
            for i in range(data_objects.size):
                value=data_objects[i]
                if value.seen_width > 0.2 or value.speed < 0.3:
                    a=np.append(a,i)
            data_objects_new_array=np.delete(data_objects, a)
            if data_objects_new_array.size == 1 and self.active_face == False:
                data_objects_new=data_objects_new_array[0]
                object_angle=(data_objects_new.angle_begin+data_objects_new.angle_end)/2
                rospy.loginfo("I use lidir find object_angle:%s",object_angle)
                if object_angle <= 0:
                    goal = self.PI+object_angle
                else:
                    goal=object_angle - self.PI
                velocity = Twist()
                velocity.linear = Vector3(0,0,0)
                velocity.angular= Vector3(0., 0.,goal)
                self.cmdVelPublisher.publish(velocity)
                # rospy.loginfo("I start turn to :%s",goal)
                # rospy.sleep(3)
                # if self.active_face == True:
                #     rospy.loginfo("I find people:%s",goal)
                # else:
                #     rospy.loginfo("I can't find people")

# 避免小车碰撞,紧急刹车程序
    def stopMoving(self):
        velocity = Twist()
        velocity.linear = Vector3(0.,0.,0.)
        velocity.angular= Vector3(0.,0.,0.)
        self.cmdVelPublisher.publish(velocity)
    def controllerLoss(self):
        self.stopMoving()
        rospy.loginfo('lost connection')
    


class simplePID:
	'''very simple discrete PID controller'''
	def __init__(self, target, P, I, D):
        # rospy.loginfo('PID initialised with P:{}, I:{}, D:{}'.format(P,I,D))
		self.Kp		=np.array(P)
		self.Ki		=np.array(I)
		self.Kd		=np.array(D)
		self.setPoint   =np.array(target)
		
		self.last_error=0
		self.integrator = 0
		self.integrator_max = float('inf')
		self.timeOfLastCall = None 
		
		
	def update(self, current_value):
		current_value=np.array(current_value)
		if(self.timeOfLastCall is None):
			# the PID was called for the first time. we don't know the deltaT yet
			# no controll signal is applied
			self.timeOfLastCall = time.clock()
			return np.zeros(np.size(current_value))

		
		error = self.setPoint - current_value
		P =  error
		# rospy.loginfo(P)
		currentTime = time.clock()
		deltaT      = (currentTime-self.timeOfLastCall)

		# integral of the error is current error * time since last update
		self.integrator = self.integrator + (error*deltaT)
		I = self.integrator
		
		# derivative is difference in error / time since last update
		D = (error-self.last_error)/deltaT
		
		self.last_error = error
		self.timeOfLastCall = currentTime
		
		# return controll signal
		return self.Kp*P + self.Ki*I + self.Kd*D    
         
if __name__ == '__main__':
	rospy.init_node('find_person', anonymous=True)
	findperson = findPerson()
	try:
		rospy.spin()
	except rospy.ROSInterruptException:
		print('exception')

实现效果:https://v.douyin.com/eFvRMQk/


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明智能机器人基于激光雷达和摄像头实现360°跟踪移动人物
喜欢 (0)

您必须 登录 才能发表评论!

加载中……