阅读说明：本技术 一种基于等弦分割采样的球形灌木球面插补算法 (Spherical shrub spherical interpolation algorithm based on equal-chord segmentation sampling ) 是由 李传军 陈玺 王立萍 杨保鹏 于 2021-08-03 设计创作，主要内容包括：本发明公开一种基于等弦分割采样的球形灌木球面插补算法,给定球心坐标P-(0),球心半径R。步骤：首先求得球面插补辅助点P-(1)-P-(6),球面可以看做多条径向半圆组成,使用本发明算法求得P-(1)-P-(4)所在球面大圆m+1插补点坐标用于计算机器人在每段径向半圆姿态；设每段径向半圆起始点与终止点为P-(start),P-(end),令P-(start)＝P-(5),P-(end)＝P-(6),P-(j)为P-(1)-P-(4)所在球面大圆第j个插补点,令j＝0；使用本发明算法求解圆弧P-(star)P-(j)与圆弧P-(j)P-(end)组成的径向半圆中n+1个插补点坐标；求解机器人在圆弧P-(star)P-(j)与圆弧P-(j)P-(end)插补过程中姿态。本发明降低了运算量,提高了运行速度。(The invention discloses a spherical shrub spherical interpolation algorithm based on equal chord segmentation sampling, which gives a spherical center coordinate P 0 The radius of the sphere center R. The method comprises the following steps: firstly, a sphere interpolation auxiliary point P is obtained 1 ‑P 6 The spherical surface can be regarded as a plurality of radial semicircles, and the P is obtained by using the algorithm of the invention 1 ‑P 4 The coordinate of the interpolation point m +1 of the spherical surface is used for calculating the radial semicircular posture of the robot at each section; setting the starting point and the ending point of each radial semicircle to be P start ，P end Let P start ＝P 5 ,P end ＝P 6 ，P j Is P 1 ‑P 4 The j of the jth interpolation point of the great circle of the sphere is 0; solving the arc P using the algorithm of the present invention star P j And the arc P j P end N +1 interpolation point coordinates in the formed radial semicircle; solving robot in arc P star P j And the arc P j P end Attitude during interpolation. The invention reduces the operation amount and improves the operation speed。)

一种基于等弦分割采样的球形灌木球面插补算法

技术领域

本发明属于机器人控制技术领域，特别是一种基于等弦分割采样的球形灌木球面插补算法。

背景技术

城市化的扩张,造成城市的绿化不足。城市绿化中,球形灌木以其生命力旺盛、易存活、造型美观和四季常绿等优点,在绿化建设中得到青睐。目前，球形灌木主要依赖于花匠手持修剪刀具凭借经验进行修剪，其它岗位人员无法进行兼任，而且球形灌木修剪存在一定的季节性，造成用工不稳定，管理成本上升，导致球形灌木修剪的效率低，维护成本高。不同规格的球形灌木依赖于视觉尺寸进行修剪，把握不够精准，而且修剪不规范后，只能越修越小，无法达成批量化、规格化，常常造成参差不齐的现象。

目前，关于机器人关于圆弧和球面的加工，大都采用矢量法，坐标转换法，积分法。所求的插补点需要经过坐标变换,积分运算，微分运算增加了运算量。需要一种算法简单，运算量小，运算速度快，插补坐标精度高的方法。

发明内容

本发明所要解决的技术问题是：提供一种基于等弦分割采样的空间圆弧插补算法，并把这种算法扩展到球形灌木球面插补中，求得机器人在修剪过程中空间球面插补点坐标和位姿。本算法没有理论误差，提高了插补坐标精度，运算量小，提高了运算速度。

为解决上述技术问题，本发明提供的技术方案是：一种基于等弦分割采样的空间圆弧插补算法，并把这种算法扩展到球形灌木球面插补中，已知球形灌木球心相对于基坐标系位置P₀＝(x₀,y₀,z₀)以及球的半径R。

步骤1，由已知条件，求解球面插补辅助点坐标P₁-P₆。其中点P₁，P₂；P₃，P₄；P₅，P₆均为直径对点。

设θ＝atan2(y₀,x₀)。则P₁-P₆的坐标为：

步骤2，基于等弦分割采样的球形灌木球面插补算法是把等弦分割采样空间圆弧算法应用到空间球面插补。使用等弦分割采样空间圆弧算法求得P₁-P₄所在球面大圆m+1个插补点坐标。等弦分割采样空间圆弧算法原理为：

此算法分为两种情况圆弧的圆心角0<θ<π和π<θ<2π。

(1)当圆弧圆心角为0<θ<π时

设给定某圆弧起始点为P_s＝(x_s,y_s,z_s),终止点为P_e＝(x_e,y_e,z_e),圆心坐标为P₀＝(x₀,y₀,z₀)，圆弧半径为R。则弦P_sP_e长度L为:

根据圆弧起始点P_s、终止点P_e求插补点P_i的空间坐标，首先对弦P_sP_e等分，等分精度为△L。插补次数n为

圆弧上第i个插补点为P_i，则直线P₀P_i与弦P_sP_e的交点为P_i'。P_sP_i'长度为

P_sP_i'＝i×△L

P_i'的坐标为

直线P₀P_i'长度为

插补点P_i坐标为

(2)当圆弧圆心角为π<θ<2π时

设给定某圆弧起始点为P_s＝(x_s,y_s,z_s),终止点为P_e＝(x_e,y_e,z_e),圆心坐标为P₀＝(x₀,y₀,z₀)，圆弧半径为R。可以求得P_e与P_s直径对点坐标P_e'，P_s'坐标。圆弧P_sP_e'，P_e'P_s'，P_s'P_e的圆心角均属于0<θ<π，每一段可根据圆心角0<θ<π时进行插补。

