项目——基于GPS的种树机器人路径规划实战及详解
- 一、前言
- 二、设计思路
- 1、坐标系的转换
- 2、输入的区域摆法及关系式
- 3、设计流程图
- 三、算法实现（C语言开发）
- 四、运行结果
一、前言
种树机器人实现精准，高效的种树，离不开精准的定位系统和运动控制系统等，但是好的路径规划能大大提高种树机器人的种树效率。
二、设计思路
1、坐标系的转换
首先，我们利用GPS系统，我们获得机器人当前经纬度和我们要种植的区域四个顶点的经纬度。又由于经纬度坐标系不适合直接使用，需转化为平面坐标系,才可以让机器人高效利用。经纬度坐标系转换为平面坐标系如下:
XY平面被当作Mercator投影平面，Y轴和X轴原点分别设为0纬度和0经度。通过如下公式将地理坐标变换到Mercator投影平面。由于标准电子海图/航道图使用WGS84坐标系，本文使用WGS84参数进行变换。投影基准纬度为0度。
公式参数如下:
X：水平直角坐标，单位为米(m);
Y：纵向直角坐标，单位为米(m);
B：纬度，单位为弧度(rad);
L：经度，单位为弧度(rad);
Bo：投影基准纬度，Bo =0，单位为弧度((rad);
Lo：坐标原点的经度，Lo =0，单位为弧度(rad);
a：地球椭球体长半轴，a=6378137.0000，单位为米(m);
b：地球椭球体短半轴，b=6356752.3142，单位为米(m);
e：第一偏心率；
e’：第二偏心率。
N-卯酉圈曲率半径，单位为米（m）。
2、输入的区域摆法及关系式
3、设计流程图
三、算法实现（C语言开发）
/*
函数功能:
机器人路径规划
输入:
机器人当前位置的x,y坐标，四个顶点的x,y坐标
输出:
机器人最优路径点的x,y坐标
使用注意:
输入的时候，四个顶点的坐标可以无序输入
最好输入的点构成的是一个平行四边形，因为这样会最大限度提取
若输入点不为平行四边形，函数内部会自动提取出平行四边形
*/
#include<stdio.h>
#include<math.h>
int main()
{
int min(int a,int b); //求最小值函数声明
int distance_piont(int x0,int y0,int x1,int y1,int x2,int y2,int x3,int y3,int x4,int y4);//求(x0,y0)到哪一点距离最小声明
int vertex_input[4][2] = {{100,0},{200,0},{200,100},{300,100}};//输入的四个顶点坐标
//机器人当前的位置
int x0 = 200;
int y0 = 200;
float plant_trees_point[65535][2] = {{0,0}}; //种树点数组初始化 ，初始化最多种65535棵树
int tree_distance = 2; //树苗间距
/*将输入的四个顶点坐标，按照如下格式进行排列
1 3
2 4
*/
int vertices_input_Sorted[4][2] = {{0,0}}; //定义初始化排序顶点数组
int mid_Sorted[4][2]; //定义中间存放数据的数组
//将1,2点(横坐标最小值，次小值)混合存入mid_Sorted[0][0]和mid_Sorted[1][0] 且把他们的纵坐标也存入
//将3,4点混合存入mid_Sorted[2][0]和mid_Sorted[3][0] 且把他们的纵坐标也存入
int input_x = 0;
int input_y = 0;
int min_x = vertex_input[0][0];
int min_y = vertex_input[0][1];
int min_next_x = vertex_input[1][0];
int min_next_y = vertex_input[1][1];
if (min_x > min_next_x)
{
min_x = vertex_input[1][0];
min_y = vertex_input[1][1];
min_next_x = vertex_input[0][0];
min_next_y = vertex_input[0][0];
}
for (int i = 0; i < 2; i++)
{
input_x = vertex_input[i+2][0];
input_y = vertex_input[i+2][1];
if (input_x < min_x)
{
//将原来的次小值保存至3或4点
//先把最小值赋给次小值
//再把输入的值赋给最小值
//这两个赋值式顺序不能调换
mid_Sorted[i+2][0] = min_next_x;
mid_Sorted[i+2][1] = min_next_y;
min_next_x = min_x;
min_next_y = min_y;
min_x = input_x;
min_y = input_y;
}
else if ((input_x >= min_x) && (input_x < min_next_x))//不能写数学式min<=input<min_next
{
mid_Sorted[i+2][0] = min_next_x;
mid_Sorted[i+2][1] = min_next_y;
min_next_x = input_x;
min_next_y = input_y;
}
else if(input_x == min_next_x)
{
if(input_y > min_next_y)
{
mid_Sorted[i+2][0] = min_next_x;
mid_Sorted[i+2][1] = min_next_y;
min_next_x = input_x;
min_next_y = input_y;
}
}
else
{
mid_Sorted[i+2][0] = input_x;
mid_Sorted[i+2][1] = input_y;
}
mid_Sorted[0][0] = min_x;
mid_Sorted[0][1] = min_y;
mid_Sorted[1][0] = min_next_x;
mid_Sorted[1][1] = min_next_y;
}
//确定1,2
if(mid_Sorted[0][1] > mid_Sorted[1][1])
{
vertices_input_Sorted[0][0] = mid_Sorted[0][0];
vertices_input_Sorted[0][1] = mid_Sorted[0][1];
vertices_input_Sorted[1][0] = mid_Sorted[1][0];
vertices_input_Sorted[1][1] = mid_Sorted[1][1];
}
else
{
vertices_input_Sorted[1][0] = mid_Sorted[0][0];
vertices_input_Sorted[1][1] = mid_Sorted[0][1];
vertices_input_Sorted[0][0] = mid_Sorted[1][0];
vertices_input_Sorted[0][1] = mid_Sorted[1][1];
};
//确定3,4
if(mid_Sorted[2][1] > mid_Sorted[3][1])
{
vertices_input_Sorted[2][0] = mid_Sorted[2][0];
vertices_input_Sorted[2][1] = mid_Sorted[2][1];
vertices_input_Sorted[3][0] = mid_Sorted[3][0];
vertices_input_Sorted[3][1] = mid_Sorted[3][1];
}
else
{
vertices_input_Sorted[3][0] = mid_Sorted[2][0];
vertices_input_Sorted[3][1] = mid_Sorted[2][1];
vertices_input_Sorted[2][0] = mid_Sorted[3][0];
vertices_input_Sorted[2][1] = mid_Sorted[3][1];
};
//计算矩形各边的长度
int distance_1_2,distance_1_3,distance_2_4,distance_3_4;
distance_1_2 = sqrt(pow((vertices_input_Sorted[1][0] - vertices_input_Sorted[0][0]),2) + pow((vertices_input_Sorted[1][1] - vertices_input_Sorted[0][1]),2));
distance_1_3 = sqrt(pow((vertices_input_Sorted[2][0] - vertices_input_Sorted[0][0]),2) + pow((vertices_input_Sorted[2][1] - vertices_input_Sorted[0][1]),2));
distance_2_4 = sqrt(pow((vertices_input_Sorted[3][0] - vertices_input_Sorted[1][0]),2) + pow((vertices_input_Sorted[3][1] - vertices_input_Sorted[1][1]),2));
distance_3_4 = sqrt(pow((vertices_input_Sorted[3][0] - vertices_input_Sorted[2][0]),2) + pow((vertices_input_Sorted[3][1] - vertices_input_Sorted[2][1]),2));
//图形矩形化，且得到新的合适的矩形边长
int points_2_1 = (int)(min(distance_1_2,distance_3_4) / tree_distance) + 1; //2，1方向树的棵树
int points_2_4 = (int)(min(distance_1_3,distance_2_4) / tree_distance) + 1; //2，4方向树的棵树
distance_1_2 = (points_2_1 - 1) * tree_distance;
distance_3_4 = distance_1_2;
distance_1_3 = (points_2_4 - 1) * tree_distance;
distance_2_4 = distance_1_3;
//得到合适的矩形顶点及其坐标
double theta; //2,4与x轴正的夹角，有正有负
double beta; //beta是12边与24边的夹角，只有正
theta = atan((double)(vertices_input_Sorted[3][1] - vertices_input_Sorted[1][1])/(double)(vertices_input_Sorted[3][0] - vertices_input_Sorted[1][0]));
/*
求12与24的角度beta
1
2 4
cos(beta) = (d12^2 + d24^2 - d14^2)/2*d12*d24
*/
beta = acos(
(double)(pow((vertices_input_Sorted[1][1] - vertices_input_Sorted[0][1]),2) + pow((vertices_input_Sorted[1][0] - vertices_input_Sorted[0][0]),2)
+ pow((vertices_input_Sorted[3][1] - vertices_input_Sorted[1][1]),2) + pow((vertices_input_Sorted[3][0] - vertices_input_Sorted[1][0]),2)
- pow((vertices_input_Sorted[3][1] - vertices_input_Sorted[0][1]),2) - pow((vertices_input_Sorted[3][0] - vertices_input_Sorted[0][0]),2))
/(double)(2 * sqrt(pow((vertices_input_Sorted[1][1] - vertices_input_Sorted[0][1]),2) + pow((vertices_input_Sorted[1][0] - vertices_input_Sorted[0][0]),2))
* sqrt(pow((vertices_input_Sorted[3][1] - vertices_input_Sorted[1][1]),2) + pow((vertices_input_Sorted[3][0] - vertices_input_Sorted[1][0]),2))));
int x1,y1,x2,y2,x3,y3,x4,y4; //定义变量
x2 = vertices_input_Sorted[1][0];
y2 = vertices_input_Sorted[1][1];
x1 = ceil(x2 + distance_1_2*cos(theta + beta)); //向正无穷取整
y1 = ceil(y2 + distance_1_2*sin(theta + beta));
x3 = ceil(x1 + distance_1_3*cos(theta));
y3 = ceil(y1 + distance_1_3*sin(theta));
x4 = ceil(x2 + distance_2_4*cos(theta));
y4 = ceil(y2 + distance_2_4*sin(theta));
// 找出距离机器人最近的顶点，并记录机器人依次要走的数据点的xy坐标
// 让机器人先行不变，沿着列方向走，列方向走完了，就走再走行方向，依次循环。
switch(distance_piont(x0,y0,x1,y1,x2,y2,x3,y3,x4,y4))
{
case 1:
{
int plant_number = 1;
float plant_x = x1;
float plant_y = y1;
//存入机器人起始点
plant_trees_point[plant_number-1][0] = x0;
plant_trees_point[plant_number-1][1] = y0;
for(int i = 0;i < points_2_1;i++)
{
for(int j = 0;j < points_2_4;j++)
{
plant_trees_point[plant_number][0] = plant_x;
plant_trees_point[plant_number][1] = plant_y;
if(j < points_2_4 - 1)
{
if(i%2 == 0)
{
plant_x += tree_distance*cos(theta);
plant_y += tree_distance*sin(theta);
}
else
{
plant_x -= tree_distance*cos(theta);
plant_y -= tree_distance*sin(theta);
}
}
plant_number++;
}
plant_x -= tree_distance*cos(theta + beta);
plant_y -= tree_distance*sin(theta + beta);
}
break;
}
case 2:
{
int plant_number = 1;
float plant_x = x2;
float plant_y = y2;
//存入机器人起始点
plant_trees_point[plant_number-1][0] = x0;
plant_trees_point[plant_number-1][1] = y0;
for(int i = 0;i < points_2_1;i++)
{
for(int j = 0;j < points_2_4;j++)
{
plant_trees_point[plant_number][0] = plant_x;
plant_trees_point[plant_number][1] = plant_y;
if(j < points_2_4 - 1)
{
if(i%2 == 0)
{
plant_x += tree_distance*cos(theta);
plant_y += tree_distance*sin(theta);
}
else
{
plant_x -= tree_distance*cos(theta);
plant_y -= tree_distance*sin(theta);
}
}
plant_number++;
}
plant_x += tree_distance*cos(theta + beta);
plant_y += tree_distance*sin(theta + beta);
}
break;
}
case 3:
{
int plant_number = 1;
float plant_x = x3;
float plant_y = y3;
//存入机器人起始点
plant_trees_point[plant_number-1][0] = x0;
plant_trees_point[plant_number-1][1] = y0;
for(int i = 0;i < points_2_1;i++)
{
for(int j = 0;j < points_2_4;j++)
{
plant_trees_point[plant_number][0] = plant_x;
plant_trees_point[plant_number][1] = plant_y;
if(j < points_2_4 - 1)
{
if(i%2 == 0)
{
plant_x -= tree_distance*cos(theta);
plant_y -= tree_distance*sin(theta);
}
else
{
plant_x += tree_distance*cos(theta);
plant_y += tree_distance*sin(theta);
}
}
plant_number++;
}
plant_x -= tree_distance*cos(theta + beta);
plant_y -= tree_distance*sin(theta + beta);
}
break;
}
case 4:
{
int plant_number = 1;
float plant_x = x4;
float plant_y = y4;
//存入机器人起始点
plant_trees_point[plant_number-1][0] = x0;
plant_trees_point[plant_number-1][1] = y0;
for(int i = 0;i < points_2_1;i++)
{
for(int j = 0;j < points_2_4;j++)
{
plant_trees_point[plant_number][0] = plant_x;
plant_trees_point[plant_number][1] = plant_y;
if(j < points_2_4 - 1)
{
if(i%2 == 0)
{
plant_x -= tree_distance*cos(theta);
plant_y -= tree_distance*sin(theta);
}
else
{
plant_x += tree_distance*cos(theta);
plant_y += tree_distance*sin(theta);
}
}
plant_number++;
}
plant_x += tree_distance*cos(theta + beta);
plant_y += tree_distance*sin(theta + beta);
}
break;
}
}
//写Excel表
FILE *fp = NULL ;
fp = fopen("E:\\gaohongzhi.csv","w");
for (int i=0;i<65535;i++)
{
//把原来数据中的无用元素给剔除掉
if(((int)plant_trees_point[i][0] == 0)&&((int)plant_trees_point[i][1] == 0))
if(((int)plant_trees_point[i+1][0] == 0)&&((int)plant_trees_point[i+1][1] == 0))
break;
fprintf(fp,"%f,%f\n",plant_trees_point[i][0],plant_trees_point[i][1]);
}
fclose(fp);
return 0;
}
//求两个数的最小值
int min(int a,int b)
{
if(a >= b)
{return b;}
return a;
}
//求(x0,y0)到哪一点距离最小
int distance_piont(int x0,int y0,int x1,int y1,int x2,int y2,int x3,int y3,int x4,int y4)
{
float d_01 = sqrt(pow((x0 - x1),2) + pow((y0 - y1),2));
float d_02 = sqrt(pow((x0 - x2),2) + pow((y0 - y2),2));
float d_03 = sqrt(pow((x0 - x3),2) + pow((y0 - y3),2));
float d_04 = sqrt(pow((x0 - x4),2) + pow((y0 - y4),2));
if((d_01 <= d_02)&&(d_01 <= d_03)&&(d_01 <= d_04))
return 1;
else if((d_02 <= d_01)&&(d_02 <= d_03)&&(d_02 <= d_04))
return 2;
else if((d_03 <= d_01)&&(d_03 <= d_02)&&(d_03 <= d_04))
return 3;
else
return 4;
}
四、运行结果
将代码输出的csv文件中的数据转移到xlsx（EXCEL）中，进行matlab绘图.
matlab绘图代码:
G = xlsread('gaohongzhi.xlsx','Sheet1','A:B');
plot(G(:,1),G(:,2))得到的部分结果如下:
