# 一、项目背景

![xiaobai2](imgs/xiaobai2.jpg)

相信大家都知道超能陆战队中的大白，拥有一台能行走，能看，能听会说，会思考的机器人，一定是很多科技爱好者的愿望，随着人工智能的发展，以及供应链和加工制造业的完善，离普通人造一台属于自己的大白机器人不再那么遥远。

![dabai](imgs/dabai.jpg)

由于时间关系和技术水平有限，所以本项目尽量简化了机器人的机械结构，设计了一台外观简约但是功能强大的桌面机器人，并且发挥我本职工作算法调包侠的优势，让他拥有了一颗聪明的AI大脑。

![xiaobai1](imgs/xiaobai1.jpg)

对于这个机器人的外观，有的人说像墨水瓶，有的人说像电饭锅，有的人说像托马斯小火车，有的人说像赛尔号，还有说像地雷的。我把它取名为“小白”，首先这个名字来源于电影中的“大白”，而它的外观小巧并且是白色的，其次“小白”在现代汉语中也有新手、初学者的意思，用来提醒自己要拓宽视野不断学习，也提醒机器人的AI模型要不断学习。

为了和大家一起交流和进步，现在将小白机器人的软硬件所有资料全部开源。下面分硬件、结构、软件三个方面向大家汇报一下小白机器人的技术细节。

# 二、硬件

## 1.硬件总体设计

做硬件，首先要确定一下小白机器人的核心CPU，有近些年火热的ESP32和昂贵的Linux SOC两种选择，最终还是选择了能跑Linux的RK3566芯片，虽然价格昂贵、功耗高、发热重、体积大，但是在Linux系统下编程更加自由，功能更加容易扩展，有大量的开源项目可供使用，不必自己重新造轮子，例如下文中会提到的强大的ros系统。
确定了处理器之后，机器人上肯定少不了一块液晶屏，为的是和用户进行人机交互，显示个表情包啥的，提供必要的情绪价值。
要想让机器人能看见这个世界，必须需要安装一个摄像头；要机器人能听会说，麦克风和扬声器也是必须要的；要实现机器人能走路，最简单的方式就是安装两个差速轮，并且为了支持它到处跑，电池供电也是必不可少的。
另外为了机器人在室内环境中能够避障和导航，我还为小白机器人的顶部配备了单线激光雷达；为了实现机器人常见的离线命令词控制，内置了离线语音芯片；为了实现待机充电，在机器人内部做了电源切换和充电电路。

最后确定下来的硬件整体框架如下：

![hardware](imgs/hardware.png)

## 2.采购清单

硬件采购清单如下，仅供参考（以下不包括底盘PCB单面SMT用到的元器件，如果两面全手工焊接需要根据原理图另行购买元器件）：

![bom](imgs/bom.png)

## 3.原理图和PCB

主板采用了现成的香橙派3B，而STM32控制板需要在嘉立创打样完成，主要是用于电源管理和移动底盘控制：

![base1](imgs/base1.png)

![base2](imgs/base2.png)

![base3](imgs/base3.png)

![base4](imgs/base4.png)

另外还有比较简单的电池盒转接板、USB线转接板、USB串口板：

![bat](imgs/bat.png)

![usb](imgs/usb.png)

![uart](imgs/uart.png)

## 4.硬件参数

|     项目     |                    参数                    |
| :----------: | :----------------------------------------: |
|   产品型号   |                 newbot1.0                  |
|  主芯片SOC   |              Rockchip RK3566               |
|     CPU      |         四核64位Cortex-A55 1.8GHz          |
|     NPU      | 1T算力 支持TensorFlow/ONNX/PyTorch模型转换 |
|     VPU      |         H.264/H.265/JPEG硬件编解码         |
|     RAM      |                 LPDDR4 2GB                 |
|     ROM      |                 eMMC 32GB                  |
|  Wi-Fi+蓝牙  |               Wi-Fi 5+BT 5.0               |
| 控制芯片MCU  |               STM32F103C8T6                |
|   电池容量   |          两节18650共7000mAh 3.7V           |
|  机器人尺寸  |            直径12cm 高度约10cm             |
| 摄像头分辨率 |               1280x800 30FPS               |
|   屏幕尺寸   |             1.28寸SPI圆形屏幕              |
|   操作系统   |                Ubuntu20.04                 |
|   ROS版本    |                ROS1 Noetic                 |



