上篇给大家分享了微型巡逻摄像头的整体硬件方案及各单元的工作原理。本篇将重点分享微型巡逻摄像头自平衡功能和移动控制功能的实现。
MCU固件开发
本demo直接使用涂鸦智能SC012-WD2摄像头，因此摄像头和APP相关固件程序，并不需要开发者进行开发。开发者只需要开发底盘小车平衡运动这方面即可。
1.姿态读取
在控制平衡之前，开发者需要先获取当前平衡姿态，即先读取MPU6050数据，参考如下
void Get_Angle(uint8_t way)
{
float Accel_Y,Accel_Z,Gyro_X,Gyro_Z;
Temperature=Read_Temperature(); //===读取MPU6050内置温度传感器数据，近似表示主板温度。
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,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L); //读取Y轴陀螺仪
Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪
Accel_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取X轴加速度计
Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_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) Yijielvbo(Accel_Angle,Gyro_X); //互补滤波
Angle_Balance=angle; //更新平衡倾角
Gyro_Turn=Gyro_Z+Gyro_Z_OFFSET; //更新转向角速度
Acceleration_Z=Accel_Z; //===更新Z轴加速度计
}
}
2.姿态平衡控制
获取姿态之后，开发者就可以着手控制小车保持平衡了。
在直立控制里面加入速度负反馈无法达到速度闭环的目的，而且还会破坏直立控制系统，因此在保证直立控制的优先级条件下，开发者们要把速度控制放在直立控制的前面，也就是速度控制调节的结果仅仅是改变直立控制的目标值。根据经验可知，小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度，就需要增加小车向前倾斜的角度，倾斜角度加大之后，车轮在直立控制的作用下需要向前运动保持小车平衡，速度增大；如果要降低小车向前行驶的速度，就需要减小小车向前倾斜的角度，倾斜角度减小之后，车轮在直立控制的作用下向后运动保持小车平衡，速度减小。
开发者们把速度和直立两个控制器串联起来工作，其中速度控制的输出作为直立控制的输入，而直立控制的输出作为系统的输出，这其实就是一个串级控制系统。其中直立控制使用PD控制。因为编码器可能存在的噪声，为防止噪声被放大并消除系统的静差，这里速度控制使用PI控制。
3. 直立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;
}
4.速度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;
}
5.转向控制
除了保持平衡之外，小车也涉及到左右转动，因此还需要加入转向的控制，可参考如下
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;
}
6.电机PWM控制
通过上述一系列控制计算后，开发者就得到了使小车平衡的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));
}
结语
以上就是带有可视通话功能且可以行走微型巡逻摄像头的实现方案。感兴趣的同学可以尝试动手操作，这种巡逻摄像头体积小巧可爱，可以放在家里和家人互动，也可以解决很多因空间原因无法解决的很多工作场景问题。
上篇给大家分享了微型巡逻摄像头的整体硬件方案及各单元的工作原理。本篇将重点分享微型巡逻摄像头自平衡功能和移动控制功能的实现。
MCU固件开发
本demo直接使用涂鸦智能SC012-WD2摄像头，因此摄像头和APP相关固件程序，并不需要开发者进行开发。开发者只需要开发底盘小车平衡运动这方面即可。
1.姿态读取
在控制平衡之前，开发者需要先获取当前平衡姿态，即先读取MPU6050数据，参考如下
void Get_Angle(uint8_t way)
{
float Accel_Y,Accel_Z,Gyro_X,Gyro_Z;
Temperature=Read_Temperature(); //===读取MPU6050内置温度传感器数据，近似表示主板温度。
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,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L); //读取Y轴陀螺仪
Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪
Accel_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取X轴加速度计
Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_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) Yijielvbo(Accel_Angle,Gyro_X); //互补滤波
Angle_Balance=angle; //更新平衡倾角
Gyro_Turn=Gyro_Z+Gyro_Z_OFFSET; //更新转向角速度
Acceleration_Z=Accel_Z; //===更新Z轴加速度计
}
}
2.姿态平衡控制
获取姿态之后，开发者就可以着手控制小车保持平衡了。
在直立控制里面加入速度负反馈无法达到速度闭环的目的，而且还会破坏直立控制系统，因此在保证直立控制的优先级条件下，开发者们要把速度控制放在直立控制的前面，也就是速度控制调节的结果仅仅是改变直立控制的目标值。根据经验可知，小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度，就需要增加小车向前倾斜的角度，倾斜角度加大之后，车轮在直立控制的作用下需要向前运动保持小车平衡，速度增大；如果要降低小车向前行驶的速度，就需要减小小车向前倾斜的角度，倾斜角度减小之后，车轮在直立控制的作用下向后运动保持小车平衡，速度减小。
开发者们把速度和直立两个控制器串联起来工作，其中速度控制的输出作为直立控制的输入，而直立控制的输出作为系统的输出，这其实就是一个串级控制系统。其中直立控制使用PD控制。因为编码器可能存在的噪声，为防止噪声被放大并消除系统的静差，这里速度控制使用PI控制。
3. 直立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;
}
4.速度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;
}
5.转向控制
除了保持平衡之外，小车也涉及到左右转动，因此还需要加入转向的控制，可参考如下
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;
}
6.电机PWM控制
通过上述一系列控制计算后，开发者就得到了使小车平衡的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));
}
结语
以上就是带有可视通话功能且可以行走微型巡逻摄像头的实现方案。感兴趣的同学可以尝试动手操作，这种巡逻摄像头体积小巧可爱，可以放在家里和家人互动，也可以解决很多因空间原因无法解决的很多工作场景问题。
举报
