3.5 移动机器人系统模型
相信大家的机器人平台STM32端底层控制和机器人urdf建模都已经顺利完成了，在正式开始ros端编写机器人启动功能包之前，我们还不得不学习一些必要的理论知识。别担心数学不好，这里基本都是高中数学。下面我们开始，Are you ready？ 没准备好也开始了。
3.5.1 常用坐标系系统模型
在移动机器人技术研究中，最为常用的坐标系统是笛卡尔坐标系统。而在SLAM算法研究中，有如下几个比较常见的笛卡尔坐标系统，它们分别为 机器人坐标系XR YR OR 、 传感器坐标系XS YS OS 、 世界坐标系XW YW OW 。顾名思义，世界坐标系是描述机器人全局信息的坐标系；机器人坐标系是描述机器人自身信息的坐标系；传感器坐标系是描述传感器信息的坐标系。他们之间的关系如下图所示：
从图中我们可以知道，首先世界坐标系是固定不变的，机器人坐标系和传感器坐标系是在世界坐标系下描述的。这里我们看到的是俯视图，机器人坐标系和传感器坐标系原点重合但是存在一定的角度，不同的机器人坐标系关系是不同的。当我们使用传感器数据时，这些坐标系间的关系就是我们变换矩阵的参数，因为传感器的数据必定是要变换到机器人坐标系或者世界坐标系中使用的。
3.5.2 移动机器人位姿模型
移动机器人的位姿模型就是机器人在世界坐标系下的状态。 常用随机变量Xt =(xt ,yt ,θt )来描述t时刻的机器人在世界坐标系下的状态，简称位姿 。其中(xt ,yt )表示的在t时刻机器人在世界坐标系下的位置，θt 表示机器人的方向。 默认世界坐标系的X正半轴为正方向，逆时针旋转为旋转正方向，初始时刻机器人坐标系和世界坐标系重合 。某时刻t机器人的位姿描述如下图所示：
3.5.3 移动机器人里程计模型
简单的说， 移动机器人的里程计就是机器人每时每刻在世界坐标系下位姿状态 。 常用的激光SLAM和导航算法通常都需要移动机器人的里程计作为控制输入 。这也是我们对本章理论内容讲解的必要性。
不同底盘的里程计模型有所不同，本文以两轮差分轮式机器人为例介绍里程计模型以及如何计算里程计。在介绍里程计模型之前，要先介绍差分轮式机器人的运动学模型，这样可以先了解一下机器人的物理特性。 首先明确差分模型的机器人始终做的是以R为半径的圆弧运动 。如下图所示，机器人的线速度V、角速度ω，左右轮速用VL和VR表示，用D表示轮间距，D=2d，右轮到旋转中心的距离为L。
ROS端给机器人底盘（STM32端）发送的是机器人要达到的线速度V和角速度ω，而我们底盘控制板需要的是左右轮速VL和VR来进行速度控制。所以，我们通过高中知识可以得到他们之间的关系：
上面的公式是由角速度和线速度的关系得来，如果不知道这个关系，那就自行百度吧，我相信你一定知道。 从这些公式我们是不是发现了什么，机器人的轮间距影响着我们向左右轮分发速度以及合成角速度，所以这是个我们需要注意的参数，不同的机器人结构，该参数就不同。
有了上面坐标系系统模型、位姿模型的基础，里程计就非常简单了。 里程计的计算是指以机器人上电时刻为世界坐标系的起点（机器人的航向角是世界坐标系X正方向）开始累积计算任意时刻机器人在世界坐标系下的位姿 。通常计算里程计方法是 速度积分推算 ： ==通过左右电机的编码器测得机器人的左右轮的速度VL和VR，在一个短的时刻△t内，认为机器人是匀速运动，并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理，关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计== 。具体计算如下图所示：
至此，里程计模型原理及计算就结束了。
3.5.4 2D激光雷观测模型
我们做移动机器人平台少不了使用2D激光雷达，所以了解激光雷达的观测模型也是有必要的。
激光雷达通常由精准控制的旋转电机、红外激光发射器、红外接收视觉系统和主控组成。
激光雷达的测距原理分为两种，一种是基于三角测距，另一个是基于TOF（飞行时间）。基于三角测距的激光雷达表现出的特点，价格便宜、中近距离测距较准确、远距离精度差。基于TOF的激光雷达表现出来的特点，价格昂贵、测距精度高、测距范围广、扫描频率高。
本文采用市面上比较便宜的基于三角测距的低成本激光雷达RPLIDAR A1。该激光雷达在每次测距过程中，发射器发射红外激光信号，视觉采集系统接收激光反射信号。在经过主控实时解算后，将激光雷达几何中心到被测物体的距离值以及当时的角度值，通过主控的通信接口发出。RPLIDAR A1 工作原理如图所示。
激光雷达在整个SLAM和导航中起着不可替代的作用。 第一，通过激光雷达观测数据与地图进行匹配，估计出机器人的位姿 ；第 二，当机器人估计出较准确的位姿时，通过激光雷达的观测数据建立环境地图 ； 第三，在机器人导航过程中，检测地图中的未知障碍物 。本文主要使用的观测传感器是2D激光雷达RPLIDAR A1如下图所示：
RPLIDAR A1是一款360度激光雷达，角度分辨率≤1，输出的每一个激光点的数据都是使用极坐标的方式描述，一个距离值以及一个对应的角度值。
TurtleBot3 Burger: 运行ROS的强大的模块化机器人!
Turtlebot3是2017年推出的一款新的ROS学习套件，适合于新手入门。
具体参数见下图：
TurtleBot 3 Burger 使用Raspberry Pi 3开发板来跑ROS,通过OpenCR主板控制底部的运动执行部分（两个带编码器的减速电机，差动），顶部有一个激光雷达模块，用于导航。机体采用模块化，堆叠式的方式组合。
1.Raspberry Pi 3 中安装系统版本为：Ubuntu Mate
集成的 ROS版本为： Kinetic
2.Open CR 是TurtleBot3的主控制器板，STM32F7系列是主芯片，具有非常强大的ARM Cortex-M7浮点单元，开发环境为：Arduino IDE
具体参数，见下图：
本篇只是简单介绍一下 TurtleBot 3 Burger的组成部分，从下一篇开始会依次介绍TurtleBot 3 Burger的开发过程！
大致分为三个部分：
1.OpenCR板的固件安装；(下位机)
2.树莓派的Ubuntu系统的安装和ROS环境安装;（上位机）
3.PC或Mac上Ubuntu系统的安装和ROS环境安装；（控制主机）
下一篇：
玩转 TurtleBot 3 Burger 之OpenCR板的固件安装
内核不同，进口芯片采用二代的M3内核，STM32主要采用一代M3内核。
stm32芯片在程序方面拥有很好的兼容性，使用了相同的Flash，使程序开发及升级变得更加方便。
stm32为ST公司推出的Cortex-M3内核微控芯片，分成两个系列，分别为，工作频率在36MHZ的STM32F101系列和工作频率在72MHZ的STM32F103系列。
P-PJ1接口是外接8v电源输入的接口引脚。给lm2956做输入的。
DC-DC功能是把通过LM2956转换出来的5V通过DC-DC隔离出来一个专门的5V，这个隔离出来的5v在转化为3.3v给stm32供电。
杭州艾豆智能激光SLAM 智能扫地机器人开源系统
一、说明
杭州艾豆智能科技有限公司，专注于机器人的室内定位与导航、自主运动，智能避障和视觉技术的研究。在SLAM算法，扫地机的运动控制，视觉等技术领域有六年多技术积累。
我们致力于为高性能消费级机器人提供室内定位导航及视觉解决方案，主要的产品有：360°扫描激光雷达SLAM定位导航套件，固定式激光雷达定位导航套件，及深度摄像头定位导航套件，陀螺仪惯导套件、智能扫地机器人控制主板、通用型激光SLAM 机器人底盘，智能消毒机器人。
我们积累了丰富的智能机器人室内导航定位系统，产品广泛应用与智能扫地机器人，智能全自动消毒机器人。
杭州艾豆智能，基于激光SLAM的智能扫地机人源码是一套完整的量产的源码。基于STM32和linux下C语言开发，基于本代码可以创建完整的商业级激光SLAM智能扫地机器人。
二、功能简介
1．功能
杭州艾豆智能科技有限公司的激光SLAM智能扫地机人源码，具备完整的智能扫地机器人功能，功能包括。
1）LDS激光雷达360度全方位扫描，10赫兹自适应扫描频率。
2）不低于8米的测距范围，测量量程1%的解析。
3） Class 1 激光安全标准。
4）激光SLAM定位，建图，导航功能。
5）快速全屋扫描地图算法，快速生成室内二维地图。
6）弧形掉头，工字清扫功能。
7）沿边清扫功能，支持激光沿边和红外沿边。
8）红外碰撞，碰撞块处理机制。
9）悬崖传感器，跌落计算处理。
10） 以房间为单位划分区域，智能清扫策略，路径规划功能。
11） 计算导航路径，导航算法。
12） 后轮电机、滚刷电机、边刷电机，风机的驱动和调速功能。
13） 后轮电机、滚刷电话，边刷电机，风机堵转保护功能。
14） 自动回充对接充电桩功能。
15） APP地图显示、控制功能。
16） 遥控器功能
17） 手动遥控功能
18） 智能避障
19） 预约功能
20） 虚拟墙功能
21） 禁区功能
22） 指哪去哪功能。
23） 区域清扫功能
24） 断点续扫功能。
25） 智能语音功能。
26） OTA远程升级功能
2．技术特色
杭州艾豆智能科技有限公司激光SLAM智能扫地机器人采用自持专利算法，有别其他开源项目。
1) 完全自主知识产权激光SLAM算法。
2) 不采用传统开源SLAM算法，无需操作系统支持，支持裸奔，支持嵌入式linux，抛弃臃肿的ubuntu和ROS系统。
3) 快速识别门和房间，全屋快速扫描，生成地图算法。适用于室内自动扫描建图系统。
4) 快速重定位功能，只要建好图，机器人随便扔，都能快速拟合重定位。
三、系统结构
1．系统结构
本激光SLAM导航智能扫地机器人系统由以下单元组成：
1) 主控系统
2) 激光SLAM算法板
3) 传感器板
4) 电机驱动器
系统结构图如下：
2．主控系统
主控系统采用STM32或GD32系统，用于电机运动控制和清扫策略计算，以及各个传感器的数据采集，分析。
3．激光SLAM算法板
激光SLAM算法板实现SLAM算法，房屋识别，门识别，分区算法。
4．传感器模块
传感器模块用于采集各种外围传感器数据。
电机控制模块
电机控制模块用于控制左行动轮，右行动轮，边刷电机，滚刷电机，风机，并通过编码器反馈形成电机闭环控制。
反馈监视电机电流，以便主控系统计算电机堵转。
四、快速入门
1．快速使用
从艾豆智能科技有限公司获取到激光SLAM智能扫地机器人源码。源码分三部分：
1) 主控板源码，采用Keil uVision编译。
2) SLAM算法源码，基于linux编译。
3) APP代码，基于linux编译。
第一步：打开主控板源码。
使用Keil uVision V5.21.1.0打开“RE830\USER”目录下的irobot.uvprojx 文件，编译，生成hex或bin文件，烧录hex或bin文件。
本代码使用Source Insight编辑，建议使用者也采用SI编辑，如使用Keil编辑，可能会存在代码不对齐的情况。
第二步：打开SLAM源码：
进入slam 的build目录，执行make命令，生成slam文件，将slam
文件通过网络上传到linux算法板的update目录下。
第三步：打开APP源码。
在linux下，进入app的build目录，执行make命令，生成app文件，将app上传到linux算法板的update目录下。
重启系统，听到“系统载入中”的语音，稍后，开始按键，配网，使用APP进行控制，建图。
先将STM32初始化配置：需要初始化USART1和USART2。
再将激光雷达输出数据信息转化为距离：
因为我们使用的是USART2串口接收数据，所以我们先使能串口2的接收中断。
HAL_UART_Receive_IT(huart2,uart,1);
然后设置一些需要用到的量值。
int dist=99;
int check;
int count=10000;
int strength;
uint8_t uart[9];
const int HEADER=0X59;
在回调函数中，我们首先判断第一个数据包帧头，再判断第二个数据包帧头。如果都是0x59，则继续接收剩下数据填满数组，并按照协议对收到的数据进行校验，然后就可以计算对应的参数了。
拓展：
STM32文件夹说明：
Libraries:存放库文件（驱动）
Output:存放生成的项目文件，如HEX文件
Startup:存放启动文件，STM32需要一个启动文件才能工作
User:存放自己编写的函数文件，如main函数，中断函数等
APP:存放一些外设程序
本文标签：stm32和激光雷达