# 三、结构

## 1.外观设计

对于结构部分，小白机器人的外壳采用工业界常用的Solidworks软件进行绘制。

**注意**：3D打印采用光固化打印，没有尝试过普通的FDM打印机。

![3d1](imgs/3d1-1.png)

![3d2](imgs/3d2-1.png)

## 2.内部结构设计

小白机器人充分利用了壳体内的空间，在直径12厘米的圆柱体内，从下至上大致分成了电机、电池、主板、激光雷达四层空间，做到了体积小巧但功能丰富。

![3d4](imgs/3d4.png)



## 3.激光雷达的安装

目前支持YDLidar X2、M1C1_MINI、乐动LD14等多款雷达，YDLidar X2的雷达孔位，也适用于顶部有个小红点的乐动LD14，但是M1C1_MINI的孔位距离更长，具体细节可参考雷达手册。

小白机器人的接口线序和组装详细步骤也放到了项目主页，您可以从零开始一步步组装好机器人的每一个零件。

**注意**：根据激光雷达的类型不同，孔位位置不同，要选用不同的上壳结构：

![ydlidar](imgs/ydlidar.png)

![m1c1_mini](imgs/m1c1_mini.png)



# 四、软件

## 1.软件整体框架

### (1)ROS系统简介

ROS（机器人操作系统，Robot Operating System），是专为机器人软件开发所设计出来的一套电脑操作系统架构。小白机器人的软件在ROS框架下构建，需要掌握ROS基本的命令行工具、可视化工具、工程编译方法、ROS Launch文件、C++和Python语言等ROS开发基础。

推荐ROS入门课程：

ROS官方教程  https://wiki.ros.org/cn

