平衡車電源供應與pid平衡與卡爾曼率波思維調整 與製作平衡小車分享 依情況更新
本帖最后由 lieak59 于 2016-3-2 19:41 编辑如題 小弟最近遇到了兩個奇怪的問題
一個是 atmega328p+直流馬達後用橋式連接器 可以正常供電跟自己做平衡
但換一般9v直流電源或6*1.5v的直流電源無法運作(有上穩壓器7805) 這是為何?
加了兩個104電容後 不但沒動而且還讓接電腦後車子無法自己做平衡....
第2個是pid與卡爾曼濾波器思維
每次調整程式都要調整pid的比例系數，積分系數，微分系數
多半都調整Kp居多為啥? 我調查到說調整ki與kd會很傷元件
以下是我之前發文的程式碼.....
#include "Wire.h"
#include <Servo.h>
#include <Kalman.h>
#include "I2Cdev.h"
#include "MPU6050.h"
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int nCalibTimes = 1000; //校准时读数的次数
int calibData; //校准数据
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
char flag = false; //打印开关
unsigned long nLastTime = 0; //上一次读数的时间
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
/*********** PID控制器参数 *********/
float ITerm, lastInput; // 积分项、前次输入
float Output = 0.0; // PID输出值
/***********电机参数**************/
Servo ServoL;
Servo ServoR;
/***********四元数参数***********/
#define KP 0.025f//10.0f
#define KI 0.0f//0.008f
#define halfT 0.001f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle_roll, Angle_pitch, Angle_yaw;
byte ENA = 11;byte IN1 = 9; byte IN2 = 10;
byte ENB = 6;byte IN3 = 7;byte IN4 = 8;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
for( byte i = 0; i <= 12 ; i++){
pinMode(i,OUTPUT);
}
// configure Arduino LED for
}
void loop() {
int16_t readouts;
ReadAccGyr(readouts); //读出测量值
float realVals;
Rectify(readouts, realVals); //根据校准的偏移量进行纠正
//四元数解析出欧拉角
// AngleUpdate(realVals);
//计算加速度向量的模长，均以g为单位
float fRoll = GetRoll(realVals); //计算Roll角
float fPitch = GetPitch(realVals); //计算Pitch角
//计算两次测量的时间间隔dt，以秒为单位
unsigned long nCurTime = micros();
float dt = (double)(nCurTime - nLastTime) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
float fNewRoll = kalmanRoll.getAngle(fRoll, realVals, dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals, dt);
//更新本次测的时间
nLastTime = nCurTime;
//PID控制
nCurTime = millis(); // 当前时间(ms)
float TimeCh = (nCurTime - nLastTime) / 1000.0; // 采样时间(s)
float Kp = 5.0, Ki = 0.0, Kd = 0.0; // 比例系数、积分系数、微分系数
float SampleTime = 0.1; // 采样时间(s)
float Setpoint = 0; // 设定目标值(degree)
float outMin = -255, outMax = +255; // 输出上限、输出下限
if (TimeCh >= SampleTime) { // 到达预定采样时间时
float Input = fNewPitch; // 输入赋值
float error = Setpoint - Input; // 偏差值
ITerm += (Ki * error * TimeCh); // 计算积分项
ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeCh; // 计算微分项
Output = Kp * error + ITerm - DTerm; // 计算输出值
Output = constrain(Output, outMin, outMax); // 限定值域
//servoL.write(Output + servoL_offset); // 控制左驱
//servoR.write(Output + servoR_offset); // 控制右驱
lastInput = Input; // 记录输入值
nLastTime = nCurTime; // 记录本次时间
}
if (Serial.available() > 0) {
char rece = Serial.read();
delay(10);
if (rece == (char)'#')
{
Serial.println("Nano is Activity...");
}
flag = rece;
}
if (flag == (char)'R')
{
Serial.print("Roll:(");
Serial.print(fNewRoll);
Serial.print(")\t");
Serial.print("Pitch:(");
Serial.print(fNewPitch);
Serial.print(")\r\n");
}
if(Output > 0){
analogWrite(ENA,Output);
digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);
analogWrite(ENB,Output);
digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);
}
else{analogWrite(ENA,Output);
digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);
analogWrite(ENB,Output);
digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);
}
}
void ReadAccGyr(int16_t *pVals)
{
accelgyro.getMotion6(&pVals, &pVals, &pVals, &pVals, &pVals, &pVals);
}
//对大量读数进行统计，校准平均偏移量
void Calibration()
{
int16_t valSums = {0};
//先求和
for (int i = 0; i < nCalibTimes; ++i)
{
ReadAccGyr(valSums);
}
//再求平均
for (int i = 0; i < 6; ++i)
{
calibData = int(valSums / nCalibTimes);
}
calibData += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}
//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
for (int i = 0; i < 3; ++i)
{
pRealVals = (float)(pReadout - calibData) / 16384.0f;
}
for (int i = 3; i < 6; ++i)
{
pRealVals = (float)(pReadout - calibData) / 131.0f;
}
}
//算得Roll角。算法见文档。
float GetRoll(float *pRealVals)
{
float fNorm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormXZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormXZ / fNorm;
if (pRealVals > 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
float fNorm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormYZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormYZ / fNorm;
if (pRealVals < 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
void AngleUpdate(float *pRealVals)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
if (pRealVals*pRealVals*pRealVals == 0)
return;
norm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
vx = 2 * (q1 * q3 - q0 * q2);
vy = 2 * (q0 * q1 - q3 * q2);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
ex = pRealVals / norm * vz - pRealVals / norm * vy;
ey = pRealVals / norm * vx - pRealVals / norm * vz;
ez = pRealVals / norm * vy - pRealVals / norm * vx;
exInt = exInt + ex * KI;
eyInt = eyInt + ey * KI;
ezInt = ezInt + ez * KI;
q0 = q0 + (-q1 * pRealVals + KP * ex + exInt - q2 * pRealVals + KP * ey + eyInt - q3 * pRealVals + KP * ez + ezInt) * halfT;
q1 = q1 + (q0 * pRealVals + KP * ex + exInt + q2 * pRealVals + KP * ez + ezInt - q3 * pRealVals + KP * ey + eyInt) * halfT;
q2 = q2 + (q0 * pRealVals + KP * ey + eyInt - q1 * pRealVals + KP * ez + ezInt + q3 * pRealVals + KP * ex + exInt) * halfT;
q3 = q3 + (q0 * pRealVals + KP * ez + ezInt + q1 * pRealVals + KP * ey + eyInt - q2 * pRealVals + KP * ex + exInt) * halfT;
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Angle_roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * fRad2Deg;
Angle_pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * fRad2Deg;
Angle_yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * fRad2Deg;
}
以上的程式碼是用卡爾曼濾波+pid系統做出來的程式碼
看完之後我覺得很新奇 因為這個程式碼沒有用到網路上面大家都在用的pid_v1文件庫
檢測完以我的車子是要用pitch來檢測 趨近於0做平衡
目前成果是 車子接電腦會自己做平衡
但接一般直流電壓就會變第一個問題
看板上車子立起來的思維是 輪子做正反轉 搖晃 最後慢慢立起來
我的車子是 兩個輪子往一個地方衝.....
車子俯視圖
藍芽孔有架喔 但怕會有干擾就暫時把藍芽拔掉
rx tx與橋式連接器共用
pcb圖
這個小弟失敗了很多次最後做出來....不過也算很開心了 因為就算失敗了也能做一般四輪遙控玩具車(小時候玩到大的玩具)
這些圖都是用平板拍後在弄到電腦做切圖好不容易才能上傳....
現在要重弄關於用電腦的電壓 立起來的問題....
請給個建議 每有一些進展 更新一次 願互相學習
------------------------------------------------------------------------------------------------------------------------------------------------------------
程式庫:
關於卡爾曼率波若有錯要報我這邊光卡爾曼率波就有3個程式庫 小车做的很精致，如果是简体中文字就更好啦。 先說說 用了啥元件吧
Atmega328p*1
(主晶片)
22p電容*2
104電容*2
16MHZ*1
(我不會用Atmega328p內建的8MHZ)
CP212102 USB-TTL BOARD 橋式連接器*1
(自製PCB版子溝通電腦)
SJ 藍芽模組(找不到排字 有找到標籤) 可接5.0V 3.3V 藍芽*1
(若要自己做 請注意電壓 最好是5V 不然會壞)
MPU6050 GY-521*1
(這種的比較多 我也是努力拿常見的做)
L298N*1
(接香蕉電機)
香蕉電機*2
7805穩壓器
25V 10uF *2(接穩壓器的)
1.5v*6電池盒(电源) 目前發現要手碰才會讓 才會讓一邊的輪子動
懷疑是電流不夠
可是接電腦 電流照理說因該不會不夠阿
馬達也拆開來看過都還好好的...通電也會動阿
傾另一邊 另一邊馬達不會做修正阿
看來要慢慢搞了...
你的电机都没看到码盘，怎么采样转速信号的？
没有转速采样如何做负反馈，PID控制呢？
还有，这个TT电机玩玩可以，如果做控制，赶紧换个好点的带编码器的减速电机。 干电池，即时是南孚电池，在面对电机运动的时候，电压降低特别明显。我的PID调整老不成功，不知道换航模锂电池会不会好点。 艰苦奋斗 发表于 2016-2-27 20:13 static/image/common/back.gif
你的电机都没看到码盘，怎么采样转速信号的？
没有转速采样如何做负反馈，PID控制呢？
还有，这个TT电机玩 ...
恩 大致上理解 小弟會研究一下如何在這馬達上放測速碼盤
畢竟當初拿到並沒有測速馬達這東西 只有香蕉電機....
只是沒想到要做測速與pid控制要用到測速模組 146行，void Calibration()函数没看到在那儿用阿？ 本帖最后由 lieak59 于 2016-3-2 16:46 编辑
如果是以Arduino控制馬達轉速的話 是不是還要讓馬達分個接頭到a0-a3的角位?
arduino上面也是有pwm 不知道可不可以共用 但本來就有馬達模組L298N了 會不會多此一舉?
或是使用ARDUINO的PWM接L298N的ENA跟ENB抓轉速?
小弟最近入手了 減速馬達(沒編碼盤) 到時候可能要再入手個編碼盤 或測速模組?
Kalman.h库哪位大侠有啊，官网下不下来
85行：nLastTime = nCurTime; 与105行nLastTime = nCurTime; 是否冲突？
RE: 平衡車電源供應與pid平衡與卡爾曼率波思維調整 與製作平衡小車分享 依情況更新
本帖最后由 lieak59 于 2016-3-8 16:00 编辑新增更新 之前發過的4到3輪的小車控制程式碼
#include <SoftwareSerial.h> // 引用「軟體序列埠」程式庫
SoftwareSerial BT(3, 2); // 設定軟體序列埠(接收腳, 傳送腳)
char command; // 設定啟動或停止馬達的參數
// 一開始先設定「停止」
boolean run = false;
// 左馬達控制設定
const byte LEFT1 = 10;
const byte LEFT2 = 9;
const byte LEFT_PWM = 5;
// 右馬達控制設定
const byte RIGHT1 = 8;
const byte RIGHT2 = 7;
const byte RIGHT_PWM = 6;
// 設定PWM輸出值（註：FA-130馬達供電不要超過3v）
byte motorSpeed = 100;
void forward() { // 馬達轉向：前進
digitalWrite(LEFT1, HIGH);
digitalWrite(LEFT2, LOW);
digitalWrite(RIGHT1, HIGH);
digitalWrite(RIGHT2, LOW);
}
void backward() { // 馬達轉向：後退
digitalWrite(LEFT1, LOW);
digitalWrite(LEFT2, HIGH);
digitalWrite(RIGHT1, LOW);
digitalWrite(RIGHT2, HIGH);
}
void turnLeft() { // 馬達轉向：左轉
digitalWrite(LEFT1, LOW);
digitalWrite(LEFT2, HIGH);
digitalWrite(RIGHT1, HIGH);
digitalWrite(RIGHT2, LOW);
}
void turnRight() { // 馬達轉向：右轉
digitalWrite(LEFT1, HIGH);
digitalWrite(LEFT2, LOW);
digitalWrite(RIGHT1, LOW);
digitalWrite(RIGHT2, HIGH);
}
void setup() {
BT.begin(9600);
pinMode(LEFT1, OUTPUT);
pinMode(LEFT2, OUTPUT);
pinMode(LEFT_PWM, OUTPUT);
pinMode(RIGHT1, OUTPUT);
pinMode(RIGHT2, OUTPUT);
pinMode(RIGHT_PWM, OUTPUT);
}
void loop() {
if (BT.available() > 0) {
command = BT.read();
switch (command) {
case 'w': // 接收到'w'，前進。
forward();
run = true; // 啟動馬達
break;
case 'x': // 接收到'x'，後退。
backward();
run = true; // 啟動馬達
break;
case 'a': // 接收到'a'，左轉。
turnLeft();
run = true; // 啟動馬達
break;
case 'd': // 接收到'd'，右轉。
turnRight();
run = true; // 啟動馬達
break;
case 's':
run = false; // 停止馬達
break;
}
}
if (run) {
// 如果要啟動馬達…
// 向馬達輸出指定的類比電壓值
analogWrite(LEFT_PWM, motorSpeed);
analogWrite(RIGHT_PWM, motorSpeed);
} else {
// 否則…
// 將馬達的電壓值設定成0
analogWrite(LEFT_PWM, 0);
analogWrite(RIGHT_PWM, 0);
}
}
若是要製作功能性小車可以以這程式碼為底 這程式碼只有控制2個電機 剩下輪子可以用一般小輪子替代 記得要會轉的...
關於安卓控制可以用網路上的控制藍芽程式 弄完後小車可以拿手機控制了 碼盤已經買到 要照著接抓轉速吧?
页:
[1]