自平衡巡逻摄像车原型开发

整体数据信号交互如下

接下来分三个方面进行详细介绍。

控制单元

该部分原理图如下

MCU

该部分为主控芯片 STM32G071RBT6 相关电路，主要控制引脚如下

• NEST 为单片机的复位按键，USER 为用户自定义按键，可以通过编程赋予特定的按键功能。
• A_SA，A_SB 和 B_SA，B_ SB 这两组具备定时器的编码器功能，需接两个轮子的霍尔编码器输出脚。编码器是一种将角位移或者角速度转换成一连串电数字脉冲的旋转式传感器，开发者们可以通过编码器测量位移或者速度信息。
• PWMA 和 PWMB 这两组具备定时器的 PWM 功能，通过调节占空比来实现电机转速的控制。
• AIN1，AIN2 和 BIN1，BIN2 这两组为普通 IO 口，通过改变高低电平来控制电机正转和反转，具体方式会在下文详细介绍。
• MOTO_OUT3，MOTO_OUT4 和 MOTO_OUT7，MOTO_ OUT8 这两组为单片机的外部中断输入口，通过该信号，可以实现手机 app 控制设备移动。
• ADC 引脚具备单片机的 ADC 功能，通过该引脚可以测量当前电池电压。
• BEEP 引脚用于控制蜂鸣器。
• MPU_ INT 为单片机外部中断输入口，传感器通过该引脚通知单片机读取数据。
• SCL 和 SDA 为传感器的 IIC 接口，单片机通过该接口读取传感器数据。
• SWDIO 和 SWCLK 为单片机的程序下载口。
• USART1_TX 和 USART1_ RX 为通讯串口，用于用户和单片机的数据交互。

POWER

该部分为系统的电源电路。本系统输入电压 VPP 为 12V 左右，VPP 一部分直接给两个电机供电，还有一部分通过 MT2492 DCDC 芯片降压到 5V，给摄像头单元和霍尔编码器模块供电。

其中 MT2492 是一款高效的 DCDC 芯片，转换效率最高可达 96%。该芯片工作时，开关频率为 600KHz，最大的可以输出 2A 电流，输入电压支持 4.5V~16V，具有短路保护和内部失效保护，非常安全可靠。

MT2492 降压得到的 5V 电压，还会通过一颗 LDO 芯片进一步降压到 3.3V，给 MCU 和其他外设供电。

该 LDO 选择了 AMS1117-33，这是一个正向低压降稳压器，内部集成过热保护和限流电路，最大可以输出 1A 电流，输入电压最高允许 12V，输出稳压到 3.3V，输出精度误差仅在 1%以内。

MOTOR

该部分为电机驱动电路和电机接口电路。电机驱动采用 TB6612FNG 驱动芯片，这是东芝半导体公司生产的一款直流电机驱动器件，它具有大电流 MOSFET-H 桥结构，VM 电源电压最大支持 15V，输出平均电流可达 1.2A，峰值电流允许 3.2A，导通电阻也只有 0.5 欧姆，内置过热保护和低压检测电路，PWM 信号输入频率可允许 100KHz。

相比 L298N 的热耗性和外围二极管续流电路，TB6612FNG 无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，利于减小系统尺寸。

TB6612FNG 驱动芯片双通道电路输出，可同时驱动两个电机。两个电机的正负极分别接 MA+，MA- 和 MB+，MB-。

另外，当 STBY 处于高电平的时候，开发者们可利用芯片上的 IN1 和 IN2 这两个输入信号，选择 正转，反转，短路制动和停机四种模式的其中一种模式，真值表如下：

IN1	In2	模式
0	1	反转
1	0	正转
1	1	短路制动
0	0	停机

另外电机上集成了霍尔编码器，因此接口中含有 VCC_ 5V 的电源为其供电。

SENSOR

该部分为传感器相关电路。

这里给大家介绍一款特别实用的芯片 MPU-6050。它是 InvenSense 公司推出的全球首个整合性 6 轴运动处理器件，内带 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二个 IIC 接口，可用于连接外部磁力传感器，利用数字运动处理器（DMP:Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，可以向应用端输出完整的 9 轴姿态融合演算数据。

