小型雙輪差速底盤視覺循跡功能的實現(xiàn)

1. 任務描述

在機器人小車上搭載攝像頭，攝像頭采集圖像信息并通過WiFi將信息傳遞給PC端，然后PC端使用OpenCV對攝像頭讀取到的視頻進行灰度化、高斯濾波、腐蝕、膨脹等處理，使圖像分為黑白兩色。PC端進行圖像信息處理并將處理結(jié)果傳遞為下位機，下位機接收上位機處理的圖像信息結(jié)果后便會控制小車相應運動，小車運動包含前進、左轉(zhuǎn)、右轉(zhuǎn)、停止。

?2. 電子硬件

在這個示例中，我們采用了以下硬件，請大家參考：

3. 功能實現(xiàn)

視覺小車巡黑線工作原理：

(1) 攝像頭采集圖像信息；

(2) 通過 WiFi 將圖像信息傳遞給 PC 端（VS2015 配置的 OpenCV 環(huán)境）；

(3) 在 PC 端使用 OpenCV 對攝像頭讀取到的視頻進行灰度化、高斯濾波、腐蝕、膨脹等處理，使圖像分為黑白兩色，采用 RGB 顏色模型作為黑白顏色判斷；

(4) 將圖像對稱分成左右兩半，分別判斷左、右計算檢測在顯示的攝像范圍內(nèi)的黑色像素區(qū)域所占比例=黑色像素范圍/顯示的攝像范圍；

(5) 比較兩側(cè)黑色像素區(qū)域所占比例大小確定前進方向，如果左側(cè)比例大于右側(cè)，則小車左偏離，進行右轉(zhuǎn)；

(6) PC端進行圖像信息處理，將處理結(jié)果傳遞為下位機，下位機控制小車進行相應的運動；

3.1硬件連接

接線說明：

① 將2510通信轉(zhuǎn)接板連接到擴展板的擴展塢上面；

② 用3根母對母杜邦線將2510通信轉(zhuǎn)接板與WiFi路由器連接起來，GND-GND、RX-RX、TX-TX；

③ 找到1根USB線，一端連接到2510通信轉(zhuǎn)接板接口上，另一端連接到WiFi路由器USB接口上；

④ 將攝像頭線連接到WiFi路由器接口上。

3.2示例程序

編程環(huán)境：Arduino 1.8.19

① 下位機例程：

下位機接收上位機處理的圖像信息結(jié)果控制小車相應運動，小車運動包含前進、左轉(zhuǎn)、右轉(zhuǎn)、停止。

參考例程代碼（car.ino）如下：【詳細例程源代碼詳見https://www.robotway.com/h-col-113.html】

/*------------------------------------------------------------------------------------

  版權(quán)說明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 機器譜 2023-02-02 https://www.robotway.com/

  -----------------------------------------------------------------------------------

/*

  wift car:

  2019/08/19:

  JN

  left: 9, 5;

  right: 10, 6;  

*/



const String FORWARD = "F";

const String BACK = "B";

const String LEFT = "L";

const String RIGHT = "R";

const String STOP = "S";



int speed_left = 41;

int speed_right = 41;



void setup() {

  Serial.begin(9600);

  pinMode(5, OUTPUT);

  pinMode(6, OUTPUT);

 

 

 

  pinMode(9, OUTPUT);

  pinMode(10, OUTPUT);

  Stop();

  delay(1000);

}



void loop() {

  String data = SerialRead();

 

  //if(data != ""){   

    if(data == FORWARD)

      Forward();

    else if(data == BACK)

      Back();

    else if(data == LEFT)

      Left();

    else if(data == RIGHT)

      Right();   

    else if(data == STOP)

      Stop();

// }

}



String SerialRead(){

  String str;

  while(Serial.available()){

    str += char(Serial.read());

  }

  return str;

}



void Forward(){

  analogWrite(9, speed_left);

  analogWrite(5, 0);

  analogWrite(6, 0);

  analogWrite(10, speed_right);

}



void Back(){

  analogWrite(9, 0);

  analogWrite(5, speed_left);

  analogWrite(6, speed_right);

  analogWrite(10, 0);

}



void Left(){

  analogWrite(9, 0);

  analogWrite(5, speed_left);

  analogWrite(6, 0);

  analogWrite(10, speed_right);

}



void Right(){

  analogWrite(9, speed_left);

  analogWrite(5, 0);

  analogWrite(6, speed_right);

  analogWrite(10, 0);

}



void Stop(){

  analogWrite(9, speed_left);

  analogWrite(5, speed_left);

  analogWrite(6, speed_right);

  analogWrite(10,speed_right);

}

② 上位機例程：

上位機（Visual Studio 2015.net下配置OpenCV環(huán)境）進行圖像信息處理。下面提供一個參考例程（MainWindow.xaml.cs），大家可嘗試根據(jù)實驗效果改寫。

