自动跟随机器人：一种简易的自动跟随方案，自动跟随小车、自动跟随平衡小车、STM32、基于超声波的自动跟随小车

目的：一种廉价的跟随方案，让大家都能够参与进来，技术难度不大，一些人也能够DIY一些属于自己的“跟随”机器人！并不是要做工业应用什么的。只是做出来玩玩~

/ 1 / 介绍

先看视频，视频中是一个简单的4轮模型的小型机器人，做了一个简单的应用（智能行李车）

自动跟随机器人视频1

自动跟随机器人视频2

在这我们只介绍自动跟随的方案，牵扯到机器人底盘移动等其他控制部分，在这不做过多赘述了。

如果读者有机器人相关控制基础或者做过智能小车类的项目，很容易处理好。没有也没关系，

继续关注我的知乎【张巧龙】或者公众号，后面会开源相关机器人的控制方案。

/ 2 / 硬件方案

选用单发单收的超声波模块进行测距，以此来获取人与车的距离，模块可在某宝上购买，也可自己制作。由于有广告嫌疑，我这里不放购买链接。

（关注微信公号【大鱼机器人】，后台回复【自动跟随】即可获得电路等资料）

在机器人的两端各安装一个单收超声波，人手持一个单发的超声波模块。

这样左侧与右侧离人的距离就构成了一个三角形。简单示意图如下图所示：

当机器人正对着人时，装在机器人上的2个超声波模块，距离A=B，

当人左拐时，A必定小于B，

同理当人右拐时，A大于B。

当人向前走时，A和B的距离必定大于设定距离。我们只需要控制好这几个距离即可。

由此可获得人与机器人的距离以及人的行走路线，来实现机器人的自动跟随功能。

【注意】如果选用下面这收发集成一体的超声波 例如这种：

这种超声波模块也能做跟随，但是非常有限制性，为什么呢？

用收发一体的超声波模块也能做超声波跟随，但是只能做直线跟随和障碍物跟随，也就是说只能跟随前方的物体，并不能跟随特定的人。

因为该超声波只能测得前方障碍物的距离，小车通过前进后退去控制与障碍物的距离从而实现跟随功能。并且，只能使用一个收发一体超声波模块，如果2个超声波同时发射，肯定会互相干扰，因此不能实现三角形跟随算法。

所以只能做直线跟随！并不能转弯跟随！

/ 3 / 基于超声波的跟随算法设计

本设计选用单发单收的超声波模块进行测距，在车上的前方左右两端各安装一个单收超声波模块，人手持一个单发超声波模块，这样左右两端的超声波与移动目标的距离三点之间就构成了一个三角形。

假设左超声波A点距离移动目标C点（超声波发射器）的距离为a，右超声波B点距离移动目标C点的距离为b。

当小车与人正对时，距离a = b；当人左拐时，a < b；同理，当人右拐时，a > b。当人往前方走时，A和B距离人的距离a和b必定大于预先设定距离。综上所述可知人与车的距离，由此来实现小车的自动跟随功能。

/ 4/ 程序设计 4.1 主程序思路

4.2 跟随关键程序

选当获取到A、B距离后，如何控制呢？

对距离做一个简易的PID中的P控制即可。

#include "control.h"	

int line_Pwm,Turn_Pwm;//计算出来的pwm

int  Moto1,Moto2;//赋给电机的pwm值

void	TIM3_IRQHandler(void)   //TIM3中断
{
		if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源 
		{
			TIM_ClearITPendingBit(TIM3, TIM_IT_Update  )                                     ;  //清除TIMx的中断待处理位:TIM 中断源 
			line_Pwm =line_follow_pwm(distance_left,distance_right);         //===直线跟随环PID控制	
			Turn_Pwm =turns_follow_pwm(distance_left,distance_right);        //===转向跟随环PID控制	
			Moto1=line_Pwm+Turn_Pwm;                 //===计算左轮电机最终PWM
			Moto2=line_Pwm-Turn_Pwm;                 //===计算右轮电机最终PWM
			Set_Pwm(Moto1,Moto2);                                    //===赋值给PWM寄存器  	
			if(distance_left>4000||distance_right>4000||distance_right0)||(line_distance14000) line_pwm=0;   
		return line_pwm;
}
/**************************************************************************
函数功能：转向PD控制
入口参数：电机编码器的值、Z轴角速度
返回  值：转向控制PWM
作    者：张巧龙
**************************************************************************/
int turns_follow_pwm(int distance_left,int distance_right)//转向控制
{
		static int turn_pwm;
		static float difference;			//当人移动时，小车左右接收超声波的距离必不相等，此时产生的偏差将成为跟随转向PID的入口参数
		float kP_ultrasonic_turn=45;				//转向跟随环PID的P参数
		difference=distance_left-distance_right;//计算出左右超声波的差
	
		//=============自动跟随部分=======================//
		if(distance_left>20&&distance_left20&&distance_right