双轮云台小车实现追踪彩色目标功能

机器谱 · 发表于 2023-5-25 09:35:56

本帖最后由机器谱于 2023-5-25 09:35 编辑

1. 功能说明
在R216a样机上安装一个摄像头，本文示例将实现双轮小车通过二自由度云台自主寻找彩色目标的功能。

2. 结构说明
R216a样机主要是由一个双轮小车【https://www.robotway.com/h-col-113.html】和一个 2自由度云台【https://www.robotway.com/h-col-126.html】组合而成。

3. 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
电池	7.4V锂电池
通信	2510通信转接板
通信	WiFi路由器
其它	摄像头
其它	配置OpenCV的Visual Studio 2015.net环境的计算机一台

电路连接说明：
   ① 将2510通信转接板连接到Bigfish扩展板的扩展坞上面；
   ② 用3根母对母杜邦线将2510通信转接板与WiFi路由器连接起来：GND-GND、RX-RX、TX-TX；
   ③ 找到1根USB线，一端连接到2510通信转接板接口上，另一端连接到WiFi路由器USB接口上；
   ④ 将摄像头线连接到WiFi路由器接口上。

4. 功能实现
   实现思路：实现双轮云台小车追踪蓝色小球。
4.1 工作原理
   ① 摄像头采集图像信息；
   ② 通过WiFi将信息传递给PC端（VS2015配置的OpenCV环境）；
   ③ 使用OpenCV的目标颜色跟踪camshift算法取得目标物体的中心点坐标；
   ④ 采用九宫格方式对摄像显示图像进行分割；
   ⑤ 确定目标物体在显示图像的所处九宫格位置；
   ⑥ 如果目标图像超出九宫格位置的中心，调整摄像头矫正偏移使目标物体在屏幕中心位置；
   ⑦ 调整摄像头需要上位机通过WiFi给下位机发送矫正指令，下位机需要接收信号，并且让安装了摄像头的智能小车做出相应的矫正动作，如果丢失目标，智能车上的云台会转动以寻找目标。

4.2 示例程序
   编程环境：Arduino 1.8.19
   ① 下位机例程
   将参考例程（example.ino）下载到主控板，打开路由器，待路由器完成启动后，将路由器与主控板的TX、RX串口连接，同时将PC连接至路由器WIFI网络。下位机接收上位机处理的图像信息结果控制云台相应运动，云台跟随目标物体运动。