MPU-6050 对陀螺仪和加速度计分别用了三个 16 位的 ADC（0~65535），将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的。

MPU-6050 三个独立的振动 MEMS 速率陀螺仪，可检测旋转角度 X 轴，Y 轴和 Z 轴。当陀螺仪围绕任何感应轴旋转时，科里奥利效应就会产生电容式传感器检测到的振动。所得到的信号被放大，解调和滤波产生与角速度成比例的电压。该电压使用单独的片内数字化 16 位模数转换器（ADC）对每个轴进行采样。陀螺仪传感器可以全面范围的被数字编程为每秒±250，±500，± 1000 或± 2000 度（dps）。ADC 样本速率可以从每秒 8,000 个采样点编程到每秒 3. 9 个采样点，并且可由用户选择低通滤波器可实现广泛的截止频率。

MPU-6050 的 3 轴加速度计为每个轴使用单独的检测质量。加速沿着一条特定轴在相应的检测质量上引起位移，并且电容式传感器检测到该位移位移有差别。MPU-6050 的架构降低了加速度计的敏感度制造变化以及热漂移。当设备放置在平坦的表面上时，将进行测量在 X 和 Y 轴上为 0g，在 Z 轴上为+ 1g。加速度计的比例因子在工厂进行校准并且在名义上与电源电压无关。每个传感器都有一个专用的 sigma-delta ADC 来提供数字输出。数字输出的满量程范围可以调整到±2g，±4g，± 8g 或±16g。

CAMERA

该部分为摄像头接口相关电路。

FACTORY_ RST 为摄像头的配网按键脚，当需要重新配网时，长按 S3 按键，按提示操作即可。

SPK+ 和 SPK-需要接到摄像头的内置蜂鸣器。

LED_RED 和 LED_ GREEN 为网络状态指示灯，当配上网络时，绿灯常亮，正在连接网络时，红灯绿灯交替闪烁。

BEEP

该部分为蜂鸣器电路。当巡逻摄像头正常工作时发出轻微的提示音，也会在低电量的时候进行声音报警。

点击这里查看完整原理图

点击这里查看完整 PCB

摄像头单元

摄像头采用涂鸦智能出品的 SC012- WD2 摄像头，具体参数如下。

处理器	高性能 DSP 处理器
传感器类型	1/2. 9 英寸 CMOS
最大分辨率	200W 1080P
红外灯	圈灯，红外距离可达 7 米
日夜切换	IR- CUT 自动切换
降噪	3D 降噪
镜头焦距	4mm
视场角（对角）	94 度
光圈	固定，F2.1
视频编码标准	H.264/MJPEG
视频码率	主码流（高清）：1.2Mbps；子码流（标清）：256Kbps
视频帧率	主码流（高清）：1080P@20fps；子码流（标清）：VGA@20fps
音频输入	1 路 Mic
音频输出	1 路 speaker
恢复默认	支持一键恢复默认
移动侦测	支持
SD 卡存储	支持 SD 卡存储连续录像/事件录像，最大支持 128G SD 卡
涂鸦云存储	支持连续录像/事件录像
隐私模式	支持
移动侦测追踪	支持
连接方式	Wi-Fi IEEE 802.11 b/g/n 2.4 GHz
电源输入	DC 5V/2A
工作温度	-10～ 45 度
工作湿度	≤95%

动力单元

本方案采用聚合物锂电池和减速电机作为巡逻摄像头的动力。

锂聚合物电池，又称高分子锂电池，是一种化学性质的电池。相对以前的电池来说，具有能量高、小型化、轻量化的特点。锂聚合物电池具有超薄化特征，可以配合一些产品的需要，制作成不同形状与容量的电池。

电池规格，选择 3S（11.1V），容量在 2200mAH 左右即可。

减速电机就是在直流电机基础上，增加一个高精密的减速器。一般直流电机的转速都是一分钟几千上万转的，增加减速器可以降低电机转速，增加电机扭矩，使得可控性更强。

选择减速电机的时候，电机的额定电压需要和开发者们选择的电池相匹配，常见的 12V 额定电压电机供电范围在 11~ 16V 之间，12V 最佳。

