利用绝对值编码器计算odom&阿克曼运动学模型

一、绝对值编码器：增量编码器有一个缺点：即当发生电源故障时丢失轴位置。然而，对于绝对编码器来说，即使发生电源故障也不丢失轴位置。绝对编码器由机械位置确定编码，它无需记忆，无需找参考点，而且不用一直计数，什么时候需要知道位置，什么时候就去读取它的位置。这样，编码器的抗干扰特性、数据的可靠性大大提高了。

二、编码器参数：64圈绝对值编码器；12位（4096）即一圈4096个脉冲；Modbus RTU协议。38mm外径/6mm轴。

三、Modbus RTU协议：当通讯命令由发送设备（主机）发送至接收设备（从机）时，符合相应地址码的从机（编码器）接收通讯命令，并根据功能码及相关要求读取信息，如果CRC校验无误，则执行相应的任务，然后把执行结果（数据）返送给主机。返回的信息中包括地址码、功能码、执行后的数据以及CRC校验码。如果CRC校验出错就不返回任何信息。此编码器支持 03H/06H/10H 功能码，分别是读取编码器数据、修改站号、设置原点。
四、计算里程：
编码器值 = 圈数单圈分辨率 + 单圈值。
例如圈数 10 位 1024 圈,单圈 12 位 4096P 编码器。编码器值 = 圈数4096 + 单圈值。
角度转换方法:单圈 0x01D7 转 10 进制为 471,圈数 0x0209 转 10 进制共 521 圈。
单圈角度 = (471/ 1024) * 360° = 165.59°
总角度 = 圈数*360 + 单圈角度

五、串口读取与解析（源码）：

#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include "std_msgs/UInt8.h"
#include "std_msgs/UInt8MultiArray.h"
#include "encode/mobileRobot_msgs.h"
#include "std_msgs/UInt8MultiArray.h"
#include "std_msgs/MultiArrayDimension.h"
#include "string.h"
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

//ros::Publisher read_pub;
ros::Publisher odom_pub;
serial::Serial ser;

    
int main (int argc, char** argv)
{
    
    // initial parameters
    std::string node_name;
    static uint8_t lcmd_buffer[8]={0x01, 0x03, 0x00, 0x00, 0x00, 0x02, 0xC4, 0x0B};  
    //static uint8_t reset[8]={0x01, 0x06, 0x00, 0x02, 0x00, 0x02, 0xA9, 0xCB};
    uint8_t serial_data[9];
    float d=0.36;//轮子直径
    float pai=3.1415925;
    

    // initial parameter service

     ros::init(argc, argv, "encode_node");
     node_name=ros::this_node::getName();
     ROS_INFO("%s is starting...", node_name.c_str());
     ros::NodeHandle nh;
    tf::TransformBroadcaster odom_broadcaster;
    odom_pub = nh.advertise<nav_msgs::Odometry>("my_odom", 5);

     try 
     {
         ser.setPort("/dev/ttyUSB0");
         ser.setBaudrate(9600);
         serial::Timeout to = serial::Timeout::simpleTimeout(1000);
         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 opened");
     }
     else
     {
         return -1;
     }
     ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();
    ros::Rate loop_rate(50); 
      while(ros::ok()) 
      {ser.write(lcmd_buffer,8);
        //ROS_INFO_STREAM("Write to serial port");
        //获取缓冲区的字节数
        size_t n = ser.available();
        bool ifread=false;
        bool ifhead=false;
        int count=0;
        
        if (n!=0)
        {
              uint8_t buffer[1024];
              //读取数据
              n = ser.read(buffer , n);
              for (int i=0; i<n; i++)
              {
                    if(ifhead)
                    {
                        if(buffer[i]==0x03)
                        {
                            ifread=true;
                            count=1;
                        }
                        ifhead=false;
                    }
                    if (buffer[i]==0x01)
                    {
                        ifhead=true;
                    }
                    
                    if (ifread)
                    {           
                        serial_data[count]=buffer[i];
                        count++;
                        // std::cout<<count<<std::endl;
                        if(count>8)
                        {
                            
                             ifread=false;
                             //m.data.resize(9);
                             for (int j=1;j<8;j++)
                             {
                                //m.data[0]=0x01;
                                //m.data[j]=serial_data[j];
                                serial_data[0]=0x01;
                                int Single_cycle_pulse=int(serial_data[3]*256+serial_data[4]);
                                int cycle_number=int(serial_data[5]*256+serial_data[6]);
                                float Total_angle=(Single_cycle_pulse/4096)*360+cycle_number*360;
                                //std::cout << "Total_angle:"<<Total_angle<<" ";
                                float total_odom = pai*d*Total_angle/360;
                                    double x = 0.0;
                                    double y = 0.0;
                                    double th = 0.0;

                                    double vx = 0.1;
                                    double vy = -0.1;
                                    double vth = 0.1;
                                    current_time = ros::Time::now();
                                    double dt = (current_time - last_time).toSec();

                                        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
                                        
                                        geometry_msgs::TransformStamped odom_trans;
                                        odom_trans.header.stamp = current_time;
                                        odom_trans.header.frame_id = "odom";
                                        odom_trans.child_frame_id = "base_link";
                                        odom_trans.transform.translation.x = total_odom;
                                        odom_trans.transform.translation.y = y;
                                        odom_trans.transform.translation.z = 0.0;
                                        odom_trans.transform.rotation = odom_quat;

                                        //send the transform
                                        odom_broadcaster.sendTransform(odom_trans);

                                        //next, we'll publish the odometry message over ROS
                                        nav_msgs::Odometry odom;
                                        odom.header.stamp = current_time;
                                        odom.header.frame_id = "odom";

                                        //set the position
                                        odom.pose.pose.position.x = total_odom;
                                        odom.pose.pose.position.y = y;
                                        odom.pose.pose.position.z = 0.0;
                                        odom.pose.pose.orientation = odom_quat;

                                        //set the velocity
                                        odom.child_frame_id = "base_link";
                                        odom.twist.twist.linear.x = vx;
                                        odom.twist.twist.linear.y = vy;
                                        odom.twist.twist.angular.z = vth;

                                        //publish the message
                                        odom_pub.publish(odom);
                                        last_time = current_time;
                                        

        }
                             
                        }
                    }
              }
        }
        loop_rate.sleep();
      }
}

以上是仅通过一个编码器读出来里程数，要想应用到机器人上，至少需要两个（左右轮），先来分析运动学模型再去买另一个编码器构建里程计模型。
六、阿克曼运动学模型

像汽车一样，前轮转向，后轮驱动的模型叫阿克曼模型。

如上图所示阿克曼转向几何模型， δ o ​和δ i 分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角 δ i 较左前轮胎转角 δ o ​更大。 ℓ w ​为轮距， L 为轴距，后轮两轮胎转角始终为 0 ° 。当以后轴中心为参考点时，转向半径 R为下图中红线。