>假设我有一个数组
sensor_data=[10,0,5,1,10,1,20,1,20,1,15]
现在在这个:
0 表示机器人/无人机已右转,并且
1 表示机器人/无人机已左转。
其余数字是行进的距离。
因此,根据上述阵列,机器人/无人机首先行进 10 厘米的距离。然后它向右转。右转后,机器人/无人机行驶 5 厘米,然后向左行驶。左转后,它会行驶 10 厘米,依此类推。所以最初机器人/无人机在 (0,0(。然后它直线行进,即沿 y 方向行进。
因此,坐标将为 (0,10(。然后在右转并行驶 5 厘米后,坐标将是 (-5,10(。按照这样的模式,其余坐标是:(-5,20(、(15,20( 和 (15,0(。可以编写哪些代码,以便可以从上述给定数组生成这些坐标。
咀嚼这个直到你想通,哈哈。
import numpy as np
from numpy import cos,sin,pi
import matplotlib.pyplot as plt
# Convert data into floats, for
sensor_data = tuple(map(lambda x: float(x),[10,0,5,1,10,1,20,1,20,1,15]))
# Start at 0,0 in a 2D plane and start out in x-Direction
Starting_Position = np.array((0.,0.))
Starting_Direction = np.array((1.,0.))
def Rotation_Matrix(direction):
'''Can be expanded for any angle of rotation in a 2D plane. Google rotation matrix in 2D space.'''
a = {'left':pi/2,'right':-pi/2}[direction]
matrix = np.array(((round(cos(a),7),round(-sin(a),7)),
(round(sin(a),7),round(cos(a),7))))
return matrix
def DronePosition(Number_input,Current_Position,Current_Direction):
if Number_input == 1.:
New_Direction = Current_Direction.dot(Rotation_Matrix('left'))
New_Position = Current_Position
elif Number_input == 0.:
New_Direction = Current_Direction.dot(Rotation_Matrix('right'))
New_Position = Current_Position
else:
New_Position = Current_Position + Current_Direction*Number_input
New_Direction = Current_Direction
return New_Position,New_Direction
Drone_Path = np.zeros(shape=(len(sensor_data),2))
for step in range(len(sensor_data)):
Drone_Path[step,0] = Starting_Position[0]
Drone_Path[step,1] = Starting_Position[1]
Starting_Position, Starting_Direction = DronePosition(sensor_data[step],Starting_Position,Starting_Direction)
fig, ax = plt.subplots(figsize=(6,6))
ax.plot(Drone_Path[:,0],Drone_Path[:,1])
plt.show()