这里推荐一款 MG513P30_ 12V 电机，这款电机额定电压为 12V，1： 30 的减速比，额定输出电流为 0.36A，减速后空载转速为 366rpm 左右，扭矩 1kg*cm，可以允许 3KG 负载，再搭配 65mm 直径轮胎，速度大概 1m/s，完全满足平衡小车的运动需求。

首先，开发者需要采购涂鸦智能的 Wi-Fi 全高清 1080P 云台摄像机 SC012-WD2（若选用其他型号，可以咨询涂鸦相关人员）。

将摄像机的下底座拆除，根据 3 个定位孔，将安装到硬件设计中提到的 PCB 板上，并逐一装好接插件，固定好电池。

将上述硬件与两轮平衡小车组合，便可以得到一个完整的微型巡逻摄像头。

本 Demo 直接使用涂鸦智能 SC012- WD2 摄像头，因此摄像头和 App 相关固件程序，并不需要开发者进行开发。开发者只需要开发底盘小车平衡运动这方面即可。

姿态读取

在控制平衡之前，开发者需要先获取当前平衡姿态，即先读取 MPU-6050 数据，参考如下

void Get_Angle(uint8_t way)
{ 
	float Accel_Y,Accel_Z,Gyro_X,Gyro_Z;
	Temperature=Read_Temperature();      //=== 读取  MPU-6050 内置温度传感器数据，近似表示主板温度。
	if(way==1)                           //=== DMP 的读取在数据采集中断读取，严格遵循时序要求
	{	
			Read_DMP();                      //===读取加速度、角速度、倾角
			Angle_Balance=-Roll;             //===更新平衡倾角
			Gyro_Balance=-gyro[0];            //===更新平衡角速度
			Gyro_Turn=gyro[2];               //===更新转向角速度
			Acceleration_Z=accel[2];         //=== 更新 Z 轴加速度计
	}			
	else
	{
		Gyro_X=(I2C_ReadOneByte(devAddr, MPU-6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr, MPU-6050_RA_GYRO_XOUT_L);    // 读取 Y 轴陀螺仪
		Gyro_Z=(I2C_ReadOneByte(devAddr, MPU-6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr, MPU-6050_RA_GYRO_ZOUT_L);    // 读取 Z 轴陀螺仪
		Accel_Y=(I2C_ReadOneByte(devAddr, MPU-6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr, MPU-6050_RA_ACCEL_YOUT_L); // 读取 X 轴加速度计
		Accel_Z=(I2C_ReadOneByte(devAddr, MPU-6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr, MPU-6050_RA_ACCEL_ZOUT_L); // 读取 Z 轴加速度计
		if(Gyro_X>32768)  Gyro_X-=65536;                       //数据类型转换也可通过short 强制类型转换
		if(Gyro_Z>32768)  Gyro_Z-=65536;                       //数据类型转换
		if(Accel_Y>32768) Accel_Y-=65536;                      //数据类型转换
		if(Accel_Z>32768) Accel_Z-=65536;                      //数据类型转换
		Gyro_Balance=Gyro_X+Gyro_X_OFFSET;                                  //更新平衡角速度
		Accel_Angle=atan2(Accel_Y,Accel_Z)*180/PI;                 //计算倾角	
		Gyro_X=Gyro_X/16.4;                                    //陀螺仪量程转换	
		if(way==2)		  	Kalman_Filter(Accel_Angle,Gyro_X);//卡尔曼滤波	
		else if(way==3)   First_order_Filter(Accel_Angle,Gyro_X);    //互补滤波
		Angle_Balance=angle;                                     //更新平衡倾角
		Gyro_Turn=Gyro_Z+Gyro_Z_OFFSET;                                      //更新转向角速度
		Acceleration_Z=Accel_Z;                                //=== 更新 Z 轴加速度计	
	}
}

这里的 Accel_ Angle 就是通过计算得出的原始角度值。

当然，也可以用公式 Accel_Angle=arcsin(Accel_Y,Accel_G)*180/PI;

其中 Accel_G=sqrt(pow(Accel_X, 2) + pow(Accel_Y, 2) + pow(Accel_Z, 2));

由于物理震动等原因，姿态数据波动较大，难以获取真正的姿态状态，无形中增加了后期平衡算法的难度，如图。

因此，开发者需要先将 MPU-6050 采集到的数据，进行数字滤波处理。

以角度采集为例，这里给大家推荐两种滤波算法，分别是卡尔曼滤波算法和一阶互补滤波算法。

卡尔曼滤波算法

其中函数如下

void Kalman_Filter(float Accel,float Gyro)		
{
	angle+=(Gyro - Q_bias) * dt; //先验估计
	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1]=-PP[1][1];
	Pdot[2]=-PP[1][1];
	Pdot[3]=Q_gyro;
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	angle_dot   = Gyro - Q_bias;	 //输出值(后验估计)的微分=角速度
}

将数据绘制出来，如图

对比发现，加了卡尔曼滤波之后，数据更加平滑，贴近真实值。

一阶互补滤波算法

一阶互补滤波的程序非常简单，如下

void First_order_Filter(float angle_m, float gyro_m)
{
   angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
}

使用的时候，开发者们只需要如下调用函数：

First_order_Filter(Accel_Angle,Gyro_X); 

其中

• Accel_ Angle 就是原始计算出的角度值，具体可以查看上面卡尔曼滤波；
• Gyro_X 就是 X 轴的角速度，直接从传感器读取，不过要注意单位转换，如图所示

对于 MPU-6050，开发者们若选择 FS_SEL=3，即± 2000 度/秒的量程，则需要如下处理

Gyro_X=Gyro_X/16.4; 

式子中，

angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);

• angle_ m 是根据传感器原始数据计算出的角度；

• gyro_ m 是对应轴的角速度；

• dt 的值，根据开发者们选择的采样周期来定。如果 5ms 采样一次，则 dt=0.05。在单片机处理能力许可的情况下，建议开发者采样频率适当大一些。

• gyro_m* dt，就是在 dt 时间内，设备转过的角度。

• K1 的值，根据大家实际需求来，如果希望动态响应快，增大灵敏度，K 取值可以大一些；如果想减小传感器本身的系统误差，减少干扰抖动，增强平滑性，K 取值可以小一点；这里可已令 K1=0.02。

将原始数据和一阶互补滤波后的数据绘制出来，如图：

可以看到，加了一阶互补滤波后，数据非常平滑，也很吻合实际情况。

姿态平衡控制

获取姿态之后，开发者就可以着手控制小车保持平衡了。

在直立控制里面加入速度负反馈无法达到速度闭环的目的，而且还会破坏直立控制系统，因此在保证直立控制的优先级条件下，开发者们要把速度控制放在直立控制的前面，也就是速度控制调节的结果仅仅是改变直立控制的目标值。根据经验可知，小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度，就需要增加小车向前倾斜的角度，倾斜角度加大之后，车轮在直立控制的作用下需要向前运动保持小车平衡，速度增大；如果要降低小车向前行驶的速度，就需要减小小车向前倾斜的角度，倾斜角度减小之后，车轮在直立控制的作用下向后运动保持小车平衡，速度减小。

开发者们把速度和直立两个控制器串联起来工作，其中速度控制的输出作为直立控制的输入，而直立控制的输出作为系统的输出，这其实就是一个串级控制系统。其中直立控制使用 PD 控制。因为编码器可能存在的噪声，为防止噪声被放大并消除系统的静差，这里速度控制使用 PI 控制。

直立 PD 控制

int balance(float Angle,float Gyro)
{ 
   float Bias;
   int balancePID;
	 Bias=Angle-Angle_OFFSET;                       //===求出平衡的角度中值 和机械相关
	 balancePID=Balance_Kp*Bias+Gyro*Balance_Kd;   //=== 计算平衡控制的电机 PWM PD 控制 kp 是 P 系数 kd 是 D 系数 
	 return balancePID;
}

速度 PI 控制

int velocity(int encoder_left,int encoder_right)
{ 
	static float Velocity,Encoder_Least,Encoder,Movement;
	static float Encoder_Integral,Target_Velocity;
	//=============遥控前进后退部分=======================// 
	Target_Velocity=40;           
	if(Direction.Current==GO_STRAIGHT)   	Movement=-Target_Velocity/Flag_speed;	     //=== 前进标志位置 1 
	else if(Direction.Current==GO_BACK)	Movement=Target_Velocity/Flag_speed;         //=== 后退标志位置 1
	else  Movement=0;	
    //============= 速度  PI 控制器=======================//	
	Encoder_Least =(encoder_left+encoder_right)-0;                  
	Encoder *= 0.8;		                                                //===一阶低通滤波器       
	Encoder += Encoder_Least*0.2;	                                    //===一阶低通滤波器    
	Encoder_Integral +=Encoder;                                       //===积分出位移 积分时间：10ms
	Encoder_Integral=Encoder_Integral-Movement;                       //===接收遥控器数据，控制前进后退
	if(Encoder_Integral>8000)  	Encoder_Integral=8000;             //===积分限幅
	if(Encoder_Integral<-8000)	Encoder_Integral=-8000;              //===积分限幅	
	Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;        //===速度控制	
	if(Turn_Off(Angle_Balance,BAT_VOL)==1||Direction.Current==TURN_OFF)   
	  Encoder_Integral=0;      
	return Velocity;
}

转向控制

除了保持平衡之外，小车也涉及到左右转动，因此还需要加入转向的控制，可参考如下

int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
	static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; 
    float Turn_Amplitude=30/Flag_speed,Kp=32,Kd=0; 	
  	if(Direction.Current==TURN_LEFT||Direction.Current==TURN_RIGHT)                      
		{
			if(++Turn_Count==1)
			Encoder_temp=myabs(encoder_left+encoder_right);
			Turn_Convert=50/Encoder_temp;
			if(Turn_Convert<0.6)Turn_Convert=0.6;
			if(Turn_Convert>3)Turn_Convert=3;
		}	
	  else
		{
			Turn_Convert=0.9;
			Turn_Count=0;
			Encoder_temp=0;
		}			
		if(Direction.Current==TURN_LEFT){
			Turn_Target+=Turn_Convert;
		}
		else if(Direction.Current==TURN_RIGHT){
			Turn_Target-=Turn_Convert; 
		}
		else Turn_Target=0;
	
    if(Turn_Target>Turn_Amplitude)  Turn_Target=Turn_Amplitude;    //===转向速度限幅
	  if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
		if(Direction.Current==GO_STRAIGHT||Direction.Current==GO_BACK||Direction.Current==KEEP_STOP)  
		  Kd=-1 ;        
		else 
		  Kd=0;   
  	//============= 转向 PD 控制器=======================//
	Turn=-Turn_Target*Kp-gyro*Kd;                 //=== 结合 Z 轴陀螺仪进行 PD 控制
	  return Turn;
}

电机 PWM 控制

通过上述一系列控制计算后，开发者就得到了使小车平衡的 PWM 值（ 即下面程序中的 Moto1 和 Moto2）。

	Balance_Pwm = balance(Angle_Balance,Gyro_Balance);//=== 平衡 PID 控制	
	Velocity_Pwm= velocity(Enconder_left,Enconder_right);//=== 速度环 PID 控制
	Turn_Pwm   = turn(Enconder_left,Enconder_right,Gyro_Turn);//=== 转向环 PID 控制	
	Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm;//=== 计算左轮电机最终 PWM
	Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm;//=== 计算右轮电机最终 PWM
	Limit_Pwm();                            //=== PWM 限幅	
	Set_Pwm(Moto1,Moto2);                   //=== 赋值给 PWM 寄存器 

将该 PWM 进行限幅处理之后，就可以幅值给对应寄存器，随后就能查看小车的实际运动状态了。

void Set_Pwm(int moto1,int moto2)
{     
    if(moto2>0)		{AIN2_RESET;AIN1_SET;}
    else 	        {AIN2_SET;AIN1_RESET;}		
    TIM16_PWM_Set(myabs(moto2));

    if(moto1>0)	{BIN1_RESET;BIN2_SET;}
    else        {BIN1_SET;BIN2_RESET;}
    TIM17_PWM_Set(myabs(moto1));
}