odometry based motion model 基于里程计的机器人移动模型

odometry_based_motion_model.py

# -*- coding: utf-8 -*-
"""
https://blog.csdn.net/varyshare/article/details/100061173
@author: 知乎@Ai酱
"""
import numpy as np
def odometry_motion_model():
    # 机器人初始位置
    (x,y,theta) = (0,0,0)
    """
    机器人控制命令：
    1. 转90度
    2. 直行1米
    """
    theta_cmd = np.pi/2
    distance_cmd = 1
    
    # 机器人移动模型
    scale_factor1 = 0.09 # 用于调节机器人转动操作的噪声大小
    theta_predict = theta + theta_cmd + np.random.normal(0, scale_factor1*theta_cmd)
    
    scale_factor2 = 0.06 # 用于调节机器人直行操作的噪声大小
    distance_predict = distance_cmd + np.random.normal(0, scale_factor2*distance_cmd)
    x_predict = x + distance_predict*np.cos(theta_predict)
    y_predict = y + distance_predict*np.sin(theta_predict)
    
    return x_predict,y_predict

x_arr = []
y_arr = []

for _ in range(5000):
    x_predict,y_predict = odometry_motion_model()
    x_arr.append(x_predict)
    y_arr.append(y_predict)
    
import matplotlib.pyplot as plt
plt.scatter(0,0)
plt.scatter(x_arr,y_arr,c='r',s=1)
plt.show()