部署在Go1身上的手势识别

最近花了两个礼拜的时间完成初步完成了Unitree Go1 机器狗的手势识别，花点时间写篇博客记录一下。（ps：这篇博客不含部署环境的任何细节，碰了太多壁想不起来了😅😅😅）

这个项目的目的是要让做出相对应的手势，让机器狗做出相应的动作。

1.了解Go1的内部结构

看图可以了解到Go1 身上一共有5块主板

Nano1，Nano2 ，Nan3（或者选择nx），Raspberry（树莓派）,MCU（主控版）

由于实验室选购的是nx版，所以下面的教程都是采用nx版的视角来进行。

2.跨板块传输

        Unitree公司给出的demo是用udp的传输协议，我也就按着这个模式写了下去，去网上搜udp的相关代码很多都是一大串，但其实有用的只有3步

        1.初始化服务器

        2.绑定地址

        3.接收，发送

        这是接受端的代码（在真正用的适合需要循环，一直接受消息）

import socket
import cv2

socket_server = socket(AF_INET, SOCK_DGRAM) #初始化服务器

ip= ('192.168.108.15', 9999)#第一个参数是设定地址，第二个则是你选择的端口号，端口号自定义，具体能                    
                            #开多大我也没试过，不过接受端和发送端一定要一样

socket_server.bind(ip)#绑定服务器

recv_data = socket_server.recvfrom(65535)#这个65535是指一次从网络中获取多少bytes

#这个recv_data是一个数组recv_data[0]是从网络中获取信息
                      #recv_data[1]是ip，端口号

image = np.frombuffer(recv_data[0],np.unit8)#暂存一下，其实我不确定有没有用这一步

image = cv2.imdecode(image,cv2.IMREAD_COLOR)#变成图片

        这是发送端代码（发送端的代码不能停）

import socket
import cv2

socket_server = socket(AF_INET, SOCK_DGRAM) #初始化服务器

ip= ('192.168.108.43', 9999)

socket_server.bind(ip)

cap = cv2.VideoCapture(1)#获得头部相机

while True:
    ret,frame = cap.read()
    
    frame_data =cv2.imencode('.jpg',frame)[1].tobytes()#把图片解析成为udp可以传输的形式

    udp_socket.sendto(frame_data,ip)

如果有错误欢迎私信我，看到第一时间改正

3.相机标定

        如下图，左边是标定完后的，右边是标定前的。虽然依旧存在的一定程度的拉伸，但是已经大大提高了识别的精确度。

标定代码    

import socket
import cv2
import numpy as np

udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
local_addr = ('192.168.123.15', 10000)
udp_socket.bind(local_addr)


def udp_save():
    recv_data = udp_socket.recvfrom(65535) 

    image = np.frombuffer(recv_data[0],np.uint8)
                
    image = cv2.imdecode(image,cv2.IMREAD_COLOR)

    return image


def calibrate():

    # 找棋盘格角点
    # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # 阈值

    #棋盘格模板规格
    w = 11   # 8 - 1  
    h = 8   # 6  - 1

    # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
    objp = np.zeros((w*h,3), np.float32)
    objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
    objp = objp*16  # 18.1 mm 焦距 要根据自己相机修改

    # 储存棋盘格角点的世界坐标和图像坐标对
    objpoints = [] # 在世界坐标系中的三维点
    imgpoints = [] # 在图像平面的二维点
    i=0
    while True:
        img =udp_save()
        cv2.imshow('img', img)
        cv2.waitKey(1)  
        # 获取画面中心点
        #获取图像的长宽
        h1, w1 = img.shape[0], img.shape[1]
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
        u, v = img.shape[:2]
        # 找到棋盘格角点
        ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
        # 如果找到足够点对，将其存储起来
        if ret == True:
            print("i:", i)
            i = i+1
            # 在原角点的基础上寻找亚像素角点
            cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
            #追加进入世界三维点和平面二维点中
            objpoints.append(objp)
            imgpoints.append(corners)
            #将角点在图像上显示
            cv2.drawChessboardCorners(img, (w,h), corners, ret)
            cv2.imshow('findCorners',img)
            cv2.waitKey(200)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break


    print('正在计算')
    #标定
    ret, mtx, dist, rvecs, tvecs = \
        cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)


    print("ret:",ret  )
    print("mtx:\n",mtx)      # 内参数矩阵
    print("dist畸变值:\n",dist   )   # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
    print("rvecs旋转（向量）外参:\n",rvecs)   # 旋转向量  # 外参数
    print("tvecs平移（向量）外参:\n",tvecs  )  # 平移向量  # 外参数
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
    print('newcameramtx外参',newcameramtx)


