遗忘越来越快
之前做仿真时学看ros，然后休息了几个月，现在要做试验了，感觉已经忘得差不多了，所以体会到了做笔记的重要性，因此这里做个试验笔记。
参考资料:http://www.autolabor.com.cn/book/ROSTutorials/
对于试验，本质就是搭建一个机器人，从控制的角度看，机器人系统可以分为：传感系统、控制系统、驱动系统、执行机构。
执行机构: 执行机构只要满足能在蔗田环境下工作，采用四个直流编码电机带动四个主动轮进行行走，由于执行机构比较简单，不再做单独介绍。
驱动系统: 电池、arduino 以及电机驱动模块；
控制系统: 树莓派；
传感系统: 编码器、单线激光雷达、IMU；
其中，执行机构与驱动系统构成了机器人底盘。
执行机构和驱动系统
执行结构
硬件介绍
底盘是淘宝买的四驱mini小车，由于上位机是树莓派下的ROS，因此为了省事，把重点放在算法的研究上，驱动控制板选择了另一家和ros深度配合的驱动板，该驱动板基于stm32，并且集成了imu惯性传感器，所以把原来买的IMU退了。
经过仔细阅读说明书，发现小车底盘和深度定制的ros驱动板不太兼容，目前有两种方案：
1.驱动板和树莓派ros深度定制，例程丰富，开发手册详细，但是其配的小车不太适合我的的试验环境
2.例程很粗糙，开发手册没有，对应的小车比较适合我们的试验环境
最终我选择了第二种，因为首先试验要保证的是硬件条件能满足试验环境，对于软件环境，我觉得可以多看多学，慢慢研究，并且我也不是完全没基础，对于1中的教程有的显得太过于多余。对于方案2，要考虑的是使用arduino进行四驱小车的pid控制，对于imu信息的接收，可以由arduino承担，也可以直接由树莓派承担，最终的目的就是树莓派中的ros里有imu数据的话题，有编码器的话题，同时考虑将他们如何做成odom信息。
STM32控制器程序执行图
初始化和任务对应的代码接线
驱动系统
对于I2C,其实还有个更新的库，但是这里驱动板的示例程序是基于上面的，为了节约时间，新库以后有时间再研究，新库：
驱动板使用代码学习：
ros_arduino_bridge
上述代码只有驱动板与arduino交互的驱动，对于arduino与ros的交互，则需要自己编写才行，好在有相关的开源项目，因此在上面进行二次开发即可。我只使用其中的和ros进行交互的部分，对于其中的底盘控制，舵机控制则不使用。
rosserial arduino
ros_arduino-bridge问题较多，换成研究下rosserial arduino试试
1.安装Arduino（树莓派没安装桌面环境，这步可以忽略，编译上传在安装桌面环境的电脑上进行即可）
sudo apt-get install arduino
2.启动arduino并进行相关设置（树莓派没安装桌面环境，这步可以忽略，编译上传在安装桌面环境的电脑上进行即可）
arduino
3.安装软件包rosserial arduino
sudo apt-get install ros-melodic-rosserial-arduinosudo apt-get install ros-melodic-rosserial
4.编译ros_lib文件，并复制到arduino libraries里（这里是负责编译和上传的电脑的arduino的库里，我的是Windows上的，压缩成zip,然后导入库）
编译：
rosrun rosserial_arduino make_libraries.py [这里写输出目录，中括号要去掉]
在windows上下载下来
然后压缩成zip,在arduino里导入
5.测试，创建helloworld话题
#include <ros.h>
#include <std_msgs/String.h>
ros::NodeHandle nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup()
{
nh.initNode();
nh.advertise(chatter);
}
void loop()
{
str_msg.data = hello;
chatter.publish( &str_msg );
nh.spinOnce();
delay(1000);
}
然后插入arduino编译上传后把arduino插到树莓派上
接着在树莓派上分别开终端运行
终端1：
roscore终端2：（这里硬件端口根据实际情况修改）
rosrun rosserial_python serial_node.py /dev/ttyACM0终端3：
rostopic echo /chatter如下图
参考文章
https://www.guyuehome.com/38503
实测电机参数
经过实际测试，电机数据和淘宝上以及卖家给的说明书都不太一致（淘宝的坑？），但是应该可以用。
| 平均每10圈的脉冲数 | 5850 |
| 每一圈脉冲数 | 585 |
| 轮子直径 | 67.5mm |
| 轮子周长 | 212.06mm |
| 两个脉冲之间，小车前进距离 | 0.36mm |
知道以上参数，可以编写函数来实现测速，即单位位移除以单位时间。
测速函数如下：
long start_time = millis();//一个计算周期的开始时刻，初始值为 millis();
long interval_time = 50;//一个计算周期 50ms
double current_vel[4];
int32_t start_EncodeTotal[4];//构造用于储存开始时的脉冲数的数组
int32_t current_EncodeTotal[4];//构造用于储存当前脉冲数的数组
void get_current_vel()
{
long right_now = millis();
long past_time = right_now - start_time;//计算逝去的时间
if(past_time >= interval_time){//如果逝去时间大于等于一个计算周期
//1.禁止中断
// noInterrupts();
//2.计算转速 转速单位可以是秒，也可以是分钟... 自定义即可
WireReadDataArray(MOTOR_ENCODER_TOTAL_ADDR,(uint8_t*)current_EncodeTotal,16);//获取当前的转动量
for(int i=0;i<4;i++)
{
current_vel[i]=(current_EncodeTotal[i]-start_EncodeTotal[i])*0.3625/past_time;//求当前各个轮子的速度，单位m/s
}
//3.重置计数器
for(int i=0;i<4;i++)
{
start_EncodeTotal[i]=current_EncodeTotal[i];//求当前各个轮子的速度，单位m/s
}
//4.重置开始时间
start_time = right_now;
//5.重启中断
// interrupts();
Serial.println("四个轮子的速度分别为：");
Serial.print(current_vel[0]);Serial.print("\t");Serial.print(current_vel[1]);Serial.print("\t");Serial.print(current_vel[2]);Serial.print("\t");Serial.print(current_vel[3]);Serial.println("\t");
}
}同时在setup中加入以下代码，获取开始时的转动脉冲数
WireReadDataArray(MOTOR_ENCODER_TOTAL_ADDR,(uint8_t*)start_EncodeTotal,16);//获取开始时的转动量编码器重置函数和各个电机速度控制函数如下
void rest_encode()//编码器计数清零
{
EncodeTotal[0]=0,EncodeTotal[1]=0,EncodeTotal[2]=0,EncodeTotal[3]=0;//数组赋值0
WireWriteDataArray(MOTOR_ENCODER_TOTAL_ADDR,(uint8_t*)EncodeTotal,16);//把驱动板上的编码器计数器归0
start_EncodeTotal[0]=0;start_EncodeTotal[1]=0;start_EncodeTotal[2]=0;start_EncodeTotal[3]=0;//避免清0后第一次测速数据异常
}
void contrl_vel(int16_t *val) //指定各轮的速度，单位：m/s
{//要把速度转化成脉冲数每10ms,result=val*10/0.3625
int8_t vel2pulse[4]={0,0,0,0};
for(int i=0;i<4;i++){
//vel2pulse[i]=val[i]/100/0.3625;//这里会先把val[i]/100取整型再除以0.3625
vel2pulse[i]=val[i]/36.25;
}
WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR,vel2pulse,4);//由于输入的数据只能为整型，控制精度低，后期考虑自己写pid,用pwm
Serial.println("四个轮子的目标速度（单位mm/s）为：");
Serial.print(val[0]);Serial.print("\t");Serial.print(val[1]);Serial.print("\t");Serial.print(val[2]);Serial.print("\t");Serial.print(val[3]);Serial.println("\t");
Serial.println("四个轮子的目标脉冲(单位脉冲/10ms)为：");
Serial.print(vel2pulse[0]);Serial.print("\t");Serial.print(vel2pulse[1]);Serial.print("\t");Serial.print(vel2pulse[2]);Serial.print("\t");Serial.print(vel2pulse[3]);Serial.println("\t");
for(int i=0;i<4;i++)
{
delay(2000);
get_current_vel();
}
}
上面我们把各个轮子单位时间内的脉冲数转化成了小车各个轮子单位时间内能行进的距离，但是要想让机器人运动，我们还是要进行机器人的运动学分析。
机器人运动学分析
参考文献1：https://mp.weixin.qq.com/s/Fzrpn5_3TB6apqG2Ds1v5Q
参考文献2：王雪松. 四驱轮式移动机器人运动控制系统研究与设计[D].上海电机学院,2016.
把机器人的目标速度转换成每个电机的目标速度，这个叫做机器人的运动学分析。
通过机器人各轮的速度求解出机器人沿X、Y、Z轴方向的速度，叫做运动学正解；
通过机器人沿X、Y、Z轴方向的速度求解出机器人各轮的速度，叫做运动学逆解。
细看卖家的最后的四个式子感觉推导的有点尴尬，好在通过阅读资料，我做了简化，控制时保证左侧轮子速度一致，右侧轮子速度也一致，就可以简化为两轮差速控制，如下：
简化正运动学模型是基于虚拟左右驱动轮的速度来计算几何质心COM的速度，可表示为
简化逆运动学模型是基于几何质心COM的速度分解出左右驱动轮的速度，可表示为
若采用公式（9-10）来描述四轮驱动移动机器人(SSMR)，这里有两个点需要进一步讨论：
问题：在正运动学模型（9）中，左右虚拟轮的线速度vl和vr如何得到？
根据公式（7）可知，两左（或右）侧轮的纵向线速度大小与左（或右）虚拟轮的线速度相同。但两轮的实际转速可能不同。
从简化实际运动控制的角度出发，两左（或右）侧轮应该尽可能保持一致。这里做一个假象实验：若机器人的两前轮的速度相同，两后轮的速度相同，但前后轮的速度不同，机器人肯定是以介于前轮速度和后轮速度之间的某一速度直线运动，可想象前轮拖着后轮加快运动，后轮扯着前轮减缓运动。
所以无论前后轮的速度如何变化，机器人只会以一个速度运动，由于前后轮速度不相等会引起轮胎与地面在纵向上同时存在滚动和滑动摩擦，这会加速磨损轮胎，且不利于准确运动控制。所以，结论是尽可能保持两左（或右）侧轮速度相同，用数学语言描述就是：、
式中，[wM1 wM2 wM3 wM4]表示对应轮子的转动角速度，[RM1 RM2 RM3 RM4]表示对应的轮子的半径。
4
问题：虚拟轮间距dLR具体是多少？
这也是四轮驱动移动机器人(SSMR)简化模型最难的一个参数，是一个随着工况变化的参数，且无法求得解析解。在参考文献[2]中给出的方法，是引入了无量纲参数γ：
式中，dwb表示机器人的轮间距。
问题则转化为如何求γ，该参数与机器人的总负载、轮胎与地面的相对摩擦系数、转弯半径及质心位置都是有关系，是一个非常复杂的参数，所以常用的方法就是做实验，控制不再改动的机器人在特定地面上做差速转向运动，采集到多组实验数据后，拟合估算γ。
再回过头来看四轮驱动移动机器人(SSMR)运动学模型，基于公式（9-12）可知
正运动学模型为
逆运动学模型为
踩坑
1.断电时一般先断开arduino和上位机（电脑或者树莓派的连线），再断开驱动板和电池的连线，特别是电机失控，要断电急停的时候，否则可能导致驱动板损坏。
2.arduino可以给驱动板供电，驱动板也可以给反向给arduino供电，不知道我是不是在某次操作中，进行了1中的操作，后来发觉驱动板不能给arduino供电了，检查原来是r如下图，重新在后面5v标注的焊点处焊上排针，耶，也可以用。
3.不知道是不是1的操作还把SCL 和SDA的上拉电阻给烧了（新板在路上，保险起见，又买了块），在调试程序中，发现Wire.endTransmission() 函数无响应，并且是间歇性出现，有时重新断开所有电源好了，但是过一会又死在那，然后查了资料（https://arduino.stackexchange.com/questions/5339/wire-endtransmission-hangs），把SCL 和SDA外接一个4.7K的5V上拉电阻，貌似再也没有卡住了
4.自带的示例程序里pwm控制根本没用，轮子只是抖一下，我以为是这板子太垃圾，不支持，于是就用了他的闭环控制，然后我自己又套了层PID，相当于套了2层PID,反应慢。突然我想起了，或许他示例的pwm只是给个脉冲，用循环持续的给脉冲，不就可以了吗，改了下，果真可以。唉，基础不牢，地动山摇。
5.但是使用4中的pwm控制发觉不能低速，最低速度也五秒3米左右，远高于试验要求，然后使用自带的闭环控制，速度可以慢一点，但是还是偏大（整体来说，买的电机减速比偏低），无奈，只能更改ros-arduino-bridge里的pid函数了
6.发觉就算加了上拉电阻，也会卡死，嗐，等新驱动板到了再试
7.为了验证是否是因为基于wire.h库的I2c挂起导致ros_arduino_bridge无法连接，编译上传不含有i2c的ros_arduino_bridge项目到arduino试试。经验证，果真是，嗐，卖家配的示例真坑
8.使用github另外一位大佬改编的IC2库（https://github.com/rambo/I2C），成功移植之前的代码，然后成功连接，现在就是改ros_arduino_python里的关于pid的代码，把他屏蔽掉就行
9.实测在连接书梅派串口后，i2c总线会卡死，导致连接不上，就算使用了新的I2C.h库，超时可以跳出，可以连接上，但三仍然功能不正常，暂时找不到解决方案，时间关系，以后再研究。
10.问题解决，arduino的供电来自树莓派，驱动板的供电来自电源，两个之前的vcc不用连接，连接gnd即可，在arduino程序启动的时候可以先不建立I2c通信，后面再初始话硬件然后通信
控制系统
使用ros控制电机转动从而控制小车运动，之前尝试有太多bug,并且也不是我试验的重点，花大量时间不值得，所以就修改为使用蓝牙遥控器控制，和arduino上的驱动系统相集成，因此arduino上一共集成了以下代码：I2C通信代码 红外控制电机代码 ros_arduino_bridge代码
首先，检测遥控器的按键编码：（灯没必要接，接好红外传感器就行）
然后编写控制逻辑代码，最后集成的时候在相关地方加入控制函数即可
#include <IRremote.h>
int RECV_PIN = 11;//11号引脚为数据引脚
long accelerate = 0x00FF629D;//2号数字键,前进/加速
long decelerate = 0x00FF02FD;//5号键，后退/减速
long left= 0x00FFA25D;//1号数字键，左转
long right= 0x00FFE21D;//3号数字键，右转
long carstop = 0x00FF38C7;//停止，OK键
IRrecv irrecv(RECV_PIN);
decode_results results;
void setup() {
pinMode(RECV_PIN, INPUT);
Serial.begin(9600);
irrecv.enableIRIn(); // Start the receiver
}
int on = 0;
unsigned long last = millis();
void loop() {
if (irrecv.decode(&results))
{
// If it's been at least 1/4 second since the last
// IR received, toggle the relay
if (millis() - last > 250)
{
on = !on;
digitalWrite(13, on ? HIGH : LOW);
//dump(&results);
}
if (results.value == accelerate )
{
Serial.println("前进");
}
if (results.value == decelerate )
{
Serial.println("后退");
}
if (results.value == left )
{
Serial.println("左转");
}
if (results.value == right )
{
Serial.println("右转");
}
if (results.value == carstop )
{
Serial.println("停止");
}
last = millis();
irrecv.resume(); // Receive the next value
}
}传感系统
sudo apt install arp-scan -ysudo arp-scan -I wlp3s0 --localnetubuntu镜像下载：http://cdimage.ubuntu.com/releases/
由于我用到的ros arduino bridge 不支持我的ros-noetic,所以树莓派上必须安装ros-melodic,笔记本上可以用原来的不变。而ros-melodic是在ubuntu18.04环境下，故树莓派的安装版本为：ubuntu18.04+rosmelodic,但是官方的软件刷ubuntu18.04设置得wifi不生效，于是https://blog.csdn.net/qq_30613365/article/details/120739069
树莓派上部署ROS：
一行代码搭建机器人开发环境(ROS/ROS2/ROSDEP)
wget http://fishros.com/install -O fishros && . fishros使用树莓派官方的工具刷入ubuntu20.04，刷入之前设置好用户名、密码、wifi等信息（这里我设置成收手机的，因为以后试验也是用手机共享热点当成无线路由器），同时记得打开ssh，为了方便远程连接(这里主要是不想电脑也连接手机wifi)，我们进入系统后首先安装zerotier，然后再使用上面小鱼的一键安装命令安装无桌面版ros，安装结束后添加到环境变量：
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc然后用小鱼工具箱安装rosdepc
rosdepc init
rosdepc update最后roscore启动，没错误，ros环境部署成功。
踩坑
创建工作空间并初始化
mkdir -p field_trial/src
cd field_trial/
catkin_make如果提示没安装catkin，安装又发现和ros的依赖冲突，这里可以先卸载ros然后先安装catkin，再安装ros
1.catkin_make报错：IOError: [Errno 13] Permission denied:
https://answers.ros.org/question/246820/ros_arduino_bridge-reconnection-error/
https://blog.csdn.net/gongdiwudu/article/details/123983828
原因是创建工作空间时用了sudo或者在root下，解决方案：
sudo chown $USER: -R /home/pi/ros_catkin_ws
2.diagnostic_updater“,CMake did not find diagnostic_updater.
缺少相应的依赖包，可以：
sudo apt-cache search diagnostics updater #在apt源里寻找同样名字的包根据返回安装合适的依赖包
sudo apt install ros-humble-diagnostic-updater激光雷达相关
连接上激光雷达，并认当前的 USB 转串口终端并修改权限，
USB查看命令:
ll /dev/ttyUSB*
如果找不到，可能是数据线问题，换一根数据线即可。
使用
ls -l /dev/ttyUSB0发现，当前用户没有使用它的权限，因此要添加权限
sudo usermod -a -G dialout 你的用户名之后要重启才能生效
由于本次试验有多个外接设备，为了不造成串口混乱，可以绑定串口设备
雷达驱动包安装：
进入src工作空间，然后下载驱动包
git clone https://github.com/slamtec/rplidar_ros返回工作空间，并catkin_make编译，并source ./devel/setup.bash，把功能包里的launch文件的设备端口改成之前映射好的端口。
测试
启动驱动包里的例程，如果不报错且能正确获取到当前雷达的信息，则没问题，我这里暂时没有做分布式，树莓派上安装的是无桌面版本，所以就不能通过rviz查看。
roslaunch rplidar_ros rplidar.launch
通过 rostopic list和rostopic echo /scan也能看到相关信息输出
IMU相关
参考资料：https://blog.csdn.net/ganjb1/article/details/118153554
参考项目：https://github.com/COONEO/Arduino_Jetson_nano_ROS_Car/tree/melodic/Jetson_nano_ROS_code/catkin_ws/src/imu_901
如何将 IMU （惯性测量单元） 传感器与 Arduino 对接
开源！手把手教你搭建Arduino+英伟达Jetson的ROS小车（中）
https://blog.csdn.net/zhuoyueljl/article/details/75453808
1.在windows电脑上使用商家提供的上位机软件设置IMU参数
2.然后把设备插到ubuntu设备的usb口上
安装串口功能包：
sudo apt-get install ros-melodic-serial然后创建imu信息读取解析的相关功能包（参考教程：https://blog.csdn.net/qqliuzhitong/article/details/114384297）
catkin_create_pkg serial_demo roscpp serial能再屏幕上打印后根据芯片的开发手册对串口数据进行解码，最后编写数据发布代码发布/imu
相关代码如下：
编码器相关
我的是arduino接收编码器的信息，然后arduino再通过ros_arduino_bridge和编码器交互，在ros端，我主要使用的是ros_arduino_python 下的arduino_drive.py.
查看权限
ls -l /dev/ttyACM0授权（前面雷达部分已经弄过就可以不弄了）
sudo usermod -a -G dialout your_user_nameros_arduino_bridge是依赖于python-serial功能包的
sudo apt-get install python-serial在ros_arduino_msg中添加自定义编码器消息
Header header
int16 voltage
int32[4] enc然后按照ros自定义消息的步骤进行配置和编译，最后在arduino_drive.py中进行解析和发布话题
odom里程计
odom里程计数据由编码器和imu数据融合而成，https://blog.csdn.net/baimei4833953/article/details/80768762
imu tool:
http://wiki.ros.org/imu_complementary_filter?distro=noetic
论文：https://www.mdpi.com/1424-8220/15/8/19302
两种融合的方法
（1） 一种简单的方法
从imu得到的数据为一个相对角度(主要使用yaw，roll和pitch 后面不会使用到)，使用该角度来替代由编码器计算得到的角度。
这个方法较为简单，出现打滑时候因yaw不会受到影响，即使你抬起机器人转动一定的角度，得到的里程也能正确反映出来
（2）扩展的卡尔曼滤波
官方提供了个扩展的卡尔曼滤波的包robot_pose_ekf，robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态，支持
odom（编码器）
imu_data（IMU）
vo（视觉里程计）
还可以支持GPS
引用官方图片
chatGPT真是个神器：
集成
TF坐标变换
预备知识：https://www.jianshu.com/p/cb99188fec49
在launch文件中增加以下节点的代码：
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_transform_publisher" args="0 0 0 0 0 0 base_link imu_link 100" />
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_static_transform_publisher" args="x y z yaw pitch roll base_link imu_link 100" />
ROS std_msgs/Header 数据含义
https://blog.csdn.net/qq_18676517/article/details/109270525
创建功能包
catkin_create_pkg integration roscpp rospy std_msgs ros_arduino_python usb_cam rplidar_ros功能包下创建launch文件夹，launch文件夹中新建run.launch文件,内容如下：
<!-- 机器人启动文件：
1.启动底盘
2.启动激光雷达
3.IMU
4.启动编码器
5.启动odom
6.录制数据
-->
<launch>
<!-- <include file="$(find ros_arduino_python)/launch/arduino.launch" /> -->
<!-- <include file="$(find usb_cam)/launch/usb_cam-test.launch" /> -->
<!-- 2.启动激光雷达 -->
<include file="$(find rplidar_ros)/launch/rplidar.launch" />
<!-- 6.录制数据 -->
<!-- <node pkg="rosbag" type="record" name="record" args="/imu /odom /scan /tf /tf_static -o /home/daoker/vscode_ws/src/sugarcane_car/bag/onece.bag"/> -->
<node pkg="rosbag" type="record" name="record" args="/scan -o /home/daoker/field_trial/src/integration/bag/onece.bag"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /front_bumper_link /front_bumper_link_depth" />
<node pkg="tf" type="static_transform_publisher" name="odom_to_world" args="0.0 0.0 0.0 0 0 0.0 /odom /world 1000"/>
<node pkg="rosbag" type="record" name="record" args="/imu /odom /scan /tf /tf_static -o /home/daoker/vscode_ws/src/sugarcane_car/bag/onece.bag"/>
</launch>