tomaz-suller / hector_slam_fpga Goto Github PK

View Code? Open in Web Editor NEW

7.0 3.0 0.0 381 KB

FPGA implementation of the hector_slam algorithm using only lidar data

License: BSD 3-Clause "New" or "Revised" License

Python 9.21% VHDL 0.98% SystemVerilog 0.22% Julia 0.77% Jupyter Notebook 88.83%

hector_slam_fpga's Issues

Definir representação numerica

Implementar Simulação via hardware
Analisar os valores
Definir representação

Implementar Operações trigonometricas

Seno (0 a 2π)
Coseno (0 a 2π)
Tangente (0 a π/4)

Sugestãoes de interface

Seno

module op_sin (
  input [`VSIZE-1:0] a,
  output [`VSIZE-1:0] r
);

Coseno

module op_sub (
  input [`VSIZE-1:0] a,
  output [`VSIZE-1:0] r
);

Tangente

module op_mul (
  input [`VSIZE-1:0] a,
  output [`VSIZE-1:0] r
);

Implementação do Hector SLAM iterativo

Rounding instead of truncating indices could reduce error

During conversion from world coordinated to grid indices, we use BinaryFixedPoint.to_int():

hector_slam_fpga/software_simulation/occupancy_grid_dont_draw_lidar.py

Lines 194 to 196 in 19b208f

    
           def world_coordinate_to_grid(coordinate: BinaryFixedPoint) -> BinaryFixedPoint: 
        
               return BinaryFixedPoint.from_numeric( 
        
                   (coordinate>>RIGHT_SHIFTS_WORLD_TO_GRID).to_int())

This function is implemented to truncate the fixed point representation by always rounding it down:

hector_slam_fpga/software_simulation/fixed_point.py

Lines 199 to 200 in 19b208f

    
           def to_int(self) -> int: 
        
               return math.floor(self.to_float())

It is possible that rounding instead of truncating would benefit algorithm execution, at the cost of adding extra logic to compare the world coordinate with half of a grid cell.

Apresentação

Ler os dados a partir de uma memoria
Executar o algoritimo na FPGA fisica
Mostrar cada interação no display

Implementar operações aritimeticas

Soma (+)
Subtração (-)
Multiplicação (*)
Divisão (/)

Sugestãoes de interface

Soma

module op_sum (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r
);

Subtração

module op_sub (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r
);

Multiplicação

module op_mul (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r
);

ou caso demore mais de um ciclo de clock:

module op_mul (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r,
  output buzy,
);

Divisão

module op_div (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r
);

ou caso demore mais de um ciclo de clock:

module op_div (
  input [`VSIZE-1:0] a,
  input [`VSIZE-1:0] b,
  output [`VSIZE-1:0] r,
  output buzy,
);

Implementar operações matriciais

Sugestãoes de interface

Soma de matrizes

module mat_sum
# (
  parameter M,
  parameter N
) (
  input [M[N][`VSIZE-1:0] a,
  input [M[N][`VSIZE-1:0] b,
  output [M][N][`VSIZE-1:0] r
);

Multiplicação por escalar

module mat_escalar_mult
# (
  parameter M,
  parameter N
) (
  input [`VSIZE-1:0] a,
  input [M[N][`VSIZE-1:0] b,
  output [M][N][`VSIZE-1:0] r
);

Multiplicação de matrizes/vetores

-- tal a \in \mathbb{R}^{m \times n} e B \in \mathbb{R}^{n \times p} realiza A \cdot B
module mat_mult
# (
  parameter M,
  parameter N,
  parameter P
) (
  input [M][N][`VSIZE-1:0] a,
  input [N][P][`VSIZE-1:0] b,
  output [M][P][`VSIZE-1:0] r
);

Transposição de matriz

module mat_transpose
# (
  parameter M,
  parameter N
) (
  input [M[N][`VSIZE-1:0] a,
  output [M][N][`VSIZE-1:0] r
);

Inversão de um matriz 3x3

module mat_inv
# (
  parameter M,
  parameter N
) (
  input [M[N][`VSIZE-1:0] a,
  output [M][N][`VSIZE-1:0] r
);

Implementação da occupancy grid

Algoritimo de linha de Bresenham
Numero para posição
Ocuppacy grid

SLAM simulation converts polar coordinates to map frame, not cartesian

During execution of main, the result from lidar is converted to the coordinate system of the map:

hector_slam_fpga/software_simulation/slam.py

Lines 271 to 272 in 3223e58

    
           S = lidar(xi, env, 720, use_inf=False) 
        
           S_map = [np.array([world2map(si[0]), si[1]]) for si in S]

However, the output of lidar is in polar, rather than in cartesian, coordinates:

hector_slam_fpga/software_simulation/slam.py

Lines 17 to 34 in 3223e58

    
           def lidar(xi, env, n_measures=360, use_inf=False): 
        
               sensor_px, sensor_py, psi = xi 
        
               size_x, size_y = env.T.shape 
        
               size_max = max(size_x, size_y) 
        
               acc = [] 
        
               for theta in np.linspace(0, 2*pi, n_measures): 
        
                   theta += psi  
        
                   measure = np.array([np.Inf if use_inf else size_max, theta]) 
        
                   for rho in range(1, size_max+1): 
        
                       check_px = math.ceil(sensor_px + rho*cos(theta)) 
        
                       check_py = math.ceil(sensor_py + rho*sin(theta)) 
        
                       if (0 < check_px <= size_x and  
        
                               0 < check_py <= size_y and  
        
                               not env[check_py, check_px]): 
        
                           measure = np.array([rho-1, theta]) 
        
                           break 
        
                   acc.append(measure) 
        
               return acc

Therefore, this operation is bound to produce invalid results when executed.

	def world_coordinate_to_grid(coordinate: BinaryFixedPoint) -> BinaryFixedPoint:
	return BinaryFixedPoint.from_numeric(
	(coordinate>>RIGHT_SHIFTS_WORLD_TO_GRID).to_int())

	S = lidar(xi, env, 720, use_inf=False)
	S_map = [np.array([world2map(si[0]), si[1]]) for si in S]

	def lidar(xi, env, n_measures=360, use_inf=False):
	sensor_px, sensor_py, psi = xi
	size_x, size_y = env.T.shape
	size_max = max(size_x, size_y)
	acc = []
	for theta in np.linspace(0, 2*pi, n_measures):
	theta += psi
	measure = np.array([np.Inf if use_inf else size_max, theta])
	for rho in range(1, size_max+1):
	check_px = math.ceil(sensor_px + rho*cos(theta))
	check_py = math.ceil(sensor_py + rho*sin(theta))
	if (0 < check_px <= size_x and
	0 < check_py <= size_y and
	not env[check_py, check_px]):
	measure = np.array([rho-1, theta])
	break
	acc.append(measure)
	return acc

tomaz-suller / hector_slam_fpga Goto Github PK

hector_slam_fpga's Issues

Sugestãoes de interface

Seno

Coseno

Tangente

Sugestãoes de interface

Soma

Subtração

Multiplicação

Divisão

Sugestãoes de interface

Soma de matrizes

Multiplicação por escalar

Multiplicação de matrizes/vetores

Transposição de matriz

Inversão de um matriz 3x3

Recommend Projects

Recommend Topics

Recommend Org