if __name__=='__main__':
    calibrate()

k和d都是标定代码跑出来的参数，根据不同的相机出来的结果肯定是不一样的

def undistort(frame):
    d=np.array([-0.16036848 , 0.0189364 , -0.00221216 , 0.00121543, -0.00085047])
    k=np.array([[108.32753648,0.,213.67262523],[0.,107.71983139 ,227.61521286],[0.,0.,1.]])
    h,w=frame.shape[:2]
    mapx,mapy=cv2.initUndistortRectifyMap(k,d,None,k,(w,h),5)
    return cv2.remap(frame,mapx,mapy,cv2.INTER_LINEAR)

4.运动控制

        运动控制我并没有做出自己的姿态（确实能力不行做不出来😅），直接采用了官方的unitreerobotics/unitree_legged_sdk: SDK tools for control robots. (github.com)

       里面有python和c++的两种，c++做的介绍很全面，至于python的话，使用的类，类里面的属性，函数都是差不多的。（）

        不过python的控制需要在循环里面，而c++的却可以不用，只需要设定一次就好了。

运动控制也是通过udp从nx版传输到树莓派上的，nx版在整个系统中的作用就类似大脑，不断接收反馈信息控制手脚。

重点！！！：在调试控制前一定一定要看文档（下方链接），充分的阅读文档可以给你减少很多尝试的时间并且减少对电机的损耗。

宇树科技开发知识库 · 语雀

        至于上手可以优先在仿真模型上尝试，在b站搜索Unitree仿真就可以获得官方出版的很多视频。

这里分别给出c++和python两个转圈的例子

C++

void Custom::round(){
  cmd.mode = 1;
  cmd.mode = 2;//这个模式要按顺序切换，否则容易进入断电保护
  cmd.yawSpeed = M_PI/2.0;//角速度
  udp.SetSend(cmd);
  sleep((2*M_PI)/(M_PI/2.5));//延时，速度归0
  cmd.yawSpeed = 0;
}

Python

def Round():
    delay_time = datetime.timedelta(seconds=Round_time)
    start_time = datetime.datetime.now()
    end_time = start_time + delay_time#延时
    while True: 
        cmd.mode = 1
        cmd.mode = 2
        cmd.yawSpeed = M_PI/2.0
        udp.SetSend(cmd)
        udp.Send()
        if datetime.datetime.now() >= end_time:
            break

5.手势识别

   

    采用yolov8的框架，以HaGRID手势数据集为数据集，训练出模型并且部署在Go1上  

    推荐一个很好用的yolo8的框架，连README都是中文实在是太感动了bubbliiiing/yolov8-pytorch: 这是一个yolov8-pytorch的仓库，可以用于训练自己的数据集。 (github.com)

     

def video(img):

        fps = 0.0
        t1 = time.time()
            # 读取某一帧
        frame = img
            # 格式转变，BGRtoRGB
        frame = cv2.cvtColor(frame,cv2.COLOR_BGR2RGB)
            # 转变成Image
        frame = Image.fromarray(np.uint8(frame))
            # 进行检测
        frame = np.array(yolo.detect_image(frame))
            # RGBtoBGR满足opencv显示格式
        frame = cv2.cvtColor(frame,cv2.COLOR_RGB2BGR) 
        fps  = ( fps + (1./(time.time()-t1)) ) / 2
        name = yolo.name#这个name是我在yolo那份文件下自己增加的属性
 
        frame = cv2.putText(frame, "fps= %.2f"%(fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) 
                  
        cv2.imshow("video",frame)
        
        return name


#这个是我主函数获取识别完的图像和获取标签的部分，我就细说我自己修改的部分，对于原作者的代码可以去慢慢品味

我在这个类下面 加入了name 属性 以便于我获取标签

最后在这一块，用self name = predicted_class 来获取标签。（对于小白来说可以慢慢的用调试或者print尝试去找到需要的变量）

6.待完成

        原本是想再用tensort加速模型的，但是我思考了下，其实没什么必要对于手势识别来说，识别了一个手势进程就卡在了做动作的那个部分，加速了对效果其实并没有什么很大的帮助（换句话来说，有点懒）。