这里分享下机器人结构和电控的设计，以及驱动相关程序。

机器人为两轮差速移动底盘，整体使用亚克力板拼接而成，各层载板均有固定空位可用于驱动板、主控板等器件的固件。机械臂使用不锈钢件拼接得到，有五个关节自由度+夹爪组成。

机械结构部分分为SLAM底盘载体，接入电机、轮子、电池等零件，是底盘的结构设计；控制载板部分为各个功能模块及下位机驱动板、安卓手机，是机器人运控载板；机械臂部分为采购的不锈钢连接件拼接而得到。

底盘结构包括电池位、前辅助轮以及尾部两个MG317直流电机固定孔位。整体尺寸为23cm*23cm(包含轮宽距)，其中轮距15cm。使用4cm宽橡胶轮，具备良好抓地性能，底盘载板结构如下图所示。

机器人控制载板部分是两块相同的亚克力板，第一块安装固定激光雷达、SLAM底盘驱动板、树莓派开发板，第二块安装固定机械臂及机械臂驱动控制板。

在此将激光雷达中心、惯导单元(IMU)中心放置在Z轴垂直中心位置，后续坐标变换只需计算Z轴高度即可，可以减少部分TF坐标计算，控制载板结构如下图所示。

机械臂部分采用不锈钢件进行拼接，使用六个MG996R伺服电机进行驱动，通过PWM脉冲占宽信号驱动，机械臂结构及参数如下图所示。

电气系统设计主要是对工业机器人的各个驱动电路设计，其中主要有供应系统、电源驱动系统、单机臂的舵机驱动系统、SLAM底盘驱动系统四部分组成。其中SLAM底盘驱动板是淘宝购买的，这里使用其提供的接口，将自己的算法接入。

供电部分使用XL4016E1电源芯片，为运行系统、激光雷达等提供供电。XL4016E1支持宽电压输入，为高低压一体的开关电源芯板，支持5~40V的宽电压输入，以及3~36V电压调整输入输出，最大输入输出电压可达8A，基本原理图如下图所示。

接口使用了一个型号为5.5-2.0的DC头作为12V供电接口，接开关电源的输出。在12V位置使用电容C7（470uF）和电容C8（1uF）并联的方式进行滤波；VC是内测电压调整，使用一个电容C9（1uF）接到12V即可；FB是分压反馈引脚，检测当前的输出电压并进行调整；SW为功率输出引脚。

在SW功率输出后，需要使用一个快恢复二极管D14（20100）、一个电感L1（47uH）和一个电容C10（1000uF）进行高频滤波处理，电容C11（1uF）作为输出滤波。

电阻R25（10K）和电阻R26（3.3K）作为分压检测使用，FB的检测电压是1.25V，电阻R25接5V，电阻R26接GND，则两电阻之间和GND的电压差值为5V*3.3/(3.3+10)≈1.25V(1:4的比例)，当输出电压不足或者过高的情况下，XL4016E1会自动进行调整。

机械臂驱动板部分使用Atmega 328P芯片，使用Arduino Nano核心板为主控进行设计。机械臂需要六路PWM脉冲信号来驱动，在此使用该芯片的D3、D5、D6、D9、D10、D11六个引脚产生500Hz的脉冲信号进行驱动。供电部分使用LM2596S-5.0降压模块，每个伺服电机堵转的最大电流约为1.2A，这里使用5V 3A的电源模块可以避免电机堵转导致电路烧毁。如果出现六个伺服电机同时堵转的情况，驱动板会直接断电来实现对系统的保护。同时板载一路蜂鸣器驱动电路，作为机械臂部分上电/掉电检测提示信号。机械臂驱动板原理图和PCB如下图所示。

SLAM驱动器部分，采用STM32F103RCT6开发板为主控，板载电源设计模组、电源电压测量模组、MPU6050陀螺仪模组、电机驱动器模组。其电机驱动部分使用了TB6612供电驱动模块驱动两路直流电机，通过IO中断模式捕获编码器信号进行中断计数实现编码器闭环控制系统。IMU部分将线加速度、角速度数据进行DMP四元数姿态解算，通过在传感器联合标定部分通过协方差矩阵来降低噪声。SLAM底盘驱动的原理图和PCB如下图所示。

下位机部分需要编写驱动程序来实现控制系统。SLAM底盘部位需要设计PID控制实现电机转速转PWM值，陀螺仪部分需要进行DMP姿态算法实现XYZ三轴线速度和角速度转姿态的算法设计，机械臂驱动和底盘驱动，也需要相应的驱动程序设计。

在SLAM系统中必须建立机器人的轮组运动学模型，将运动学模型放到虚拟的2D环境中进行控制，通过轮子反馈的位置变化和速度信息间接转化为机器人中心在2D坐标系中的移动。在此采用增量式PID实现速度控制，原理如下图所示。

