1、首先声明v,w分别为cmd_vel中给出的机器人实际线速度与角速度
2、输出:lp,rp分别为左右轮电机输出的转速与最大转速的百分比
其他参数:
wb:左右轮轴距
d:轮子直径
ratio:电机减速比
rmp:电机每分钟转的圈数 (我们的电机是3000转/min)
先根据v,w算出左右轮的速度vl,vr
vl=v-w*(wb/2)
vr=v+w*(wb/2)
之后计算lp,rp
lp = (vl / ((rpm / 60) / ratio) * (PI * d)) * 100
rp = (vr / ((rpm / 60) / ratio) * (PI * d)) * 100