/*------------------------------------------------------------------------------------
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2023-04-24 https://www.robotway.com/
------------------------------*/
/*
car_exapmle
2018/06/07
---------------------
Motor:
left: 9,5
right: 10,6
Servo:
bottom:4
top:7
*/
#include <Servo.h>
#define BOTTOM_SERVO_MIN 60
#define BOTTOM_SERVO_MAX 150 //175
#define BOTTOM_SERVO_MIDDLE 85
#define TOP_SERVO_MIN 85
#define TOP_SERVO_MAX 175
const String CMD_LEFT = "L";
const String CMD_RIGHT = "R";
const String CMD_STOP = "S";
const String CMD_FORWARD = "F";
const String CMD_LOST = "N";
Servo myServo[2];
int port0 = 4;
int port1 = 7;
int servo_value[2] = {85, 105};
int servo_move_angle = 1;
void setup() {
Serial.begin(9600);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
ServoInit();
delay(1000);
}
void loop() {
String data = SerialRead();
if(data == CMD_STOP)
{
Stop();
}
else if(data == CMD_FORWARD)
{
Forward();
}
else if(data == CMD_LEFT)
{
Left();
delay(60);
Stop();
}
else if(data == CMD_RIGHT)
{
Right();
delay(60);
Stop();
}
else if(data == CMD_LOST)
{
FindObject();
}
}
String SerialRead()
{
String str = "";
while(Serial.available())
{
str += char(Serial.read());
}
return str;
}
int Angle2Pwm(int n)
{
return map(n, 0, 180, 500, 2500);
}
void ServoInit()
{
myServo[0].attach(port0);
myServo[1].attach(port1);
for(int i=0;i<2;i++)
{
myServo[i].write(Angle2Pwm(servo_value[i]));
}
}
void FindObject()
{
const int times = 30;
int dir = 0;
for(;;)
{
String data = SerialRead();
if(data != CMD_LOST && data != "")
{
if(servo_value[0] <= BOTTOM_SERVO_MIDDLE)
{
dir = 1; // turn left
for(int i=0;i<abs(servo_value[0] - BOTTOM_SERVO_MIDDLE);i++)
{
servo_value[0] += 1;
myServo[0].write(Angle2Pwm(servo_value[0]));
delay(times);
}
}
else if(servo_value[0] > BOTTOM_SERVO_MIDDLE)
{
dir = 2; // turn right
for(int i=0;i<abs(servo_value[0] - BOTTOM_SERVO_MIDDLE);i++)
{
servo_value[0] -= 1;
myServo[0].write(Angle2Pwm(servo_value[0]));
delay(times);
}
}
break;
}
if(servo_value[0] <= BOTTOM_SERVO_MIN)
{
servo_move_angle = 1;
servo_value[0] = BOTTOM_SERVO_MIN;
}
else if(servo_value[0] >= BOTTOM_SERVO_MAX)
{
servo_move_angle = -1;
servo_value[0] = BOTTOM_SERVO_MAX;
}
servo_value[0] += servo_move_angle;
myServo[0].write(Angle2Pwm(servo_value[0]));
delay(times);
}
if(dir == 1)
{
Left();
delay(500);
Stop();
}
else if(dir == 2)
{
Right();
delay(500);
Stop();
}
}
void Forward()
{
analogWrite(5,120);
analogWrite(6,0);
analogWrite(9,120);
analogWrite(10,0);
}
void Left()
{
analogWrite(5,105);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,105);
}
void Right()
{
analogWrite(5,0);
analogWrite(6,105);
analogWrite(9,105);
analogWrite(10,0);
}
void Stop()
{
digitalWrite(5,HIGH);
digitalWrite(6,HIGH);
digitalWrite(9,HIGH);
digitalWrite(10,HIGH);
}

复制代码

