Estou tentando implementar o planejamento do 'espaço de crença' para um robô que tenha uma câmera como sensor principal. Semelhante ao SLAM, o robô possui um mapa de pontos 3D e é localizado executando a correspondência 2D-3D com o ambiente a cada passo. Para o propósito desta pergunta, estou assumindo que o mapa não muda.

Como parte do planejamento do espaço de crenças, desejo planejar caminhos para o robô que o leva do início ao objetivo, mas de uma maneira que sua precisão de localização seja sempre maximizada. Portanto, eu teria que provar possíveis estados do robô, sem realmente ir para lá, e as observações que o robô faria se estivesse nesses estados que, juntos (me corrijam se estiver errado), formam a "crença" do robô , posteriormente codificando sua incerteza de localização nesses pontos. E então meu planejador tentaria conectar os nós que me dão a menor incerteza (covariância).

Como minha incerteza de localização para esse robô baseado em câmera depende inteiramente de coisas como quantos pontos de recurso são visíveis em um determinado local, o ângulo de direção do robô etc.: preciso de uma estimativa de quão ruim é minha localização em uma determinada amostra seria, para determinar se devo descartá-lo. Para chegar lá, como defino o modelo de medição para isso, seria o modelo de medição da câmera ou algo relacionado à posição do robô? Como 'adivinho' minhas medições de antemão, e como computo a covariância do robô através dessas medições calculadas?

EDIT: A principal referência para mim é a ideia de explorar rapidamente as árvores aleatórias de crenças , que é uma extensão do método Mapas de estradas de crenças . Outro artigo relevante usa RRBTs para planejamento restrito. Neste artigo, os estados são amostrados de forma semelhante aos TRR convencionais, representados como vértices como um gráfico, mas quando os vértices devem ser conectados, o algoritmo propaga a crença do vértice atual para o novo (função PROPAGATE na seção V de 1 ) , e é aqui que estou preso: não entendo completamente como posso propagar a crença ao longo de uma borda sem realmente atravessá-la e obter novas medidas, assim novas covariâncias da localização. O artigo RRBT diz que "as equações de previsão de covariância e expectativa de custo são implementadas na função PROPAGATE": mas se apenas a previsão é usada, como ela sabe, por exemplo, se existem recursos suficientes na posição futura que poderiam melhorar / degradar a precisão da localização?

Use a localização somente de rolamentos para modelar a informatividade da câmera e simular medições com ruído zero (por exemplo, sem inovação).

Por várias razões, essa é realmente uma maneira teoricamente sólida de estimar a informatividade de um caminho.

Existem muitas métricas informativas "sem medições", como a Fisher Information Matrix . Tudo o que você precisa são as posições do robô e as posições dos pontos de referência no mapa para determinar quanta informação sobre a posição do robô seria obtida medindo os locais dos pontos de referência. (Ou vice-versa, a inovação das medições é aplicada ao alvo e ao robô (é SLAM certo?), Portanto, a mesma métrica funciona para ambos).

Eu começaria com um sensor de rolamento, pois esse é um modelo bom e bem aceito de sensor de visão. Descubra o "ruído" nas medições de rolamentos assumindo alguns pixels de erro na localização de recursos no mundo. Deixe o estado do sistema ser a posição do robô mais sua incerteza e, em seguida, faça a amostra de caminhos (como você sugere). De cada posição no caminho amostrado, eu recomputiria a incerteza prevista usando o FIM. Isso não é difícil de fazer, apenas assuma que não há erro nas medições (ou seja, não haverá "inovação" na crença do robô, mas você ainda experimentará uma queda na incerteza representada pela covariância reduzida na estimativa de posição do robô. não atualize os locais ou incertezas dos pontos de referência, apenas para simplificar o problema.

Essa é uma abordagem bem compreendida do que me lembro na minha última revisão desta literatura, mas não aceite minha palavra (revise você mesmo!). Pelo menos isso deve formar uma abordagem de linha de base fácil de simular. Vamos usar o poder da literatura. Você pode ler esta tese para a configuração e equações.

Resumindo

1. $x$$\Sigma$
2. $\Sigma_i$
3. Que o "custo" da trajetória seja uma combinação convexa de progresso em direção à meta e o inverso da covariância (por exemplo, a matriz da informação )

Algumas sutilezas

Use o menor vetor de estado que faça sentido. Se você puder assumir que o robô pode apontar a câmera independentemente do movimento ou possuir várias câmeras, ignore a orientação e apenas rastreie a posição. Vou prosseguir apenas nas posições 2D.

Você terá que derivar o sistema linearizado, mas pode emprestá-lo da tese acima. certifique-se de não se incomodar com a simulação de medições (por exemplo, se você fizer apenas atualizações do EKF com "medições simuladas", assuma que as medições são verdadeiras e sem ruído.

Se aplicarmos a identidade da matriz Woodbury

$n$

$R$$\sum_{i=1}^n H_i^TR^{-1}H_i$

$H$$H$$nx2$$n$$n$$2$$R$$n\times n$$\sigma I_{n\times n}$$\sigma$

Qual é a equação da medição? Está

$t$$r$

Desenrolando a recursão. Eu procederia da seguinte maneira:

1. Escreva um algoritmo de busca de caminho que encontre seu caminho sem considerar a incerteza.
2. $H$
3. $trace(H^TRH)$
4. Observe que o resultado corresponde ao FIM da trajetória (exercício deixado para o leitor) e você determinou corretamente e de maneira teoricamente correta a trajetória mais informativa.