先通过SPI读取加速度和角速度，然后在单片机里通过通过kalman滤波进行数据融合，输出X轴角度，同时通过CAN口将融合前的原始数据和融合的角度发送给上位机，上位机借用基于ROS的Plotjuggler绘图工具，进行数据的图形化显示，极大的方便了滤波参数的调整。
下面贴出调试好的kalman滤波部分代码，以及ROS部分代码。
kalman滤波代码（借用他人代码修改了极少部分，原作者看到请留意我好署名原作者）：
其中参数acc_x,y,z为三轴加速度值，有的代码只取了x轴，考虑非理想状态，最好是三轴都取
#define DEG2RAD 0.017453292519943295769236907684886f // 角度转弧度
#define RAD2DEG 57.295779513082320876798154814105f // 弧度转角度
float Q_angle = 0.0008f; // 陀螺仪噪声的协方差
float R_measure = 0.05f; // 加速度计的协方差
float Q_bias = 0.0008f; // Q_bias为陀螺仪漂移
float dt = 0.005f;
float bias = 0;
float k_0 = 0,k_1 = 0; // 卡尔曼增益
float pdot[4] = { 0.0f,0.0f,0.0f,0.0f };
float p[2][2] = { {1.0f,0.0f},{0.0f,1.0f} };
float kalman(float acc_x,float acc_y,float acc_z,float gyro_x)
{
static float angle = 0.0f; // 卡尔曼滤波器的输岀值，最优估计的角度
float angle_acc,acc1_z,acc1_x,acc1_y;
acc1_z = acc_z * 0.00025;
acc1_x = acc_x * 0.00025;
acc1_y = acc_y * 0.00025;
angle_acc = atan2f(acc1_x,sqrtf(acc1_z*acc1_z + acc1_y*acc1_y)) * RAD2DEG;
//1、卡尔曼第一个公式(状态预测):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle += (gyro_x/40.0 - bias) * dt;
//2、卡尔曼第二个公式(计算误差协方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡尔曼第三个公式(计算卡尔曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系统测量噪声协方差
k_0 = p[0][0]/(p[0][0]+R_measure);
k_1 = p[1][0]/(p[0][0]+R_measure);
//4、卡尔曼第四个公式(修正估计):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle += k_0 * (angle_acc - angle);
bias += k_1 * (angle_acc - angle);
//5、卡尔曼第五个公式(更新误差协方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
return angle;
}ROS下CAN部分代码（使用的珠海创芯科技的USB转CAN），main部分主要是进行can设备的初始化，以及初始化话题等，在主循环进行can消息的发送、IMU数据话题发布，IMU数据在后面的can主代码里通过读取can口获取。其中bms结构为发布的的IMU数据（修改了之前代码，懒得重命名，将就着看）将在后面的Plotjuggler里订阅。
int main(int argc, char** argv)
{
pthread_t tid;
ros::init(argc, argv, "abc");
ros::NodeHandle n,nh,ns;
std::string can_device;
ros::Publisher bat_pub = n.advertise<std_msgs::Float32MultiArray>("bms", 1000);
std_msgs::Float32MultiArray bms;
bms.layout.dim.push_back(std_msgs::MultiArrayDimension());
bms.layout.dim[0].size = 6;
bms.layout.dim[0].stride = 6;
bms.layout.dim[0].label = "bms";
bms.data.resize(6);
signal(SIGALRM, sigHandler);
signal(SIGINT, sigHandler);
if(initCAN(VCI_USBCAN2) == 0) // Initialization device
exit(0);
getCANInfo(VCI_USBCAN2); // get device information
if(pthread_create(&tid,NULL,receiveFrame,(void *)&run) != 0)
ROS_INFO("Thread: Create can1 thread failed!");
frame = (PVCI_CAN_OBJ)malloc(sizeof(VCI_CAN_OBJ));
memset(frame,0x00,sizeof(VCI_CAN_OBJ));
ros::Rate loop_rate(100);
while (ros::ok())
{
frame->id = 0x361;
frame->dlc = 4;
frame->data[0] = 0;
frame->data[1] = 200;
frame->data[2] = 0;
frame->data[3] = 0;
frame->is_extended = false;
frame->is_rtr = false;
sendCTLFrame(frame);
bms.data[0] = s_bms.acc_x;
bms.data[1] = s_bms.acc_y;
bms.data[2] = s_bms.acc_z;
bms.data[3] = s_bms.gyro;
bms.data[4] = s_bms.angle_acc;
bms.data[5] = s_bms.angle_kalman;
bat_pub.publish(bms);
ros::spinOnce();
loop_rate.sleep();
}
pthread_join(tid,NULL);
}can驱动部分代码，receiveFrame函数读取所有帧，通过帧ID 0x333 跳转到adis16450()
函数进行IMU原始数据以及融合数据的格式转换，并写入s_bms结构，用于消息发布，注意变量类型(此处必须为short 因为涉及负数数据)：
#include <ros/ros.h>
#include <memory>
#include <string>
#include <csignal>
#include <geometry_msgs/Twist.h>
#include "can/can.h"
#include "std_msgs/UInt8MultiArray.h"
#include "std_msgs/MultiArrayDimension.h"
#define S_FLAG 0
#define R_FLAG 1
double g_csb_dis[4] = {5.0,5.0};
static union {
char c[4];
unsigned long l;
}endian_test;
#define ENDIANNESS ((char)endian_test.l)
PVCI_CAN_OBJ frame;
Quaternion q;
S_BMS s_bms;
S_MOTOR s_motor;
void adis16450(PVCI_CAN_OBJ frame)
{
float acc_x,acc_y,acc_z,gyro,k_angle,a_angle;
//short gryo_x_r = (frame->data[0]<<8) | frame->data[1];
short acc_x_r = (frame->data[0]<<8) | frame->data[1];
// short acc_y_r = (frame->data[2]<<8) | frame->data[3];
short acc_z_r = (frame->data[2]<<8) | frame->data[3];
// short gyro_r = (frame->data[6]<<8) | frame->data[7];
union{
float ram;
unsigned char bytes[4];
}thing;
thing.bytes[0] = frame->data[4];
thing.bytes[1] = frame->data[5];
thing.bytes[2] = frame->data[6];
thing.bytes[3] = frame->data[7];
acc_x = acc_x_r * 0.00025;
// acc_y = acc_y_r * 0.00025;
acc_z = acc_z_r * 0.00025;
// gyro = gyro_r/40.0 + 0.124;
s_bms.acc_x = acc_x;
// s_bms.acc_y = acc_y;
s_bms.acc_z = acc_z;
s_bms.gyro = gyro;
s_bms.angle_acc = a_angle;
s_bms.angle_kalman = thing.ram;
//ROS_INFO("Q: %f %f",thing.ram,acc_x);
}
void c620InfoProc(PVCI_CAN_OBJ frame)
{
s_motor.l_current = frame->data[1] | (frame->data[0]<<8);
s_motor.r_current = frame->data[3] | (frame->data[2]<<8);
s_motor.l_rpm = frame->data[5] | (frame->data[4]<<8);
s_motor.r_rpm = frame->data[7] | (frame->data[6]<<8);
// ROS_INFO("RPM: %d %d",s_motor.l_rpm,s_motor.r_rpm);
}
void bldcInfoProc(PVCI_CAN_OBJ frame)
{
int i = 0;
double vl,vr,vxy;
vl = frame->data[0] | (frame->data[1]<<8);
vr = frame->data[2] | (frame->data[3]<<8);
if(vl > 20000)
vl -= 65536;
if(vr > 20000)
vr -= 65536;
vl = vl/600 * 0.439822964; // pi*d = 0.439822964
vr = vr/600 * 0.439822964;
vxy = (vr-vl) / 2.0 * -1.0;
vth = (vr+vl) / 0.327 * -1.0;
vx = cos(vth) * vxy;
vy = -sin(vth) * vxy;
}
void csbInfoProc(PVCI_CAN_OBJ frame)
{
g_csb_dis[0] = (frame->data[0]<<8 | frame->data[1]) / 1000.0;
g_csb_dis[1] = (frame->data[2]<<8 | frame->data[3]) / 1000.0;
}
void batInfoProc(PVCI_CAN_OBJ frame)
{
s_bms.coulomb = ((frame->data[0]<<8) | frame->data[1]) / 56470.0;
s_bms.voltage = (70.8 * (frame->data[2]<<8 | frame->data[3]) / 65535.0);
s_bms.current = frame->data[4]<<8 | frame->data[5];
s_bms.current = 64.0 * (s_bms.current - 32767.0) / 32767.0;
s_bms.temperature = 510.0 * (frame->data[6]<<8 | frame->data[7]) / 65535.0 - 273.0;
}
int printFrame(PVCI_CAN_OBJ frame,unsigned char flag)
{
int i = 0;
if(flag)
ROS_INFO("RX(0x%08X): ",frame->id);
else
ROS_INFO("TX(0x%08X): ",frame->id);
if(frame->is_extended==0) ROS_INFO(" Standard ");
if(frame->is_extended==1) ROS_INFO(" Extend ");
if(frame->is_rtr==0) ROS_INFO(" Data ");
if(frame->is_rtr==1) ROS_INFO(" Remote ");
ROS_INFO("DLC:0x%02X ",frame->dlc);
for(i=0;i<(frame->dlc);i++)
printf("%02X ",frame->data[i]);
printf("\n");
return 1;
}
void *receiveFrame(void *arg)
{
int r_len = 0;
int j = 0;
int *run = (int*)arg;
VCI_CAN_OBJ rec[2500]; // 接收缓存，设为3000为佳
if('b' == ENDIANNESS) // big endian
ROS_INFO("Big endian");
else
ROS_INFO("Little endian");
while(*run & 0xff)
{
if((r_len = VCI_Receive(VCI_USBCAN2,0,0,rec,2500,100))>0) //调用接收函数，如果有数据，进行数据处理显示。
{
for(j=0; j<r_len; j++)
{
switch(rec[j].id)
{
case 0x382:
bldcInfoProc(&rec[j]);
break;
case 0x383:
csbInfoProc(&rec[j]);
// printFrame(&rec[j],R_FLAG);
break;
case 0x377:
batInfoProc(&rec[j]);
break;
case 0x332:
c620InfoProc(&rec[j]);
break;
case 0x333:
adis16450(&rec[j]);
break;
default:
break;
}
}
}
usleep(10000);
}
pthread_exit(0);
}
int initFrame(PVCI_CAN_OBJ frame) // 需要发送的帧，结构体设置
{
int i = 0;
frame->id = 0x381;
frame->sendType = 0;
frame->is_rtr = 0;
frame->is_extended = 0;
frame->dlc=8;
for(i=0; i<(frame->dlc); i++)
frame->data[i] = 0;
return 1;
}
int getCANInfo(unsigned int DevType) //获取CAN设备信息
{
int i = 0;
VCI_BOARD_INFO pInfo;
if(VCI_ReadBoardInfo(DevType,0,&pInfo)==1)//读取设备序列号、版本等信息。
{
ROS_INFO("Get device info success!");
/*
printf("Serial Number:");
for(i=0; i<20; i++)
printf("%c", pInfo.str_Serial_Num[i]);
printf("\n");
printf("HW Type:");
for(i=0; i<10; i++)
printf("%c", pInfo.str_hw_Type[i]);
printf("\n");
*/
return 1;
}
else
{
ROS_INFO("Get can info error!");
return 0;
}
}
int sendCTLFrame(PVCI_CAN_OBJ frame)
{
if(VCI_Transmit(VCI_USBCAN2, 0, 0, frame, 1) == 1)
{
// printFrame(frame,S_FLAG);
return 1;
}
else
return 0;
}
int initCAN(unsigned int DevType)
{
if(VCI_OpenDevice(VCI_USBCAN2,0,0) == 1) //打开设备
{
ROS_INFO("Open CAN1 success!");
}
else
{
ROS_INFO("Open CAN1 error!");
return 0;
}
//初始化参数，严格参数二次开发函数库说明书
VCI_INIT_CONFIG config;
config.AccCode=0x80000000;
config.AccMask=0xFFFFFFFF;
config.Filter=1; //接收所有帧
config.Timing0=0x00; //波特率125 Kbps 0x03 0x1C
config.Timing1=0x1C;
config.Mode=0; //正常模式
if(VCI_InitCAN(DevType,0,0,&config)!=1)
{
ROS_INFO("Init CAN1 error!");
VCI_CloseDevice(DevType,0);
return 0;
}
if(VCI_InitCAN(DevType,0,1,&config)!=1)
{
ROS_INFO("Init CAN2 error!");
VCI_CloseDevice(DevType,0);
return 0;
}
if(VCI_StartCAN(DevType,0,0)!=1)
{
ROS_INFO("Start CAN1 error!");
VCI_CloseDevice(DevType,0);
return 0;
}
if(VCI_StartCAN(DevType,0,1)!=1)
{
ROS_INFO("Start CAN2 error!");
VCI_CloseDevice(DevType,0);
return 0;
}
return 1;
}
void closeCAN(PVCI_CAN_OBJ frame)
{
VCI_ResetCAN(VCI_USBCAN2, 0, 0); // 复位CAN1通道
usleep(100000);// delay 100ms
VCI_ResetCAN(VCI_USBCAN2, 0, 1);
usleep(100000);
VCI_CloseDevice(VCI_USBCAN2,0); // 关闭设备
free(frame);
}
头文件：
#ifndef CAN_H
#define CAN_H
//接口卡类型定义
#define VCI_PCI5121 1
#define VCI_PCI9810 2
#define VCI_USBCAN1 3
#define VCI_USBCAN2 4
#define VCI_PCI9820 5
#define VCI_CAN232 6
#define VCI_PCI5110 7
#define VCI_CANLite 8
#define VCI_ISA9620 9
#define VCI_ISA5420 10
//CAN错误码
#define ERR_CAN_OVERFLOW 0x0001 //CAN控制器内部FIFO溢出
#define ERR_CAN_ERRALARM 0x0002 //CAN控制器错误报警
#define ERR_CAN_PASSIVE 0x0004 //CAN控制器消极错误
#define ERR_CAN_LOSE 0x0008 //CAN控制器仲裁丢失
#define ERR_CAN_BUSERR 0x0010 //CAN控制器总线错误
//通用错误码
#define ERR_DEVICEOPENED 0x0100 //设备已经打开
#define ERR_DEVICEOPEN 0x0200 //打开设备错误
#define ERR_DEVICENOTOPEN 0x0400 //设备没有打开
#define ERR_BUFFEROVERFLOW 0x0800 //缓冲区溢出
#define ERR_DEVICENOTEXIST 0x1000 //此设备不存在
#define ERR_LOADKERNELDLL 0x2000 //装载动态库失败
#define ERR_CMDFAILED 0x4000 //执行命令失败错误码
#define ERR_BUFFERCREATE 0x8000 //内存不足
//函数调用返回状态值
#define STATUS_OK 1
#define STATUS_ERR 0
#define USHORT unsigned short int
#define BYTE unsigned char
#define CHAR char
#define UCHAR unsigned char
#define UINT unsigned int
#define DWORD unsigned int
#define PVOID void*
#define ULONG unsigned int
#define INT int
#define UINT32 UINT
#define LPVOID void*
#define BOOL BYTE
#define TRUE 1
#define FALSE 0
#if 1
//1.ZLGCAN系列接口卡信息的数据类型。
typedef struct _VCI_BOARD_INFO{
USHORT hw_Version;
USHORT fw_Version;
USHORT dr_Version;
USHORT in_Version;
USHORT irq_Num;
BYTE can_Num;
CHAR str_Serial_Num[20];
CHAR str_hw_Type[40];
USHORT Reserved[4];
}VCI_BOARD_INFO,*PVCI_BOARD_INFO;
//2.定义CAN信息帧的数据类型。
typedef struct _VCI_CAN_OBJ{
UINT id;
UINT timeStamp;
BYTE timeFlag;
BYTE sendType;
BYTE is_rtr; // 是否是远程帧
BYTE is_extended; // 是否是扩展帧
BYTE dlc; // data len
BYTE data[8];
BYTE reserved[3];
}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
//3.定义CAN控制器状态的数据类型。
typedef struct _VCI_CAN_STATUS{
UCHAR ErrInterrupt;
UCHAR regMode;
UCHAR regStatus;
UCHAR regALCapture;
UCHAR regECCapture;
UCHAR regEWLimit;
UCHAR regRECounter;
UCHAR regTECounter;
DWORD Reserved;
}VCI_CAN_STATUS,*PVCI_CAN_STATUS;
//4.定义错误信息的数据类型。
typedef struct _ERR_INFO{
UINT ErrCode;
BYTE Passive_ErrData[3];
BYTE ArLost_ErrData;
}VCI_ERR_INFO,*PVCI_ERR_INFO;
//5.定义初始化CAN的数据类型
typedef struct _INIT_CONFIG{
DWORD AccCode;
DWORD AccMask;
DWORD Reserved;
UCHAR Filter;
UCHAR Timing0;
UCHAR Timing1;
UCHAR Mode;
}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
struct Quaternion
{
float w, x, y, z;
float vx, vy, vz;
float ax, ay, az;
};
typedef struct bms{
float_t voltage;
float_t current;
float_t temperature;
float_t coulomb;
float_t acc_x;
float_t acc_y;
float_t acc_z;
float_t gyro;
float_t angle_acc;
float_t angle_kalman;
}S_BMS;
typedef struct motor{
short l_rpm;
short r_rpm;
short l_current;
short r_current;
}S_MOTOR;
struct EulerAngles
{
float roll, pitch, yaw;
};
extern Quaternion q;
extern P_V_INFO pVinfo;
extern S_BMS s_bms;
extern S_MOTOR s_motor;
extern void framePrint(unsigned int cid,unsigned char *buf);
extern int initCAN(unsigned int DevType);
extern void closeCAN(PVCI_CAN_OBJ frame);
extern int getCANInfo(unsigned int DevType);
extern void *receiveFrame(void *arg);
extern void *can2RecvFrame(void *arg);
extern int sendCTLFrame(PVCI_CAN_OBJ frame);
extern double g_csb_dis[];
extern PVCI_CAN_OBJ frame;
extern double vx,vy,vth;
#ifdef __cplusplus
extern "C" {
#endif
DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
DWORD VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo);
DWORD VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus);
DWORD VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
#ifdef __cplusplus
}
#endif
#endif
#endif
具体编译的时候请参考硬件厂家提供的资料，还有引入对应的库等，有的版本硬件驱动库还存在bug，本文章主要用作自己资料记录，仅贴出部分代码，如有技术方面问题可留言共同交流探讨，仅此而已。
最终效果图：
完全调好之后的忘了拍照，贴几个随便拍的，最终效果在静止状态角度在正负0.005之间变化 （好像是的）。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