② 上位机例程
下面提供一个可以实现双轮智能小车追踪蓝色小球的参考例程（MainWindow.xaml.cs），大家可参考演示视频完成该实验。

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Forms;
using System.Runtime.InteropServices;
using System.Threading;
using System.Net;
using System.Net.Sockets;
namespace Project
{
/// <summary>
/// 云台跟踪
/// </summary>
public partial class MainWindow : Window
{
//导入 camshift.dll 动态链接库
[DllImport("Camshift_DLL.dll", CharSet = CharSet.Ansi)]
public static extern void Camshift([MarshalAs(UnmanagedType.LPStr)]string ip_address, //图片或视频地址
ref int xpos, //检测框中心X坐标
ref int ypos, //检测框中心Y坐标
ref int td); //检测区域对角线长度
//定义窗口大小
int cap_w = 320, cap_h = 240;
//跟踪物体中心 x, y 坐标值
int x = 0, y = 0, d = 0;
//定义命令变量
string CMD_FORWARD = "", CMD_TURN_LEFT = "", CMD_TURN_RIGHT = "", CMD_STOP = "", CMD_LOST = "";
//结构体
public struct Boundaries
{
public int x_left;
public int x_right;
public int y_up;
public int y_down;
public int d_min;
public int d_max;
}
Boundaries boundaries = new Boundaries();
public MainWindow()
{
InitializeComponent();
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
GetIni();
SetPosition();
CmdInit();
StructInit();
}
//变量初始化
private void CmdInit()
{
CMD_FORWARD = "F";
CMD_TURN_LEFT = "L";
CMD_TURN_RIGHT = "R";
CMD_STOP = "S";
CMD_LOST = "N";
}
//结构体初始化
private void StructInit()
{
boundaries.x_left = 120;
boundaries.x_right = 240;
boundaries.y_up = 80;
boundaries.y_down = 160;
boundaries.d_min = 50;
boundaries.d_max = 150;
}
//获取ini配置文件信息
private void GetIni()
{
ini_RW.FileName = System.Windows.Forms.Application.StartupPath + "\\Config.ini";
this.videoAddress.Text = ini_RW.ReadIni("VideoUrl", "videourl", "");
this.ipAddress.Text = ini_RW.ReadIni("ControlUrl", "controlUrl", "");
this.portBox.Text = ini_RW.ReadIni("ControlPort", "controlPort", "");
}
//修改配置
private void setBtn_Click(object sender, RoutedEventArgs e)
{
ini_RW.WriteIni("VideoUrl", "videourl", this.videoAddress.Text);
ini_RW.WriteIni("ControlUrl", "controlUrl", this.ipAddress.Text);
ini_RW.WriteIni("ControlPort", "controlPort", this.portBox.Text);
System.Windows.MessageBox.Show("配置成功！请重启程序以使配置生效。",
"配置信息", MessageBoxButton.OK,
MessageBoxImage.Information);
//this.Close();
}
//命令发送函数
void SendData(string data)
{
try
{
IPAddress ips = IPAddress.Parse(ipAddress.Text.ToString());//("192.168.8.1");
IPEndPoint ipe = new IPEndPoint(ips, Convert.ToInt32(portBox.Text.ToString()));//把ip和端口转化为IPEndPoint实例
Socket c = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);//创建一个Socket
c.Connect(ipe);//连接到服务器
byte[] bs = Encoding.ASCII.GetBytes(data);
c.Send(bs, bs.Length, 0);//发送测试信息
c.Close();
}
catch (Exception e)
{
System.Windows.Forms.MessageBox.Show(e.Message);
}
}
//跟踪物体位置界限判断
private void LineDetect(int _x, int _y, int _d)
{
try
{
//判断转动方向
if (_x > 0 && _x <= boundaries.x_left)
{
SendData(CMD_TURN_LEFT);
}
else if (x > boundaries.x_right && x < cap_w)
{
SendData(CMD_TURN_RIGHT);
}
//判断是否前进
else if (_d > boundaries.d_min && _d < boundaries.d_max)
{
SendData(CMD_FORWARD);
}
else if ((_x > boundaries.x_left && _x < boundaries.x_right)&&
(_d >= 0 && _d <= boundaries.d_min ))
{
SendData(CMD_LOST);
}
else
{
SendData(CMD_STOP);
}
}
catch { };
}
//物体位置初始化
private void SetPosition()
{
var color = new SolidColorBrush((System.Windows.Media.Color)System.Windows.Media.ColorConverter.ConvertFromString("#FFACAAAA"));
objEllipse.Height = 30;
objEllipse.Width = 30;
objEllipse.Fill = color;
var left_distance = (cap_w - objEllipse.Width) / 2;
var top_distance = (cap_h - objEllipse.Height) / 2;
Canvas.SetLeft(objEllipse, left_distance);
Canvas.SetTop(objEllipse, top_distance);
}
//跟踪物体位置更新函数
private void PositionUpdate(int x, int y, int d)
{
LineDetect(x, y, d);
Canvas.SetLeft(objEllipse, x);
Canvas.SetTop(objEllipse, y);
posLable.Content = x + " , " + y + " , " + d;
}
//线程函数
private void ThreadCapShow()
{
try
{
while (true)
{
this.Dispatcher.Invoke(
new Action(
delegate
{
string ip = this.videoAddress.Text;
Camshift(ip, ref x, ref y, ref d);
PositionUpdate(x - 15, y - 15, d);
}
));
}
}
catch { };
}
//打开跟踪窗口
private void openBtn_Click(object sender, RoutedEventArgs e)
{
try
{
Thread m_thread = new Thread(ThreadCapShow);
m_thread.IsBackground = true;
m_thread.Start();
}
catch { };
}
}
}

复制代码

5. 资料下载
资料内容：
①追踪彩色目标-例程源代码
②追踪彩色目标-样机3D文件
资料下载地址：https://www.robotway.com/h-col-214.html

想了解更多机器人开源项目资料请关注机器谱网站 https://www.robotway.com

帐号		自动登录	找回密码
密码			注册