概要

2024睿抗机器人比赛,物资运输使用宇树Unitree-GO1完成视觉巡线.

要求使用宇树四足机器人(go1、go2、A1等)并搭载机械臂，进行物资识别、物资抓取、物资运输、物资卸载、回到出发点

整体架构流程

将任务分为以下部分：

1.视觉巡线

2.二维码扫描（为是否进入环岛、物资选择等准备）

比赛地图

图1 比赛地图

技术名词解释

• nano13->该nano为头部下巴相机，主要进行巡线

• nano14->该nano对机械臂（ESP32主控），通过串口与nano14进行通讯

• nano15->该nano对应go1的腹部相机，主要负责识别Aruco二维码

• 树莓派->主要负责运动部分

• 图2 控制框架

技术细节

nano巡线方法:

盲道的颜色为黄色,其余地区颜色为白色,使用HSV颜色空间进行过滤.后续使用最长白列法进行盲道的中线的计算,

先写一个使用HSV的巡线函数:这是主函数,阈值分割的部分放在trans.h和line_follow.h文件中

#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <unistd.h>
#include <stdlib.h>
#include <UnitreeCameraSDK.hpp>
#include "viz.h"
#include "opencv.h"
#include "LinuxUdp.h"
#include <trans.h>
#include <line_follow_h.h>
#include <PID.h>
using namespace std;
using namespace cv;
using namespace LinuxUdp;


struct PACK
{
    int mode=0;//运动模式
    double v=0;//直线速度
    double yaw=0;//转向速度
};//这里是这个包是需要发送给树莓派的  



int main(int argc, char *argv[]){
    UdpClient udp_client("192.168.123.161", 8900);//初始化UDP通讯

    PACK pack;

    //下面部分是初始化并调用相机的程序  具体可以参考Unitree的camera_sdk
    int deviceNode = 0; ///< default 0 -> /dev/video0
    cv::Size frameSize(1856, 800); ///< default frame size 1856x800
    int fps = 30; ///< default camera fps: 30
    
    if(argc >= 2){
        deviceNode = std::atoi(argv[1]);
        if(argc >= 4){
            frameSize = cv::Size(std::atoi(argv[2]), std::atoi(argv[3]));
        }
        if(argc >=5)
            fps = std::atoi(argv[4]);
    }
    
    UnitreeCamera cam("stereo_camera_config.yaml"); ///< init camera by device node number
    if(!cam.isOpened())   ///< get camera open state
        exit(EXIT_FAILURE);
    
    cam.setRawFrameSize(frameSize); ///< set camera frame size
    cam.setRawFrameRate(fps);       ///< set camera camera fps
    cam.setRectFrameSize(cv::Size(frameSize.width >> 2, frameSize.height >> 1)); ///< set camera rectify frame size
    cam.startCapture(); ///< disable image h264 encoding and share memory sharing
    
    usleep(5000);
    
        //下面部分是为了方便调试  做的一个窗口可视化跳帧HSV的范围
        int hmin = 0, hmax = 179, smin = 0, smax = 255, vmin = 0, vmax = 255;
        namedWindow("window");
        createTrackbar("hmin", "window", &hmin, 179);
        createTrackbar("hmax", "window", &hmax, 179);
        createTrackbar("smin", "window", &smin, 255);
        createTrackbar("smax", "window", &smax, 255);
        createTrackbar("vmin", "window", &vmin, 255);
        createTrackbar("vmax", "window", &vmax, 255);




    int count0=0;
    while(cam.isOpened()){
        cv::Mat left,right;
        if(!cam.getRectStereoFrame(left,right)){ ///< get rectify left,right frame  
            usleep(1000);
            continue;
        }
	Mat temp,temp2,hsv,hsv2;
        cv::flip(left,left, -1);
	cv::flip(right,right,-1);
	cvtColor(left,temp,COLOR_BGR2HSV);
	cvtColor(right,temp2,COLOR_BGR2HSV);
	Scalar low(hmin,smin,vmin);
	Scalar high(hmax,smax,vmax);

	inRange(temp,low,high,hsv);
	inRange(temp2,low,high,hsv2);
	imshow("hsv2",hsv2);
	imshow("HSV",hsv);
       // Mat Thr=hsvimage(left);
       // Mat Thr=hsvimage2(left);
        int size=find_line(hsv,left).size;
        double error=find_line(hsv,left).err+find_line(hsv2,right).err;
	
        
        double kp = 0.03; // 比例系数  
        double ki = 0.0001; // 积分系数  
        double kd = 0.0001; // 微分系数  
        double dt = 0.01; // 时间间隔（秒）  
        PIDController pid(kp, ki, kd, dt);  

        // 假设的偏转值（偏差）  
        float setpoint = 0.0; // 期望的偏转值（例如，0表示直线行驶）  
        float measured_value = error; // 测量的偏转值  
  
       // 直线速度和转向速度输出  
        float  linear_speed = 0.0;  
        float  steering_speed = 0.0;  

        pid.Update(setpoint, measured_value , linear_speed, steering_speed);  
        
        pack.mode=1;
        pack.v=0.1f;//中速
        pack.yaw=steering_speed;
	if(pack.yaw>=0.6){
  	pack.yaw=0.6;
	}
	if(pack.yaw<=-0.6){
	pack.yaw=-0.6;
	}	


        //cv::imshow("Longlat_Rect", stereo);
        char key = cv::waitKey(10);
        if(key == 27) // press ESC key
           break;

        cout<<"mode="<<pack.mode<<"\t"<<"直线速度"<<pack.v<<"\t"<<"转向速度"<<pack.yaw<<endl;
        udp_client.UdpSend((void*)&pack, sizeof(PACK));
    }
    
    cam.stopCapture(); ///< stop camera capturing
    
    return 0;
}

