室外gps+imu小车系列（二）——双天线gps下用MPC控制松灵两轮差速车跑起来

辉——书生意气

已于 2024-02-26 16:24:07 修改

阅读量2.7k

点赞数 9

文章标签： matlab 开发语言 c++

于 2023-06-02 21:28:43 首次发布

本文链接：https://blog.csdn.net/qq_45391544/article/details/130458049

版权

补充：

1.关于双天线的航向精度，可以参照下这张图
在这里插入图片描述 >文献来源《基于双天线 GNSS 的拖拉机自动导航关键技术研究及系统集成应用》

前言

公式部分自己随便找份文献或者博客都可以看下。我在做这部分时候发现都是前篇一律的各种公式介绍，或者拿个matlab跑下仿真，但在落地时候，这些资料意义不大。
让两轮差速车跑起来其实就是：实时姿态获取（位置，速度，航向）、路径规划、路径跟踪（mpc）。
参考博客：MPC路径跟踪原理推导

平台

某双天线设备、松灵公司的tracer_mini系列小车、一台装有vm虚拟机的电脑（配置的melodic版本的ros、matlab2022a,cmake version 3.22.0-rc1,Python 3.9.9，VS2019)各种串口线、PCAN(或者用松灵送的usb转can模块）

准备工作

写个脚本启动小车串口，小车地盘节点，gps话题发布

//为了方便后续使用，建议把启动脚本放在桌面:
//创建shell脚本
touch mini.sh

打开脚本

sudo gedit mini.sh

在脚本输入下述代码，并保存。保存退出保存的文件

#！/bin/bash
#pcan init
sudo modprobe gs_usb
sudo ip link set can0 up type can bitrate 500000
#candump can0

#延时几秒
sleep 1


#开启小车节点
source /opt/ros/melodic/setup.sh 
source /home/hui/MINI_ws/devel/setup.bash
#启动小车控制节点和GPS话题发布节点
roslaunch control_car start.launch 
exit 0

其中start.launch中的内容为：

<launch>
    <!--启动小车节点-->
    <include file="$(find tracer_bringup)/launch/tracer_robot_base.launch" />
    <!-- 启动GPS话题发布-->
    <node pkg="pub_gps" type="pub_Android" name="pubGps"/> 
    <!--start MPC-->
    <node pkg="mpc_diff_new" type="mpc_diff_new" name=""/>
    <!--数据包记录-->
     <node pkg="rosbag" type="record" name="bag_record" args="-a -o /home/hui/Documents/car_data/1.bag"/>
</launch>

当启动后使用rqt或者rostopic list查看下发布的话题，确保cmd_vel和/GpsData话题发布成功。然后就可以进行下一步的路径跟踪了。
注意：建议按照本系列（一）中绑定下串口

MPC路径跟踪部分

一些碎碎嘴的话

MPC原理不加叙述，建议查看古月居中关于matlab和ros联合仿真的相关课程MATLAB+ROS开发MPC轨迹跟踪算法 • 董昊天和b站的视频：路径规划与轨迹跟踪系列算法学习_第13讲_模型预测控制(MPC)法。感谢课程老师的详细介绍和答疑。我做的其实是改了老师的模型和换了传感器。主要涉及到实际落地场景中一些经验和GPS双天线导航的基本知识。
双天线实时航向获取是通过本系列第一个里发布的话题，根据双天线安装位置的不同，可以分为双天线垂直于车辆前进方向（右边为主天线）、平行于前进方向两种。我这里的双天线是垂直于安装在小车前进方向的。
双天线设备输出的航向角根据不同厂家的定义可以采用不同的协议。我这里是以主天线指向从天线（右天线->左天线）的向量与y轴正方向的夹角，y轴正方向为起点，顺时针方向绕动[0°，360°）。但在实际计算目标路径的目标航向时候，由于要投影到本地坐标系下，目标航向角一般是以x轴正方向为起点，逆时针为正，顺时针为负，范围为[-pi,pi)的角度。这就导致了双天线输出的航向角并不能直接用于导航计算。这里我们可以画图说明下。
其中G1为主天线，G2为从天线，东北天坐标系下，y轴指向正北，x轴指向正东，yaw为设备输出航向角（度），框出来的为最终带入导航里的弧度制角度。注意区分车头方向，导航是以车头方向为追踪参考的：
在这里插入图片描述
车头方向反过来绕一圈同样满足上述结论的，即最终用于控制的角度大小以设备输出的180°分割点。

放一张测试航向时候现场图帮助理解，北方是用手机的指南针测量出来的。这时候设备输出的航向值为283°左右。

导航系统组成

导航系统主要包含：实时数据获取部分+路径规划部分+路径跟踪部分+数据分析部分
在这里插入图片描述

其中：
①init.m——在确保ros里roscore并连接网络后，通过ifconfig获取到ip,然后在init.m中改成自己的ip。
在这里插入图片描述
②generatePath.m——用于路径规划算法，这里我用函数一个个封装，自己写自己的路径规划算法后注意在这个文件的最上面按照我的格式进行添加下，否则其他界面没法调用这里的函数

③main.m——用于路径规划算法的调用，用于生成参考路径和仿真用，同时生成下述的mat文件用于后续的simulink调用 x.mat,y.mat,theta.mat 在这里插入图片描述

④analyseData.m——是配合后面ros实时跑的时候录制的rosbag包，用于分析结果用。
⑤adrc.slx——一个网上找的adrc模块，准备用于控制纵向速度的，但这里我没加。
⑥mpc_diff_new.slx——主程序，整个导航系统都在这里。
在这里插入图片描述
整天框架如图所示。
其中前面main.m中生成的mat文件在MPC路径跟踪中直接由file from模块导入

写这个导航系统的目的，是为了能够仿真+实物操作能够很好的结合，结构清晰，针对一个大团队，有人负责的融合定位，位姿估计，有人负责路径规划，有人负责路径追踪。但彼此之间交流是最没效率的事，能用文本搞定的就用文本搞定，能自己搞定就自己搞定。同时自己负责部分的东西可以随时落地，仿真测试。对于新手或者老手都比较适合。

仿真

仿真包含两部分：matlab仿真+ros（小乌龟/gazebo）仿真。纯matlab仿真有时候看不出来啥问题，只有实际上机才会发现算法合不合适，利用ros的仿真功能，我在gazebo里也添加了小车模型和小乌龟。用于仿真验证，可以极大的减少实机调试的时间
在这里插入图片描述

这里是用的里程计作为传感器，后续再慢慢添加双天线gps插件，主要没有现成得，我要自己写，不会机械
这里用了里程计作为传感器，后续再慢慢添加双天线gps插件进行仿真，主要没有现成的，我又不会机械，不会画。

实物

放一小段跑直线的视频，参数没咋调好，但起码完成了走个直线的功能：给定起始点经纬度，让它距离起点一定距离上线跑到终点停下来
（放在我这个csdn账号的视频栏里，由于写到这里时候还没给我审核通过，我就懒得等到审核通过放链接了）
在这里插入图片描述

具体的代码，我准备暂时放实验室内部使用，等改进差不多，后续再添加自己做的课题，组合导航部分，再进行开源。期待它能够被喜欢和被使用。算作我第一个开源项目。

后续的计划

准备把西工大严老师那套组合导航移植一点到这套导航系统里。用自己笔记本电脑就可以控制车跑严老师那套岂不是美哉？再后续就是加点纵向控制？再把自己单天线导航系统移植进去？反正基础框架搭建好了，后面就是填充就成了。但愿在自己学生生涯结束前（但愿能申请到心仪的学校的博士），把这套系统弄成国内最好的一套教学导航系统。集合matlab仿真+ros仿真+实车,那我再封装成虚拟机镜像，然后直接安装，岂不是美滋滋。做一个快乐的技术人。