人体跟随
-
上位机使用的是工控机，下位机主控为stm32
-
上位机使用通过Intel的Realsense D435i来读取相关相关信息，使用串口和下位机进行通信
-
通过深度相机来对人体进行判别，根据相关的距离向下位机发出不同的信息，来控制小车是否跟进
-
上位机主要分为两个部分，一个部分是用来检测，另外一个部分是用来和下位机进行通信
-
上层的检测是利用OpenCV来调用D435i的图像信息并利用Intel官方提供的Caffe模型进行检测（百度网盘链接放在了文末）
-
根据检测到的不同信息，来向下位机发送相关信息，控制小车的运行
-
主要检测的源码如下：
#include <opencv2/dnn.hpp> #include <librealsense2/rs.hpp> #include "../cv-helpers.hpp" #include "send.h" const size_t inWidth = 300; const size_t inHeight = 300; const float WHRatio = inWidth / (float)inHeight; const float inScaleFactor = 0.007843f; const float meanVal = 127.5; const char* classNames[] = { "background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor" }; int main(int argc, char** argv) try { using namespace cv; using namespace cv::dnn; using namespace rs2; /**********/ WZSerialPort w; /**********/ Net net = readNetFromCaffe("MobileNetSSD_deploy.prototxt", "MobileNetSSD_deploy.caffemodel"); // Start streaming from Intel RealSense Camera pipeline pipe; auto config = pipe.start(); auto profile = config.get_stream(RS2_STREAM_COLOR) .as<video_stream_profile>(); rs2::align align_to(RS2_STREAM_COLOR); Size cropSize; if (profile.width() / (float)profile.height() > WHRatio) { cropSize = Size(static_cast<int>(profile.height() * WHRatio), profile.height()); } else { cropSize = Size(profile.width(), static_cast<int>(profile.width() / WHRatio)); } Rect crop(Point((profile.width() - cropSize.width) / 2, (profile.height() - cropSize.height) / 2), cropSize); const auto window_name = "Display Image"; namedWindow(window_name, WINDOW_AUTOSIZE); while (getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0) { // Wait for the next set of frames auto data = pipe.wait_for_frames(); // Make sure the frames are spatially aligned data = align_to.process(data); auto color_frame = data.get_color_frame(); auto depth_frame = data.get_depth_frame(); // If we only received new depth frame, // but the color did not update, continue static int last_frame_number = 0; if (color_frame.get_frame_number() == last_frame_number) continue; last_frame_number = static_cast<int>(color_frame.get_frame_number()); // Convert RealSense frame to OpenCV matrix: auto color_mat = frame_to_mat(color_frame); auto depth_mat = depth_frame_to_meters(depth_frame); Mat inputBlob = blobFromImage(color_mat, inScaleFactor, Size(inWidth, inHeight), meanVal, false); //Convert Mat to batch of images net.setInput(inputBlob, "data"); //set the network input Mat detection = net.forward("detection_out"); //compute output Mat detectionMat(detection.size[2], detection.size[3], CV_32F, detection.ptr<float>()); // Crop both color and depth frames color_mat = color_mat(crop); depth_mat = depth_mat(crop); float confidenceThreshold = 0.8f; for (int i = 0; i < detectionMat.rows; i++) { float confidence = detectionMat.at<float>(i, 2); if (confidence > confidenceThreshold) { size_t objectClass = (size_t)(detectionMat.at<float>(i, 1)); int xLeftBottom = static_cast<int>(detectionMat.at<float>(i, 3) * color_mat.cols); int yLeftBottom = static_cast<int>(detectionMat.at<float>(i, 4) * color_mat.rows); int xRightTop = static_cast<int>(detectionMat.at<float>(i, 5) * color_mat.cols); int yRightTop = static_cast<int>(detectionMat.at<float>(i, 6) * color_mat.rows); Rect object((int)xLeftBottom, (int)yLeftBottom, (int)(xRightTop - xLeftBottom), (int)(yRightTop - yLeftBottom)); object = object & Rect(0, 0, depth_mat.cols, depth_mat.rows); // Calculate mean depth inside the detection region // This is a very naive way to estimate objects depth // but it is intended to demonstrate how one might // use depth data in general Scalar m = mean(depth_mat(object)); std::ostringstream ss; ss << classNames[objectClass] << " "; ss << std::setprecision(2) << m[0] << " meters away"; String conf(ss.str()); /******************************/ if (m[0] <= 1.0 && objectClass == 15) { string i = "1"; if (w.open("COM3", 19200, 0, 8, 1)) { cout << "人体距离摄像机小于1m，前进" << endl; w.send(i); } else cout << "打开失败" << endl; w.close(); } else { string i = "0"; if (w.open("COM3", 19200, 0, 8, 1)) { w.send(i); } else cout << "打开失败" << endl; w.close(); } /*******************************/ rectangle(color_mat, object, Scalar(0, 255, 0)); int baseLine = 0; Size labelSize = getTextSize(ss.str(), FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); auto center = (object.br() + object.tl())*0.5; center.x = center.x - labelSize.width / 2; rectangle(color_mat, Rect(Point(center.x, center.y - labelSize.height), Size(labelSize.width, labelSize.height + baseLine)), Scalar(255, 255, 255), FILLED); putText(color_mat, ss.str(), center, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 0)); } } imshow(window_name, color_mat); if (waitKey(1) >= 0) break; } return EXIT_SUCCESS; } catch (const rs2::error & e) { std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl; return EXIT_FAILURE; } catch (const std::exception& e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; } -
串口发送程序如下：
#include "send.h" #include <stdio.h> #include <string.h> #include <WinSock2.h> #include <windows.h> #include<iostream> using namespace std; WZSerialPort::WZSerialPort() { } WZSerialPort::~WZSerialPort() { } bool WZSerialPort::open(const char* portname, int baudrate, char parity, char databit, char stopbit, char synchronizeflag) { this->synchronizeflag = synchronizeflag; HANDLE hCom = NULL; if (this->synchronizeflag) { //同步方式 hCom = CreateFileA(portname, //串口名 GENERIC_READ | GENERIC_WRITE, //支持读写 0, //独占方式，串口不支持共享 NULL,//安全属性指针，默认值为NULL OPEN_EXISTING, //打开现有的串口文件 0, //0：同步方式，FILE_FLAG_OVERLAPPED：异步方式 NULL);//用于复制文件句柄，默认值为NULL，对串口而言该参数必须置为NULL } else { //异步方式 hCom = CreateFileA(portname, //串口名 GENERIC_READ | GENERIC_WRITE, //支持读写 0, //独占方式，串口不支持共享 NULL,//安全属性指针，默认值为NULL OPEN_EXISTING, //打开现有的串口文件 FILE_FLAG_OVERLAPPED, //0：同步方式，FILE_FLAG_OVERLAPPED：异步方式 NULL);//用于复制文件句柄，默认值为NULL，对串口而言该参数必须置为NULL } if (hCom == (HANDLE)-1) { return false; } //配置缓冲区大小 if (!SetupComm(hCom, 1024, 1024)) { return false; } // 配置参数 DCB p; memset(&p, 0, sizeof(p)); p.DCBlength = sizeof(p); p.BaudRate = baudrate; // 波特率 p.ByteSize = databit; // 数据位 switch (parity) //校验位 { case 0: p.Parity = NOPARITY; //无校验 break; case 1: p.Parity = ODDPARITY; //奇校验 break; case 2: p.Parity = EVENPARITY; //偶校验 break; case 3: p.Parity = MARKPARITY; //标记校验 break; } switch (stopbit) //停止位 { case 1: p.StopBits = ONESTOPBIT; //1位停止位 break; case 2: p.StopBits = TWOSTOPBITS; //2位停止位 break; case 3: p.StopBits = ONE5STOPBITS; //1.5位停止位 break; } if (!SetCommState(hCom, &p)) { // 设置参数失败 return false; } //超时处理,单位：毫秒 //总超时＝时间系数×读或写的字符数＋时间常量 COMMTIMEOUTS TimeOuts; TimeOuts.ReadIntervalTimeout = 1000; //读间隔超时 TimeOuts.ReadTotalTimeoutMultiplier = 500; //读时间系数 TimeOuts.ReadTotalTimeoutConstant = 5000; //读时间常量 TimeOuts.WriteTotalTimeoutMultiplier = 500; // 写时间系数 TimeOuts.WriteTotalTimeoutConstant = 2000; //写时间常量 SetCommTimeouts(hCom, &TimeOuts); PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);//清空串口缓冲区 memcpy(pHandle, &hCom, sizeof(hCom));// 保存句柄 return true; } void WZSerialPort::close() { HANDLE hCom = *(HANDLE*)pHandle; CloseHandle(hCom); } int WZSerialPort::send(string dat) { HANDLE hCom = *(HANDLE*)pHandle; if (this->synchronizeflag) { // 同步方式 DWORD dwBytesWrite = dat.length(); //成功写入的数据字节数 BOOL bWriteStat = WriteFile(hCom, //串口句柄 (char*)dat.c_str(), //数据首地址 dwBytesWrite, //要发送的数据字节数 &dwBytesWrite, //DWORD*，用来接收返回成功发送的数据字节数 NULL); //NULL为同步发送，OVERLAPPED*为异步发送 if (!bWriteStat) { return 0; } return dwBytesWrite; } else { //异步方式 DWORD dwBytesWrite = dat.length(); //成功写入的数据字节数 DWORD dwErrorFlags; //错误标志 COMSTAT comStat; //通讯状态 OVERLAPPED m_osWrite; //异步输入输出结构体 //创建一个用于OVERLAPPED的事件处理，不会真正用到，但系统要求这么做 memset(&m_osWrite, 0, sizeof(m_osWrite)); m_osWrite.hEvent = CreateEvent(NULL, TRUE, FALSE, L"WriteEvent"); ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误，获得设备当前状态 BOOL bWriteStat = WriteFile(hCom, //串口句柄 (char*)dat.c_str(), //数据首地址 dwBytesWrite, //要发送的数据字节数 &dwBytesWrite, //DWORD*，用来接收返回成功发送的数据字节数 &m_osWrite); //NULL为同步发送，OVERLAPPED*为异步发送 if (!bWriteStat) { if (GetLastError() == ERROR_IO_PENDING) //如果串口正在写入 { WaitForSingleObject(m_osWrite.hEvent, 1000); //等待写入事件1秒钟 } else { ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误 CloseHandle(m_osWrite.hEvent); //关闭并释放hEvent内存 return 0; } } return dwBytesWrite; } } string WZSerialPort::receive() { HANDLE hCom = *(HANDLE*)pHandle; string rec_str = ""; char buf[1024]; if (this->synchronizeflag) { //同步方式 DWORD wCount = 1024; //成功读取的数据字节数 BOOL bReadStat = ReadFile(hCom, //串口句柄 buf, //数据首地址 wCount, //要读取的数据最大字节数 &wCount, //DWORD*,用来接收返回成功读取的数据字节数 NULL); //NULL为同步发送，OVERLAPPED*为异步发送 for (int i = 0; i < 1024; i++) { if (buf[i] != -52) rec_str += buf[i]; else break; } return rec_str; } else { //异步方式 DWORD wCount = 1024; //成功读取的数据字节数 DWORD dwErrorFlags; //错误标志 COMSTAT comStat; //通讯状态 OVERLAPPED m_osRead; //异步输入输出结构体 //创建一个用于OVERLAPPED的事件处理，不会真正用到，但系统要求这么做 memset(&m_osRead, 0, sizeof(m_osRead)); m_osRead.hEvent = CreateEvent(NULL, TRUE, FALSE, L"ReadEvent"); ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误，获得设备当前状态 if (!comStat.cbInQue)return 0; //如果输入缓冲区字节数为0，则返回false //std::cout << comStat.cbInQue << std::endl; BOOL bReadStat = ReadFile(hCom, //串口句柄 buf, //数据首地址 wCount, //要读取的数据最大字节数 &wCount, //DWORD*,用来接收返回成功读取的数据字节数 &m_osRead); //NULL为同步发送，OVERLAPPED*为异步发送 if (!bReadStat) { if (GetLastError() == ERROR_IO_PENDING) //如果串口正在读取中 { //GetOverlappedResult函数的最后一个参数设为TRUE //函数会一直等待，直到读操作完成或由于错误而返回 GetOverlappedResult(hCom, &m_osRead, &wCount, TRUE); } else { ClearCommError(hCom, &dwErrorFlags, &comStat); //清除通讯错误 CloseHandle(m_osRead.hEvent); //关闭并释放hEvent的内存 return 0; } } for (int i = 0;i < 1024; i++) { if (buf[i] != -52) rec_str += buf[i]; else break; } return rec_str; } } -
底层主要是由stm32来进行小车的控制,使用stm32的定时计数器1的的PWM进行互补的输出，控制电机的运转
-
底层的相关程序如下：
#include "Motor.h" /***********************************/ // TIM1->CCRx等于99,电机全速反转 // TIM1->CCRx等于0，电机全速正转 // TIM1->CCRx等于49,电机停止转动 /***********************************/ #define kp1 0.82 #define ki1 0.45 #define kd1 0.00 #define kp2 0.80 #define ki2 0.45 #define kd2 0.00 #define kp3 0.75 #define ki3 0.55 #define kd3 0.0005 #define kp4 0.75 #define ki4 0.45 #define kd4 0.00 float TargetSpeed1=0.1; float TargetSpeed2=0.1; float TargetSpeed3=0.1; float TargetSpeed4=0.1; float PID_CallBack1(float Speed,float *error) { float Error = TargetSpeed1 - Speed; static float Error_last1 = 0,Error_prev1 = 0; float Speed_add=0; *error = Error; Speed_add = (float)((float)kp1*(Error - Error_last1) + (float)ki1*Error + (float)kd1*(Error-2.0f*Error_last1+Error_prev1)); Error_prev1 = Error_last1;// 保存上上次误差 Error_last1 = Error; // 保存上次偏差 return Speed_add; } float PID_CallBack2(float Speed,float *error) { float Error = TargetSpeed2 - Speed; static float Error_last2 = 0,Error_prev2 = 0; float Speed_add=0; *error = Error; Speed_add = (float)((float)kp2*(Error - Error_last2) + (float)ki2*Error + (float)kd2*(Error-2.0f*Error_last2+Error_prev2)); Error_prev2 = Error_last2;// 保存上上次误差 Error_last2 = Error; // 保存上次偏差 return Speed_add; } float PID_CallBack3(float Speed,float *error) { float Error = TargetSpeed3 - Speed; static float Error_last3 = 0,Error_prev3 = 0; float Speed_add=0; *error = Error; Speed_add = (float)((float)kp3*(Error - Error_last3) + (float)ki3*Error + (float)kd3*(Error-2.0f*Error_last3+Error_prev3)); Error_prev3 = Error_last3;// 保存上上次误差 Error_last3 = Error; // 保存上次偏差 return Speed_add; } float PID_CallBack4(float Speed,float *error) { float Error = TargetSpeed4 - Speed; static float Error_last4 = 0,Error_prev4 = 0; float Speed_add=0; *error = Error; Speed_add = (float)((float)kp4*(Error - Error_last4) + (float)ki4*Error + (float)kd4*(Error-2.0f*Error_last4+Error_prev4)); Error_prev4 = Error_last4;// 保存上上次误差 Error_last4 = Error; // 保存上次偏差 return Speed_add; } void Set_Speed(float speed,uint8_t i) { float PWM=0; if(i==1) { if(speed>0) { PWM=48.0f-((float)(speed/0.2)*48.0f); TIM1->CCR1=(uint16_t)PWM; } else { if(speed<0) { PWM=51.0f+((float)(speed/0.2)*48.0f); TIM1->CCR1=(uint16_t)PWM; } else TIM1->CCR1=49; } } if(i==2) { if(speed>0) { PWM=48.0f-((float)(speed/0.2)*48.0f); TIM1->CCR2=(uint16_t)PWM; } else { if(speed<0) { PWM=51.0f+((float)(speed/0.2)*48.0f); TIM1->CCR2=(uint16_t)PWM; } else TIM1->CCR2=49; } } if(i==3) { if(speed>0) { PWM=48.0f-((float)(speed/0.2)*48.0f); TIM1->CCR3=(uint16_t)PWM; } else { if(speed<0) { PWM=51.0f+((float)(speed/0.2)*48.0f); TIM1->CCR3=(uint16_t)PWM; } else TIM1->CCR3=49; } } if(i==4) { if(speed>0) { PWM=48.0f-((float)(speed/0.2)*48.0f); TIM8->CCR4=TIM1->CCR4=(uint16_t)PWM; } else { if(speed<0) { PWM=51.0f+((float)(speed/0.2)*48.0f); TIM8->CCR4=TIM1->CCR4=(uint16_t)PWM; } else TIM8->CCR4=TIM1->CCR4=49; } } } -
底层使用中断来接收上位机的信息
void USART1_IRQHandler(void) { /* USER CODE BEGIN USART1_IRQn 0 */ /* USER CODE END USART1_IRQn 0 */ //HAL_UART_IRQHandler(&huart1); /* USER CODE BEGIN USART1_IRQn 1 */ if(__HAL_UART_GET_FLAG( &huart1, UART_FLAG_RXNE ) != RESET) { Receive_data=( uint16_t)READ_REG(huart1.Instance->DR); } /* USER CODE END USART1_IRQn 1 */ } -
主函数根据接收到的相关信息，进行控制
while(1) { if(Receive_data=='0') { Set_Speed(0,1); Set_Speed(0,2); Set_Speed(0,3); Set_Speed(0,4); } else { Set_Speed(0.15,1); Set_Speed(0.15,2); Set_Speed(0.15,3); Set_Speed(0.15,4); } }
Caffe模型链接提取码：8888
扫一扫
1679
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