/*******************************************************************************************

版權(quán)說明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 機器譜 2023-02-02 https://www.robotway.com/

---------------------------------------------------------------------------------------

using System;

using System.IO;

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.Media.Animation;

using System.Threading;

using OpenCvSharp;

using System.Drawing;

using System.Drawing.Imaging;

using System.Net;

using System.Net.Sockets;



namespace Tracking_Car

{

    /// 

    /// Tracking_Car

    /// 

    public partial class MainWindow : System.Windows.Window

    {

        //定義視頻，控制地址以及控制端口變量

        static string CameraIp = "http://192.168.8.1:8083/?action=stream";

        static string ControlIp = "192.168.8.1";

        static string Port = "2001";



        //定義上位機發(fā)送的控制命令變量

        //定義命令變量

        string CMD_FORWARD = "", CMD_TURN_LEFT = "", CMD_TURN_RIGHT = "", CMD_STOP = "";



        /*

         * 指針角度對應各顏色

         * 25 -> 紅色

         * 90 -> 綠色

         * 150 -> 藍色

         */

        int ANGLE_LEFT = 0;

        int ANGLE_GO = 0;

        int ANGLE_RIGHT = 0;



        //黑色像素在左右兩側(cè)所占比例

        double numOfleft = 0.0;

        double numOfright = 0.0;



        //創(chuàng)建視頻圖像實例

        VideoCapture capture = new VideoCapture(CameraIp); //圖像大?。簩挾?X 長度 = 160 X 120;

        Mat frame = new Mat();   //存儲視頻每一幀圖像像素

        Mat result = new Mat(); //存儲二值化圖像



        static byte[] kernelValues = { 0, 1, 0, 1, 1, 1, 0, 1, 0 }; // cross (+)

        Mat kernel = new Mat(3, 3, MatType.CV_8UC1, kernelValues);



        //圖像中心線坐標

        int x1, y1, x2, y2;

        //窗口面積

        float area;



        //視頻顯示切換變量

        Boolean isChange = false;

        //循跡開始開關變量

        Boolean isBegin = false;



        public MainWindow()

        {

            InitializeComponent();

        }



        private void Window_Loaded(object sender, RoutedEventArgs e)

        {

            Assignment();

        }



        //變量賦值函數(shù)

        private void Assignment()

        {

            ANGLE_LEFT = 25;

            ANGLE_GO = 90;

            ANGLE_RIGHT = 150;



            rateLeft.Height = 10;

            rateRight.Height = 10;



            x1 = 80;

            y1 = 0;

            x2 = x1;

            y2 = 120;

            area = 160 * 120 / 2;



            CMD_FORWARD = "F";

            CMD_TURN_LEFT = "L";

            CMD_TURN_RIGHT = "R";

            CMD_STOP = "S";

        }



        /// 

        /// MatToBitmap(Mat image)

        /// 

        public static Bitmap MatToBitmap(Mat image)

        {

            return OpenCvSharp.Extensions.BitmapConverter.ToBitmap(image);

        }



        /// 

        /// BitmapToBitmapImage(System.Drawing.Bitmap bitmap)

        /// 

        public static BitmapImage BitmapToBitmapImage(Bitmap bitmap)

        {

            using (MemoryStream stream = new MemoryStream())

            {

                bitmap.Save(stream, ImageFormat.Png); //格式選Bmp時，不帶透明度



                stream.Position = 0;

                BitmapImage result = new BitmapImage();

                result.BeginInit();

                // According to MSDN, "The default OnDemand cache option retains access to the stream until the image is needed."

                // Force the bitmap to load right now so we can dispose the stream.

                result.CacheOption = BitmapCacheOption.OnLoad;

                result.StreamSource = stream;

                result.EndInit();

                result.Freeze();

                return result;

            }

        }



        //顏色指示動畫函數(shù)

        int angelCurrent = 0;

        private void ColorIndicate(int where)

        {

            RotateTransform rt = new RotateTransform();

            rt.CenterX = 130;

            rt.CenterY = 200;



            this.indicatorPin.RenderTransform = rt;



            double timeAnimation = Math.Abs(angelCurrent - where) * 5;

            DoubleAnimation da = new DoubleAnimation(angelCurrent, where, new Duration(TimeSpan.FromMilliseconds(timeAnimation)));

            da.AccelerationRatio = 0.8;

            rt.BeginAnimation(RotateTransform.AngleProperty, da);



            switch (where)

            {

                case 25:

                    dirDisplay.Content = "左轉(zhuǎn)";

                    break;

                case 90:

                    dirDisplay.Content = "前進";

                    break;

                case 150:

                    dirDisplay.Content = "右轉(zhuǎn)";

                    break;

                default:

                    dirDisplay.Content = "方向指示";

                    break;

            }



            angelCurrent = where;

        }



        //檢測函數(shù)

        private void ColorDetect() {

            //將攝像頭RGB圖像轉(zhuǎn)化為灰度圖，便于后續(xù)算法處理

            Mat gray = frame.CvtColor(ColorConversionCodes.BGR2GRAY);

            //進行高斯濾波

            Mat binary = gray.Threshold(0, 255, ThresholdTypes.Otsu | ThresholdTypes.Binary);

            //閉運算，先膨脹后腐蝕，消除小型黑洞

            Cv2.Dilate(binary, binary, null);

            Cv2.Erode(binary, binary, kernel);



            result = binary.Clone();

            result.Line(new OpenCvSharp.Point(x1, y1), new OpenCvSharp.Point(x2, y2), new Scalar(255, 255, 255), 1);



            float rateOfleft = 0, rateOfRight = 0;

            var indexer = result.GetGenericIndexer();

            for (int i = 0; i < result.Rows; i++) {

                for (int j = 0; j < result.Cols; j++) {

                    int B = indexer[i, j][0];

                    int G = indexer[i, j][1];

                    int R = indexer[i, j][2];



                    if (B == 0 && G == 0 && R == 0) {

                        if (j <= x1) {

                            numOfleft++;

                        }

                        else

                        {

                            numOfright++;

                        }

                    }

                }

            }



            rateOfleft = (float)(numOfleft) / area * 100;

            rateOfRight = (float)(numOfright) / area * 100;

            rateLeft.Height = rateOfleft;

            rateRight.Height = rateOfRight;

            numOfleft = 0;

            numOfright = 0;

        }



        //命令發(fā)送函數(shù)

        void SendData(string data)

        {

            try

            {

                IPAddress ips = IPAddress.Parse(ControlIp.ToString());//("192.168.8.1");

                IPEndPoint ipe = new IPEndPoint(ips, Convert.ToInt32(Port.ToString()));//把ip和端口轉(zhuǎn)化為IPEndPoint實例

                Socket c = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);//創(chuàng)建一個Socket



                c.Connect(ipe);//連接到服務器



                byte[] bs = Encoding.ASCII.GetBytes(data);

                c.Send(bs, bs.Length, 0);//發(fā)送測試信息

                c.Close();

            }

            catch (Exception e)

            {

                MessageBox.Show(e.Message);

            }

        }



        //方向指示更新及命令發(fā)送

        private void CommandSend() {

            double l = rateLeft.Height;

            double r = rateRight.Height;



            if (isBegin) {

                if (Math.Abs(l - r) < 20)   //兩側(cè)黑色軌跡基本相同，前進

                {

                    ColorIndicate(ANGLE_GO);

                    SendData(CMD_FORWARD);

                }

                else if ((l - r) < -50)     //左側(cè)黑色軌跡小于右側(cè)，右轉(zhuǎn)

                {

                    ColorIndicate(ANGLE_RIGHT);

                    SendData(CMD_TURN_RIGHT);



                }

                else if ((l - r) > 50)      //右側(cè)黑色軌跡小于左側(cè)，左轉(zhuǎn)

                {

                    ColorIndicate(ANGLE_LEFT);

                    SendData(CMD_TURN_LEFT);

                }

            }

        }



        //視頻顯示函數(shù)

        private void ThreadCapShow()

        {



            while (true)

            {

                try

                {

                    capture.Read(frame); // same as cvQueryFrame

                    if (frame.Empty())

                        break;



                    this.Dispatcher.Invoke(

                        new Action(

                            delegate

                            {

                                if (isChange)

                                {

                                    //檢測圖像左右兩側(cè)黑色像素所占的比例，并顯示圖像

                                    ColorDetect();

                                    originImage.Source = BitmapToBitmapImage(MatToBitmap(result));

                                    CommandSend();

                                    result = null;

                                }

                                else

                                {

                                    originImage.Source = BitmapToBitmapImage(MatToBitmap(frame));

                                }

                            }

                            ));

                    //Cv2.WaitKey(100);

                    //bitimg = null;

                }

                catch { }

            }

        }



        //加載視頻

        private void loadBtn_Click(object sender, RoutedEventArgs e)

        {

            if (originImage.Source != null) return;

            Thread m_thread = new Thread(ThreadCapShow);

            m_thread.IsBackground = true;

            m_thread.Start();

        }



        //切換視頻顯示，顯示檢測結(jié)果

        private void changeBtn_Click(object sender, RoutedEventArgs e)

        {

            if (!isChange)

            {

                isChange = true;

                changeBtn.Content = "返回";

            }

            else

            {

                isChange = false;

                changeBtn.Content = "切換";

                //指針角度歸零

                ColorIndicate(0);

                rateLeft.Height = 10;

                rateRight.Height = 10;

                result = null;

            }

        }


        //循跡開始

        private void bgeinBtn_Click(object sender, RoutedEventArgs e)

        {

            isBegin = true;

        }

        //循跡停止

        private void stopBtn_Click(object sender, RoutedEventArgs e)

        {

            isBegin = false;

            SendData(CMD_STOP);

        }

    }

}

4. 資料內(nèi)容

?①視覺循跡-程序源代碼
?②視覺循跡-樣機3D文件

審核編輯黃宇

?