黄山派小智平衡车【雏形】
○如上图：静止倾斜时通过x轴测得的加速度与z轴测得的加速度值的反正切函数即可计算出对应的俯仰角度（pitch），翻滚角（roll）同理。●Angle（输出角度） = Angle _g（角速度角） + a * (Angle_a（加速度角） - Angle_g)●Angle（输出角度） = Angle _g（角速度角） + a * (Angle_a（加速度角） - Angle_g)●消除长期积累的误差
一、引脚配置
#define PWMA_PIN 29
#define PWMB_PIN 20
#define AIN1_PIN 35
#define AIN2_PIN 36
//#define STBY_PIN 20
#define BIN1_PIN 27
#define BIN2_PIN 28
#define ENC1A_PIN 25
#define ENC1B_PIN 24
#define ENC2A_PIN 32
#define ENC2B_PIN 18 -> RX
//#define STBY_PIN 20 -> 固定3.3V
小智 sensor 传感器控制需要拉高PA30
二、硬件选择&参数
主控黄山派 6轴陀螺仪 3轴地磁传感器
N20减速电机 3V-12V 100转/min 减速比150 7PPR 尺寸【图】 额定电压6V
电机驱动 TB6612模块 <- IC ->
三、设计流程
四、角度获取
参考例程：https://docs.sifli.com/projects/sdk/latest/sf32lb52x/example/rt_device/sensor/README.html#sensor
lsm6d:6轴陀螺仪
mmc56x3：3轴地磁传感器
0.0 坐标系选取
●以地面为坐标，物体的运动视为空间中的运动
●物体坐标系
○无论物体在哪，都已物体本身为坐标系中心
0.1 俯仰角、偏航角、翻滚角
首先明确物体三轴对应的方向角度
●物体三轴分为 x-y-z
○绕z轴水平旋转为 - 偏航角（yaw）
○绕x轴左右翻转为 - 翻滚角（yoll）
○绕y轴前后翻转为 - 俯仰角（pitch）
1.lsm6d
加速度计：
加速度易受影响（外部或、包括自身移动）
●测量 x-y-z 三轴的加速度
○静止时 z轴加速度为重力加速度，x - y 轴无加速度
○当设备倾斜，即可测得对应轴向上的重力加速度的分速度
●通过方向加速度 & 反正切函数 计算角度
○如上图：静止倾斜时通过x轴测得的加速度与z轴测得的加速度值的反正切函数即可计算出对应的俯仰角度（pitch），翻滚角（roll）同理
○需要注意，水平的偏航角无法通过加速度计测量
■无论如何水平旋转，x-y轴加速度无变化
●加速度计优缺点
○水平 yaw 角度无法计算
○动态漂移严重
■静止或者匀速运动时，物体加速度为0，可准确计算出实际角度
■物体运动时，测量值带有物体自身移动加速度，导致物体动态时漂移严重
○长时间不漂移
○以重力加速度为准，测量绝对角度（水平方向除外）
acce_angle.pitch = atan2(lsm6d_acce.data.acce.x, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //俯仰
acce_angle.roll = atan2(lsm6d_acce.data.acce.y, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //翻转
acce_angle.yaw = 0;
角速度计：
受设备零偏影响
●测量 x-y-z 三轴的角速度
○角速度积分得角度
●角速度计优缺点
○零偏误差：静止不动时由于设备固有差异，会出现不同的零偏值，零偏的不断累积导致稳态误差（无法完全消除）
○只能测量相对值，需要初始明确值（初始化）
○动态稳定性强，不会受运动加速度影响
/*其中0.005 为设定时间dt*/
gyro_offset.pitch += (lsm6d_gyro.data.gyro.y*0.005*25/32768);
gyro_offset.yaw += (lsm6d_gyro.data.gyro.z*0.005*25/32758);
gyro_offset.roll += (lsm6d_gyro.data.gyro.x*0.005*25/32768);
2、mmc56x3
- 受磁场电磁、强磁影响
- 磁感应器主要用于修正无法通过加速度计修正的水平yaw角度
- 测量地磁，地磁确定
●测量 x-y-z 三轴的地磁强度
○角度计算方式类似加速度计，参考地磁北
●主要校准水平 yaw 角度
●归一化处理角度，水平角度限制在-180 ~ 180°之间
mmc56x3_angle.yaw = -atan2f(mmc56x3.data.mag.y, mmc56x3.data.mag.x) * 180.0f / 3.1415927f;
float diff = mmc56x3_angle.yaw - gyro_angle.yaw;
if (diff > 180.0f) diff -= 360.0f;
if (diff < -180.0f) diff += 360.0f;
3.普通加权计算角度
●获取各个传感器的值，并分别计算出角度数据，然后根据稳定新计算加权计算出角度
●只是单纯叠加，无法解决零偏
●Angle（输出角度） = Angle _g（角速度角） + a * (Angle_a（加速度角） - Angle_g)
4.互补滤波计算角度
融合加速度计与角速度计优缺点计算角度
- pitch
- roll
融合角速度计与磁感应计计算角度
- yaw
●互补滤波融合两者间的有点，消除两者的缺点
●角速度计计算角度为主、加速度计计算角度为辅（加速度计容易受自身运动影响）
●加速度计角度和陀螺仪角度取加权平均值 得到互补滤波后角度
●Angle（输出角度） = Angle _g（角速度角） + a * (Angle_a（加速度角） - Angle_g)
●注意：滤波后，下一次Angle_g 的累加要在Angle基础上加【互补滤波公式关键】
●互补滤波相当于使用加速度计角度/地磁感应器角度来修复加速度角度
●互补滤波系数
○互补滤波系数的大小决定了加速度计对角速度计的影响
○系数过大会导致设备抖动，体现：设备开始小幅度摆动，且摆动幅度会越来越大（噪声振荡）
gyro_angle.pitch = angle.pitch - ((lsm6d_gyro.data.gyro.y )* 0.005*25/32768 - gyro_offset.pitch) ;// + gyro_offset.pitch 前倾为正
gyro_angle.roll = angle.roll + ((lsm6d_gyro.data.gyro.x )* 0.005*25/32768 - gyro_offset.roll) ;//+ gyro_offset.roll 右翻为负
gyro_angle.yaw += ((lsm6d_gyro.data.gyro.z )* 0.005 * 25 / 32758 * 1.5 - gyro_offset.yaw);//- gyro_offset.yaw- gyro_offset.yaw 右转为负
acce_angle.pitch = atan2(lsm6d_acce.data.acce.x, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //俯仰
acce_angle.roll = atan2(lsm6d_acce.data.acce.y, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //翻转
acce_angle.yaw = 0;
mmc56x3_angle.yaw = -atan2f(mmc56x3.data.mag.y, mmc56x3.data.mag.x) * 180.0f / 3.1415927f;
float diff = mmc56x3_angle.yaw - gyro_angle.yaw;
if (diff > 180.0f) diff -= 360.0f;
if (diff < -180.0f) diff += 360.0f;
angle.pitch = gyro_angle.pitch + a *(acce_angle.pitch - gyro_angle.pitch);
angle.roll = gyro_angle.roll + a * (acce_angle.roll - gyro_angle.roll);
angle.yaw = gyro_angle.yaw * 0.95 + 0.05 * diff;// + 0.02 * diff
5.消除零偏
通过一定时间测量的平均初始值来得到传感器的初始偏移值
●器件均有零偏，无法完全消除，只能尽可能减小
其他
- 为实现更加精确的角度计算可以再换更为复杂的算法（滤波）计算
●四元数、卡尔曼滤波
五、电机控制
N20编码器电机控制
参考例程：https://docs.sifli.com/projects/sdk/latest/sf32lb52x/example/rt_device/motor/README.html
编码器使用：gptim1/2
pwm使用atim channl 1/2
0. PID控制
PID负反馈调节
P：比例
I：积分
D：微分
P:比例
P_term = Kp * error
error(目标值 - 测量值）
●根据当前误差大小纠正测量值逼近目标值
●当误差较大时，P_term值大，更快接近目标值
●kp越大 -> 响应越快 -> 振荡
●kp越小 -> 响应越慢 -> 稳定（对应直立无法快速直立）
I：积分
integral += error;
I_term = Ki * integral;
●消除稳态误差
●消除长期积累的误差（仅kp计算，会有累计误差，无法到达目标值）
●ki值过大：引起超调（抖动）
D:微分
D_term = Kd * (Error - LastError)
LastError（前一次误差值）
●预测误差变化，抑制振荡
●当误差值减小时，D_term为负，可抑制输出值
输出
P_out = P_term + I_term + D_term;
●可根据实际情况自由匹配PID算法
●PID | PI | PD
●算法优化（不完全）
○根据需求添加适用
○积分限幅：防止积分深度饱和（输出被限制在100%，电机全速转动）
○微分先行：对误差的积分转为对实际值的微分（解决目标值跳变问题，更加平滑）
○输出偏移：输出非0时添加固定偏移值，跳出输出死区
○输入死区：误差较小时不调控
pid->LastError = pid->NowError;
pid->NowError = pid->Target - pid->Measure;
if (pid->Ki != 0)
{
pid->Integral += pid->NowError;
}
else
{
pid->Integral = 0;
}
float max_integral = pid->MaxOutput / 2.0 / pid->Ki; // 防止积分过大
if (pid->Integral > max_integral)
pid->Integral = max_integral;
if (pid->Integral < -max_integral)
pid->Integral = -max_integral;
pid->Result = pid->Kp * pid->NowError
+ pid->Ki * pid->Integral
- pid->Kd * (pid->Measure - pid->LastMeasure); //weifenxiangxing
//+ pid->Kd * (pid->NowError - pid->LastError)*2;
if (pid->Result > pid->MaxOutput) {pid->Result = pid->MaxOutput;}
if (pid->Result < pid->MinOutput) {pid->Result = pid->MinOutput;}
if(pid->Result >0)
{
pid->Result += pid->Resultoffset;
}
if(pid->Result <0)
{
pid->Result -= pid->Resultoffset;
}
pid->LastMeasure = pid->Measure;
return pid->Result;
1.角度环控制
输入 ：目标 需要的目标 yaw 角度
：测量 当前物体yaw角度
：设定pid
：输出 双轮差速
控制需求：
调节PID值使得角度控制趋于稳定
●通过输入目标角度和当前角度到PID计算器，输出值为左右轮的差速
●通过两轮的差速实现转动改变当前实际角度
●反馈实际角度与目标角度误差，实现单闭环控制
if(cnt %10 ==0&& cnt != 0)
{
turn_pid.Measure = measured_angle.yaw;
//turn_pid.Target = 0;
dif_pulse = pid_controller_cal_sat123(&turn_pid); //转向环输出，差分速度
}
p值小
p值增大
2.直立环控制
输入 ：目标 需要的目标 pitch 角度 （单直立环）
：目标 速度环外环输出角度值 （串级PID）
：测量 当前物体 pitch 角度
：输出 双轮平均速度
：设定pid
控制需求：
单直立环控制PID参数，调节PID值使得角度控制趋于稳定
●单直立环控制可以实现小车直立
○当小车pitch角度前倾时，给一个向前的速度来保持小车的直立平衡
●单直立环实现直立无法控制速度，小车会前后"摆动"移动，无法静止直立
●加入速度环可解决
angle_pid.Measure = measured_angle.pitch;//0;//
ave_pulse = -pid_controller_cal_sat123(&angle_pid); //直立（角度）环控制
3.速度环控制
输入 ：目标 需要的目标 速度
：测量 轮子转速
：输出 一个目标角度
：设定pid
控制需求：
调节PID值使得角度控制趋于稳定
●速度环的控制为一个目标角度
○该目标角度为直立环目标角度输入
○静止时，目标速度0 计算目标倾角 0保持直立
○运动时，目标速度x，计算目标角度y，目标实际偏差，电机向前运动
●需要平衡角度环与速度环的执行间隔
○角度环作为内环需要快速响应
○速度环作为外环需要更慢速的响应
○当外环接近内环响应时，出现内外环耦合现象（内环还未完成偏移调整，外环继续给出响应，导致无法平衡，实际现象可为设备剧烈抖动调节PID难解决）
if (cnt >= 100) //need to adjust the print frequency to avoid overwhelming the console
{
cnt = 0;
ave_speed = (pulse_to_rpm(enc_l) + pulse_to_rpm(enc_r)) / 2.0; //当前平均速度
dif_speed = pulse_to_rpm(enc_l) - pulse_to_rpm(enc_r); //当前差速
speed_pid.Measure = ave_speed;
//speed_pid.Target = tar_speed; //目标速度
angle_pid.Target = -pid_controller_cal_sat123(&speed_pid); //速度环输出,串级PID -> 外环输出给内环当目标值
}
前后摆动移动
硬件问题
- 电机摩擦力
- 减速箱齿轮
- 可用无刷电机
●当实现直立环 + 角度环，小车仍旧一定幅度的前后摇摆
○由于电机齿轮/减速比 造成的死区问题
○在一个小角度的范围变化内电机无法实现执行PID输出的调节作用（响应速度变慢了）
○解决 pid输出加上 输出偏移（当输出不为0时 ，加或减一个固定值）
if(pid->Result >0)
{
pid->Result += pid->Resultoffset;
}
if(pid->Result <0)
{
pid->Result -= pid->Resultoffset;
}
●
MCP服务
语音控制
void angle_turn(float angle)
{
angle_pid.Target += angle;
LOG_I("Target angle: %f", angle_pid.Target);
}
//MCP Code
AddTool("self.turn.right",
"Set the left turn function.",
PropertyList(
{
Property("angle", kPropertyTypeInteger, 0, 180)
}
),
[=](const PropertyList& properties) -> ReturnValue
{
int angle = properties["angle"].value<int>();
angle_turn(-angle);
return true;
});
AddTool("self.turn.left",
"Set the left turn function.",
PropertyList(
{
Property("angle", kPropertyTypeInteger, 0, 180)
}
),
[=](const PropertyList& properties) -> ReturnValue
{
int angle = properties["angle"].value<int>();
angle_turn(angle);
return true;
});
六、硬件结构设计
硬件结构设计
●当前缺陷：
○优化设计：需更新改进IO线路
○黄山派固定结构需要更新
■孔位稍微偏移
■孔大小匹配 / 排针插槽短于现有铜柱螺丝
○需添加挖槽
■电机编码器挖槽
■显示屏排线挖槽
作为“人工智能6S店”的官方数字引擎，为AI开发者与企业提供一个覆盖软硬件全栈、一站式门户。
更多推荐
0
0- 0
已为社区贡献4条内容
所有评论(0)