如何得到曲线（如直线、椭圆线）依次经过的栅格索引序列——基于python的一种解决方法

问题描述

在一个项目中需要得到智能体在栅格中的运动轨迹，实质上是求一个曲线依次经过的栅格索引序列的问题。如下图1所示，对曲线 $\Gamma$与栅格地图$S$，则$\Gamma$的运动轨迹为其经过的栅格索引序列 { S 1 , S 2 , . . . . . . , S n } {\{S_1,S_2,......,S_n\}} {S1​,S2​,......,Sn​}。

问题分析

开始的时候觉得这个问题比较复杂，所以先在网上搜索了一下有没有其他人分享的代码。搜索到的内容大部分是图形学中的Bresenham算法，看起来比较复杂。后来又找到一篇很有价值的博客python解决直线过网格问题_numpy_matplotlib。这篇文章解决了直线情形的问题，代码提供得非常完整，作者大佬的解释也十分详尽，主要关注直线情形的朋友可以移步去那篇文章交流。

本文考虑一般情形，针对任意曲线如何得到索引坐标。思路是对曲线采样然后遍历整个栅格地图。

设曲线的参数方程为$$\Gamma =\Gamma (x(t),y(t))$$地图的大小为$[row,col]$，进行两次遍历，伪代码如下。

//首先遍历曲线
for t in Γ:
	//其次遍历栅格地图
	for i in row:
		for j in col:
			judge if Γ(x(t),y(t)) belong to index[j,i]
			judge if index repeat
return index list

这种方法比较暴力，对栅格地图大小有限，曲线采样点数有限的情形可以直接使用，否则求解时间过长，主程序迭代次数多的话需要考虑其他方法进行优化。

参考代码

import numpy as np
import matplotlib.pyplot as plt

def plotSpiral(core, b, theta, sigma, a):
    """绘制椭圆螺线
    core		- 中心坐标(x0,y0)，tuple类型
    b           - 螺距，由side-scan sonar、以及洋流决定
    theta       - 转角，决定螺旋大小（圈数）
    sigma       - 椭圆形状，半短长轴比
    a           - 倾斜度，逆时针，角度输入,初始状态长轴水平，和终点指向
    return
                - 返回路径和轨迹末点坐标
    xy_path     - 路径
    xy_end      - 轨迹末点坐标
    """
    plt.figure()
    plt.axis()
    plt.plot([core[0]], [core[1]], c='red', marker='+', markersize=10)
    the = np.linspace(0, theta * 2 * np.pi, num=theta * 360)
    a=np.radians(a)
    b=b/(2*np.pi) #原文没有仔细说，此处b需要调整
    x = core[0] + b * the * np.cos(the)  *np.cos(a)- b * the * sigma * np.sin(the)*np.sin(a)
    y = core[1] + b * the * sigma * np.sin(the)*np.cos(a)+b * the * np.cos(the)*np.sin(a)
    plt.plot(x, y, c='blue')
    plt.grid(True)
    plt.show()
    xy_path=[[a,b] for a,b in zip(x,y)]
    xy_end=[x[-1],y[-1]]

    return xy_path,xy_end

def gridOval(path,map_size):
    path=np.array(path)
    path_oval=[]
    for k in range(len(path[:,0])):
        for i in range(map_size[0]):
            for j in range(map_size[1]):
                if((path[k][0]>j)and(path[k][0]<(j+1))and(path[k][1]>i)and(path[k][1]<(i+1))):
                    if (len(path_oval)):
                        if(path_oval[-1]!=[j,i]):
                            path_oval.append([j,i])
                    else:
                        path_oval.append([j, i])
    return  path_oval

if __name__=="__main__":
    plt.close("all")
    map_size=[50,50]
    mid_path,mid_end=plotSpiral((25, 25), 3, 6, 1/3,0)
    grid_oval=np.array(gridOval(mid_path,map_size))
    plt.figure()
    plt.plot(grid_oval[:,0],grid_oval[:,1])
    plt.grid(True)
    plt.show()

效果如下图2，3所示。