无人驾驶——1、GNSS+IMU定位（基于三阶互补算法）
简单的搭建工作空间就不说了，从节点的编写开始，详细讲一下怎么进行多线程节点的读取和GNSS及IMU数据的处理以及飞控中三阶互补算法的实现过程。
1.编写我们自己的msg文件，在src目录下构建msg文件，里面创建一个data.msg用于自定义我们要传输的消息类型。这里我定义了2个double型的用于传输GNSS坐标点，3个float型用于传输IMU的数据。
在编写完成之后不要忘记在Cmakelist里面加入声明message_generation
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
serial
message_generation
)
Serial是用于串口读取的，这一部分可以百度找到对应的配置说明。
## Generate messages in the 'msg' folder
add_message_files(
FILES
data.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)与此同时在package.xml中找到下面两句取消注释。
至此，消息配置完成。
2.编写串口读取GNSS与IMU的节点cpp文件
代码如下：（1）GNSS
#include
#include //ROS已经内置了的串口包
#include
#include
#include
#include
using namespace serial;
serial::Serial ser; //声明串口对象
float Naxis_buff[5]={0},Eaxis_buff[5]={0},Naxis_sum=0,Eaxis_sum=0;
unsigned char axis_num=0;
//滑动平均法
void slid_aver(float Naxis,float Eaxis)
{
unsigned char i=0;
if(axis_num<5)
{
Naxis_buff[axis_num]=Naxis;Naxis_sum+=Naxis;
Eaxis_buff[axis_num]=Eaxis;Eaxis_sum+=Eaxis;
axis_num++;
}
else
{
Naxis_sum-=Naxis_buff[0];Eaxis_sum-=Eaxis_buff[0];
for(i=0;i("GNSS", 100);
try
{
//设置串口属性，并打开串口
ser.setPort("/dev/ttyUSB0");
ser.setBaudrate(115200);
serial::Timeout to = serial::Timeout::simpleTimeout(100);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
//检测串口是否已经打开，并给出提示信息
if (ser.isOpen())
{
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
return -1;
}
//指定循环的频率
ros::Rate loop_rate(1);
while (ros::ok())
{
data_fusion::data msg2;
size_t n = ser.available();
std::string gstart = "$GNRMC",gend = "\r\n";
std::string ss,cut;
int i = 0, sta = -1, nd = -1;
if (n) {
ss=ser.read(ser.available());
//截取GNSS对应区域的字符串
sta = ss.find("A,"); nd = ss.find(",N");
cut=ss.substr(sta+2,nd);
std::cout< 这里要注意，serial读出来的GNSS数据我们以string格式接收，GNSS输出格式为NEMA格式，因此GNRMC~\r\n的区域就是我们的坐标存的区域，再从其中截取N坐标和E坐标。
截取的方式可以采用find来找到对应的区域，并用substr进行截取，最后再采用atof（）函数将字符串转化为浮点数，这里atof（）函数由于有效位数的原因，后几位数会不太准确，如果想要准确读到全部位数可以自己写一个转化函数。
最后采用滑动平均法来对获得的坐标数据进行滤波
效果如下：
（2）IMU
#include
#include //ROS已经内置了的串口包
#include
#include
#include
#include
#include
using namespace serial;
serial::Serial ser; //声明串口对象
int main(int argc, char** argv)
{
unsigned char i=0,j=0,k=0,send_buff[]={0x01,0x03,0x00,0x02,0x00,0x06,0x64,0x08};
unsigned char buff[20]={0};
int Z_rate=0,Y_acc=0,Z_angle=0;float Z=0.41,Y=1.514,A=5.12;
serial::parity_t parity = parity_even;
//初始化节点
ros::init(argc, argv, "imu");
//声明节点句柄
ros::NodeHandle nh;
//发布主题
ros::Publisher IMU_pub = nh.advertise("IMU", 100);
try
{
//设置串口属性，并打开串口
ser.setPort("/dev/ttyUSB1");
ser.setParity(parity); //偶校验
ser.setBaudrate(115200);
serial::Timeout to = serial::Timeout::simpleTimeout(100);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
//检测串口是否已经打开，并给出提示信息
if (ser.isOpen())
{
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
return -1;
}
//指定循环的频率
ros::Rate loop_rate(2);
while (ros::ok())
{
data_fusion::data msg1;
size_t n = ser.available();
ser.write(send_buff,8);
if (n) {
ser.read(buff,n);
Z_rate=buff[6]*pow(16,6)+buff[5]*pow(16,4)+buff[4]*pow(16,2)+buff[3];Z=(float)(Z_rate-150000)/100;
Y_acc=buff[10]*pow(16,6)+buff[9]*pow(16,4)+buff[8]*pow(16,2)+buff[7];Y=(float)(Y_acc-20000)/1000;
Z_angle=buff[14]*pow(16,6)+buff[13]*pow(16,4)+buff[12]*pow(16,2)+buff[11];A=(float)(Z_angle-18000)/100;
printf("Z_rate=%.2f°/s,Y_acc=%.3fg,Z_angle=%.2f°\n",Z,Y,A);
msg1.Z_rate=Z;msg1.Y_acc=Y;msg1.Z_angle=A;
IMU_pub.publish(msg1);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
} 这里我采用的IMU是TL740款，存在偶校验码串口形式，对应配置即可。最后取出来的数据进行相应转化，得到Z轴偏转角度，Y轴的前进加速度，Z轴偏转速率。
3.3阶互补滤波实现定位
3阶互补滤波的实现框图如下：
通过读取IMU的数值进行转化，得到相应的X轴（对应E），Y轴（对应N）两个方向的加速度，进行积分后得到对应的移动距离，这个移动距离与GNSS读取的坐标点的距离进行比较，能够得到相应误差，将这种误差反馈回IMU加速度、速度、距离三个分量，从而实现互补，由于存在积分，系数的关系：Ka 对应代码如下： 最后，别忘记配置Cmakelists文件，在末尾添加对应项： #include add_executable(GNSS src/GNSS.cpp)
add_dependencies(GNSS ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(GNSS
${catkin_LIBRARIES}
)
add_executable(IMU src/IMU.cpp)
add_dependencies(IMU ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(IMU
${catkin_LIBRARIES}
)
add_executable(order_comple src/order_comple.cpp)
add_dependencies(order_comple ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(order_comple
${catkin_LIBRARIES}
)