那些你必须知道的关于机器人的基础知识
我的图书馆
那些你必须知道的关于机器人的基础知识
机器人的历史并不算长，1959年美国英格伯格和德沃尔制造出世界上第一台工业机器人，机器人的历史才真正开始。英格伯格在大学攻读伺服理论，这是一种研究运动机构如何才能更好地跟踪控制信号的理论。德沃尔曾于1946年发明了一种系统，可以“重演”所记录的机器的运动。1954年,德沃尔又获得可编程机械手专利，这种机械手臂按程序进行工作，可以根据不同的工作需要编制不同的程序，因此具有通用性和灵活性，英格伯格和德沃尔都在研究机器人，认为汽车工业最适于用机器人干活，因为是用重型机器进行工作，生产过程较为固定。1959年，英格伯格和德沃尔联手制造出第一台工业机器人。
机器人的分类
关于机器人如何分类，国际上没有制定统一的标准，有的按负载重量分，有的按控制方式分，有的按自由度分，有的按结构分，有的按应用领域分。一般的分类方式：
示教再现型机器人：通过引导或其它方式，先教会机器人动作，输入工作程序，机器人则自动重复进行作业。
数控型机器人：不必使机器人动作，通过数值、语言等对机器人进行示教，机器人根据示教后的信息进行作业。
感觉控制型机器人：利用传感器获取的信息控制机器人的动作。
适应控制型机器人：机器人能适应环境的变化，控制其自身的行动。
学习控制型机器人：机器人能“体会”工作的经验，具有一定的学习功能，并将所“学”的经验用于工作中。
智能机器人：以人工智能决定其行动的机器人。
我国的机器人专家从应用环境出发，将机器人分为两大类，即工业机器人和特种机器人。所谓工业机器人就是面向工业领域的多关节机械手或多自由度机器人。而特种机器人则是除工业机器人之外的、用于非制造业并服务于人类的各种先进机器人，包括：服务机器人、水下机器人、娱乐机器人、军用机器人、农业机器人、机器人化机器等。在特种机器人中，有些分支发展很快，有独立成体系的趋势，如服务机器人、水下机器人、军用机器人、微操作机器人等。目前，国际上的机器人学者，从应用环境出发将机器人也分为两类：制造环境下的工业机器人和非制造环境下的服务与仿人型机器人，这和我国的分类是一致的。
机器人的优缺点
机器人使用的优点：
机器人和自动化技术在多数情况下可以提高生产率，安全性，效率，产品质量和产品的一性；
机器人可以在危险的环境下工作，而无需考虑生命保障或安全的需要；
机器人无需舒适的环境，例如考虑照明，空调，通风以及噪音隔离等。
机器人能不知疲倦，不知厌烦地持续工作，他们不会有心理问题，做事不拖沓，不需要医疗保险或假期；
机器人除了发生故障或磨损外，将始终如一地保持精确度；
机器人具有比人高得多的精确度。直线位移精度可达千分之几英寸（1英寸= 2.54cm），新型的半导体晶片处理机器人具有微英寸级的精度；
机器人和其附属设备及传感器具有某些人类所不具备的能力；
机器人可以同时响应多个激励或处理多项任务，而人类只能响应一个现行激励。
机器人使用的负面：
机器人替代了工人，由此带来经济和社会问题；
机器人缺乏应急能能力，除非该紧急情况能够预知并已在系统中设置了应对方案，否则不能很好地处理紧急情况。同时，还需要有安全措施来确保机器人不会伤害操作人员以及与他一起工作的机器（设备）。这些情况包括：不恰当或错误的反应、缺乏决策的能力、断电、机器人或其它设备的损伤、人员伤害；
机器人尽管在一定情况下非常出众，但其能力在以下方面仍具有局限性（与人相比），表现在：自由度、灵巧度、传感器能力视觉系统、实时响应。
机器人的组成部件
机器人作为一个系统，它由如下部件构成：
机械手或移动车，这是机器人的主体部分，由连杆，活动关节以及其它结构部件构成，使机器人达到空间的某一位置。如果没有其它部件，仅机械手本身并不是机器人。（相当于人的身体或手臂）；
末端执行器，连接在机械手最后一个关节上的部件，它一般用来抓取物体，与其他机构连接并执行需要的任务。机器人制造上一般不设计或出售末端执行器，多数情况下，他们只提供一个简单的抓持器。（相当于人的手）
末端执行器安装在机器人上以完成给定环境中的任务，如焊接，喷漆，涂胶以及零件装卸等就是少数几个可能需要机器人来完成的任务。通常，末端执行器的动作由机器人控制器直接控制，或将机器人控制器的信号传至末端执行器自身的控制装置（如PLC）；
驱动器，驱动器是机械手的“肌肉”。常见的驱动器有伺服电机，步进电机，气缸及液压缸等，也还有一些用于某些特殊场合的新型驱动器，它们将在第6章进行讨论。驱动器受控制器的控制。
传感器，传感器用来收集机器人内部状态的信息或用来与外部环境进行通信。机器人控制器需要知道每个连杆的位置才能知道机器人的总体构型。人即使在完全黑暗中也会知道胳膊和腿在哪里，这是因为肌腱内的中枢神经系统中的神经传感器将信息反馈给了人的大脑。大脑利用这些信息来测定肌肉伸缩程度进而确定胳膊和腿的状态。对于机器人，集成在机器人内的传感器将每一个关节和连杆的信息发送给控制器，于是控制器就能决定机器人的构型。机器人常配有许多外部传感器，例如视觉系统，触觉传感器，语言合成器等，以使机器人能与外界进行通信。
控制器，机器人控制器从计算机获取数据，控制驱动器的动作，并与传感器反馈信息一起协调机器人的运动。假如要机器人从箱柜里取出一个零件，它的第一个关节角度必须为35°，如果第一关节尚未达到这一角度，控制器就会发出一个信号到驱动器（输送电流到电动机），使驱动器运动，然后通过关节上的反馈传感器（电位器或编码器等）测量关节角度的变化，当关节达到预定角度时，停止发送控制信号。对于更复杂的机器人，机器人的运动速度和力也由控制器控制。机器人控制器与人的小脑十分相似，虽然小脑的功能没有人的大脑功能强大，但它却控制着人的运动。
处理器，处理器是机器人的大脑，用来计算机器人关节的运动，确定每个关节应移动多少和多远才能达到预定的速度和位置，并且监督控制器与传感器协调动作。处理器通常就是一台计算机（专用）。它也需要拥有操作系统，程序和像监视器那样的外部设备等。
软件，用于机器人的软件大致有三块。第一块是操作系统，用来操作计算机。第二块是机器人软件，它根据机器人运动方程计算每一个关节的动作，然后将这些信息传送到控制器，这种软件有多种级别，从机器语言到现代机器人使用的高级语言不等。第三块是例行程序集合和应用程序，它们是为了使用机器人外部设备而开发的（例如视觉通用程序），或者是为了执行特定任务而开发的。
在许多系统中，控制器和处理器放置在同一单元中。虽然这两部分放在同一装置盒内甚至集成在同一电路中，但他们有各自的功能。
机器人的性能指标
以下几项用来定义机器人的性能指标：
负荷能力，负荷能力是机器人在满足其它性能要求的情况下，能够承担的负荷重量。例如，一台机器人的最大负荷能力可能远大于它的额定负荷能力，但是达到最大负荷时，机器人的工作精度可能会降低，可能无法准确地沿着预定的轨迹运动，或者产生额外的偏差。机器人的负荷量与其自身的重量相比往往非常小。例如，Fanuc Robotics LR Mate机器人自身重86磅，而其负荷量仅为6.6磅；M16机器人自身重594磅，而其负荷量仅为35磅。
运动范围，运动范围是机器人在其工作区域内可以达到的最大距离。器人可按任意的姿态达到其工作区域内的许多点(这些点称为灵巧点)。然而，对于其他一些接近于机器人运动范围的极限线，则不能任意指定其姿态(这些点称为非灵巧点)。说明：运动范围是机器人关节长度和其构型的函数。
精度，定义：精度是指机器人到达指定点的精确程度 说明：它与驱动器的分辨率以及反馈装置有关。大多数工业机器人具有0.001英寸或更高的精度。
重复精度，（变化性） 定义：重复精度是指如果动作重复多次，机器人到达同样位置的精确程度。举例：假设驱动机器人到达同一点100次，由于许多因素会影响机器人的位置精度，机器人不可能每次都能准确地到达同一点，但应在以该点为圆心的一个圆区范围内。该圆的半径是由一系列重复动作形成的，这个半径即为重复精度。说明：重复精度比精度更为重要，如果一个机器人定位不够精确，通常会显示一固定的误差，这个误差是可以预测的，因此可以通过编程予以校正。举例：假设一个机器人总是向右偏离0.01mm，那么可以规定所有的位置点都向左偏移0.01mm英寸，这样就消除了偏差。说明：如果误差是随机的，那它就无法预测，因此也就无法消除。重负精度限定了这种随机误差的范围，通常通过一定次数地重复运行机器人来测定。
机器人学是一个交叉学科，涉及的知识范围非常广泛，各位金粉如果想了解更详细的机器人相关知识，还需要阅读更多的相关书籍。在这里小编为金粉推荐两本机械工业出版社出版的两本机器人基础知识的图书，希望能够对大家了解机器人行业有所帮助！
《机器人概论》，2011
图书介绍：系统而全面地介绍了机器人的基本知识，适合刚刚进入机器人领域人员，本书出版后即被多所院校选为机器人教学教材。
《机器人概论》生动地介绍了机器人的起源、发展、分类、应用、组成、功能及应用前景，较系统地介绍了机器人技术的基础知识，在相关章节介绍了若干机器人应用实例，如特种机器人、生物生产机器人、足球机器人、仿生机器人等。并结合大学生的特点，介绍了机器人大赛的有关知识，比较全面地反映了国内外机器人研究和应用的最新进展。
发表评论：
馆藏&22634
TA的最新馆藏大家好，在下又回来了。今天开一个新坑，算是小小地总结一下之前的工作。
在这个系列教程中，我会尝试教大家一步一步从底层开始，构建属于自己的移动机器人。为了开发的简单方便，上层使用了装有ROS（robot operating&system）的linux板卡和台式电脑（台式机），而下层使用了STM32F407作为MCU的嵌入式系统，并且使用了开源项目HANDS-FREE[2]的大部分代码并在此基础上做了一些移植工作。
直入主题。轮式机器人可以简单定义为以轮子作为运动机构的移动机器人。可以将轮式机器人分为两轮（平衡车），三轮和四轮，当然还有六轮的形式。其实都大同小异。
下表引自[1]。
除了两轮车外，其他轮式的移动机器人都是天然鲁棒系统，所以这对高性能控制的要求几乎为零。
明显地，这是一个项目教程，因此我将以三轮全向轮式移动机器人为例，讲一讲如何从零完成一个机器人项目。
第一部分：机械
淘宝搜索&全向移动机器人&，本例中用的是这款
注意，电机为普通的直流电机，因为是室内移动机器人，机器人的速度小于1m/s，速度不需要很高。要求每个电机上都要有里程计（编码器），目的是获得小车上每个电机的里程与比较粗糙的速度信息。
第二部分：嵌入式系统-硬件
为了解除广告之嫌，我不会放链接。如果有兴趣请留言联系我。
首先，为了要控制电机，需要两到三个电机驱动。我选择的是具有L298N逻辑的双路电机驱动，满足三个电机的控制要求。淘宝上随便找种L298N模块即可。&
电源系统：需要给单片机最小系统板提供5v供电，仅需要一个能提供5v的LM25XX开关电源模块即可。非常轻松愉快。
单片机最小系统板：一个STM32F407VET6最小系统板即可。需要板上自带能提供3.3v的LDO，还有三到四个开关和两盏LED，可以不需要RTC电池。
在Embedded/doc/下会此版的原理图。
传感器：最少需要一个MPU6050模块，一个HMC5883L模块。这是为了融合出机器人的yaw，这是三个机器人的状态变量之一。单独一个MPU6050测出的yaw角度会随着时间漂移，需要HMC5883磁力计矫正。最好可以有一个到两个普通的超声波测距模块（如HC-SR04）,主要作为壁障用。下图是集成了MPU6050，HMC5883，BMP180的GY-87模块
电池：12V动力锂电池即可。&
其他：①舵机：普通数字舵机或者高级的带串口的数字舵机均可，可以组成简单机械臂或者机器人头；
第三部分：嵌入式系统-软件
本例中的代码结构和大部分的源代码来自开源工程HANDS-FREE。它是一个由西北工大的团队维护的开源项目。在本文中，我会比较详尽的对代码进行讲解。
首先整体介绍以下嵌入式软件部分。这里没有使用实时操作系统。开发环境为keil-ARM V5.17+ windows 10 enterprise。使用C++编程。
[这里为嵌入式软件整体功能以及算法流程图]
工程文件结构如下
USER文件夹内主要是用户文件。Start_Code文件夹内主要是底层库函数代码。而BSP文件夹内放有用户库函数。剩下的就是外设功能包。
首先介绍一下主函数。
//main.cppint main(void)
constructorInit();
systemInit();
if(usart1_queue.emptyCheck()==0){
hf_link_pc_node.byteAnalysisCall(usart1_queue.getData());
if ( t_1ms &= 1 )
imu.topCall();
// stm32f4 -- 631us(fpu)
if ( t_2ms &= 2 )
if ( t_5ms &= 5 )
if ( t_10ms &= 10 )
board.boardBasicCall();
// need time stm32f1
if ( t_20ms &= 20 )
t_20ms = 0 ;
motor_top.motorTopCall();
//motor speed control
if ( t_50ms &= 50 )
t_50ms = 0 ;
//robot_head.headTopCall();
hands_free_robot.robotWheelTopCall();
//robot control interface
if ( t_500ms &= 500 )
t_500ms = 0 ;
//robot_head.headTopCall();
board.setLedState(0,2);
可以大致看到整个机器人底层的执行流程：先进行系统架构的初始化和系统外设初始化。然后进入主循环：每一毫秒都会检查接收串口1数据的队列是否有数，如果有数据就对数据帧作分析，否则就更新imu的数据并处理（或者只将数据更新之后交由上位机处理）；每10ms更新一次系统数据（CPU温度，系统时间和电池电压）；每20ms控制一次电机（对电机底层的控制）；每50ms控制一次机器人运动（对机器人姿态的控制）；每500ms翻转一下led电平（指示系统正在运行）。
下面简单讲解一下系统架构初始化和系统外设初始化函数。
下面是系统外设初始化。
//main.cppvoid constructorInit(void)
board = Board();
Ultrasonic = ULTRASONIC();
my_robot = RobotAbstract();
motor_top = MotorTop();
//robot_head = HeadAX();
hands_free_robot = RobotWheel();
//sbus = SBUS();
imu = IMU();
usart1_queue = Queue();
hf_link_pc_node =
HFLink(0x11,0x01,&my_robot);
hf_link_node_pointer=&hf_link_pc_
这里将描述系统外设的所有抽象类进行初始化，我将以board类为例大致讲一讲C++在嵌入式编程中的具体运用，因为确实好用。
//board.hclass Board
float cpu_
float cpu_
float battery_
float system_
//system working time (unit:us), start after power-on
uint8_t system_ //state of system: 0--&not initialize
1--&initialized
uint16_t cnt_1
uint16_t cnt_2
uint16_t cnt_5
uint16_t cnt_10
uint16_t cnt_20
uint16_t cnt_50
uint16_t cnt_500
uint32_t chipUniqueID[3];
uint16_t flashS
//Unit: KB
void boardBasicInit(void);
void boardBasicCall(void);
/**********************************************************************************************************************/
void debugPutChar(uint8_t tx_byte_);
//system support functions
void setLedState(uint8_t led_id, uint8_t operation);
void setBeepState(uint8_t operation);
uint8_t getKeyState(uint8_t key_id){return key_state[key_id] ;}
/**********************************************************************************************************************/
void motorInterfaceInit(uint8_t motor_id , float motor_pwm_T); //package "PAKG_MOTOR" support functions
void motorEnable(uint8_t motor_id);
void motorDisable(uint8_t motor_id);
void motorEnableForward(uint8_t motor_id);
void motorEnableBackward(uint8_t motor_id);
float getMotorEncoderCNT(uint8_t motor_id);
float getMotorCurrent(uint8_t motor_id);
void motorSetPWM(uint8_t motor_id , int pwm_value);
/**********************************************************************************************************************/
void axServoInterfaceInit(void); //package " PAKG_SERVO" support
void axServoTxModel(void);
void axServoRxModel(void);
void axServoSendTxByte(uint8_t tx_byte);
uint8_t axServoGetRxByte(uint8_t *error);
/**********************************************************************************************************************/
void sbusInterfaceInit(void); //package " SBUS_PPM" support
void ppmInterfaceInit(void);
/**********************************************************************************************************************/
//package " ANALOG_SERVO" support
void analogServoInterfaceInit(void);
void analogServoSetPWM(uint8_t servo_id , int pwm_value);
/**********************************************************************************************************************/
void imuI2CInit(void);
//package "PAKG_IMU" support
void imuI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
uint8_t reg_data , uint8_t fastmode);
uint8_t imuI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
uint8_t imuI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t* pt_char , uint8_t size , uint8_t fastmode);
uint8_t imuI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t * pt_char , uint8_t size , uint8_t fastmode);
void gpsInterfaceInit(void);
void gpsSendTxByte(uint8_t tx_byte);
/**********************************************************************************************************************/
void eepromI2CInit(void); //package "AT24Cxx" support functions
void eepromI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
uint8_t reg_data , uint8_t fastmode);
uint8_t eepromI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
uint8_t eepromI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t* pt_char , uint8_t size , uint8_t fastmode);
uint8_t eepromI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t * pt_char , uint8_t size , uint8_t fastmode);
/**********************************************************************************************************************/
//extend interface support functions
void extendI2CInit(void);
// extend iic interface is using for extend other sensors
void extendI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
uint8_t reg_data , uint8_t fastmode);
uint8_t extendI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
uint8_t extendI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t* pt_char , uint8_t size , uint8_t fastmode);
uint8_t extendI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
uint8_t* pt_char , uint8_t size , uint8_t fastmode);
//extend spi interface
//extend can interface
//extend PWM interface
//mode=1 high
mode = 2 for
electric modulation
void pwmInterfaceInit(uint8_t mode , float pwm_t);
void setPWMValue(uint8_t channel_x , float pwm_value);
//channel_x 1~6
/**********************************************************************************************************************/\
float battery_voltage_;
float battery_voltage_alarm_ ;
float battery_proportion_ ;
float temperature_;
uint16_t beep_alarm_cnt_ ;
uint16_t board_call_i , board_call_j , board_call_k;
uint8_t key_state[5];
void systemMonitoring(void);
void ledInit(void);
void keyInit(void);
void keyStateRenew(void);
void beepInit(void);
void debugInterfaceInit(void);
float getBatteryVoltage(void);
float getCPUUsage(void);
float getCPUTemperature(void);
void getCPUInfo(void);
C++是一种面向对象的编程语言，C++ 在 C 语言的基础上增加了面向对象编程，C++ 支持面向对象程序设计。类是 C++ 的核心特性，通常被称为用户定义的类型。[3]
类本身是抽象的。定义一个类本质上是定义了一个数据类型的蓝图。程序员将一种事物定义为一种类，然后把这个类的属性与能进行的操作包含在类下。如此例，定义board为描述主控板的类。public标识符下的数据成员为共有的，也就是说其他的类可以调用这些数据成员。而private下则是私有的，也就是说这些数据成员只能在这个类里面使用。主要的原因是C++数据封装与信息隐藏的功能。在大型项目的开发中，代码应该简明并且合理，应该在类里完成的工作就让他在类里面完成就行了。
在这个类下，定义了板子的初始化函数（这里初始化了单片机的时钟和开发板的外设，如定时器、key、led、蜂鸣器、ADC模块等）
//control_unit_v2.cppvoid Board::boardBasicInit(void)
for(i=0;i&0x8fff;i++);
HF_System_Timer_Init();
//Initialize the measurement systemm
debugInterfaceInit();
ledInit();
keyInit();
beepInit();
//HF_RTC_Init();
//Initialize the RTC, if return 0:failed,else if return 1:succeeded
//HF_IWDG_Init();
//Initialize the independed watch dog, system will reset if not feeding dog over 1s
HF_ADC_Moder_Init(0X3E00 , 5 , 2.5f);
//ADC init
HF_Timer_Init(TIM6 , 1000);
//timer6 init , 1000us
setBeepState(1);
delay_ms(500);
setBeepState(0);
analogServoInterfaceInit();
后面还提供了可供用户调用的其他外设包的函数，比如设定led状态的
void setLedState(uint8_t led_id, uint8_t operation);
和设定使能和失能电机的
void motorEnable(uint8_t motor_id);
void motorDisable(uint8_t motor_id);
其他类的具体实现可以参考其源码。
回到主函数，我们这时其实还没有进行任何初始化，只是对类进行了个&抽象&的初始化（具体的初始化还没有实现），接下来就开始进行真正的硬件外设初始化。
//main.cppvoid systemInit(void)
//INTX_DISABLE();
//close all interruption
board.boardBasicInit();
motor_top.motorTopInit(3 , 1560 , 0.02 , 0);
//robot_head.axServoInit();
hands_free_robot.robotWheelTopInit();
//sbus.sbusInit();
imu.topInit(1,1,1,0,0,1);
Ultrasonic.Ultra_Init(1);
//INTX_ENABLE();
//enable all interruption
printf("app start \r\n");
这里的所有函数除了printf，都是各种外设的初始化。上面已经提到了boardBasicInit()，那就以Motor_top类下的motorTopInit()为例吧。
//motor_top.cppvoid MotorTop::motorTopInit(float motor_enable_num_ , float motor_encoder_num_ ,
float motor_pid_t_
, unsigned char motor_simulation_model_)
motor_enable_num = motor_enable_num_;
motor_encoder_num = motor_encoder_num_;
motor_pid_t = motor_pid_t_;
board.motorInterfaceInit(1, motor_pwm_max); //motor_x init
motor1.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
motorPWMRenew(1,0);
board.motorInterfaceInit(2, motor_pwm_max);
motor2.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
motorPWMRenew(2,0);
if(motor_enable_num &= 3)
board.motorInterfaceInit(3, motor_pwm_max);
motor3.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
motorPWMRenew(3,0);
if(motor_enable_num &= 4)
board.motorInterfaceInit(4, motor_pwm_max);
motor4.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
motorPWMRenew(4,0);
这个函数有两大功能，一是初始化STM32的TIM1，二是初始化里程计（编码器）用到的TIM2、TIM3、TIM4定时器。具体的实现函数为motorInterfaceInit()
//control_unit_v2.cppvoid Board::motorInterfaceInit(uint8_t motor_id , float motor_pwm_T)
GPIO_InitTypeDef
GPIO_InitS
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
if(motor_id == 1 ){
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_10;
GPIO_Init(GPIOE , &GPIO_InitStructure);
GPIO_ResetBits(GPIOE , GPIO_Pin_8);
GPIO_ResetBits(GPIOE , GPIO_Pin_10);
HF_Encoder_Init(TIM2,0);
//encoder init for PID speed control
else if(motor_id == 2){
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOC , &GPIO_InitStructure);
GPIO_ResetBits(GPIOC , GPIO_Pin_0);
GPIO_ResetBits(GPIOC , GPIO_Pin_1);
HF_Encoder_Init(TIM3,2);
else if(motor_id == 3){
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
GPIO_Init(GPIOC , &GPIO_InitStructure);
GPIO_ResetBits(GPIOC , GPIO_Pin_2);
GPIO_ResetBits(GPIOC , GPIO_Pin_3);
HF_Encoder_Init(TIM4,1);
else if(motor_id == 4){
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_Init(GPIOE , &GPIO_InitStructure);
GPIO_ResetBits(GPIOE , GPIO_Pin_15);
HF_Encoder_Init(TIM5,0);
//motor_pwm_T = 5000 , TIM1 motor pwm frequency
= (168M/4) / motor_pwm_T
HF_PWMOut_Init(TIM1 , 2-1 , motor_pwm_T , 1);
//motor_pwm_T = 5000 , TIM9 motor pwm frequency
= (168M/4) / motor_pwm_T = 16.8K
HF_PWMOut_Init(TIM9 , 2-1 , motor_pwm_T , 0);
//motor_pwm_T = 5000 , TIM12 motor pwm frequency = (84M/2) / motor_pwm_T
HF_PWMOut_Init(TIM12 , 0 , motor_pwm_T , 0);
因为使用了L298N逻辑的电机驱动，所以一路电机需要两个GPIO和一路PWM控制，因此上面这个函数主要是初始化使用的gpio和pwm功能的定时器TIM1，因为TIM1能发出四路不同的PWM信号，因此，三个电机仅仅需要一个TIM1。编码器也用到了TIM，具体的代码解析就不做了，[4]为介绍STM32编码器接口库函数的解析文档，讲得不错。
回到systemInit函数。剩下分别初始化了motorTop的一些pid参数。这个函数属于RobotWheel类，这个类主要包含了一些控制机器人姿态和运动的接口函数。它主要将上层发出的期望机器人速度或位置信号转换成三个电机的控制信号。imu.topInit()主要完成了IMU的初始化。Ultra_Init完成了超声波模块的初始化。
接下来开始程序主循环的介绍。
首先每次都检查队列（queue）中的数据，调用queue.emptyCheck()。因为每次串口收到数据都会进入串口中断接收数据并保存在队列中，具体的代码实现如下：
//stm32f4xx_it.cppvoid USART1_IRQHandler(void)
unsigned char
if(USART1-&SR&(1&&5))
data=USART1-&DR;
if(usart1_queue.fullCheck()==0){
usart1_queue.putData(data);
USART_ClearITPendingBit(USART1, USART_IT_RXNE);
// clear interrupt flag
如果检测到队列中有数，就要开始对数据帧做解析了，实现函数如下。这个函数先将协议去除，获得上位机发出的命令所代表的字，然后在通过调用packageAnalysis()对命令一一对应地做出相应。
//hf_link.cppunsigned char HFLink::byteAnalysisCall(const unsigned char rx_byte)
unsigned char package_update=0;
unsigned char receive_message_update=0;
receive_message_update=receiveFiniteStates(rx_byte) ;
//Jump communication status
if( receive_message_update ==1 )
receive_message_update = 0;
receive_message_count++;
package_update=packageAnalysis();
if(package_update==1) receive_package_count++;
return package_
下面就是去除协议帧的函数。它是通过一个状态机来解析协议的。
//hf_link.cppunsigned char HFLink::receiveFiniteStates(const unsigned char rx_data)
switch (receive_state_)
//协议解析状态机
case WAITING_FF1:
if (rx_data == 0xff)
//接收到帧头第一个数据
receive_state_ = WAITING_FF2;
receive_check_sum_ =0;
receive_message_length_ = 0;
byte_count_=0;
receive_check_sum_ += rx_
break;　　　　　　　　　　//状态机复位
case WAITING_FF2:
if (rx_data == 0xff)　　　　//接收到帧头第二个数据，下面以此类推
receive_state_ = SENDER_ID;
receive_check_sum_ += rx_
receive_state_ = WAITING_FF1;
case SENDER_ID:
rx_message_.sender_id = rx_
if (rx_message_.sender_id == friend_id)
//id check
receive_check_sum_ += rx_
receive_state_ = RECEIVER_ID;
receive_state_ = WAITING_FF1;
case RECEIVER_ID:
rx_message_.receiver_id = rx_
if (rx_message_.receiver_id == my_id)
//id check
receive_check_sum_ += rx_
receive_state_ = RECEIVE_LEN_H;
receive_state_ = WAITING_FF1;
case RECEIVE_LEN_H:
receive_check_sum_ += rx_
receive_message_length_ |= rx_data&&8;
receive_state_ = RECEIVE_LEN_L;
case RECEIVE_LEN_L:
receive_check_sum_ += rx_
receive_message_length_ |= rx_
rx_message_.length = receive_message_length_;
receive_state_ = RECEIVE_PACKAGE;
case RECEIVE_PACKAGE:
receive_check_sum_ += rx_
rx_message_.data[byte_count_++] = rx_
if(byte_count_ &= receive_message_length_)
receive_state_ = RECEIVE_CHECK;
receive_check_sum_=receive_check_sum_%255;
case RECEIVE_CHECK:
if(rx_data == (unsigned char)receive_check_sum_)
//对比发送和接收的校验和，如果不一致直接放弃此数据帧
receive_check_sum_=0;
receive_state_ = WAITING_FF1;
return 1 ;　　　　　　　　　　　　　　　　　　　　　//传回1，表示一个数据包收到
receive_state_ = WAITING_FF1;
receive_state_ = WAITING_FF1;
这里的协议为：0XFF 0XFF sender_id receiver_id length_H length_L& ****(data)& check_sum。
&解析完协议就要开始分析命令了。由packageAnalysis()来实现这一步工作。
//hf_link.cpp
unsigned char HFLink::packageAnalysis(void)
unsigned char analysis_state=0;
void* single_
command_state_ = (Command)rx_message_.data[0];
if (hf_link_node_model== 0)
//the slave need to check the SHAKING_HANDS"s state
if(shaking_hands_state==0 && command_state_ != SHAKING_HANDS) //if not
shaking hands
sendStruct(SHAKING_HANDS
, (unsigned char *)single_command, 0);
switch (command_state_)
case SHAKING_HANDS :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_global_coordinate , sizeof(my_robot-&measure_global_coordinate));
case READ_ROBOT_SYSTEM_INFO :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&robot_system_info , sizeof(my_robot-&robot_system_info));
case SET_GLOBAL_SPEED :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_global_speed , sizeof(my_robot-&expect_global_speed));
case READ_GLOBAL_SPEED :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_global_speed , sizeof(my_robot-&measure_global_speed));
case SET_ROBOT_SPEED :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_robot_speed , sizeof(my_robot-&expect_robot_speed));
case READ_ROBOT_SPEED :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_robot_speed , sizeof(my_robot-&measure_robot_speed));
case SET_MOTOR_SPEED :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_motor_speed, sizeof(my_robot-&expect_motor_speed));
case READ_MOTOR_SPEED :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_motor_speed , sizeof(my_robot-&measure_motor_speed));
case READ_MOTOR_MILEAGE :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_motor_mileage , sizeof(my_robot-&measure_motor_mileage));
case READ_GLOBAL_COORDINATE :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_global_coordinate , sizeof(my_robot-&measure_global_coordinate));
case READ_ROBOT_COORDINATE :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_robot_coordinate , sizeof(my_robot-&measure_robot_coordinate));
case CLEAR_COORDINATE_DATA :
if (hf_link_node_model==0)
sendStruct(command_state_ , (unsigned char *)single_command , 0);
analysis_state=1;
receive_package_renew[(unsigned char)command_state_] = 1 ;
case SET_ARM_1 :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_arm1_state, sizeof(my_robot-&expect_arm1_state));
case READ_ARM_1 :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_arm1_state , sizeof(my_robot-&measure_arm1_state));
case SET_ARM_2 :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_arm2_state, sizeof(my_robot-&expect_arm2_state));
case READ_ARM_2 :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_arm2_state , sizeof(my_robot-&measure_arm2_state));
case SET_HEAD_1 :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_head1_state , sizeof(my_robot-&expect_head1_state ));
case READ_HEAD_1 :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_head1_state , sizeof(my_robot-&measure_head1_state));
case SET_HEAD_2 :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&expect_head2_state, sizeof(my_robot-&expect_head2_state));
case READ_HEAD_2 :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_head2_state , sizeof(my_robot-&measure_head2_state));
case READ_IMU_DATA :
analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&measure_imu_euler_angle , sizeof(my_robot-&measure_imu_euler_angle));
case SET_ROBOY_PARAMETERS :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&robot_parameters, sizeof(my_robot-&robot_parameters));
case SAVE_ROBOY_PARAMETERS :
if (hf_link_node_model==0)
sendStruct(command_state_ , (unsigned char *)single_command , 0);
analysis_state=1;
receive_package_renew[(unsigned char)command_state_] = 1 ;
case SET_MOTOR_PARAMETERS :
analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot-&motor_parameters, sizeof(my_robot-&motor_parameters));
case SAVE_MOTOR_PARAMETERS :
if (hf_link_node_model==0)
sendStruct(command_state_ , (unsigned char *)single_command , 0);
analysis_state=1;
receive_package_renew[(unsigned char)command_state_] = 1 ;
analysis_state=0;
rx_message_.sender_id=0;
//clear flag
rx_message_.receiver_id=0;
rx_message_.length=0;
rx_message_.data[0]=0;
return analysis_
上位机在一开始时通常会发一帧握手信号，然后开始发送数据。然后在switch结构里，对应每一个命令都有相应的代码做处理。大致可以将处理分为设定数据（setCommandAnalysis）和上传数据（readCommandAnalysis）两个函数。
结束后返回分析状态，标志命令更新。在上述两个函数里，还有对应的发送数据的函数，详细请参考源码。
将收到的数据获得后，主循环进入了实际的控制模式。
首先最需要处理是IMU数据。下面的代码将陀螺仪数据的传输频率设定为500hz，bmp085为100hz。只是将数据传输给上位机，并没有做融合处理。
//imu_top.cppvoid IMU::topCall(void)
imu_call_1++;
imu_call_2++;
imu_call_3++;
imu_call_4++;
imu_call_5++;
if( imu_call_1 &= 2 ) //500HZ
imu_call_1=0;
mpu6050.dataUpdate();
if( imu_call_2 &= 5 ) //200HZ
imu_call_2=0;
if( imu_call_3 &= 10 ) //100HZ
imu_call_3 = 0;
if(bmp085_en == 1) bmp085.dataUpdate();
if(ms611_en == 1) ms611.dataUpdate();
if( imu_call_4 &= 20 ) //50HZ
imu_call_4=0;
if(hmc085_en == 1) hmc5883l.dataUpdate();
if( imu_call_5 &= 50 ) //20HZ
imu_call_5=0;
if( debug_en == 1)
printf("mpuaccx = %.4f
mpuaccy = %.4f mpuaccz = %.4f\r\n" , mpu6050.acc_normal.x , mpu6050.acc_normal.y,mpu6050.acc_normal.z);
printf("hmc_normalx = %.4f
hmc_normaly = %.4f hmc_normalz = %.4f\r\n" , hmc5883l.hmc_normal.x , hmc5883l.hmc_normal.y , hmc5883l.hmc_normal.z);
printf("temperature = %.4f pressure = %.4f altitude = %.4f altitude_offset = %.4f\r\n" , ms611.temperature , ms611.pressure , ms611.altitude , ms611.altitude_offset);
接下来就是频率为100hz的系统数据更新函数，还有50hz的底层电机控制函数。
//motor_top.cppvoid MotorTop::motorTopCall(void)
motorPWMRenew( 1 , motor1.speedControl(expect_angle_speed_m[0] , board.getMotorEncoderCNT(1) ) );
motorPWMRenew( 2 , motor2.speedControl(expect_angle_speed_m[1] , board.getMotorEncoderCNT(2) ) );
if(motor_enable_num &= 3)
motorPWMRenew( 3 , motor3.speedControl(expect_angle_speed_m[2] , board.getMotorEncoderCNT(3) ) );
if(motor_enable_num &= 4)
motorPWMRenew( 4 , motor4.speedControl(expect_angle_speed_m[3] , board.getMotorEncoderCNT(4) ) );
此函数获得此刻编码器的速度数据之后先进行速度控制，实现如下。
//robot_wheel_top.cppfloat MotorControl::speedControl(float expect_speed , float unit_count)
float filter =
0.3 * (50 * pid_t);
expect_angle_speed = (1-filter) * measure_angle_speed + filter * expect_
if(motor_simulation_model == 1)
measure_unit_encoder = (simulation_max_angel_speed/360.0f)
*( pid_parameters_.i_pidout/pwm_max ) * encoder_num * pid_t;
measure_unit_encoder =
//if you using real motor
//expect unit encoder num in one cycle to pid
expect_unit_encoder = ( expect_angle_speed / 360 ) * encoder_num * pid_t;
expect_total_encoder += expect_unit_
//recording total encoder
measure_total_encoder += measure_unit_
//recording total angle for robot coordinate calculation
d_past_angle += (measure_unit_encoder/encoder_num)*360;
past_total_angle+=(measure_unit_encoder/encoder_num)*360;
//calc motor speed
measure_angle_speed
= measure_unit_encoder * 360 / ( encoder_num*pid_t);
//motor speed pid control function
pidOrdinaryCall(expect_total_encoder , measure_total_encoder
, expect_unit_encoder , measure_unit_encoder , pwm_max);
pid_parameters_.i_
一开始，先对车轮速度进行滑动平局滤波，之后计算期望速度，实际速度，期望位置，实际位置。最后将数据送入PID函数，进行位置速度双闭环控制。
之后是20hz的机器人底盘控制robotWheelTopCall()函数。
//robot_wheel_top.cppvoid RobotWheel::robotWheelTopCall(void)
robotDataUpdate();
chassisControl();
// control your robotic chassis
robotCoordCalc();
// for robot localization
headControl();
// control your robotic head
armControl();
// control your robotic arm
&首先机器人会将所有数据更新，包括全局速度（global_speed）、机器人速度（robot_speed）、之前测量的电机转速，位置和期望电机转速，位置、数字舵机的位置（如果有），四个机器人系统数据和IMU数据，最后还有pid数据（如果底层收到上位机的设定参数的请求，便会更新参数）。
然后调用ChassisControl()控制机器人底盘，它先分别判断是否收到新的命令，如果收到便执行相应操作。
最后调用robotCoordCalc()通过电机测量的三个电机的速度与位置计算机器人坐标系的速度位置。
我已经将代码同步到git上，里面有我改好的三轮底盘机器人地层程序。
git：//Embedded/tree/my_pro
在/0_Project/STM32F4/Application_HF_Wheel_Robot_Beta/目录下有已经建好的keil工程，应该没问题。如果有问题请留言。
[1]R.西格沃特等.自主移动机器人导论[M].第二版.李人厚等译.西安：西安交通大学出版社,2013.5
[2]/HANDS-FREE
[3]/cplusplus/cpp-classes-objects.html
[4]/forum.php?mod=viewthread&tid=2411
阅读(...) 评论()