平衡車電源供應與pid平衡與卡爾曼率波思維調整與製作平衡小車分享依情況更新

lieak59 · 发表于 2016-2-25 19:08:38

本帖最后由 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[6]; //校准数据
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[6];
ReadAccGyr(readouts); //读出测量值
float realVals[6];
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[4], dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], 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[0], &pVals[1], &pVals[2], &pVals[3], &pVals[4], &pVals[5]);
}
//对大量读数进行统计，校准平均偏移量
void Calibration()
{
int16_t valSums[6] = {0};
//先求和
for (int i = 0; i < nCalibTimes; ++i)
{
ReadAccGyr(valSums);
}
//再求平均
for (int i = 0; i < 6; ++i)
{
calibData[i] = int(valSums[i] / nCalibTimes);
}
calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}
//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
for (int i = 0; i < 3; ++i)
{
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
}
for (int i = 3; i < 6; ++i)
{
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
}
}
//算得Roll角。算法见文档。
float GetRoll(float *pRealVals)
{
float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
float fCos = fNormXZ / fNorm;
if (pRealVals[1] > 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fCos = fNormYZ / fNorm;
if (pRealVals[0] < 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[0]*pRealVals[1]*pRealVals[2] == 0)
return;
norm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
vx = 2 * (q1 * q3 - q0 * q2);
vy = 2 * (q0 * q1 - q3 * q2);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
ex = pRealVals[1] / norm * vz - pRealVals[2] / norm * vy;
ey = pRealVals[2] / norm * vx - pRealVals[0] / norm * vz;
ez = pRealVals[0] / norm * vy - pRealVals[1] / norm * vx;
exInt = exInt + ex * KI;
eyInt = eyInt + ey * KI;
ezInt = ezInt + ez * KI;
q0 = q0 + (-q1 * pRealVals[3] + KP * ex + exInt - q2 * pRealVals[4] + KP * ey + eyInt - q3 * pRealVals[5] + KP * ez + ezInt) * halfT;
q1 = q1 + (q0 * pRealVals[3] + KP * ex + exInt + q2 * pRealVals[5] + KP * ez + ezInt - q3 * pRealVals[4] + KP * ey + eyInt) * halfT;
q2 = q2 + (q0 * pRealVals[4] + KP * ey + eyInt - q1 * pRealVals[5] + KP * ez + ezInt + q3 * pRealVals[3] + KP * ex + exInt) * halfT;
q3 = q3 + (q0 * pRealVals[5] + KP * ez + ezInt + q1 * pRealVals[4] + KP * ey + eyInt - q2 * pRealVals[3] + 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個程式庫

PINKWALKMAN · 发表于 2016-2-26 08:13:07

小车做的很精致，如果是简体中文字就更好啦。

lieak59 · 发表于 2016-2-26 16:48:23

先說說用了啥元件吧

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電池盒(电源)

lieak59 · 发表于 2016-2-27 15:58:58

目前發現要手碰才會讓才會讓一邊的輪子動

懷疑是電流不夠

可是接電腦電流照理說因該不會不夠阿

馬達也拆開來看過都還好好的...通電也會動阿

傾另一邊另一邊馬達不會做修正阿

看來要慢慢搞了...

艰苦奋斗 · 发表于 2016-2-27 20:13:45

你的电机都没看到码盘，怎么采样转速信号的？
没有转速采样如何做负反馈，PID控制呢？
还有，这个TT电机玩玩可以，如果做控制，赶紧换个好点的带编码器的减速电机。

wetnt · 发表于 2016-2-29 18:45:22

干电池，即时是南孚电池，在面对电机运动的时候，电压降低特别明显。我的PID调整老不成功，不知道换航模锂电池会不会好点。

lieak59 · 发表于 2016-3-1 15:43:04

艰苦奋斗发表于 2016-2-27 20:13
你的电机都没看到码盘，怎么采样转速信号的？
没有转速采样如何做负反馈，PID控制呢？
还有，这个TT电机玩 ...

恩大致上理解小弟會研究一下如何在這馬達上放測速碼盤

畢竟當初拿到並沒有測速馬達這東西只有香蕉電機....

只是沒想到要做測速與pid控制要用到測速模組

蜗牛j · 发表于 2016-3-2 09:42:27

146行，void Calibration()函数没看到在那儿用阿？

lieak59 · 发表于 2016-3-2 16:31:02

本帖最后由 lieak59 于 2016-3-2 16:46 编辑

如果是以Arduino控制馬達轉速的話是不是還要讓馬達分個接頭到a0-a3的角位?

arduino上面也是有pwm 不知道可不可以共用但本來就有馬達模組L298N了會不會多此一舉?

或是使用ARDUINO的PWM接L298N的ENA跟ENB抓轉速?

小弟最近入手了減速馬達(沒編碼盤) 到時候可能要再入手個編碼盤或測速模組?

蜗牛j · 发表于 2016-3-2 17:36:45

Kalman.h库哪位大侠有啊，官网下不下来

蜗牛j · 发表于 2016-3-5 16:52:38

85行：nLastTime = nCurTime; 与105行nLastTime = nCurTime; 是否冲突？

lieak59 · 发表于 2016-3-8 15:58:39

本帖最后由 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個電機剩下輪子可以用一般小輪子替代記得要會轉的...

關於安卓控制可以用網路上的控制藍芽程式弄完後小車可以拿手機控制了

lieak59 · 发表于 2016-3-14 21:28:20

碼盤已經買到要照著接抓轉速吧?

帐号		自动登录	找回密码
密码			注册

平衡車電源供應與pid平衡與卡爾曼率波思維調整與製作平衡小車分享依情況更新

本帖子中包含更多资源

RE: 平衡車電源供應與pid平衡與卡爾曼率波思維調整與製作平衡小車分享依情況更新

平衡車電源供應與pid平衡與卡爾曼率波思維調整 與製作平衡小車分享 依情況更新

本帖子中包含更多资源

RE: 平衡車電源供應與pid平衡與卡爾曼率波思維調整 與製作平衡小車分享 依情況更新

平衡車電源供應與pid平衡與卡爾曼率波思維調整與製作平衡小車分享依情況更新

RE: 平衡車電源供應與pid平衡與卡爾曼率波思維調整與製作平衡小車分享依情況更新