输入电机的目标转速，通过电机编码器测速来检测当前电机是否达到预设转速。电机转速是通过PWM占宽比进行控制，这里使用PID来实现速度转PWM的计算工作，实现的核心代码如下图所示。

以左电机为例，误差值=目标值-反馈值，P、I、D三个的误差值分别进行迭代计算，输出的PWM值=Kp*当前误差+Ki*上一次误差和当前误差误差的差值+Kd*当前误差与前两次误差值。在最后PWM数值输出执行时，增加了限位检测，PWM值不能超过阈值Esc_output_PWM_LIMT，当PWM超出这个值可能会造成速度过快导致机器人故障。

测试过程中PID的响应较为理想，变化曲线如下图所示。

航姿态参考系统（AHRS）常见于多旋翼无人飞行器的姿态解算部分，用以为飞行器的控制提供姿态信息(pitcℎ、roll、yaw)。AHRS算法需要以下前置信息：

（1）AHRS的计算过程包含坐标系转换，分别是地理坐标系和载体坐标系；

（2）地理坐标系为东北地（END）；

（3）坐标系关系如下图所示：

n表示地理坐标系，b表示载体坐标系，R_n^b表示从地理系旋转到载体坐标系的旋转关系；表示绕载体X轴旋转的roll，ϕ表示绕Y轴即pitcℎ，ψ表示绕Z轴即yaw。

在AHRS系统中使用了四元数来描述旋转关系，将旋转矩阵转换为四元数

表示，如下所示：

观测方程实际上就是通过加速度的测量来提供垂直向下的参考，从而提供pitcℎ、roll的准确计算，观测向量为

表示载体坐标系下重力加速度测量分量，V是观测噪声，AHRS算法有意思的是，运动加速度是不考虑在内的，也就是说滤波过程中，运动加速度将和振动噪声、零偏等统一考虑为观测噪声。

机械臂驱动部分需要输出六路PWM信号，机械臂驱动板部分使用的是Arduino Nano开发板，在此直接使用Servo库文件直接设定伺服电机角度值。

通讯协议这里使用Modbus协议，按照帧头、指令、长度、数据包、校验、帧尾的方式传输数据。在此假设机械臂各关节的角度值如下表所示，作为Modbus协议的测试数据。

则主机发送数据为:

0x55 0x5A 0x11 0x00 0x3C 0x00 0x78 0x00 0xB4 0x00 0xF0 0x01 0x2C 0x01 0x68 0xEC 0xBB

其中第1位0x55为开始读取数据标志。

第2位0X5A是机械臂从机地址。

总共有17字节数据，所以第3位数据长度是0x11(十进制17的十六进制模式)。

第4位+第5位是(0x00的十进制)+60(0x3C的十进制)=60，是关节1的数值。

第6位+第7位是(0x00的十进制)+120(0x78的十进制)=120，是关节2的数值。

第8位+第9位是(0x00的十进制)+180 (0xB4的十进制)=180，是关节3的数值。

第10位+第11位是(0x00的十进制)+240(0xF0的十进制)=240，是关节4的数值。

第12位+第13位是256 (0x01的十进制)+44 (0x2C的十进制)=300，是关节5的数值。

第14位+第15位是256 (Ox01的十进制)+104 (0x68的十进制)=360，是关节6的数值。

第16位是60+120+180+240+300+360 = 1260，转成十六进制为0x04 0xEC，取低位数据，为0xEC。

第17位0xBB停止读取数据标志。

ros-serial是用非ROS装置和ROS装置相互之间进行数据通信的一个方法。它为非ROS装置的使用提出了对ROS节点和服务的发布/订阅服务，使得在非ROS工作环境中执行的使用，可以通过串口或网络就可以很容易地和ROS装置使用实现信息通信。

ros-serial系统包含了客户端和服务器两个部分。ros-serial客户端也可以应用于执行在没有进行过ROS的环境条件下的使用中，通过使用串口或互联网将与运行在ROS环境条件中的ros-serial服务器相连，并使用服务器节点即可在ROS中自由发布/订阅话题。

SLAM底盘驱动接口使用ros-serial进行开发设计，发布Odom轮式里程计数据、发布imu_data陀螺仪数据，并订阅cmd_vel速度数据，通过运动学模型解算和PID进行转换输出控制。驱动程序框架伪代码如下图所示。

1-4行：机械臂关节回调执行函数，监听到机械臂关节数据后执行该函数，将数值传递给舵机执行。

5-8行：控制指令回调函数，监听速度控制指令并传至底盘驱动执行。

9-12行：实例化底盘需要的数据类型，并注册相关回调函数。

13-17行：硬件设备初始化，设定波特率、引脚模式。

18-23行：程序主执行逻辑，这里只发布IMU陀螺仪数据和Odom轮式里程计数据，底盘速度控制数据和机械臂关节控制数据通过订阅回调函数监听执行。