图3 HSV调整窗口

PID部分代码

#include <unistd.h>
#include <stdlib.h>
#include "viz.h"
using namespace std;

class PIDController {  
public:  
    PIDController(double kp, double ki, double kd, double dt)  
        : kp_(kp), ki_(ki), kd_(kd), dt_(dt), integral_(0.0), prev_error_(0.0) {}  
  
    // 更新PID控制器，返回直线速度和转向速度  
    void Update(float setpoint, float measured_value, 
                float & linear_speed, float & steering_speed) {  
        float error = setpoint - measured_value;  
  
        // 积分项  
        integral_ += error * dt_;  
  
        // 微分项  
        double derivative = (error - prev_error_) / dt_;  
  
        // PID计算  
        double output = kp_ * error + ki_ * integral_ + kd_ * derivative;  
  
 
        linear_speed = output * kLinearSpeedScale; // kLinearSpeedScale 是直线速度的比例系数  
        steering_speed = output * kSteeringSpeedScale; // kSteeringSpeedScale 是转向速度的比例系数  
  
        // 更新之前的误差  
        prev_error_ = error;  
    }  
  
private:  
    double kp_; // 比例系数  
    double ki_; // 积分系数  
    double kd_; // 微分系数  
    double dt_; // 时间间隔  
    double integral_; // 积分项  
    double prev_error_; // 上一次的误差  
  

    static const float kLinearSpeedScale;  
    static const float kSteeringSpeedScale;  
};  
    const float PIDController::kLinearSpeedScale = 1; 
    const float PIDController::kSteeringSpeedScale = 0.05; 

PID控制的作用是使机器人能够准确沿盲道中央行走。以图示情况为例，红色中线位于白色盲道中线的左侧，表明机器人当前姿态偏左，存在一定的横向误差。为了消除该误差，使其回到盲道中央（即期望位置），需施加一个向右的转向速度。

我们可以通过PID控制器计算该转向速度。假设图像大小为100×100像素，期望的中央位置为横坐标50像素（红色标识线），而实际检测到的机器人位置为60像素（绿色标识线），则当前误差可计算为：
error = 当前值 - 期望值 = 60 - 50 = 10

PID控制器根据该误差计算输出量，其三个组成部分如下：

• 比例项（P）：将误差乘以比例系数 kp，即
  P = kp × error
  反映当前误差的直接影响。

• 积分项（I）：对误差进行积分，用于消除稳态误差。其计算为：
  integral += error × dt
  I = ki × integral
  其中 dt 为采样时间间隔。

• 微分项（D）：反映误差变化率，提高系统稳定性。其计算为：
  derivative = (error - prev_error) / dt
  D = kd × derivative

最终的控制输出为三项之和：
output = P + I + D = kp × error + ki × integral + kd × derivative

通过调节 kp、ki、kd 三个参数，可优化机器人的横向控制性能，使其平稳、准确地跟踪盲道中心线。

树莓派控制主函数

#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <math.h>
#include <iostream>
#include <unistd.h>
#include <string.h>
#include "LinuxUdp.h"


using namespace UNITREE_LEGGED_SDK;

LinuxUdp::UdpServer udp_server0(8900);      //line_follow
LinuxUdp::UdpServer udp_server1(8800);   //8800为接受ARUCO
struct PACK0
{
    int mode;
    double v;
    double yaw;
};

struct PACK1
{
    int mode;
    int count;//起停区
    int aruco_ID;  //倾倒区
    int a_count;
    
};