步骤3，球面可以做m段径向半圆，根据步骤2算法求解出P₁-P₄所在球面大圆m+1个插补点用于计算机器人在每段径向半圆姿态。其原理为：

设第j个插补点为P_j＝(x_j,y_j,z_j)。

θ_j为机器人末端单位向量在基座标上的投影夹角。

机器人末端姿态为：

步骤4，求解机器人运动轨迹，算法步骤如下：

设每段径向半圆起始点与终止点为P_start，P_end。P_start＝P₅,P_end＝P₆。P_j为P₁-P₄所在球面大圆第j个插补点。令j＝0。

第一步：根据步骤2的方法求解第一段径向半圆：圆弧P_starP_j与圆弧P_jP_end中n+1个插补点坐标。

第二步：求解机器人在圆弧P_starP_j与圆弧P_jP_end插补过程中姿态。

第三步：当机器人位于P_end并且j≠m+1时，交换P_start，P_end。j＝j+1时返回至第一步求解下一段径向半圆位姿。当机器人位于P_end并且j＝m+1时，结束插补运动。

采用上述技术方案带来的有益效果：

(1)本发明用参数方程表示插补点坐标，插补点均落在球面上，避免了累积误差。

(2)本发明每段圆弧机器人末端姿态相同，无需时刻计算并调整机器人末端姿态，算法简单减小了计算量。

(3)本发明运算过程中不存在坐标变换，没有矩阵，微分，积分运算，提高了运算效率。

附图说明

图1本发明算法流程图

图2本发明球面插补辅助点示意图

图3本发明插补算法圆心角0<θ<π原理示意图

图4本发明劣弧P₁P₃插补点仿真示意图

图5本发明插补算法圆心角π<θ<2π原理示意图

图6本发明P₁-P₄所在球面大圆插补点仿真示意图

图7本发明机器人球面插补运动示意图

具体实施方式

下将结合附图，对本发明的技术方案进行详细说明。

步骤一，如附图2所示，坐标系单位为cm，给定球形灌木球心相对于基座标系位置P₀＝(30,-100,100)，球的半径为30cm。求解辅助点P₁-P₆坐标。

θ＝atan2(y₀,x₀)＝-73.3008°

辅助点P₁-P₆坐标为：

步骤二，使用等弦分割采样空间圆弧算法求得P₁-P₄所在球面大圆m+1个插补点坐标。如图3所示：劣弧P₁P₃起始点P_s坐标为P₁＝(38.6204,-128.7347,100)，终止点P_e坐标为P₃＝(58.7347,-91.3795,100)。圆心坐标为P₀＝(30,-100,100)。圆弧半径为30cm。弦长P_sP_e长度L为

设插补次数50。

圆弧上第i个插补点为P_i，则直线P₀P_i与弦P_sP_e的交点为P_i'。P_sP_i'长度为

P_sP_i'＝i×△L＝i×0.8485

P_i'的坐标为

直线P₀P_i'长度为

插补点P_i坐标为

劣弧P₁P₃上的插补点均已求出。仿真如附图4所示。

劣弧P₁P₃第i次插补点P_i坐标，如表1所示：

i	P<sub>ix</sub>	P<sub>iy</sub>	P<sub>iz</sub>
				0	38.6204	-128.7347	100
1	39.2049	-128.5529	100
				2	39.8092	-128.3510	100
3	40.4333	-128.1273	100
				…	…	…	…
50	58.7347	-91.3795	100

如图5所示：设给定某圆弧起始点为P_s＝(x_s,y_s,z_s),终止点为P_e＝(x_e,y_e,z_e),圆心坐标为P₀＝(x₀,y₀,z₀)，圆弧半径为R，可以求得P_e与P_s直径对点坐标P_e'，P_s'坐标，按此方法可以此求出劣弧P₁P₃,P₃P₂，P₂P₄，P₄P₁所组成的球面大圆，仿真如附图6所示。至此P₁-P₄所在球面大圆插补点均已求出。

步骤3，球面可以做m段径向半圆，根据步骤2算法求解出P₁-P₄所在球面大圆m+1个插补点用于计算机器人在每段径向半圆姿态。

设第j个插补点为P_j＝(x_j,y_j,z_j)。

θ_j为机器人末端单位向量在基座标上的投影夹角。

机器人末端姿态为：

表2为机器人P_j在圆弧P₁P₃P₂中每段径向半圆插补时的姿态。

j	a<sub>xj</sub>	a<sub>yj</sub>	a<sub>zj</sub>
				0	-0.2873	-0.9578	0
1	-0.3068	-0.9518	0
				2	-0.3270	-0.9450	0
3	-0.3478	-0.9376	0
				101	0.2873	0.9578	0

步骤4，求解机器人运动轨迹。

设每段径向半圆起始点与终止点为P_start，P_end。其中P_start＝P₅,P_end＝P₆。P_j为P₁-P₄所在球面大圆第j个插补点。令j＝0。

第一步：根据步骤2的方法求解第一段径向半圆：圆弧P_starP_j与圆弧P_jP_end中n个插补点坐标。

第二步：求解机器人在圆弧P_starP_j与圆弧P_jP_end插补过程中姿态。

第三步：当机器人末端位于P_end并且j≠m+1时，交换P_start，P_end。j＝j+1时返回至第一步求解下一段径向半圆位姿。当机器人位于P_end并且j＝m+1时，结束插补运动。

附图7所示为机器人修剪球形灌木运动仿真过程。