古月居GYH [【古月居】古月·*ROS*入门21讲 | 一学就会的*ROS*机器人入门教程](https://www.bilibili.com/video/BV1zt411G7Vn/)

机器人工匠阿杰 [机器人操作系统 *ROS* 快速入门教程](https://www.bilibili.com/video/BV1BP4y1o7pw/)



ROS包括了通信机制、开发工具、应用功能和生态系统四个部分：

![ros](imgs/ros.png)



ROS的通信机制主要有Topic、Service、Action三大方式，最常用的是Topic：

![ros_topic](imgs/ros_topic.png)

ROS常用的可视化工具有rviz、rqt、rqt_image_view、rqt_graph、rqt_plot等，ROS主要应用于移动机器人底盘（SLAM、NAV）、机械臂（MoveIt）和底盘与机械臂结合的复合机器人。

![rviz](imgs/rviz.png)

### (2)本系统的主要软件功能

这是本项目的三大主要软件功能，包括了语音交互、图像识别、移动导航。

![software](imgs/software.png)

### (3)本系统的ROS节点图

左侧是图像的采集处理流程，先读取USB相机原始的JPEG格式数据，然后解码和缩放，送入yolov6处理模块，处理得到检测框和绘制了检测框的图像，然后送入跟踪模块，得到绘制了跟踪结果的图像，最后送到JPEG编码节点，发布出去方便电脑或手机接收查看。

注意这里的解码、缩放以及最后的编码都是调用了RK3566的硬件编解码能力，而yolov6使用了RK3566 RKNN的AI加速能力。

![img_pipeline](imgs/img_pipeline.png)

右侧是其他处理流程，包括了底盘控制节点base_control，雷达驱动节点/m1c1_mini，离线唤醒命令处理节点wakeup_process，纯python编写的语音处理节点audio，精确移动和旋转命令执行节点/move_client_cmd和/move_server。下文会对核心的几个节点做简单介绍。

![graph](imgs/graph.png)



## 2.底盘控制节点

base_control节点的主要功能：和STM32串口通信，计算并发布里程计/odom和/tf，发布电池信息话题，发布离线语音命令ID；订阅速度话题转换为串口命令，订阅雷达使能话题转换为串口命令，订阅位置复位话题清零位置等。

### (1)和STM32的通信协议

底盘控制节点(base_control)每20ms都会通过串口从STM32接收到机器人的一些底层状态信息，包括了当前20ms内的编码器的脉冲数，电池电压，充电状态，离线语音命令ID；底盘控制节点每20ms都会向STM32发送控制命令，包括了两个轮子的PWM占空比，是否打开雷达电源。具体的通信协议如下：

```c++
#pragma pack(1)

typedef struct
{
unsigned char head1;//数据头1 'D'
unsigned char head2;//数据头2 'A'
unsigned char struct_size;//结构体长度

short encoder1;//编码器当前值1
short encoder2;//编码器当前值2

short vbat_mv;//电池电压 mV
unsigned char charger_connected;//是否连接充电器
unsigned char fully_charged;//是否充满电

unsigned char asr_id;//语音命令ID

unsigned char end1;//数据尾1 'T'
unsigned char end2;//数据尾2 'A'
unsigned char end3;//数据尾3 '\r' 0x0d
unsigned char end4;//数据尾4 '\n' 0x0a
}McuData;


typedef struct
{
unsigned char head1;//数据头1 'D'
unsigned char head2;//数据头2 'A'
unsigned char struct_size;//结构体长度

short pwm1;//油门PWM1
short pwm2;//油门PWM2
unsigned char enable_power;//开启5V雷达电源

unsigned char end1;//数据尾1 'T'
unsigned char end2;//数据尾2 'A'
unsigned char end3;//数据尾3 '\r' 0x0d
unsigned char end4;//数据尾4 '\n' 0x0a
}CmdData;

#pragma pack()
```



### (2)差速机器人的运动学模型

小白机器人采用两个差速轮加上一个万向球的底盘架构，差速机器人的运动学模型如下：

![base](imgs/base.png)

![base_math](imgs/base_math1.png)



![base_math2](imgs/base_math2.png)

代码实现：

```c++
//v = (vr+vl)*0.5
//w = (vr-vl)/ l(轮距)
double delta_m   =  (double)(mcu_data.encoder2 + mcu_data.encoder1) * 0.5             / pluses_m;// 当前周期的脉冲数均值 / pluses/m = 距离m
double delta_rad =  (double)(mcu_data.encoder2 - mcu_data.encoder1)/wheel_distance_m  / pluses_m;// 当前周期的脉冲数差值 / pluses/m / 轮距 = 角度rad

//vl = v - w*l(轮距)*0.5
//vr = v + w*l(轮距)*0.5
//速度转换为编码器目标值
int target1 = (target_m_s - target_rad_s * wheel_distance_m *0.5) * dt * pluses_m;//左轮速度m/s * dt * pluses/m = 左轮距离m * pluses/m = 左轮脉冲数
int target2 = (target_m_s + target_rad_s * wheel_distance_m *0.5) * dt * pluses_m;//右轮速度m/s * dt * pluses/m = 右轮距离m * pluses/m = 右轮脉冲数
```



### (3)PID控制机器人速度

PID控制分为两种，位置式PID和增量式PID：

**位置型PID**：控制输出与整个历史误差有关，包括比例项、积分项（误差的累加值）和微分项。这种算法直接计算出控制量的绝对值。

**增量型PID**：控制输出仅与当前误差及前几次误差的差值有关，计算的是控制量的增量，即每次调整的量，而不是控制量的绝对值。因此，它没有积分累加的概念。

优缺点如下：

**位置式PID**：直接基于当前误差控制输出，更适合精确的位置或速度控制，但可能会遇到积分饱和问题。

**增量式PID**：基于误差变化量控制输出，通常实现更简单，适合在对误差变化快速响应的场合，能较好地避免积分饱和。

![pid1](imgs/pid1.jpg)



![pid2](imgs/pid2.jpg)

为了达到更高的速度控制精度，小白机器人采用位置式PID控制电机速度。

PID参数调节参考链接：

电机控制进阶——PID速度控制：https://zhuanlan.zhihu.com/p/373402745

在ROS系统下可以使用rqt_reconfigure和rqt_plot工具来调节和可视化PID控制效果：

![rqt_reconfigure](imgs/rqt_reconfigure.png)

![plot](imgs/plot.jpg)

位置式PID控制器的代码如下：

```c++
#include "pid.h"

void init_pid(PidController* pid,double Kp,double Ki,double Kd)
{
	pid->Kp = Kp; // 比例系数
    pid->Ki = Ki; // 积分系数
    pid->Kd = Kd; // 微分系数
    pid->integral = 0; // 误差累计
    pid->last_error = 0; // 上一次误差
    pid->output = 0; //输出值
}

// 计算PID控制器输出
double calculate_pid_output(PidController* pid,int target,int encoder,int max_pwm) 
{
    // 计算误差
    double error,error_rate;
    error = target - encoder;

    // 计算误差变化率
    error_rate = error - pid->last_error;

    // 计算误差累计
    pid->integral += error;
    if(pid->integral>1000)//积分限幅
        pid->integral = 1000;
    else if(pid->integral<-1000)
        pid->integral = -1000;

    
    // 计算PID输出 （位置式直接赋值）
    pid->output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * error_rate;

    // 保存上一次误差
    pid->last_error = error;

    //PWM限幅
    if(pid->output>max_pwm)
        pid->output=max_pwm;
    else if(pid->output<-max_pwm)
        pid->output=-max_pwm;

    return pid->output;
}
```



## 3.语音交互节点

语音交互节点主要包括了语音识别、大模型处理和语音合成。因为目前大多数语音识别API都需要付费，因此使用Kaldi本地语音识别。大模型使用了不限次数免费的科大讯飞大模型API。语音合成可以使用免费的edge_tts、有道TTS、Kaldi本地TTS等。

### (1)语音识别

因为目前大多数语音识别API都需要付费，因此使用sherpa_onnx库实现的本地语音识别，使用cpu推理模型：

```python
class KaldiASR():
    def __init__(self):
        current_dir = os.path.dirname(os.path.realpath(__file__))  # 获取当前文件夹
        model_path_name = os.path.join(current_dir,"..","model","sherpa-onnx-paraformer-zh-small-2024-03-09")
        print("离线语音识别模型开始初始化...")
        # 初始化语音识别
        offline_asr.init(model_path_name)
        print("离线语音识别模型初始化完成")

    def audio_file_to_text(self, wav_name):
        text = offline_asr.asr(wav_name)
        #print("kaldi识别结果:",text)
        return text
```

kaldi语音识别的代码如下：

```python
def asr(*sound_files):
    global args
    global recognizer
    global contexts_list
    start_time = time.time()
 
    streams = []
    total_duration = 0
    for wave_filename in sound_files:
        samples, sample_rate = read_wave(wave_filename)
        duration = len(samples) / sample_rate
        total_duration += duration
        if contexts_list:
            s = recognizer.create_stream(contexts_list=contexts_list)
        else:
            s = recognizer.create_stream()
        s.accept_waveform(sample_rate, samples)
 
        streams.append(s)
 
    recognizer.decode_streams(streams)
    results = [s.result.text for s in streams]
    end_time = time.time()
 
    for wave_filename, result in zip(sound_files, results):
        return f"{result}"
```



### (2)大模型处理

使用了无限次免费的科大讯飞大模型API：

```python
class WsProcess():
    def __init__(self, wsParam):
        websocket.enableTrace(False)
        wsUrl = wsParam.create_url()
        self.ws = websocket.WebSocketApp(wsUrl, on_message=self.on_message, on_error=on_error, on_close=on_close, on_open=on_open)
        self.answer = ""


    # 收到websocket消息的处理
    def on_message(self, message):
        data = json.loads(message)
        code = data['header']['code']
        if code != 0:
            print(f'请求错误: {code}, {data}')
            self.ws.close()
            #self.answer += data['header']['message']
        else:
            choices = data["payload"]["choices"]
            status = choices["status"]
            content = choices["text"][0]["content"]
            self.answer += content
            if status == 2:
                self.ws.close()

    def run(self, appid, domain, query):
        self.ws.appid = appid
        self.ws.query = query
        self.ws.domain = domain
        self.timeout = False
        self.timer = threading.Timer(5, self.on_timeout) #5秒等待时间，如果大模型回复的内容多，就可能等待时间比较久
        self.timer.start()
        self.ws.run_forever(sslopt={"cert_reqs": ssl.CERT_NONE})
        self.timer.cancel()
        if self.timeout:
            print("大模型网络连接超时")
            return "抱歉，我的网络好像卡住了"
        else:
            return self.answer

    def on_timeout(self):
        print("llm请求超时!")
        self.timer.cancel()
        self.timeout = True
        self.ws.close()
```



### (3)语音合成

目前使用免费的edge_tts（前一段时间挂了，升级到最新又能用了），或者国内的有道tts：

```python
class YoudaoTranslate(BaseTranslate):
    def __init__(self):
        super(YoudaoTranslate, self).__init__()
        self.home = 'https://dict.youdao.com/'
 
    def get_tts(self, text, lan='', type_=2, *args, **kwargs):
        """ 获取发音
        :param text: 源文本
        :param lan: 文本语言
        :param type_: 发音类型
        :return: 文本语音
        """
        path = 'dictvoice'
        params = {'audio': text, 'le': lan, 'type': type_}
        response = self._get(path, params)
        if response==None:
            return None

        if len(response.content)==0:
            return None

        return response.content
```



## 4.目标检测节点

目标检测节点采用了RK3566的NPU加速处理，推荐官方的模型仓库rknn_mode_zoo，官方已经支持了各类视觉和自然语言任务。本项目选用了速度较快的yolov6检测模型，模型部署的大致流程是先将pytorch转onnx，再将onnx转rknn，最后在开发板上读取rknn模型进行推理。

瑞芯微RK3566或RK3588支持NPU深度学习推理加速，官方提供的模型库链接：https://github.com/airockchip/rknn_model_zoo

目前模型库中已经支持了图像分类、目标检测、语义分割、实例分割、人脸关键点、车牌识别、文本识别OCR、机器翻译、图像文本匹配、语音识别、语音分类、文本转语音等各类深度学习任务。

下面介绍目标检测算法yolov6的模型部署过程，模型部署的大致流程是：

电脑python训练 --> .pt --> .onnx --> .rknn --> 开发板c++推理

### (1)训练代码修改

先从官方github下载代码：

```shell
git clone https://github.com/meituan/YOLOv6.git
```

deploy/ONNX/export_onnx.py原始代码：

```python
            torch.onnx.export(model, img, f, verbose=False, opset_version=13,
                              training=torch.onnx.TrainingMode.EVAL,
                              do_constant_folding=True,
                              input_names=['images'],
                              output_names=['num_dets', 'det_boxes', 'det_scores', 'det_classes']
                              if args.end2end else ['outputs'],
                              dynamic_axes=dynamic_axes)
```

修改后，输出层改为3个，方便部署代码的后处理：

```python
            torch.onnx.export(model, img, f, verbose=False, opset_version=12,
                              training=torch.onnx.TrainingMode.EVAL,
                              do_constant_folding=True,
                              input_names=['images'],
                              output_names=['num_dets', 'det_boxes', 'det_scores', 'det_classes']
                              if args.end2end else ['output1','output2','output3'], #['outputs'],
                              dynamic_axes=dynamic_axes)
```

yolov6/models/heads/effidehead_distill_ns.py原始代码：

```python
class Detect(nn.Module):
    export = False
```

修改后：

```python
class Detect(nn.Module):
    export_rknn = True #导出rknn友好模型
    export = False
```

原始代码：

```python
    def forward(self, x):
        if self.training:
            cls_score_list = []
            reg_distri_list = []
            reg_lrtb_list = []
```

修改后：

```python
    def _rknn_opt_head(self, x): #yolov6n,s会运行这个文件
        output_for_rknn = []
        for i in range(self.nl):
            x[i] = self.stems[i](x[i])
            reg_feat = self.reg_convs[i](x[i])
            reg_output = self.reg_preds[i](reg_feat)

            cls_feat = self.cls_convs[i](x[i])
            cls_output = self.cls_preds[i](cls_feat)
            cls_output = torch.sigmoid(cls_output) #npu支持sigmoid 消耗1ms以下

            conf_max,_ = cls_output.max(1, keepdim=True) #此行实际会调用reducemax，npu支持max，但没有reducemax
            #rknn推理仅cpu支持reducemax argmax sum等操作，并且argmax量化效果差
            
            out = torch.cat((reg_output,conf_max,cls_output),dim=1) #4(reg)+1(max)+80(cls)
            
            #out = torch.cat((reg_output,cls_output),dim=1) #4+80这种组合节约npu时间，但是后处理时间增加,cpu占用高
            
            output_for_rknn.append( out )
            
        return output_for_rknn
    
	def forward(self, x):
        
        if self.export_rknn:
            return self._rknn_opt_head(x)
            
        if self.training:
            cls_score_list = []
            reg_distri_list = []
            reg_lrtb_list = []

```

代码修改完成后进行模型导出：

参考链接：https://github.com/meituan/YOLOv6/tree/main/deploy/ONNX

将img的高度设置为352，宽度设置为640，导出最小模型yolov6n.pt：

```shell
python ./deploy/ONNX/export_onnx.py \
    --weights yolov6n.pt \
    --img 352 640 \
    --batch 1 \
    --simplify
```

yolov6n.onnx模型可视化，使用Netron工具打开模型：

可以看到模型输入图像尺寸为1x3x352x640：

![onnx_in](imgs/onnx_in.png)

模型其中一个分支输出尺寸为1x85x11x20：

![onnx_out](imgs/onnx_out.png)

### (2)模型转换

模型转换之前需要在电脑安装rknn-toolkit2，大致转换代码如下：

```shell
def export_rknn_inference(img):
    # Create RKNN object
    rknn = RKNN(verbose=False)

    # pre-process config
    print('--> Config model')
    rknn.config(mean_values=[[0, 0, 0]], std_values=[[255, 255, 255]],target_platform='rk3566')
    print('done')

    # Load ONNX model
    print('--> Loading model')
    ret = rknn.load_onnx(model=ONNX_MODEL)
    if ret != 0:
        print('Load model failed!')
        exit(ret)
    print('done')

    # Build model
    print('--> Building model')
    ret = rknn.build(do_quantization=QUANTIZE_ON, dataset=DATASET)
    if ret != 0:
        print('Build model failed!')
        exit(ret)
    print('done')

    # Export RKNN model
    print('--> Export rknn model')
    ret = rknn.export_rknn(RKNN_MODEL)
    if ret != 0:
        print('Export rknn model failed!')
        exit(ret)
    print('done')

    # Init runtime environment
    print('--> Init runtime environment')
    ret = rknn.init_runtime()
    if ret != 0:
        print('Init runtime environment failed!')
        exit(ret)
    print('done')

    # Inference
    print('--> Running model')
    outputs = rknn.inference(inputs=[img])
    rknn.release()
    print('done')

    return outputs
```

转换后得到yolov6n.rknn文件，使用python接口推理可以得到绘制了检测结果的图片：

![dog](imgs/dog.jpg)

也可以用Netron工具可视化查看：



![yolov6_85_rknn](imgs/yolov6_85_rknn.png)

输出层：

![yolov6_output_layer](imgs/yolov6_output_layer.png)



### (3)模型部署代码

模型部署代码运行在开发板上，使用C++代码实现推理，主要修改后处理代码：

```c++
static int process_float(float *input,  int grid_h, int grid_w, int height, int width, int stride,
                   std::vector<float> &boxes, std::vector<float> &objProbs, std::vector<int> &classId,
                   float threshold, int32_t zp, float scale,int OBJ_CLASS_NUM)
{
    int validCount = 0;
    int grid_len = grid_h * grid_w;

    //printf("hxw %dx%d %dx%d %d\n",grid_h,grid_w,height,width,stride);

    for (int i = 0; i < grid_h; i++)
    {
        for (int j = 0; j < grid_w; j++)
        {
            if( input[4 * grid_len + i*grid_w + j] < threshold )//4+1+80
            {
               continue;//这一步可以节约大量后处理时间，并且减少CPU占用
               //但是将ReduMax放在模型推理中完成，模型推理也调用的是CPU，所以CPU会在推理时偏高
            }
            
            //4+1+80 or 4+80

            float *in_ptr = input + i*grid_w + j;

            //float prob_max = in_ptr[4 * grid_len];
            float prob_max = in_ptr[5 * grid_len];
            int maxClassId = 0;
            for (int c = 1; c < OBJ_CLASS_NUM; c++)
            {
                 //float prob = in_ptr[(4 + c) * grid_len];
                 float prob = in_ptr[(5 + c) * grid_len];
                 if (prob > prob_max)
                 {
                     maxClassId = c;
                     prob_max = prob;
                 }
            }

            if (prob_max >= threshold)
            {
            
                float box_x =  j + 0.5 - in_ptr[0 * grid_len];
                float box_y =  i + 0.5 - in_ptr[1 * grid_len];
                float box_x2 = j + 0.5 + in_ptr[2 * grid_len];
                float box_y2 = i + 0.5 + in_ptr[3 * grid_len];

                float box_w = box_x2-box_x;
                float box_h = box_y2-box_y;
            
                //printf("prob=%f\n",maxClassProbs);
                // objProbs.push_back(maxClassProbs);
                // classId.push_back(maxClassId);

                objProbs.push_back(prob_max);
                classId.push_back(maxClassId);
                
                boxes.push_back(box_x*stride);
                boxes.push_back(box_y*stride);
                boxes.push_back(box_w*stride);
                boxes.push_back(box_h*stride);
                
                validCount++;
            }
        }
    }
    
    return validCount;
}
```



## 5.SLAM和导航节点

最后是slam和导航节点，slam和导航基本都是用ROS自带的功能包，只需要根据自己机器人情况修改配置文件，例如调整机器人的尺寸配置，修改机器人的最大速度限制等。

### (1)启动功能包

在小白机器人上启动SLAM和导航的命令如下：

```shell
cd ~/newbot_ws
source devel/setup.bash 

roslaunch robot_navigation blank_map_move_base.launch #空地图下测试路径规划(map坐标系和odom坐标系永远一致)

#先要打开雷达
roslaunch robot_navigation robot_slam.launch #SLAM建图测试

cd maps && rosrun map_server map_saver -f map #进入robot_navigation包里的maps目录，执行map_server保存地图

roslaunch robot_navigation robot_navigation.launch #加载上一部保存的地图、定位和路径规划测试
```

电脑上可视化建图和导航效果：

```shell
$ rviz #在电脑上执行，打开pkg_launch/rviz/robot.rviz文件可视化建图和导航结果
```

![slam](imgs/slam.jpg)

### (2)修改配置文件

以下改动量比较大的两个配置文件，首先要调整机器人的尺寸配置，还需要修改机器人的速度限制：

newbot_ws/src/robot_navigation/config/robot/costmap_common_params.yaml配置文件：

```yaml
obstacle_range: 3.0 #障碍物范围 机器人只会更新其地图包含距离移动基座obstacle_range米以内的障碍物的信息
raytrace_range: 3.5 #光线追踪范围 机器人将尝试清除raytrace_range米外的空间，在代价地图中清除raytrace_range米外的障碍物

#footprint: [[-0.07, -0.06], [-0.07, 0.06], [0.05, 0.06], [0.05, -0.06]]
robot_radius: 0.07 #圆形机器人半径 直径12cm,半径6cm

inflation_radius: 0.2 #1.0 #膨胀半径 如果机器人经常撞到障碍物就需要增大该值!!!!!，若经常无法通过狭窄地方就减小该值!!!!!
cost_scaling_factor: 3.0 #3.0 # exponential rate at which the obstacle cost drops off(default: 10)

map_type: costmap
observation_sources: scan
scan: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true}
#marking表示是否可以使用该传感器来标记障碍物
#clearing表示是否可以使用该传感器来清除障碍物标记为自由空间

#参数参考了github turtlebot3代码
#https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param/costmap_common_params_burger.yaml

```

newbot_ws/src/robot_navigation/config/robot/dwa_local_planner_params.yaml配置文件：

```yaml
DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x:  0.10  #0.22  #机器人的最大 x 速度 默认值：0.55
  min_vel_x: -0.10 #-0.22 #机器人的最小x速度（以m / s为单位），向后运动为负 默认值：0.0

  max_vel_y: 0.0 #机器人的最大 y 速度
  min_vel_y: 0.0 #机器人的最小 y 速度

# The velocity when robot is moving in a straight line
  max_vel_trans:  0.10  #0.22 #机器人最大平移速度的绝对值 默认值：0.55
  min_vel_trans:  0.05 #0.11 #机器人最小平移速度的绝对值 默认值：0.1

  max_vel_theta: 2.0 #2.75 #机器人最大旋转速度的绝对值 默认值：1.0
  min_vel_theta: 1.0 #1.37 #机器人最小旋转速度的绝对值 默认值：0.4

  acc_lim_x: 2.5 #机器人的 x 加速度极限 默认值：2.5
  acc_lim_y: 0.0 #机器人的 y 加速度极限 默认值：2.5
  acc_lim_theta: 3.2 #机器人的旋转加速度极限 默认值：3.2

# Goal Tolerance Parameters
  xy_goal_tolerance: 0.05 #实现目标时控制器在 x 和 y 距离内的公差 默认值：0.10
  yaw_goal_tolerance: 0.17 #控制器在实现目标时偏航/旋转时的弧度公差 默认值：0.05
  latch_xy_goal_tolerance: false ##如果被锁定，如果机器人到达目标xy位置，它将简单地在原地旋转，即使它最终超出了目标公差 默认值：假

# Forward Simulation Parameters
  sim_time: 2.0 #1.5 #向前模拟轨迹的时间 默认值：1.7 太小不停地拐弯，轨迹不流畅；太大走大圈和轨迹不符合
  vx_samples: 6 #20 #探索 x 速度空间时要使用的样本数 默认值：3
  vy_samples: 0 #探索 y 速度空间时要使用的样本数 默认值：10
  vth_samples: 40 #40 #探索θ速度空间时要使用的样本数量 默认值：20
  controller_frequency: 10.0 #调用此控制器的频率（以 Hz 为单位） 默认值：20.0

# Trajectory Scoring Parameters
  path_distance_bias: 32.0 #32.0 #控制器应保持与给定路径的接近程度的权重 默认值：32.0 #本地规划器与全局路径保持一致的权重 较大的值将使本地规划器更倾向于跟踪全局路径
  goal_distance_bias: 20.0 #20.0 #控制器应尝试达到其本地目标的权重也控制速度 默认值：24.0 #会使机器人与全局路径的匹配度偏低
  occdist_scale: 0.01 #0.02 #控制器应尝试避免障碍物的权重 默认值：0.01
  forward_point_distance: 0.325 #从机器人中心点到放置一个附加得分点的距离 默认值：0.325
  stop_time_buffer: 0.2 #机器人在碰撞前必须停止的时间量，以使轨迹在几秒钟内被视为有效 默认值：0.2
  scaling_speed: 0.25 #开始缩放机器人占地面积的速度的绝对值，以 m/s 为单位 默认值：0.25
  max_scaling_factor: 0.2 #将机器人的占地面积缩小到以下列的最大系数 默认值：0.2

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05 #在振荡标志重置之前，机器人必须以米为单位行进多远 默认值：0.05

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true

  holonomic_robot: false #是否为全向机器人 值为false时为差分机器人; 为true时表示全向机器人
   
#http://wiki.ros.org/dwa_local_planner?distro=noetic
#参数参考:
#https://blog.csdn.net/weixin_42005898/article/details/98478759
#导航调优指南.pdf
#https://blog.csdn.net/luohuiwu/article/details/92985584
```