class Custom
{
public:
    Custom(uint8_t level): 
      safe(LeggedType::Go1), 
      udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
    {
        udp.InitCmdData(cmd);
    }
    void UDPRecv();
    void UDPSend();
    void RobotControl();

    Safety safe;
    UDP udp;
    HighCmd cmd = {0};
    HighState state = {0};
    int motiontime = 0;
    int time=0;


    float dt = 0.002;     // 0.001~0.01
       
};

void Custom::UDPRecv()
{
    udp.Recv();
}

void Custom::UDPSend()
{  
    udp.Send();
}



void Custom::RobotControl() 
{
    PACK0* pack0;
    pack0 = new PACK0;    //nano  13
    PACK1* pack1;
    pack1 = new PACK1;    //nano  15
    udp_server0.UdpRecv((void*)pack0);
    udp_server1.UdpRecv((void*)pack1);
    motiontime += 2;
    udp.GetRecv(state);
    cmd.mode = 0;      // 0:idle, default stand      1:forced stand     2:walk continuously
    cmd.gaitType = 0;
    cmd.speedLevel = 0;
    cmd.footRaiseHeight = 0;
    cmd.bodyHeight = 0;
    cmd.euler[0]  = 0;
    cmd.euler[1] = 0;
    cmd.euler[2] = 0;
    cmd.velocity[0] = 0.0f;
    cmd.velocity[1] = 0.0f;
    cmd.yawSpeed = 0.0f;
    cmd.reserve = 0;


    if(time==0){
            cmd.mode = 2;
            cmd.velocity[0] = 0.0f;
            cmd.velocity[1] = 0.25f;
            time=1;
    }



    
    if(pack1->mode==-2){//起停区

        
            cmd.mode = 2;
            cmd.velocity[0] = 0.0f;
            cmd.velocity[1] = -0.25f;  //给一个V=0.25的横向速度  //暂停2S；
        
    }

    if(pack1->mode==-1){//倾倒区
        if(pack1->aruco_ID==1||pack1->aruco_ID==2){
            if (pack1->a_count==1)
            {
                cmd.mode=2;
                cmd.velocity[0]=0.1f;
                cmd.yawSpeed=-0.2f;
            }
            if (pack1->a_count==2||pack1->a_count==3)
            {
                cmd.mode=2;
                cmd.velocity[0]=0.2f;
                cmd.yawSpeed=0.0f;
            }

        }
        else
        {
            if (pack1->a_count==1)
            {
                cmd.mode=2;
                cmd.velocity[0]=0.1f;
                cmd.yawSpeed=0.0f;
            }
            if (pack1->a_count==2)
            {
                cmd.mode=2;
                cmd.velocity[0]=0.1f;
                cmd.yawSpeed=0.0f;
            }


            if (pack1->a_count==3)
            {
                cmd.mode=2;
                cmd.velocity[0]=0.0f;
                cmd.yawSpeed=0.3925f;  //要等2s
            }
        }
        
        
    }
 /*  if(pack1->mode==-3)
{
    cmd.mode=0;
    cmd.yawSpeed=0;
    cmd.velocity[0]=0;
    cmd.velocity[1]=0;
    cout<<"pack1.mode=-3"<<endl;
}*/

    if(pack1->mode==0&&time==5){

        cmd.mode=2;
        cmd.velocity[0]=pack0->v;
        cmd.yawSpeed=pack0->yaw;

    }

    udp.SetSend(cmd); 
    delete pack0;
    delete pack1;

    if(time==1){
        this_thread::sleep_for(chrono::seconds(2));
        cmd.mode=0;
        cmd.yawSpeed=0;
        cmd.velocity[0]=0;
        cmd.velocity[1]=0;
        time=5;
    }

    if(pack1->mode==-2){
        
            this_thread::sleep_for(chrono::seconds(60));
        
    }
    if(pack1->mode==-1){
        
            this_thread::sleep_for(chrono::seconds(2));
        
    }



}

int main(void) 
{
    std::cout << "Communication level is set to HIGH-level." << std::endl
              << "WARNING: Make sure the robot is standing on the ground." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();

    Custom custom(HIGHLEVEL);
    // InitEnvironment();
    LoopFunc loop_control("control_loop", custom.dt,    boost::bind(&Custom::RobotControl, &custom));
    LoopFunc loop_udpSend("udp_send",     custom.dt, 3, boost::bind(&Custom::UDPSend,      &custom));
    LoopFunc loop_udpRecv("udp_recv",     custom.dt, 3, boost::bind(&Custom::UDPRecv,      &custom));

    loop_udpSend.start();
    loop_udpRecv.start();
    loop_control.start();

    while(1)
    {
        sleep(10);
    };

    return 0; 
}

小结

后续的有空再写吧  很久以前干的了 有点忘记了