读取编码器的计数脉冲转化成ROS的odom数据

1，理论模型
1.1 原理
如下图所示，蓝色圆形区域为小车的地盘，左右轮之间的间距为wheel_dist，注意算法中的所有的单位，全部是国际标准单位。移动机器人编码器，安装在左右轮上，机器人地底盘的坐标系为base_link坐标系，里程计坐标系为odom。

当移动机器人移动时，假设运动到如下图所示位置。图中的，V为移动小车中心点的运动速度方向，dxy_avg为移动小车中心点的运动距离。

根据编码器的差值，可知前一时刻与当前时刻编码器增量的计数脉冲，已知小车车轮行走一个周长的长度的编码器计数脉冲个数，可以推算出，一个计数脉冲的移动距离。代码实现如下：

# (4096 * 7.5) dright is right boot position
dright = (right_enc - enc_right) * pi * D / (4096 * 7.5)
# dleft is left boot positon
dleft = (left_enc - enc_left) * pi * D / (4096 * 7.5)

其中，dright 为右轮的移动距离，dleft 为左轮的移动距离right_enc - enc_right为右轮编码器脉冲个数的增量，left_enc - enc_left为左轮编码器脉冲个数的增量，参数pi * D / (4096 * 7.5)，需要依据自己的小车测量出，编码器一个计数脉冲对应的行走距离（单位m）。

1.2 向中心点换算
因为在使用里程计向上层的开发中，往往对于小车模型，通常会考率成中心点的运动模型，即大致认为时一个质点，这是不准确的。算法如下：

 dxy_ave = (dright + dleft) / 2.0
 dth = (dright - dleft) / wheel_track
 vxy = dxy_ave / dt
 vth = dth / dt

其中，dxy_ave为左右轮的移动距离均值，认为是中心点的移动距离；dth 为通过为中心点的转动角度，通过弧长公式 θ=l/R \theta = l/Rθ=l/R，中心点速度V=vxy 为位移对时间的微分v=dx/dt v= dx/dtv=dx/dt，vth 为角速度，为转动角度对时间的倒数vth=θ/dt vth= \theta/dtvth=θ/dt

1.3 base_link到odom坐标系的转化
在1.2节中的换算数据都是在小车底盘坐标系base_link上进行的，但是我们最终需要将这些数据转化成odom坐标系上的数据发送出去。代码如下所示：

 dx = cos(dth) * dxy_ave
 dy = -sin(dth) * dxy_ave
 x += (cos(th) * dx - sin(th) * dy)
 y += (sin(th) * dx + cos(th) * dy)

其中，dxy_ave为中心点的在dt时间内的位移；dx&&dy分别为中心点出的位移在base_link坐标系xoy轴上的投影；然后通过两个坐标系的投影转换，即旋转矩阵求解在odom坐标系上的x’轴和y’轴的位移；前一时刻的位移加当前位移增量，即为当前位移。
可以理解成，在base_link坐标系上的x轴和y轴投影的位移，需要投影到odom坐标系上的x’轴和y’轴上，所以，dx′=dx∗cos(θ)−dy∗sin(θ) dx'=dx*cos(\theta)-dy*sin(\theta)dx 
′
 =dx∗cos(θ)−dy∗sin(θ)，即在odom坐标系上的x’轴上的位移是由base_link坐标系上的x轴和y轴投影的位移的合成，至于为什么是“-”号，画出投影的位移分解图，就可以很清楚的看出来了。；同理可得dy′=dy∗sin(θ)+dy∗cos(θ) dy'=dy*sin(\theta)+dy*cos(\theta)dy 
′
 =dy∗sin(θ)+dy∗cos(θ)。

位移的分解图，如下所示

2， python源代码主要部分
下面包含了，发送odom的方式，看到这一步的大概都是理解ROS的原理了，就不细细的说明了。

now = rospy.Time.now()
            # record get encode left&right in first time, must initial enc_right&left
            get_encode_count += 1
            if get_encode_count == 1:
                enc_right = right_enc
                enc_left = left_enc
            dt = now - then
            then = now
            dt = dt.to_sec()

            # Calculate odometry
            if enc_left is None:
                dright = 0
                dleft = 0
            else:
                # (4096 * 7.5) dright is right boot position
                dright = (right_enc - enc_right) * pi * D / (4096 * 7.5)
                # dleft is left boot positon
                dleft = (left_enc - enc_left) * pi * D / (4096 * 7.5)

            enc_right = right_enc
            enc_left = left_enc

            dxy_ave = (dright + dleft) / 2.0
            dth = (dright - dleft) / wheel_track
            vxy = dxy_ave / dt
            vth = dth / dt
            rospy.loginfo('dxy_ave = %f', dxy_ave)
            if dxy_ave != 0:
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                x += (cos(th) * dx - sin(th) * dy)
                y += (sin(th) * dx + cos(th) * dy)

            if dth != 0:
                th += dth
            rospy.loginfo('x = %f, y = %f' % (x, y))
            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(th / 2.0)
            quaternion.w = cos(th / 2.0)

            # Create the odometry transform frame broadcaster.
            odomBroadcaster.sendTransform(
                (x, y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(),
                base_frame,  # sub-ordi
                "odom"  # farthe-ordi
            )

            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = x
            odom.pose.pose.position.y = y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

            odomPub.publish(odom)

转自 https://blog.csdn.net/SimileciWH/article/details/84976187